lololololol
Dependencies: mbed QEI HIDScope biquadFilter MODSERIAL FastPWM
main.cpp
- Committer:
- SjorsdeBruin
- Date:
- 2019-10-29
- Revision:
- 27:d37b3a0e0f2b
- Parent:
- 26:088e397ec26f
- Child:
- 28:8c90a46b613e
File content as of revision 27:d37b3a0e0f2b:
#include "QEI.h" #include "mbed.h" #include "BiQuad.h" #include "FastPWM.h" #include "HIDScope.h" #include "MODSERIAL.h" // Pins MODSERIAL pc(USBTX, USBRX); QEI encoder_1(D10,D11,NC,8400,QEI::X4_ENCODING); QEI encoder_2(D12,D13,NC,8400,QEI::X4_ENCODING); FastPWM PWM_motor_1(D6); FastPWM PWM_motor_2(D5); DigitalOut direction_motor_1(D7); DigitalOut direction_motor_2(D4); DigitalOut led_red(LED1); DigitalOut led_green(LED2); DigitalOut led_blue(LED3); AnalogIn emg_bl(A0); AnalogIn emg_br(A1); AnalogIn emg_leg(A2); InterruptIn button_1(SW2); InterruptIn button_2(SW3); // variables Ticker TickerStateMachine; Ticker motor_control; Ticker write_scope; Timer sinus_time; Timeout rest_timeout; Timeout mvc_timeout; Timeout led_timeout; enum states {start, motor_calibration, demo_mode, emg_calibration, vertical_movement, horizontal_movement}; states CurrentState = start; bool StateChanged = true; enum substates {rest_biceps_left, mvc_biceps_left, rest_biceps_right, mvc_biceps_right, rest_biceps_leg, mvc_biceps_leg}; substates CurrentSubstate = rest_biceps_left; bool SubstateChanged = true; volatile bool pressed_1 = false; volatile bool pressed_2 = false; HIDScope scope(6); volatile float theta_1; //volatile float theta_error_1; volatile float theta_reference_1; volatile float theta_2; //volatile float theta_error_2; volatile float theta_reference_2; float Ts = 0.01; float Kp; float Ki; float Kd; BiQuad Lowpass_bl ( 1.55148e-04, 3.10297e-04, 1.55148e-04, -1.96446e+00, 9.65081e-01 ); BiQuad Highpass_bl ( 9.40809e-01, -9.40809e-01, 0.00000e+00, -8.81619e-01, 0.00000e+00); BiQuad notch_bl (9.98432e-01, -1.89913e+00, 9.98432e-01, -1.89913e+00, 9.96886e-01); BiQuad Lowpass_br ( 1.55148e-04, 3.10297e-04, 1.55148e-04, -1.96446e+00, 9.65081e-01 ); BiQuad Highpass_br ( 9.40809e-01, -9.40809e-01, 0.00000e+00, -8.81619e-01, 0.00000e+00); BiQuad notch_br (9.98432e-01, -1.89913e+00, 9.98432e-01, -1.89913e+00, 9.96886e-01); BiQuad Lowpass_leg ( 1.55148e-04, 3.10297e-04, 1.55148e-04, -1.96446e+00, 9.65081e-01 ); BiQuad Highpass_leg ( 9.40809e-01, -9.40809e-01, 0.00000e+00, -8.81619e-01, 0.00000e+00); BiQuad notch_leg (9.98432e-01, -1.89913e+00, 9.98432e-01, -1.89913e+00, 9.96886e-01); int n = 0; float emgFiltered_bl; float emgFiltered_br; float emgFiltered_leg; float emg; float xmvc_value = 1e-11; float sum = 0; float xrest_value; float rest_value_bl; float rest_value_br; float rest_value_leg; float mvc_value_bl; float mvc_value_br; float mvc_value_leg; // functies void ledred() { led_red = 0; led_green = 1; led_blue = 1; } void ledgreen() { led_green=0; led_blue=1; led_red=1; } void ledblue() { led_green=1; led_blue=0; led_red=1; } void ledyellow() { led_green=0; led_blue=1; led_red=0; } void ledmagenta() { led_green=1; led_blue=0; led_red=0; } void ledcyan() { led_green=0; led_blue=0; led_red=1; } void ledwhite() { led_green=0; led_blue=0; led_red=0; } void ledoff() { led_green=1; led_blue=1; led_red=1; } float CalculateError(float theta_reference,float theta) { float theta_error = theta_reference-theta; return theta_error; } float Controller(float theta_error, bool motor) { if (motor == false) { float K = 1; float ti = 0.1; float td = 10; Kp = K*(1+td/ti); Ki = K/ti; Kd = K*td; } else { float K = 1; float ti = 0.1; float td = 10; Kp = K*(1+td/ti); Ki = K/ti; Kd = K*td; } static float error_integral = 0; static float error_prev = 0; static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); // Proportional part: float torque_p = Kp * theta_error; // Integral part: error_integral = error_integral + theta_error * Ts; float torque_i = Ki * error_integral; // Derivative part: float error_derivative = (theta_error - error_prev)/Ts; float filtered_error_derivative = LowPassFilter.step(error_derivative); float torque_d = Kd * filtered_error_derivative; error_prev = theta_error; // Sum all parts and return it float torque = torque_p + torque_i + torque_d; return torque; } void CalculateDirectionMotor() { direction_motor_1 = Controller(CalculateError(theta_reference_1,theta_1),0) <= 0.0f; direction_motor_2 = Controller(CalculateError(theta_reference_2,theta_2),1) <= 0.0f; } void ReadEncoder() { theta_1 = ((360.0f/64.0f)*(float)encoder_1.getPulses())/131.25f; // 360/64 voor de 64 CPR Encoder, 131.25 omdat de gear ratio 131.25:1 is. Hiermee is 1 omwenteling 360. theta_2 = ((360.0f/64.0f)*(float)encoder_2.getPulses())/131.25f; } void MotorControl() { ReadEncoder(); theta_reference_1 = 360.0f*sin(0.1f*sinus_time.read()*2.0f*3.14f); // voor test, moet weg in eindscript CalculateDirectionMotor(); PWM_motor_1.write(fabs(Controller(CalculateError(theta_reference_1,theta_1),0)/360.0f)); PWM_motor_2.write(fabs(Controller(CalculateError(theta_reference_2,theta_2),1)/360.0f)); } void go_next_1() { pressed_1 = !pressed_1; } void go_next_2() { pressed_2 = !pressed_2; } bool emg_switch(float treshold, float emg_input) { if(emg_input > treshold){ return true; } else { return false; } } float EmgCalibration(float emgFiltered, float mvc_value, float rest_value) { float emgCalibrated; if (emgFiltered <= rest_value) { return 0.0f; //emgCalibrated = 0; } if (emgFiltered >= mvc_value) { return 1.1f; //emgCalibrated = 1; } else { emgCalibrated = (emgFiltered-rest_value)/(mvc_value-rest_value); } return emgCalibrated; } void emgsample() { emgFiltered_bl = Highpass_bl.step(emg_bl.read()); emgFiltered_bl = notch_bl.step(emgFiltered_bl); emgFiltered_bl = fabs(emgFiltered_bl); emgFiltered_bl = Lowpass_bl.step(emgFiltered_bl); emgFiltered_br = Highpass_br.step(emg_br.read()); emgFiltered_br = notch_br.step(emgFiltered_br); emgFiltered_br = fabs(emgFiltered_br); emgFiltered_br = Lowpass_br.step(emgFiltered_br); emgFiltered_leg = Highpass_leg.step(emg_leg.read()); emgFiltered_leg = notch_leg.step(emgFiltered_leg); emgFiltered_leg = fabs(emgFiltered_leg); emgFiltered_leg = Lowpass_leg.step(emgFiltered_leg); } void rest() { if (CurrentSubstate == rest_biceps_left) { emg = emgFiltered_bl; //pc.printf("emg: %f \n\r",emgFiltered_bl); } if (CurrentSubstate == rest_biceps_right) { emg = emgFiltered_br; } if (CurrentSubstate == rest_biceps_leg) { emg = emgFiltered_leg; } if (n < 500) { ledred(); sum = sum + emg; //pc.printf("sum: %f \n\r",sum); n++; rest_timeout.attach(rest,0.001f); } if (n == 500) { sum = sum + emg; //pc.printf("sum: %f \n\r",sum); n++; xrest_value = float (sum/n); if (CurrentSubstate == rest_biceps_left) { rest_value_bl = xrest_value; pc.printf("rest_value_bl %f \n\r", rest_value_bl); CurrentSubstate = mvc_biceps_left; SubstateChanged = true; ledblue(); } if (CurrentSubstate == rest_biceps_right) { rest_value_br = xrest_value; pc.printf("rest_value_br %f \n\r", rest_value_br); CurrentSubstate = mvc_biceps_right; SubstateChanged = true; ledmagenta(); } if (CurrentSubstate == rest_biceps_leg) { rest_value_leg = xrest_value; pc.printf("rest_value_leg %f \n\r", rest_value_leg); pc.printf("rest_value_bl %f \n\r", rest_value_bl); CurrentSubstate = mvc_biceps_leg; SubstateChanged = true; ledwhite(); } } } void mvc() { if (CurrentSubstate == mvc_biceps_left) { emg = emgFiltered_bl; } if (CurrentSubstate == mvc_biceps_right) { emg = emgFiltered_br; } if (CurrentSubstate == mvc_biceps_leg) { emg = emgFiltered_leg; } if (emg >= xmvc_value) { xmvc_value = emg; } n++; if (n < 1000) { mvc_timeout.attach(mvc,0.001f); ledred(); } if (n == 1000) { if (CurrentSubstate == mvc_biceps_left) { mvc_value_bl = xmvc_value; pc.printf("mvc_value_bl %f \n\r", mvc_value_bl); CurrentSubstate = rest_biceps_right; SubstateChanged = true; ledyellow(); } if (CurrentSubstate == mvc_biceps_right) { mvc_value_br = xmvc_value; pc.printf("mvc_value_br %f \n\r", mvc_value_br); CurrentSubstate = rest_biceps_leg; SubstateChanged = true; ledcyan(); } if (CurrentSubstate == mvc_biceps_leg) { mvc_value_leg = xmvc_value; pc.printf("mvc_value_leg %f \n\r", mvc_value_leg); CurrentState = vertical_movement; StateChanged = true; ledoff(); } xmvc_value = 1e-11; } } void WriteScope() { emgsample(); scope.set(0, EmgCalibration(emgFiltered_bl, mvc_value_bl, rest_value_bl)); scope.set(1, EmgCalibration(emgFiltered_br, mvc_value_br, rest_value_br)); scope.set(2, EmgCalibration(emgFiltered_leg, mvc_value_leg, rest_value_leg)); scope.set(3, emgFiltered_bl); scope.set(4, emgFiltered_br); scope.set(5, emgFiltered_leg); scope.send(); } void SubstateTransition() { if (SubstateChanged == true) { SubstateChanged = false; pressed_1 = false; pressed_2 = false; if (CurrentSubstate == rest_biceps_left) { ledgreen(); pc.printf("groen \n\r"); pc.printf("Initiating rest_biceps_left\n\r"); } if (CurrentSubstate == mvc_biceps_left) { //ledblue(); pc.printf("Initiating mvc_biceps_left\n\r"); } if (CurrentSubstate == rest_biceps_right) { //ledyellow(); pc.printf("Initiating rest_biceps_right\n\r"); } if (CurrentSubstate == mvc_biceps_right) { //ledmagenta(); pc.printf("Initiating mvc_biceps_right\n\r"); } if (CurrentSubstate == rest_biceps_leg) { //ledcyan(); pc.printf("Initiating rest_biceps_leg\n\r"); } if (CurrentSubstate == mvc_biceps_leg) { //ledwhite(); pc.printf("Initiating mvc_biceps_leg\n\r"); } } } void while_start() { // Do startup stuff CurrentState = motor_calibration; StateChanged = true; } void while_motor_calibration() { // Do motor calibration stuff if (pressed_1) { // bool aanmaken voor demo (switch oid aanmaken) CurrentState = demo_mode; StateChanged = true; } if (pressed_2) { // bool aanmaken voor EMG (switch oid aanmaken) CurrentState = emg_calibration; StateChanged = true; } } void while_demo_mode() { // Do demo mode stuff if ((pressed_1) || (pressed_2)) { CurrentState = emg_calibration; StateChanged = true; } } void while_emg_calibration() { // Do emg calibration stuff switch (CurrentSubstate) { case rest_biceps_left: SubstateTransition(); if ((pressed_1) || (pressed_2)) { pressed_1 = false; pressed_2 = false; n = 0; sum = 0; rest(); } break; case mvc_biceps_left: SubstateTransition(); if ((pressed_1) || (pressed_2)) { pressed_1 = false; pressed_2 = false; n = 0; mvc(); } break; case rest_biceps_right: SubstateTransition(); if ((pressed_1) || (pressed_2)) { pressed_1 = false; pressed_2 = false; n = 0; sum = 0; rest(); } break; case mvc_biceps_right: SubstateTransition(); if ((pressed_1) || (pressed_2)) { pressed_1 = false; pressed_2 = false; n = 0; mvc(); } break; case rest_biceps_leg: SubstateTransition(); if ((pressed_1) || (pressed_2)) { pressed_1 = false; pressed_2 = false; n = 0; sum = 0; rest(); } break; case mvc_biceps_leg: SubstateTransition(); if ((pressed_1) || (pressed_2)) { pressed_1 = false; pressed_2 = false; n = 0; mvc(); } break; default: pc.printf("Unknown or unimplemented state reached!\n\r"); } } void while_vertical_movement() { // Do vertical movement stuff if ((pressed_1) || (pressed_2)) { // EMG gebaseerde threshold aanmaken CurrentState = horizontal_movement; StateChanged = true; } } void while_horizontal_movement() { // Do horizontal movement stuff if ((pressed_1) || (pressed_2)) { // EMG gebaseerde threshold aanmaken CurrentState = vertical_movement; StateChanged = true; } } void StateTransition() { if (StateChanged) { pressed_1 = false; pressed_2 = false; if (CurrentState == start) { pc.printf("Initiating start.\n\r"); } if (CurrentState == motor_calibration) { pc.printf("Initiating motor_calibration.\n\r"); } if (CurrentState == demo_mode) { pc.printf("Initiating demo_mode.\n\r"); } if (CurrentState == emg_calibration) { pc.printf("Initiating emg_calibration.\n\r"); } if (CurrentState == vertical_movement) { pc.printf("Initiating vertical_movement.\n\r"); } if (CurrentState == horizontal_movement) { pc.printf("Initiating horizontal_movement.\n\r"); } StateChanged = false; } } void StateMachine() { switch(CurrentState) { case start: StateTransition(); while_start(); break; case motor_calibration: StateTransition(); while_motor_calibration(); break; case demo_mode: StateTransition(); while_demo_mode(); break; case emg_calibration: StateTransition(); while_emg_calibration(); break; case vertical_movement: StateTransition(); while_vertical_movement(); break; case horizontal_movement: StateTransition(); while_horizontal_movement(); break; default: pc.printf("Unknown or unimplemented state reached!\n\r"); } } // main int main() { pc.baud(115200); pc.printf("Hello World!\n\r"); ledoff(); button_1.fall(go_next_1); button_2.fall(go_next_2); //sinus_time.start(); //PWM_motor_1.period_ms(10); //motor_control.attach(&MotorControl, Ts); write_scope.attach(&WriteScope, 0.01); //TickerStateMachine.attach(StateMachine,1.00f); while(true) { StateMachine(); } }