Signa-bot code for project BioRobotics, at University of Twente.
Dependencies: mbed QEI MODSERIAL FastPWM ttmath Math
Diff: Motor_tryout.cpp
- Revision:
- 25:eb3204e45d33
- Parent:
- 24:61d03692f92e
- Child:
- 26:432d3519ba86
diff -r 61d03692f92e -r eb3204e45d33 Motor_tryout.cpp --- a/Motor_tryout.cpp Fri Oct 25 13:45:39 2019 +0000 +++ b/Motor_tryout.cpp Tue Oct 29 08:48:48 2019 +0000 @@ -17,6 +17,9 @@ QEI Encoder2(D10,D11,NC,64,QEI::X4_ENCODING); QEI Encoder3(D2,D3,NC,64,QEI::X4_ENCODING); +float steps1 = 0; +float steps2 = 0; +float steps3 = 0; int check; int quit; int limit_pos = 8400; @@ -29,6 +32,9 @@ int counts1 = 0; int counts2 = 0; int counts3 = 0; +int count1old = 0; +int countdiff = 0; +int counting1 = 0; const float le = 15.0; const float f = 37.5; @@ -60,12 +66,19 @@ float oldtheta2=0; float oldtheta3=0; -Ticker pulses; -void getpulses() { +Ticker pulses1; +void getpulses1() { counts1 = Encoder1.getPulses(); + } +Ticker pulses2; +void getpulses2() { counts2 = Encoder2.getPulses(); + } +Ticker pulses3; +void getpulses3() { counts3 = Encoder3.getPulses(); } + float delta_calcangleyz(float x00, float y00, float z00) { float y2 = y00 + le; @@ -132,9 +145,10 @@ theta2 = theta2 - oldtheta2; theta3 = theta3 - oldtheta3; - float steps1 = anglestep(theta1); - float steps2 = anglestep(theta2); - float steps3 = anglestep(theta3); + steps1 = anglestep(theta1); + pc.printf("\n\rsteps1.1 %f", steps1); + steps2 = anglestep(theta2); + steps3 = anglestep(theta3); // Set the direction of the motors. if (theta1 < 0) { @@ -172,46 +186,76 @@ 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); - } + + // for (int i=0; i<100; i++) { + // pc.printf("\n\rcounts1 %i", counts1); + // wait(0.001); + // } - + pc.printf("\n\rsteps3.1 %f", steps3); + pc.printf("\n\rcounts3.1 %i", counts3); + + while (check1 || check2 || check3) { - pc.printf("\n\r counts1 %i", counts1); - 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 1 is false"); + pc.printf("\n\rsteps1 %f, steps2 %f, steps3 %f",steps1, steps2, steps3); + pc.printf("\n\rcounts1 %i, counts2 %i, counts3 %i", counts1, counts2, counts3); + + if (steps1 <= 0) { + if(counts1<=steps1) { + pulses1.detach(); + pc.printf("\n 1 is false"); + motor1_pwm.write(0); check1=false; - counts1=0; } - if (abs(counts2)>=abs(steps2)) { + if (counts2<=steps2) { + pulses2.detach(); pc.printf("\n 2 is false"); motor2_pwm.write(0); check2=false; - counts2=0; } - if (abs(counts3)>=abs(steps3)) { + if (counts3>=abs(steps3)) { + pulses3.detach(); pc.printf("\n 3 is false"); motor3_pwm.write(0); check3=false; //pc.printf("\n\rsteps %f, counts %f", steps3, counts3); - counts3=0; + pc.printf("\n\rsteps3 %f", steps3); + pc.printf("\n\rcounts3 %i", counts3); + } + } + if (steps1 >= 0) { + if(counts1>=steps1) { + pc.printf("\n 1 is false"); + motor1_pwm.write(0); + check1=false; } + if (counts2>=steps2) { + pc.printf("\n 2 is false"); + motor2_pwm.write(0); + check2=false; + } + if (counts3<=-steps3) { + pc.printf("\n 3 is false"); + motor3_pwm.write(0); + check3=false; + } + } + } + //Encoder1.reset(); + wait(1.0); + pc.printf("\n\rcounts1 %i, counts2 %i, counts3 %i", counts1, counts2, counts3); } int main(void) { + pulses1.attach(&getpulses1, 1.0/1000); + pulses2.attach(&getpulses2, 1.0/1000); + pulses3.attach(&getpulses3, 1.0/1000); + Encoder1.reset(); + Encoder2.reset(); + Encoder3.reset(); + pc.baud(115200); char cc = pc.getc();