![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Signa-bot code for project BioRobotics, at University of Twente.
Dependencies: mbed QEI MODSERIAL FastPWM ttmath Math
Diff: Motor_tryout.cpp
- Revision:
- 28:43a1d67ff8ea
- Parent:
- 27:3eb181cbe183
- Child:
- 29:7eb028b359a1
diff -r 3eb181cbe183 -r 43a1d67ff8ea Motor_tryout.cpp --- a/Motor_tryout.cpp Tue Oct 29 11:10:23 2019 +0000 +++ b/Motor_tryout.cpp Tue Oct 29 12:05:47 2019 +0000 @@ -64,6 +64,13 @@ float oldtheta2=0; float oldtheta3=0; +// Constant values for PI +double Kp = 17.5; +double Ki = 1.02; +double Ts = 0.0025; +float new_steps1 = 0; +float error1 = 0; + Ticker pulses; void getpulses() { counts1 = Encoder1.getPulses(); @@ -71,7 +78,8 @@ counts3 = Encoder3.getPulses(); } - + + float delta_calcangleyz(float x00, float y00, float z00) { float y2 = y00 + le; float y1 = f; @@ -94,8 +102,10 @@ } float theta1 = (beta - alpha)*180.0/pi; return theta1; + } } -} + + float delta_calcinverse(float x00, float y00, float z00) { theta1 = theta2 = theta3 = 0; @@ -118,16 +128,32 @@ return theta1, theta2, theta3; } - +//double error; +float PI_controller() +{ + float error1 = steps1-counts1; + static double error1_integral = 0; + + // Proportional part + double u_k = Kp * error1; + + // Integral part + error1_integral = error1_integral + error1 * Ts; + double u_i = Ki * error1_integral; + + // Sum all parts and return it + return u_k + u_i; +} +// Calculate the steps from angle float anglestep(float angle) { float steps; steps = angle / 360 * 8400; return steps; } -float movefunctioninit () { - +// Omschrijving +float movefunctioninit () { theta1 = delta_calcinverse(x00,y00,z00); pc.printf("\n\r de hoeken zijn(%f, %f, %f)", theta1, theta2, theta3); @@ -141,28 +167,33 @@ steps2 = anglestep(theta2); steps3 = anglestep(theta3); pc.printf("\n\rsteps1 %f, steps2 %f, steps3 %f", steps1, steps2, steps3); + + + pc.printf("\n\r error1: %f", error1); + new_steps1 = PI_controller(); + pc.printf("\n\rnew steps1: %f", new_steps1); // Set the direction of the motors. if (theta1 < 0) { motor1_dir.write(1); } - else { - motor1_dir.write(0); - } + else { + motor1_dir.write(0); + } if (theta2 < 0) { motor2_dir.write(0); } - else { - motor2_dir.write(1); - } + else { + motor2_dir.write(1); + } if (theta3 < 0) { motor3_dir.write(0); } - else { - motor3_dir.write(1); - } + else { + motor3_dir.write(1); + } int frequency_pwm = 10000; //10 kHz PWM motor1_pwm.period(1.0/(double)frequency_pwm); // T=1/f @@ -181,8 +212,9 @@ Encoder1.reset(); Encoder2.reset(); Encoder3.reset(); - } +} +// Omschrijving float movefunction() { while (check1 || check2 || check3) { @@ -191,16 +223,17 @@ if(abs(counts1)>=abs(steps1)) { pc.printf("\n 1 is false"); motor1_pwm.write(0); - check1=false; + check1=false; pc.printf("\n\rcounts1 %i", counts1); } + if (abs(counts2)>=abs(steps2)) { pc.printf("\n 2 is false"); motor2_pwm.write(0); check2=false; pc.printf("\n\rcounts2 %i", counts2); - } + if (abs(counts3)>=abs(steps3)) { pc.printf("\n 3 is false"); motor3_pwm.write(0); @@ -214,94 +247,102 @@ pc.printf("\n\rcounts1 %i, counts2 %i, counts3 %i", counts1, counts2, counts3); } + +/////////////////// +// MAIN FUNCTION // +/////////////////// + int main(void) { - pc.baud(115200); - + pc.baud(115200); char cc = pc.getc(); - while(true) { - pulses.attach(&getpulses, 1.0/10000); - - Encoder1.reset(); - Encoder2.reset(); - Encoder3.reset(); - counts1 = Encoder1.getPulses(); - counts2 = Encoder2.getPulses(); - counts3 = Encoder3.getPulses(); + while(true){ + pulses.attach(&getpulses, 1.0/10000); + + Encoder1.reset(); + Encoder2.reset(); + Encoder3.reset(); + counts1 = Encoder1.getPulses(); + counts2 = Encoder2.getPulses(); + counts3 = Encoder3.getPulses(); + + delta_calcinverse(x0,y0,z0); - - delta_calcinverse(x0,y0,z0); - - oldtheta1 = theta1; - oldtheta2 = theta2; - oldtheta3 = theta3; -char cc = pc.getc(); - -if (cc=='d') { - x0=x0+5.0f; - if (x0>=-75 && x0<=75) { - movefunctioninit (); - movefunction (); + oldtheta1 = theta1; + oldtheta2 = theta2; + oldtheta3 = theta3; + + char cc = pc.getc(); + + if (cc=='d') { + x0=x0+5.0f; + if (x0>=-75 && x0<=75){ + movefunctioninit (); + movefunction (); + } + else { + x0=x0-5.0f; + } } - else { - x0=x0-5.0f; - } -} - -if (cc=='a') { -x0=x0-5.0f; -if (x0>=-75 && x0<=75) { - movefunctioninit (); - movefunction (); - } - else { - x0=x0+5.0f; + + if (cc=='a') { + x0=x0-5.0f; + if (x0>=-75 && x0<=75){ + movefunctioninit (); + movefunction (); + } + else { + x0=x0+5.0f; } -} - -if (cc=='w') { -y0=y0+5.0f; -if (y0>=-75 && y0<=75) { - movefunctioninit (); - movefunction (); - } - else { - y0=y0-5.0f; + } + + if (cc=='w'){ + y0=y0+5.0f; + if (y0>=-75 && y0<=75){ + movefunctioninit (); + movefunction (); + } + else { + y0=y0-5.0f; } -} - -if (cc=='s') { -y0=y0-5.0f; -if (y0>=-75 && y0<=75) { - movefunctioninit (); - movefunction (); - } - else { - y0=y0+5.0f; + } + + if (cc=='s'){ + y0=y0-5.0f; + if (y0>=-75 && y0<=75) { + movefunctioninit (); + movefunction (); + } + else { + y0=y0+5.0f; } -} - -if (cc=='u') { -z0=z0+5.0f; -if (z0>=-210 && z0<=-130) { - movefunctioninit (); - movefunction (); - } - else { - z0=z0-5.0f; + } + + if (cc=='u') + { + z0=z0+5.0f; + if (z0>=-210 && z0<=-130) { + movefunctioninit (); + movefunction (); + } + else{ + z0=z0-5.0f; } -} - -if (cc=='j') { -z0=z0-5.0f; -if (z0>=-210 && z0<=-130) { - movefunctioninit (); - movefunction (); + } + + if (cc=='j') + { + z0=z0-5.0f; + if (z0>=-210 && z0<=-130){ + movefunctioninit (); + movefunction (); + } + else { + z0=z0+5.0f; + } + } + // end while } - else { - z0=z0+5.0f; - } -} -} +// end main } \ No newline at end of file