Paso 7: El código de Arduino
El código puede copiarse directamente desde aquí:
/*********************************************************************/ Code Author:DFRobot-Kelvin Control Protocol：x100-y100-w100 X Direction:100mm/s Y Direction:100mm/s rotation to the left:100mm/s /*********************************************************************/ #include double AcceleratedSpeed=400,whieelAcceleratedSpeed=500;// double Step=0.03;//15*3.14/200/16 int xPul=25, xDir=23, xEnable=27, yPul=31, yDir=33, yEnable=29, zPul=37, zDir=39, zEnable=35, km_1=8; Metro _Serial_=Metro(20); double xspeed,yspeed,zspeed; static double xSpeed,ySpeed,wheel; long x_t,y_t,z_t; void setup() { // put your setup code here, to run once: Serial3.begin(115200); pinMode(8,OUTPUT); pinMode(xPul,OUTPUT); pinMode(xDir,OUTPUT); pinMode(yPul,OUTPUT); pinMode(yDir,OUTPUT); pinMode(zPul,OUTPUT); pinMode(zDir,OUTPUT); pinMode(xEnable,OUTPUT); pinMode(yEnable,OUTPUT); pinMode(zEnable,OUTPUT); digitalWrite(xEnable,LOW); digitalWrite(yEnable,LOW); digitalWrite(zEnable,LOW); digitalWrite(km_1,HIGH); } void loop() { // put your main code here, to run repeatedly: static long t; if(_Serial_.check()==1) { serial(); Speed(xSpeed,ySpeed,AcceleratedSpeed); // Wheel(wheel); if(xspeed!=0) x_t=int(Step/abs(xspeed)*1000000); if(yspeed!=0) y_t=int(Step/abs(yspeed)*1000000); if(zspeed!=0) z_t=int(Step/abs(zspeed)*1000000); if(xspeed>0) digitalWrite(xDir,HIGH); else digitalWrite(xDir,LOW); if(yspeed>0) digitalWrite(yDir,HIGH); else digitalWrite(yDir,LOW); if(zspeed>0) digitalWrite(zDir,LOW); else digitalWrite(zDir,HIGH); } xMove(xspeed); yMove(yspeed); zMove(zspeed); } void serial() { if(Serial3.available()==0) return; char buf=Serial3.read(); switch(buf) { case 120://x xSpeed=Serial3.parseInt(); break; case 121://y ySpeed=Serial3.parseInt(); break; case 119://w wheel=Serial3.parsedouble(); break; } } void Speed(double XSpeed,double YSpeed,double a) { static double S_XSpeed=0,S_YSpeed=0; if (XSpeed>S_XSpeed) { S_XSpeed=S_XSpeed+a*0.02; if(S_XSpeed>XSpeed) { S_XSpeed=XSpeed; } } else{ S_XSpeed=S_XSpeed-a*0.02; if(S_XSpeedS_YSpeed) { S_YSpeed=S_YSpeed+a*0.02; if(S_YSpeed>YSpeed) { S_YSpeed=YSpeed; } } else{ S_YSpeed=S_YSpeed-a*0.02; if(S_YSpeedwheelspeed) { wheelspeed=wheelspeed+whieelAcceleratedSpeed*0.02; if(_wheelwheelspeed) { wheelspeed=_wheel; } } xspeed=xspeed+wheelspeed; yspeed=yspeed+wheelspeed; zspeed=zspeed-wheelspeed; } void xMove(double xs) { static long x_time=micros(); if(xs!=0) { if(micros()-x_time>=x_t) { digitalWrite(xPul,HIGH); digitalWrite(xPul,LOW); x_time=micros(); } } } void yMove(double ys) { static long y_time=micros(); if(ys!=0) { if(micros()-y_time>=y_t) { digitalWrite(yPul,HIGH); digitalWrite(yPul,LOW); y_time=micros(); } } } void zMove(double zs) { static long z_time=micros(); if(zs!=0) { if(micros()-z_time>=z_t) { digitalWrite(zPul,HIGH); digitalWrite(zPul,LOW); z_time=micros(); } } }
