![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Robot tryout
Dependencies: mbed QEI biquadFilter MODSERIAL FastPWM ttmath Math
Diff: Motor_tryout.cpp
- Revision:
- 23:18b0be02187f
- Parent:
- 22:9f911405e096
- Child:
- 24:61d03692f92e
diff -r 9f911405e096 -r 18b0be02187f Motor_tryout.cpp --- a/Motor_tryout.cpp Fri Oct 25 11:44:33 2019 +0000 +++ b/Motor_tryout.cpp Fri Oct 25 12:13:44 2019 +0000 @@ -1,7 +1,5 @@ #include "mbed.h" #include "MODSERIAL.h" -#include "QEI1.h" -#include "QEI2.h" #include "QEI.h" #include "Math.h" #include "ttmath.h" @@ -15,9 +13,9 @@ PwmOut motor3_pwm(PTC4); DigitalOut motor3_dir(PTC12); -QEI1 Encoder1(D12,D13,NC,64,QEI1::X4_ENCODING); -QEI2 Encoder2(D10,D11,NC,64,QEI2::X4_ENCODING); -QEI Encoder(D2,D3,NC,64,QEI::X4_ENCODING); +QEI Encoder1(D12,D13,NC,64,QEI::X4_ENCODING); +QEI Encoder2(D10,D11,NC,64,QEI::X4_ENCODING); +QEI Encoder3(D2,D3,NC,64,QEI::X4_ENCODING); int check; int quit; @@ -62,6 +60,13 @@ float oldtheta2=0; float oldtheta3=0; +Ticker pulses; +void getpulses() { + counts1 = Encoder1.getPulses(); + counts2 = Encoder2.getPulses(); + counts3 = Encoder3.getPulses(); + } + float delta_calcangleyz(float x00, float y00, float z00) { float y2 = y00 + le; float y1 = f; @@ -108,12 +113,7 @@ return theta1, theta2, theta3; } -//Ticker pulse; -void pulseget() { - counts1 = Encoder1.getPulses(); - counts2 = Encoder2.getPulses(); - counts3 = Encoder.getPulses(); - } + float anglestep(float angle) { float steps; @@ -173,30 +173,25 @@ check3 = true; while (check1 || check2 || check3) { - counts1 = Encoder1.getPulses(); - counts2 = Encoder2.getPulses(); - counts3 = Encoder.getPulses(); - - wait(1.0/500); - + pulses.attach(&getpulses, 1.0/1000); + pc.printf("\n\r counts1 %i", counts1); + wait(1/500); if(abs(counts1)>=abs(steps1)) { + pc.printf("\n\r counts1 %i and steps1 %f", counts1, 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); } } } @@ -207,9 +202,6 @@ char cc = pc.getc(); while(true) { - counts1 = 0; - counts2 = 0; - counts3 = 0; delta_calcinverse(x0,y0,z0);