基于arduino的记时机器人.docx
《基于arduino的记时机器人.docx》由会员分享，可在线阅读，更多相关《基于arduino的记时机器人.docx（13页珍藏版）》请在冰豆网上搜索。
基于arduino的记时机器人
//units:
mm;microseconds;radians
//origin:
bottomleftofdrawingsurface
//deleteormarkthenextlineascommentwhendonewithcalibration
//#defineCALIBRATION
//Whenincalibrationmode,adjustthefollowingfactoruntiltheservosmoveexactly90degrees
#defineSERVOFAKTOR620
//Zero-positionofleftandrightservo
//Whenincalibrationmode,adjusttheNULL-valuessothattheservoarmsareatalltimesparallel
//eithertotheXorYaxis
#defineSERVOLEFTNULL1900
#defineSERVORIGHTNULL984
#defineSERVOPINLIFT2
#defineSERVOPINLEFT3
#defineSERVOPINRIGHT4
//liftpositionsofliftingservo
#defineLIFT0980//1080//ondrawingsurface
#defineLIFT1825//925//betweennumbers
#defineLIFT2625//725//goingtowardssweeper
//speedofliftimgarm,higherisslower
#defineLIFTSPEED1500
//lengthofarms
#defineL135
#defineL255.1
#defineL313.2
//originpointsofleftandrightservo
#defineO1X22
#defineO1Y-25
#defineO2X47
#defineO2Y-25
#include//seehttp:
//playground.arduino.cc/Code/time
#include
intservoLift=1500;
Servoservo1;//
Servoservo2;//
Servoservo3;//
volatiledoublelastX=75;
volatiledoublelastY=47.5;
intlast_min=0;
voidsetup（）
{
//Setcurrenttimeonlythefirsttovalues,hh,mmareneeded
setTime（12,18,0,0,0,0）;
drawTo（75.2,47）;
lift（0）;
servo1.attach（SERVOPINLIFT）;//liftingservo
servo2.attach（SERVOPINLEFT）;//leftservo
servo3.attach（SERVOPINRIGHT）;//rightservo
delay（1000）;
}
voidloop（）
{
#ifdefCALIBRATION
//Servohornswillhave90°betweenmovements,paralleltoxandyaxis
drawTo（-3,29.2）;
delay（500）;
drawTo（74.1,28）;
delay（500）;
#else
inti=0;
if（last_min!
=minute（））{
if（!
servo1.attached（））servo1.attach（SERVOPINLIFT）;
if（!
servo2.attached（））servo2.attach（SERVOPINLEFT）;
if（!
servo3.attached（））servo3.attach（SERVOPINRIGHT）;
lift（0）;
hour（）;
while（（i+1）*10<=hour（））
{
i++;
}
number（3,3,111,1）;
number（5,25,i,1）;
number（19,25,（hour（）-i*10）,1）;
number（28,25,11,1）;
i=0;
while（（i+1）*10<=minute（））
{
i++;
}
number（34,25,i,1）;
number（48,25,（minute（）-i*10）,1）;
lift
（2）;
drawTo（74.2,47.5）;
lift
（1）;
last_min=minute（）;
servo1.detach（）;
servo2.detach（）;
servo3.detach（）;
}
#endif
}
//Writingnumeralwithbxbybeingthebottomleftoriginpoint.Scale1equalsa20mmhighfont.
//Thestructurefollowsthisprinciple:
movetofirststartpointofthenumeral,liftdown,drawnumeral,liftup
voidnumber（floatbx,floatby,intnum,floatscale）{
switch（num）{
case0:
drawTo（bx+12*scale,by+6*scale）;
lift（0）;
bogenGZS（bx+7*scale,by+10*scale,10*scale,-0.8,6.7,0.5）;
lift
（1）;
break;
case1:
drawTo（bx+3*scale,by+15*scale）;
lift（0）;
drawTo（bx+10*scale,by+20*scale）;
drawTo（bx+10*scale,by+0*scale）;
lift
（1）;
break;
case2:
drawTo（bx+2*scale,by+12*scale）;
lift（0）;
bogenUZS（bx+8*scale,by+14*scale,6*scale,3,-0.8,1）;
drawTo（bx+1*scale,by+0*scale）;
drawTo（bx+12*scale,by+0*scale）;
lift
（1）;
break;
case3:
drawTo（bx+2*scale,by+17*scale）;
lift（0）;
bogenUZS（bx+5*scale,by+15*scale,5*scale,3,-2,1）;
bogenUZS（bx+5*scale,by+5*scale,5*scale,1.57,-3,1）;
lift
（1）;
break;
case4:
drawTo（bx+10*scale,by+0*scale）;
lift（0）;
drawTo（bx+10*scale,by+20*scale）;
drawTo（bx+2*scale,by+6*scale）;
drawTo（bx+12*scale,by+6*scale）;
lift
（1）;
break;
case5:
drawTo（bx+2*scale,by+5*scale）;
lift（0）;
bogenGZS（bx+5*scale,by+6*scale,6*scale,-2.5,2,1）;
drawTo（bx+5*scale,by+20*scale）;
drawTo（bx+12*scale,by+20*scale）;
lift
（1）;
break;
case6:
drawTo（bx+2*scale,by+10*scale）;
lift（0）;
bogenUZS（bx+7*scale,by+6*scale,6*scale,2,-4.4,1）;
drawTo（bx+11*scale,by+20*scale）;
lift
（1）;
break;
case7:
drawTo（bx+2*scale,by+20*scale）;
lift（0）;
drawTo（bx+12*scale,by+20*scale）;
drawTo（bx+2*scale,by+0）;
lift
（1）;
break;
case8:
drawTo（bx+5*scale,by+10*scale）;
lift（0）;
bogenUZS（bx+5*scale,by+15*scale,5*scale,4.7,-1.6,1）;
bogenGZS（bx+5*scale,by+5*scale,5*scale,-4.7,2,1）;
lift
（1）;
break;
case9:
drawTo（bx+9*scale,by+11*scale）;
lift（0）;
bogenUZS（bx+7*scale,by+15*scale,5*scale,4,-0.5,1）;
drawTo（bx+5*scale,by+0）;
lift
（1）;
break;
case111:
lift（0）;
drawTo（70,46）;
drawTo（65,43）;
drawTo（65,49）;
drawTo（5,49）;
drawTo（5,45）;
drawTo（65,45）;
drawTo（65,40）;
drawTo（5,40）;
drawTo（5,35）;
drawTo（65,35）;
drawTo（65,30）;
drawTo（5,30）;
drawTo（5,25）;
drawTo（65,25）;
drawTo（65,20）;
drawTo（5,20）;
drawTo（60,44）;
drawTo（75.2,44）;
lift
（2）;
break;
case11:
drawTo（bx+5*scale,by+15*scale）;
lift（0）;
bogenGZS（bx+5*scale,by+15*scale,0.1*scale,1,-1,1）;
lift
（1）;
drawTo（bx+5*scale,by+5*scale）;
lift（0）;
bogenGZS（bx+5*scale,by+5*scale,0.1*scale,1,-1,1）;
lift
（1）;
break;
}
}
voidlift（charlift）{
switch（lift）{
//roomtooptimize!
case0:
//850
if（servoLift>=LIFT0）{
while（servoLift>=LIFT0）
{
servoLift--;
servo1.writeMicroseconds（servoLift）;
delayMicroseconds（LIFTSPEED）;
}
}
else{
while（servoLift<=LIFT0）{
servoLift++;
servo1.writeMicroseconds（servoLift）;
delayMicroseconds（LIFTSPEED）;
}
}
break;
case1:
//150
if（servoLift>=LIFT1）{
while（servoLift>=LIFT1）{
servoLift--;
servo1.writeMicroseconds（servoLift）;
delayMicroseconds（LIFTSPEED）;
}
}
else{
while（servoLift<=LIFT1）{
servoLift++;
servo1.writeMicroseconds（servoLift）;
delayMicroseconds（LIFTSPEED）;
}
}
break;
case2:
if（servoLift>=LIFT2）{
while（servoLift>=LIFT2）{
servoLift--;
servo1.writeMicroseconds（servoLift）;
delayMicroseconds（LIFTSPEED）;
}
}
else{
while（servoLift<=LIFT2）{
servoLift++;
servo1.writeMicroseconds（servoLift）;
delayMicroseconds（LIFTSPEED）;
}
}
break;
}
}
voidbogenUZS（floatbx,floatby,floatradius,intstart,intende,floatsqee）{
floatinkr=-0.05;
floatcount=0;
do{
drawTo（sqee*radius*cos（start+count）+bx,
radius*sin（start+count）+by）;
count+=inkr;
}
while（（start+count）>ende）;
}
voidbogenGZS（floatbx,floatby,floatradius,intstart,intende,floatsqee）{
floatinkr=0.05;
floatcount=0;
do{
drawTo（sqee*radius*cos（start+count）+bx,
radius*sin（start+count）+by）;
count+=inkr;
}
while（（start+count）<=ende）;
}
voiddrawTo（doublepX,doublepY）{
doubledx,dy,c;
inti;
//dxdyofnewpoint
dx=pX-lastX;
dy=pY-lastY;
//pathlenghtinmm,times4equals4stepspermm
c=floor（4*sqrt（dx*dx+dy*dy））;
if（c<1）c=1;
for（i=0;i<=c;i++）{
//drawlinepointbypoint
set_XY（lastX+（i*dx/c）,lastY+（i*dy/c））;
}
lastX=pX;
lastY=pY;
}
doublereturn_angle（doublea,doubleb,doublec）{
//cosineruleforanglebetweencanda
returnacos（（a*a+c*c-b*b）/（2*a*c））;
}
voidset_XY（doubleTx,doubleTy）
{
delay
（1）;
doubledx,dy,c,a1,a2,Hx,Hy;
//calculatetrianglebetweenpen,servoLeftandarmjoint
//cartesiandx/dy
dx=Tx-O1X;
dy=Ty-O1Y;
//polarlemgth（c）andangle（a1）
c=sqrt（dx*dx+dy*dy）;//
a1=atan2（dy,dx）;//
a2=return_angle（L1,L2,c）;
servo2.writeMicroseconds（floor（（（a2+a1-M_PI）*SERVOFAKTOR）+SERVOLEFTNULL））;
//calculatejoinrarmpointfortriangleoftherightservoarm
a2=return_angle（L2,L1,c）;
Hx=Tx+L3*cos（（a1-a2+0.621）+M_PI）;//36,5°
Hy=Ty+L3*sin（（a1-a2+0.621）+M_PI）;
//calculatetrianglebetweenpenjoint,servoRightandarmjoint
dx=Hx-O2X;
dy=Hy-O2Y;
c=sqrt（dx*dx+dy*dy）;
a1=atan2（dy,dx）;
a2=return_angle（L1,（L2-L3）,c）;
servo3.writeMicroseconds（floor（（（a1-a2）*SERVOFAKTOR）+SERVORIGHTNULL））;
}
升级会员 