lololololol
Dependencies: mbed QEI HIDScope biquadFilter MODSERIAL FastPWM
main.cpp
- Committer:
- sembert
- Date:
- 2019-10-31
- Revision:
- 34:89a424fd37ce
- Parent:
- 33:1da600f06862
- Child:
- 35:ab9b1c9b6d08
File content as of revision 34:89a424fd37ce:
#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 int m = 0; const float pi = 3.1416; const float l = 0.535; 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_ref1; volatile float theta_ref2; float Ts = 0.01f; float Kp; float Ki; float Kd; float theta_1 = (40.0f*pi)/180.0f; float theta_2 = (175.0f*pi)/180.0f; float theta_error1; float theta_error2; float torque_1; float torque_2; float x; float y; volatile float EMGx_velocity = 0.0f; volatile float EMGy_velocity = 0.0f; 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; float treshold_bl = 0.5; float treshold_br = 0.5; float treshold_leg = 0.5; bool previous_value_emg_leg; bool current_value_emg_leg; volatile char command; // 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; } bool get_command_a() { command = pc.getcNb(); if (command == 'a') { pc.printf("a is ingedrukt! \n\r"); return true; } else { return false; } } bool get_command_d () { command = pc.getcNb(); if (command == 'd') { pc.printf("d is ingedrukt! \n\r"); return true; } else { return false; } } bool get_command_s () { command = pc.getcNb(); if (command == 's') { pc.printf("s is ingedrukt! \n\r"); return true; } else { return false; } } 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 Kinematics() { float DET_jacobian= 1.0f/((-l*sin(theta_1)-l*sin(theta_1+theta_2))*(l*cos(theta_1+theta_2))-(-l*sin(theta_1+theta_2))*(l*cos(theta_1)+l*cos(theta_1+theta_2))); float thetav_1=DET_jacobian*l*cos(theta_1+theta_2)*EMGx_velocity + DET_jacobian*l*sin(theta_1+theta_2)*EMGy_velocity; float thetav_2= DET_jacobian*-l*cos(theta_1)-l*cos(theta_1+theta_2)*EMGx_velocity + DET_jacobian*-l*sin(theta_1)-l*sin(theta_1+theta_2)*EMGy_velocity; theta_ref1=theta_1+thetav_1*Ts; theta_ref2=theta_2+thetav_2*Ts; x=cos(theta_ref1)*l+cos(theta_ref1+theta_ref2)*l; y=sin(theta_ref1)*l+sin(theta_ref1+theta_ref2)*l; if (sqrt(pow(x,2)+pow(y,2))>1.0f) { theta_ref1 = theta_1; theta_ref2 = theta_2; } } void CalculateDirectionMotor() { //Kinematics(); direction_motor_1 = Controller(CalculateError(theta_ref1,theta_1),0) <= 0.0f; direction_motor_2 = Controller(CalculateError(theta_ref2,theta_2),1) <= 0.0f; } void ReadEncoder() { theta_1 = (((2.0f*pi)/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 = (((2.0f*pi)/64.0f)*(float)encoder_2.getPulses())/131.25f; } void MotorControl() { ReadEncoder(); CalculateDirectionMotor(); PWM_motor_1.write(fabs(torque_1)/(2.0f*pi)); PWM_motor_2.write(fabs(torque_2)/(2.0f*pi)); } 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){ current_value_emg_leg = true; } else { current_value_emg_leg = false; } if(current_value_emg_leg == true && previous_value_emg_leg == false) { previous_value_emg_leg = current_value_emg_leg; return true; } else { previous_value_emg_leg = current_value_emg_leg; return false; } } bool emg_trigger(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, theta_ref1); scope.set(1, theta_1); scope.set(2, theta_ref2); scope.set(3, theta_2); scope.set(4, CalculateError(theta_ref1,theta_1)); scope.set(5, CalculateError(theta_ref2,theta_2)); 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 treshold_bl = 1.1f; treshold_br = 1.1f; treshold_leg = 1.1f; if ((pressed_1) || (pressed_2)) { CurrentState = vertical_movement; 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 //theta_ref1 = *(1.0f+sin(2*pi*sinus_time.read())); MotorControl(); if ((pressed_1) || (pressed_2) || (emg_switch(treshold_leg,EmgCalibration(emgFiltered_leg, mvc_value_leg, rest_value_leg)))) { // EMG gebaseerde threshold aanmaken CurrentState = horizontal_movement; StateChanged = true; } } void while_horizontal_movement() { // Do horizontal movement stuff if ((pressed_1) || (pressed_2) || (emg_switch(treshold_leg,EmgCalibration(emgFiltered_leg, mvc_value_leg, rest_value_leg)))) { // 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(1.0f/16.0f); PWM_motor_2.period_ms(1.0f/16.0f); //motor_control.attach(&MotorControl, Ts); write_scope.attach(&WriteScope, 0.01); TickerStateMachine.attach(&StateMachine,1.0f/60.0f); while(true) { return 0; } }