![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Robot tryout
Dependencies: mbed QEI biquadFilter MODSERIAL FastPWM ttmath Math
Diff: Motor_tryout.cpp
- Revision:
- 21:c826abca79c3
- Parent:
- 19:9c8ab7922191
- Child:
- 22:9f911405e096
diff -r ff45b4e1a475 -r c826abca79c3 Motor_tryout.cpp --- a/Motor_tryout.cpp Fri Oct 25 07:21:41 2019 +0000 +++ b/Motor_tryout.cpp Fri Oct 25 11:23:15 2019 +0000 @@ -49,6 +49,9 @@ float z0=-172; float y0=0; float x0=0; +float x00; +float y00; +float z00; float theta1; float theta2; @@ -57,14 +60,14 @@ float oldtheta2=0; float oldtheta3=0; -float delta_calcangleyz(float x0, float y0, float z0) { - float y2 = y0 + le; +float delta_calcangleyz(float x00, float y00, float z00) { + float y2 = y00 + le; float y1 = f; float z1 = 0.0; - float z2 = z0; - float rje2 = re*re - x0*x0; + float z2 = z00; + float rje2 = re*re - x00*x00; float rje = sqrt(rje2); - float r2 = (y1-y2)*(y1-y2) + (z1-z0)*(z1-z0); + float r2 = (y1-y2)(y1-y2) + (z1-z00)(z1-z00); float r = sqrt(r2); if ((r+rje<rf) || (r + rf <rje) || (rf+rje<r)) { @@ -82,19 +85,25 @@ } } -float delta_calcinverse(float x0, float y0, float z0) { -theta1 = theta2 = theta3 = 0; -theta1 = delta_calcangleyz(x0, y0, z0); - - if (check == 0) { - theta2 = delta_calcangleyz(x0*cos(2/3*pi) + y0*sin(2/3*pi), y0*cos(2/3*pi)-x0*sin(2/3*pi), z0); - theta3 = delta_calcangleyz(x0*cos(2/3*pi) - y0*sin(2/3*pi), y0*cos(2/3*pi)+x0*sin(2/3*pi), z0); - } - -return theta1, theta2, theta3; +float delta_calcinverse(float x00, float y00, float z00) { + theta1 = theta2 = theta3 = 0; + theta1 = delta_calcangleyz(x00, y00, z00); + + if (check == 0) { + x00=x0*cospi+y0*sinpi; + y00=y0*cospi-x0*sinpi; + z00=z0; + theta2 = delta_calcangleyz(x0*cos(2/3*pi) + y0*sin(2/3*pi), y0*cos(2/3*pi)-x0*sin(2/3*pi), z0); + x00=x0*cospi-y0*sinpi; + y00=y0*cospi+y0*sinpi; + z00=z0; + theta3 = delta_calcangleyz(x0*cos(2/3*pi) - y0*sin(2/3*pi), y0*cos(2/3*pi)+x0*sin(2/3*pi), z0); + } + + return theta1, theta2, theta3; } -Ticker pulse; +//Ticker pulse; void pulseget() { counts1 = Encoder1.getPulses(); counts2 = Encoder2.getPulses(); @@ -107,7 +116,80 @@ return steps; } -// DE MAIN FUNCTIE + +float move_steps() { + + theta1 = delta_calcinverse(x0,y0,z0); + pc.printf("\n\r de hoeken zijn(%f, %f, %f)", theta1, theta2, theta3); + pc.printf("\n\r coordinaten(%f, %f, %f)", x0, y0, z0); + + theta1 = theta1 - oldtheta1; + theta2 = theta2 - oldtheta2; + theta3 = theta3 - oldtheta3; + + float steps1 = anglestep(theta1); + float steps2 = anglestep(theta2); + float steps3 = anglestep(theta3); + + // Set the direction of the motors. + if (theta1 < 0) { + motor1_dir.write(1); + } + else { + motor1_dir.write(0); + } + + if (theta2 < 0) { + motor2_dir.write(0); + } + else { + motor2_dir.write(1); + } + + if (theta3 < 0) { + motor3_dir.write(0); + } + else { + motor3_dir.write(1); + } + + int frequency_pwm = 10000; //10 kHz PWM + motor1_pwm.period(1.0/(double)frequency_pwm); // T=1/f + motor1_pwm.write(0.7); // write Duty Cycle + + motor2_pwm.period(1.0/(double)frequency_pwm); // T=1/f + motor2_pwm.write(0.7); // write Duty Cycle + + motor3_pwm.period(1.0/(double)frequency_pwm); // T=1/f + motor3_pwm.write(0.7); // write Duty Cycle + + while (check1 || check2 || check3) { + counts1 = Encoder1.getPulses(); + counts2 = Encoder2.getPulses(); + counts3 = Encoder.getPulses(); + + wait(1.0/500); + + if(abs(counts1)>=abs(steps1)) { + motor1_pwm.write(0); + check1=false; + counts1=0; + wait(1.0/100); + } + if (abs(counts2)>=abs(steps2)) { + motor2_pwm.write(0); + check2=false; + counts2=0; + wait(1.0/100); + } + if (abs(counts3)>=abs(steps3)) { + motor3_pwm.write(0); + check3=false; + //pc.printf("\n\rsteps %f, counts %f", steps3, counts3); + counts3=0; + wait(1.0/100); + } + } int main(void) { pc.baud(115200); @@ -123,158 +205,68 @@ oldtheta1 = theta1; oldtheta2 = theta2; - oldtheta3 = theta3; - + oldtheta3 = theta3; char cc = pc.getc(); - -if (cc=='a') { - check1=true; - check2=true; - check3=true; - - z0=z0+2.0f; - - theta1 = delta_calcinverse(x0,y0,z0); - - theta1 = theta1 - oldtheta1; - theta2 = theta2 - oldtheta2; - theta3 = theta3 - oldtheta3; - - float steps1 = anglestep(theta1); - float steps2 = anglestep(theta2); - float steps3 = anglestep(theta3); - - motor1_dir.write(1); // positief - motor2_dir.write(0); // positief - motor3_dir.write(1); // positief - - if (steps1>0) { - motor1_dir.write(0); // positief +if (cc=='d') { +x0=x0+2.0f; +if (x0>=-75 && x0<=75) { + movefunction (); + } + else { + x0=x0-2.0f; } - if (steps1>0) { - motor2_dir.write(1); // positief - } - if (steps1>0) { - motor3_dir.write(0); // positief - } - - int frequency_pwm = 10000; //10 kHz PWM - motor1_pwm.period(1.0/(double)frequency_pwm); // T=1/f - motor1_pwm.write(0.7); // write Duty Cycle - - motor2_pwm.period(1.0/(double)frequency_pwm); // T=1/f - motor2_pwm.write(0.7); // write Duty Cycle - - motor3_pwm.period(1.0/(double)frequency_pwm); // T=1/f - motor3_pwm.write(0.7); // write Duty Cycle - - wait(1.0/100); +} - - while (check1 || check2 || check3) { - counts1 = Encoder1.getPulses(); - counts2 = Encoder2.getPulses(); - counts3 = Encoder.getPulses(); - - wait(1.0/1000); - //pc.printf("while"); - if(abs(counts1)>=abs(steps1)) { - motor1_pwm.write(0); - check1=false; - counts1=0; - } - if (abs(counts2)>=abs(steps2)) { - motor2_pwm.write(0); - check2=false; - counts2=0; - } - if (abs(counts3)>=abs(steps3)) { - motor3_pwm.write(0); - check3=false; - counts3=0; - } - } +if (cc=='a') { +x0=x0-2.0f; +if (x0>=-75 && x0<=75) { + movefunction (); + } + else { + x0=x0+2.0f; + } +} + +if (cc=='w') { +y0=y0+2.0f; +if (y0>=-75 && y0<=75) { + movefunction (); + } + else { + y0=y0-2.0f; + } } - -if (cc=='d') { - - check1=true; - check2=true; - check3=true; - - x0=x0-2.0f; - - theta1 = delta_calcinverse(x0,y0,z0); - pc.printf("\n\r de hoeken zijn(%f, %f, %f)", theta1, theta2, theta3); - pc.printf("\n\r coordinaten(%f, %f, %f)", x0, y0, z0); - theta1 = theta1 - oldtheta1; - theta2 = theta2 - oldtheta2; - theta3 = theta3 - oldtheta3; - - float steps1 = anglestep(theta1); - float steps2 = anglestep(theta2); - float steps3 = anglestep(theta3); - - //pc.printf("\n\r (%f, %f, %f)", steps1, steps2, steps3); - - - motor1_dir.write(0); // positief - motor2_dir.write(1); // positief - motor3_dir.write(0); // positief - - if (steps1<0) { - motor1_dir.write(1); // positief +if (cc=='s') { +y0=y0-2.0f; +if (y0>=-75 && y0<=75) { + movefunction (); + } + else { + y0=y0+2.0f; } - if (steps1<0) { - motor2_dir.write(0); // positief - } - if (steps1<0) { - motor3_dir.write(1); // positief - } - - int frequency_pwm = 10000; //10 kHz PWM - motor1_pwm.period(1.0/(double)frequency_pwm); // T=1/f - motor1_pwm.write(0.7); // write Duty Cycle - - motor2_pwm.period(1.0/(double)frequency_pwm); // T=1/f - motor2_pwm.write(0.7); // write Duty Cycle - - motor3_pwm.period(1.0/(double)frequency_pwm); // T=1/f - motor3_pwm.write(0.7); // write Duty Cycle - - while (check1 || check2 || check3) { - counts1 = Encoder1.getPulses(); - counts2 = Encoder2.getPulses(); - counts3 = Encoder.getPulses(); - - wait(1.0/500); - //pc.printf("while"); - if(abs(counts1)>=abs(steps1)) { - motor1_pwm.write(0); - check1=false; - counts1=0; - wait(1.0/100); - } - if (abs(counts2)>=abs(steps2)) { - motor2_pwm.write(0); - check2=false; - counts2=0; - wait(1.0/100); - } - if (abs(counts3)>=abs(steps3)) { - motor3_pwm.write(0); - check3=false; - //pc.printf("\n\rsteps %f, counts %f", steps3, counts3); - counts3=0; - wait(1.0/100); - } - } +} + +if (cc=='u') { +z0=z0+2.0f; +if (z0>=-210 && z0<=-130) { + movefunction (); + } + else { + z0=z0-2.0f; + } +} -} - - - wait(1.0/100); +if (cc=='j') { +z0=z0-2.0f; +if (z0>=-210 && z0<=-130) { + movefunction (); + } + else { + z0=z0+2.0f; } -} \ No newline at end of file +} + + + \ No newline at end of file