![](/media/cache/group/9e3cc099b3b04bca937a1cca1da81b19.jpg.50x50_q85.jpg)
Moet dit er bij
Dependencies: mbed QEI HIDScope biquadFilter MODSERIAL FastPWM
Diff: main.cpp
- Revision:
- 9:298469a70280
- Parent:
- 8:3cfc8be293d3
- Child:
- 10:990287a722d2
diff -r 3cfc8be293d3 -r 298469a70280 main.cpp --- a/main.cpp Tue Oct 29 16:42:46 2019 +0000 +++ b/main.cpp Tue Oct 29 18:55:53 2019 +0000 @@ -85,12 +85,11 @@ HIDScope scope(6); //Going to send x channels of data. To access data go to 'http:/localhost:18082/' after starting HIDScope application. //State maschine -enum Motor_States{motor_wait , motor_encoderset}; +enum Motor_States{motor_wait , motor_encoderset , motor_wait2 , motor_bewegen}; Motor_States motor_curr_state; bool motor_state_changed = true; -bool motor_cal1_done = false; -bool motor_cal2_done = false; - +bool motor_calibration_done = false; +bool motor_RKI=false; bool button1_pressed = false; bool button2_pressed = false; @@ -139,6 +138,131 @@ countsPrev2 = counts2; } +void PID_controller(){ + + static float error_integral1=0; + static float e_prev1=motor1error; + + //Proportional part: + u_p1=Kp*motor1error; + + //Integral part + error_integral1=error_integral1+ei1*Ts; + u_i1=Ki*error_integral1; + + //Derivative part + float error_derivative1=(motor1error-e_prev1)/Ts; + float u_d1=Kd*error_derivative1; + e_prev1=motor1error; + + // Sum and limit + up1= u_p1+u_i1+u_d1; + if (up1>1){ + controlsignal1=1;} + else if (up1<-1){ + controlsignal1=-1;} + else { + controlsignal1=up1;} + + // To prevent windup + ux1= up1-controlsignal1; + ek1= Ka*ux1; + ei1= motor1error-ek1; + +// Motor 2 + + static float error_integral2=0; + static float e_prev2=motor2error; + + //Proportional part: + u_p2=Kp*motor2error; + + //Integral part + error_integral2=error_integral2+ei2*Ts; + u_i2=Ki*error_integral2; + + //Derivative part + float error_derivative2=(motor2error-e_prev2)/Ts; + float u_d2=Kd*error_derivative2; + e_prev2=motor2error; + + // Sum and limit + up2= u_p2+u_i2+u_d2; + if (up2>1.0f){ + controlsignal2=1.0f;} + else if (up2<-1){ + controlsignal2=-1.0f;} + else { + controlsignal2=up2;} + + // To prevent windup + ux2= up2-controlsignal2; + ek2= Ka*ux2; + ei2= motor2error-ek2; +} + +void RKI(){ + + if (motor_RKI==true){ + //Vy=potmeter1.read()*10.0*motortoggle; + //Vy=potmeter2.read()*10*motortoggle; + static float t=0; + Vx=6.0f*sin(1.0f*t); + t+=Ts; + //Vx=-1.0*; + Vy=0.0f; + q1dot=(l2*cos(q1+q2)*Vx+l2*sin(q1+q2)*Vy)/((-l1*sin(q1)-l2*sin(q1+q2))*l2*cos(q1+q2)+l2*sin(q1+q2)*(l1*cos(q1)+l2*cos(q1+q2))); + q2dot=((-l1*cos(q1)-l2*cos(q1+q2))*Vx+(-l1*sin(q1)-l2*sin(q1+q2))*Vy)/((-l1*sin(q1)-l2*sin(q1+q2))*l2*cos(q1+q2)+l2*sin(q1+q2)*(l1*cos(q1)+l2*cos(q1+q2))); + q1=q1+q1dot*Ts; + q2=q2+q2dot*Ts; + + xe=l1*cos(q1)+l2*cos(q1+q2); + ye=l1*sin(q1)+l2*sin(q1+q2); + + + + if (q1<0.0f){ + q1=0.0;} + else if (q1>90.0f*deg2rad){ + q1=90.0f*deg2rad;} + else{ + q1=q1;} + + if (q2>-45.0*deg2rad){ + q2=-45.0*deg2rad;} + else if (q2<-135.0*deg2rad){ + q2=-135.0*deg2rad;} + else{ + q2=q2;} + + motor1ref=5.5f*q1+5.5f*q2; + motor2ref=2.75f*q1; + } +} + + +void motorControl(){ + motor1angle = (counts1 * factorin / gearratio)-(135.0*5.5*deg2rad); // Angle of motor shaft in rad + omega1 = deltaCounts1 / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s + motor1error=motor1ref-motor1angle; + if (controlsignal1<0){ + motordir1= 0;} + else { + motordir1= 1;} + motor1Power.write(abs(controlsignal1)); + motor1Direction= motordir1; + + motor2angle = (counts2 * factorin / gearratio); // Angle of motor shaft in rad + omega2 = deltaCounts2 / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s + motor2error=motor2ref-motor2angle; + if (controlsignal2<0){ + motordir2= 0;} + else { + motordir2= 1;} + motor2Power.write(abs(controlsignal2)); + motor2Direction= motordir2; +} + void do_motor_wait(){ // Entry function if ( motor_state_changed == true ) { @@ -171,12 +295,52 @@ // State transition guard if ( button2_pressed ) { - button2_pressed = false; - motor_curr_state = motor_wait; + button2_pressed = false; + motor_calibration_done = true; + motor_curr_state = motor_wait2; motor_state_changed = true; } } +void do_motor_wait2(){ + // Entry function + if ( motor_state_changed == true ) { + motor_state_changed = false; + // More functions + } + + // Do nothing until end condition is met + + // State transition guard +if ( button2_pressed ) { + button2_pressed = false; + motor_curr_state = motor_encoderset; + motor_state_changed = true; + // More functions + } + +} + +void do_motor_bewegen(){ + // Entry function + if ( motor_state_changed == true ) { + motor_state_changed = false; + // More functions + } + + motor_RKI=true; + +if ( button2_pressed ) { + button2_pressed = false; + motor_RKI = false; + motor_curr_state = motor_wait2; + motor_state_changed = true; + // More functions + } + + +} + void motor_state_machine() { switch(motor_curr_state) { @@ -186,6 +350,12 @@ case motor_encoderset: do_motor_Encoder_Set(); break; + case motor_wait2: + do_motor_wait2(); + break; + case motor_bewegen: + do_motor_bewegen(); + break; } } @@ -196,6 +366,10 @@ //emg_state_machine(); motor_state_machine(); readEncoder(); + PID_controller(); + motorControl(); + RKI(); + // controller(); // outputToMotors(); }