![](/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:
- 30:390cab7cd6e6
- Parent:
- 29:7eb028b359a1
- Child:
- 31:3c13f1c35ee5
diff -r 7eb028b359a1 -r 390cab7cd6e6 Motor_tryout.cpp --- a/Motor_tryout.cpp Tue Oct 29 12:45:05 2019 +0000 +++ b/Motor_tryout.cpp Thu Oct 31 09:56:54 2019 +0000 @@ -13,6 +13,10 @@ PwmOut motor3_pwm(PTC4); DigitalOut motor3_dir(PTC12); +AnalogIn potmeter1(A1); + + + 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); @@ -65,23 +69,117 @@ float oldtheta3=0; // Constant values for PI -double Kp = 17.5; -double Ki = 1.02; -double Ts = 0.0025; -float new_steps1 = 0; +float Kp; +float Kd; + +float Ts = 0.0025; float error1 = 0; +float error2 = 0; +float error3 = 0; -float u_k = 0; -float u_i = 0; +float newmotor1 = 1; +float newmotor2 = 1; +float newmotor3 = 1; + +float u_k1 = 0; +float u_k2 = 0; +float u_k3 = 0; + +float u_d1 = 0; +float u_d2 = 0; +float u_d3 = 0; + +float lasterror1 = 0; +float lasterror2 = 0; +float lasterror3 = 0; + +float derivative1 = 0; +float derivative2 = 0; +float derivative3 = 0; + +bool timer = false; + +Ticker motor_timer; +void motor() +{ + counts1 = Encoder1.getPulses(); + counts2 = Encoder2.getPulses(); + counts3 = Encoder3.getPulses(); + +// angle1 = counts1/(8400.0)*2.0*pie; +// angle2 = counts2/(8400.0)*2.0*pie; +// angle3 = counts3/(8400.0)*2.0*pie; + +// sinewave = sin(time_sin); -Ticker pulses; -void getpulses() { - counts1 = Encoder1.getPulses(); - counts2 = Encoder2.getPulses(); - counts3 = Encoder3.getPulses(); + lasterror1 = error1; + lasterror2 = error2; + lasterror3 = error3; + + error1 = counts1 - steps1; + error2 = counts2 - steps2; + error3 = counts3 - steps3; + + derivative1 = error1 - lasterror1; + derivative2 = error2 - lasterror2; + derivative3 = error3 - lasterror3; + + Kp = 50; + Kd = potmeter1 * 100; + + + // Proportional part + u_k1 = Kp * error1; + u_k2 = Kp * error2; + u_k3 = Kp * error3; + + u_d1 = Kd * derivative1; + u_d2 = Kd * derivative2; + u_d3 = Kd * derivative3; + + newmotor1 = u_k1 + u_d1; + newmotor2 = u_k2 + u_d2; + newmotor3 = u_k3 + u_d3; + + if (newmotor1>1.0f){ + newmotor1 =1.0f; } + if (newmotor1<-1.0f){ + newmotor1 = -1.0f; + } + + if (newmotor2>1.0f){ + newmotor2 =1.0f; + } + if (newmotor2<-1.0f){ + newmotor2 = -1.0f; + } + + if (newmotor3>1.0f){ + newmotor3 =1.0f; + } + if (newmotor3<-1.0f){ + newmotor3 = -1.0f; + } + + if (timer == true) { + motor1_pwm.write(fabs(newmotor1)); + motor1_dir.write(newmotor1>0); + + motor2_pwm.write(fabs(newmotor2)); + motor2_dir.write(newmotor2<0); + + motor3_pwm.write(fabs(newmotor3)); + motor3_dir.write(newmotor3>0); + } + + else { + motor1_pwm.write(0); + motor2_pwm.write(0); + motor3_pwm.write(0); + } +} - float delta_calcangleyz(float x00, float y00, float z00) { float y2 = y00 + le; @@ -132,20 +230,6 @@ } //double error; -Ticker piticker; -void PIcontroller() -{ - error1 = steps1-counts1; - static float error1_integral = 0; - - // Proportional part - u_k = Kp * error1; - - // Integral part - error1_integral = error1_integral + error1 * Ts; - u_i = Ki * error1_integral; - new_steps1 = u_k + u_i; -} // Calculate the steps from angle float anglestep(float angle) { @@ -165,94 +249,111 @@ theta2 = theta2 - oldtheta2; theta3 = theta3 - oldtheta3; - steps1 = anglestep(theta1); - steps2 = anglestep(theta2); - steps3 = anglestep(theta3); + steps1 = steps1 + anglestep(theta1); + steps2 = steps2 + anglestep(theta2); + steps3 = steps3 + anglestep(theta3); pc.printf("\n\rsteps1 %f, steps2 %f, steps3 %f", steps1, steps2, steps3); + pc.printf("\n\rcounts1 %f, counts2 %f, counts3 %f", counts1, counts2, counts3); + - pc.printf("\n\r error1: %f", error1); - pc.printf("\n\rnew steps1: %f", new_steps1); +// pc.printf("\n\r error1: %f", error1); + // 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.57); // write Duty Cycle - - motor2_pwm.period(1.0/(double)frequency_pwm); // T=1/f - motor2_pwm.write(0.57); // write Duty Cycle - - motor3_pwm.period(1.0/(double)frequency_pwm); // T=1/f - motor3_pwm.write(0.57); // write Duty Cycle - - check1 = true; - check2 = true; - check3 = true; - - Encoder1.reset(); - Encoder2.reset(); - Encoder3.reset(); + // 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.57); // write Duty Cycle +// +// motor2_pwm.period(1.0/(double)frequency_pwm); // T=1/f +// motor2_pwm.write(0.57); // write Duty Cycle +// +// motor3_pwm.period(1.0/(double)frequency_pwm); // T=1/f +// motor3_pwm.write(0.57); // write Duty Cycle +// +// check1 = true; +// check2 = true; +// check3 = true; +// +// Encoder1.reset(); +// Encoder2.reset(); +// Encoder3.reset(); +// } // Omschrijving -float movefunction() { - - while (check1 || check2 || check3) { - pc.printf("\n\rcounts1 %i, counts2 %i, counts3 %i", counts1, counts2, counts3); - - pc.printf("\n\rerror1: %f", error1); - pc.printf("\n\ru_k: %f", u_k); - pc.printf("\n\ru_i: %f", u_i); - pc.printf("\n\ru_k + u_i: %f", (u_k+u_i)); - pc.printf("\n\r"); - - if(abs(counts1)>=abs(steps1)) { - pc.printf("\n\r 1 is false"); - motor1_pwm.write(0); - check1=false; - pc.printf("\n\rcounts1 %i", counts1); - } - - if (abs(counts2)>=abs(steps2)) { - pc.printf("\n\r 2 is false"); - motor2_pwm.write(0); - check2=false; - pc.printf("\n\rcounts2 %i", counts2); - } - - if (abs(counts3)>=abs(steps3)) { - pc.printf("\n\r 3 is false"); - motor3_pwm.write(0); - check3=false; - pc.printf("\n\rcounts3 %i", counts3); - } - } - pc.printf("\n\rcounts1 %i, counts2 %i, counts3 %i", counts1, counts2, counts3); - - wait(3.0); - pc.printf("\n\rcounts1 %i, counts2 %i, counts3 %i", counts1, counts2, counts3); -} +float movestop() { +wait(0.6); +timer = false; +} +// +// while (check1 || check2 || check3) { +// +// int frequency_pwm = 10000; //10 kHz PWM +// motor1_pwm.period(1.0/(double)frequency_pwm); // T=1/f +// motor1_pwm.write(newmotor1); // write Duty Cycle +// +// motor2_pwm.period(1.0/(double)frequency_pwm); // T=1/f +// motor2_pwm.write(newmotor2); // write Duty Cycle +// +// motor3_pwm.period(1.0/(double)frequency_pwm); // T=1/f +// motor3_pwm.write(newmotor3); // write Duty Cycle +// +// if (error1==0) { +// check1=false; +// } +// if (error2==0) { +// check2=false; +// } +// if (error3==0) { +// check3=false; +// } +// +//// if(abs(counts1)>=abs(steps1)) { +//// pc.printf("\n\r 1 is false"); +//// motor1_pwm.write(0); +//// check1=false; +//// pc.printf("\n\rcounts1 %i", counts1); +//// } +//// +//// if (abs(counts2)>=abs(steps2)) { +//// pc.printf("\n\r 2 is false"); +//// motor2_pwm.write(0); +//// check2=false; +//// pc.printf("\n\rcounts2 %i", counts2); +//// } +//// +//// if (abs(counts3)>=abs(steps3)) { +//// pc.printf("\n\r 3 is false"); +//// motor3_pwm.write(0); +//// check3=false; +//// pc.printf("\n\rcounts3 %i", counts3); +//// } +// } +// wait(1.0); +// pc.printf("\n\rdone moving"); +//} /////////////////// @@ -260,39 +361,47 @@ /////////////////// int main(void) { + motor_timer.attach(&motor, 0.002); pc.baud(115200); char cc = pc.getc(); + + int frequency_pwm = 10000; //10 kHz PWM +// motor1_pwm.period(1.0/(double)frequency_pwm); // T=1/f +// motor1_pwm.write(newmotor1); // write Duty Cycle +// +// motor2_pwm.period(1.0/(double)frequency_pwm); // T=1/f +// motor2_pwm.write(newmotor2); // write Duty Cycle +// +// motor3_pwm.period(1.0/(double)frequency_pwm); // T=1/f +// motor3_pwm.write(newmotor3); // write Duty Cycle while(true){ pc.printf("\n\r----------------------------------------------------------"); pc.printf("\n\r----------------------------------------------------------"); - pulses.attach(&getpulses, 1.0/10000); - piticker.attach(&PIcontroller, 1.0/1000); - - Encoder1.reset(); - Encoder2.reset(); - Encoder3.reset(); - counts1 = Encoder1.getPulses(); - counts2 = Encoder2.getPulses(); - counts3 = Encoder3.getPulses(); + delta_calcinverse(x0,y0,z0); oldtheta1 = theta1; oldtheta2 = theta2; oldtheta3 = theta3; - + char cc = pc.getc(); + timer = true; + + if (cc=='d') { x0=x0+5.0f; if (x0>=-75 && x0<=75){ movefunctioninit (); - movefunction (); + movestop(); } else { + timer = false; x0=x0-5.0f; + } } @@ -300,9 +409,10 @@ x0=x0-5.0f; if (x0>=-75 && x0<=75){ movefunctioninit (); - movefunction (); + movestop(); } else { + timer = false; x0=x0+5.0f; } } @@ -311,9 +421,10 @@ y0=y0+5.0f; if (y0>=-75 && y0<=75){ movefunctioninit (); - movefunction (); + movestop(); } else { + timer = false; y0=y0-5.0f; } } @@ -322,9 +433,10 @@ y0=y0-5.0f; if (y0>=-75 && y0<=75) { movefunctioninit (); - movefunction (); + movestop(); } else { + timer = false; y0=y0+5.0f; } } @@ -334,9 +446,14 @@ z0=z0+5.0f; if (z0>=-210 && z0<=-130) { movefunctioninit (); - movefunction (); + movestop(); +// for (float i=0.0; i<5; i++) { +// pc.printf("\n\r dit is error1 %f, error2 %f, error3 %f", error1, error2, error3); +// wait(1.0/10.0); +// } } else{ + timer = false; z0=z0-5.0f; } } @@ -346,9 +463,10 @@ z0=z0-5.0f; if (z0>=-210 && z0<=-130){ movefunctioninit (); - movefunction (); + movestop(); } else { + timer = false; z0=z0+5.0f; } }