Robot tryout
Dependencies: mbed QEI biquadFilter MODSERIAL FastPWM ttmath Math
Diff: Motor_tryout.cpp
- Revision:
- 24:61d03692f92e
- Parent:
- 23:18b0be02187f
- Child:
- 25:eb3204e45d33
diff -r 18b0be02187f -r 61d03692f92e Motor_tryout.cpp --- a/Motor_tryout.cpp Fri Oct 25 12:13:44 2019 +0000 +++ b/Motor_tryout.cpp Fri Oct 25 13:45:39 2019 +0000 @@ -171,23 +171,38 @@ check1 = true; check2 = true; check3 = true; + + counts1=0; + counts2=0; + counts3=0; + + pulses.attach(&getpulses, 1.0/1000); + for (int i=0; i<100; i++) { + pc.printf("\n\rcounts1 %i", counts1); + wait(0.001); + } + while (check1 || check2 || check3) { - pulses.attach(&getpulses, 1.0/1000); pc.printf("\n\r counts1 %i", counts1); - wait(1/500); + pulses.attach(&getpulses, 1.0/10000); + + wait(1/1000); if(abs(counts1)>=abs(steps1)) { - pc.printf("\n\r counts1 %i and steps1 %f", counts1, steps1); + // pc.printf("\n\r counts1 %i and steps1 %f", counts1, steps1); + pc.printf("\n 1 is false"); motor1_pwm.write(0); check1=false; counts1=0; } if (abs(counts2)>=abs(steps2)) { + pc.printf("\n 2 is false"); motor2_pwm.write(0); check2=false; counts2=0; } if (abs(counts3)>=abs(steps3)) { + pc.printf("\n 3 is false"); motor3_pwm.write(0); check3=false; //pc.printf("\n\rsteps %f, counts %f", steps3, counts3); @@ -211,62 +226,62 @@ char cc = pc.getc(); if (cc=='d') { - x0=x0+2.0f; + x0=x0+5.0f; if (x0>=-75 && x0<=75) { movefunction (); } else { - x0=x0-2.0f; + x0=x0-5.0f; } } if (cc=='a') { -x0=x0-2.0f; +x0=x0-5.0f; if (x0>=-75 && x0<=75) { movefunction (); } else { - x0=x0+2.0f; + x0=x0+5.0f; } } if (cc=='w') { -y0=y0+2.0f; +y0=y0+5.0f; if (y0>=-75 && y0<=75) { movefunction (); } else { - y0=y0-2.0f; + y0=y0-5.0f; } } if (cc=='s') { -y0=y0-2.0f; +y0=y0-5.0f; if (y0>=-75 && y0<=75) { movefunction (); } else { - y0=y0+2.0f; + y0=y0+5.0f; } } if (cc=='u') { -z0=z0+2.0f; +z0=z0+5.0f; if (z0>=-210 && z0<=-130) { movefunction (); } else { - z0=z0-2.0f; + z0=z0-5.0f; } } if (cc=='j') { -z0=z0-2.0f; +z0=z0-5.0f; if (z0>=-210 && z0<=-130) { movefunction (); } else { - z0=z0+2.0f; + z0=z0+5.0f; } } }