Doing a drawbot...

using servos and arduino

First the results... it really does not work.

Lots of vibrations and errors... but a lot of fun.

Here's the GNU/Octave code to generate the .ino file.

Happy hacking!

clear all

l1=18.5;
l2=20;
d=-15;
n=200;
t=linspace(0,20*pi,n);
x=d+5*cos(t).*exp(-.05*t);
y=5*sin(t).*exp(-.05*t);
alpha(1)=   -4.5980
beta(1)=   4.3070

for i=1:n-1
  aux= newtonsys([alpha(i) beta(i)],[x(i+1) y(i+1)],1e-3,l1,l2);

  alpha(i+1)=aux(1);
  beta(i+1)=aux(2);
end


theta1=-alpha*180/pi-180;
theta2=beta*180/pi-180;


## Write to INO
filename="~/Arduino/examples/servo/servo.ino"

comment_str="This is a comment on the begining of the .ino file ;)";
fid=fopen(filename,'w');
fprintf(fid,'/* %s */\n',comment_str);

header_str="\n#include <Servo.h>\n\nServo Servo1;\nServo Servo2;\n\n";
preamble_str="\nvoid setup()\{\n    Servo2.attach(10);\n    Servo1.attach(9);\}\n\nvoid loop(){\n";

fprintf(fid,'%s',header_str);
fprintf(fid,'%s',preamble_str);

for i=1:n
  fprintf(fid,'%s%f%s',"Servo1.write(",theta1(i),");\n");
  fprintf(fid,'%s%f%s',"Servo2.write(",theta2(i),");\n");
  fprintf(fid,'%s',"delay(100);\n");


end
fprintf(fid,'%s',"}");
fclose(fid);

############################################################
figure(1)
clf
hold on
plot(x,y)
u=l1*cos(alpha)+l2*cos(beta);
v=l1*sin(alpha)+l2*sin(beta);
plot(u,v,'r-')

############################################################
figure(2)
clf
hold on
plot(l1*cos(theta1)+l2*cos(theta2),l1*sin(theta2)+l2*cos(theta2),'-')
plot(l2*cos(beta),l2*sin(beta),'r-')
plot(l1*cos(alpha(1)),l1*sin(alpha(1)),'o')

[t' alpha' beta' theta1' theta2']

function Fv=Ffun(x,xpos,l1,l2)
  Fv(1,1)=xpos(1)-l1*cos(x(1))-l2*cos(x(2));
  Fv(2,1)=xpos(2)-l1*sin(x(1))-l2*sin(x(2));
end

function Jv=Jfun(x,l1,l2)
  Jv(1,1)=l1*sin(x(1));
  Jv(1,2)=l2*sin(x(2));
  Jv(2,1)=-l1*cos(x(1));
  Jv(2,2)=-l2*cos(x(2));
end

function x=newtonsys(xo,xpos,tol,l1,l2)
  Niter=1000;
  x=xo';
  delta=eye(length(xo),1);

  i=0;
  while or(i<=Niter, abs(max(delta))>tol)
      Jv=Jfun(x,l1,l2);
      Fv=Ffun(x,xpos,l1,l2);
      delta=-Jv\Fv;
      x=x+delta;
      i=i+1;
  endwhile
end

Palavras chave/keywords: arduino, draw bot, Octave, ino

Criado/Created: 30-06-2016 [12:08]

Última actualização/Last updated: 10-10-2022 [14:25]

Voltar à página inicial.