Moet dit er bij
Dependencies: mbed QEI HIDScope biquadFilter MODSERIAL FastPWM
Diff: main.cpp
- Revision:
- 15:75070aedf4b7
- Parent:
- 14:8aa57a7c5b30
- Child:
- 16:6334ad516980
diff -r 8aa57a7c5b30 -r 75070aedf4b7 main.cpp --- a/main.cpp Wed Oct 30 12:52:26 2019 +0000 +++ b/main.cpp Wed Oct 30 13:32:33 2019 +0000 @@ -9,7 +9,7 @@ InterruptIn button1(D11); InterruptIn button2(D10); InterruptIn buttonsw2(SW2); -InterruptIn buttonsw3(SW3); +InterruptIn buttonfailure(SW3); AnalogIn potmeter1(A0); AnalogIn potmeter2(A1); AnalogIn potmeter3(A2); @@ -86,7 +86,7 @@ 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 , motor_wait2 , motor_bewegen}; +enum Motor_States{motor_wait , motor_encoderset , motor_wait2 , motor_bewegen , failuremode}; Motor_States motor_curr_state; bool motor_state_changed = true; bool motor_calibration_done = false; @@ -207,8 +207,6 @@ } void RKI(){ - - if (motor_RKI==true){ 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; @@ -219,8 +217,8 @@ - if (q1<0.0f){ - q1=0.0;} + if (q1<-5.0f){ + q1=-5.0;} else if (q1>65.0f*deg2rad){ q1=65.0f*deg2rad;} else{ @@ -228,14 +226,14 @@ if (q2>-50.0*deg2rad){ q2=-50.0*deg2rad;} - else if (q2<-135.0*deg2rad){ - q2=-135.0*deg2rad;} + else if (q2<-138.0*deg2rad){ + q2=-138.0*deg2rad;} else{ q2=q2;} motor1ref=5.5f*q1+5.5f*q2; motor2ref=2.75f*q1; - } + } @@ -272,7 +270,8 @@ // More functions } - // Do nothing until end condition is met + PID_controller(); + motorControl(); // State transition guard if ( button2_pressed ) { @@ -311,7 +310,10 @@ // More functions } - motor_RKI=true; + PID_controller(); + motorControl(); + RKI(); + // Do nothing until end condition is met // State transition guard @@ -330,11 +332,15 @@ motor_state_changed = false; // More functions } + static float t=0; Vx=10.0f*sin(1.0f*t); Vy=0.0f; + t+=Ts; - t+=Ts; + PID_controller(); + motorControl(); + RKI(); if ( button2_pressed ) { button2_pressed = false; @@ -347,6 +353,21 @@ } +void do_failuremode(){ + // Entry function + if ( motor_state_changed == true ) { + motor_state_changed = false; + // More functions + } + motor_curr_state = failuremode; + + motor1Power.write(0.0f); + motor2Power.write(0.0f); + Vx=0.0f; + Vy=0.0f; + + } + void motor_state_machine() { switch(motor_curr_state) { @@ -372,9 +393,6 @@ //emg_state_machine(); motor_state_machine(); readEncoder(); - PID_controller(); - motorControl(); - RKI(); // controller(); // outputToMotors(); @@ -393,6 +411,7 @@ button1.fall(&button1Press); button2.fall(&button2Press); + buttonfailure.fall(&do_failuremode); while (true) { pc.printf("counts1offset %i counts1af: %i counts1: %i\r\n",counts1offset,counts1af,counts1);