mag niet van hendrik D:
Dependencies: mbed MatrixMath QEI HIDScope Matrix biquadFilter MODSERIAL FastPWM
main.cpp
- Committer:
- Hendrikvg
- Date:
- 2019-10-25
- Revision:
- 25:832b26afbe0b
- Parent:
- 24:a9ec9b836fd9
File content as of revision 25:832b26afbe0b:
#include "QEI.h" #include "mbed.h" #include "BiQuad.h" #include "FastPWM.h" #include "HIDScope.h" #include "MODSERIAL.h" // Pins MODSERIAL pc(USBTX, USBRX); InterruptIn stepresponse(D0); 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; 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; bool pressed_1 = false; bool pressed_2 = false; HIDScope scope(3); QEI encoder_1(D10,D11,NC,8400,QEI::X4_ENCODING); QEI encoder_2(D12,D13,NC,8400,QEI::X4_ENCODING); volatile double theta_1; //volatile float theta_error_1; volatile float theta_reference_1; volatile double theta_2; //volatile float theta_error_2; volatile float theta_reference_2; float Ts = 0.001; float Kp; float Ki; float Kd; BiQuadChain highnotch; BiQuadChain low; BiQuad Lowpass1 (3.73938e-07, 7.47876e-07, 3.73938e-07, -1.90886e+00, 9.11279e-01); BiQuad Lowpass2 (1.00000e+00, 2.00000e+00, 1.00000e+00, -1.95979e+00, 9.62270e-01); BiQuad Highpass (9.69531e-01, -9.69531e-01, 0.00000e+00, -9.39063e-01, 0.00000e+00 ); BiQuad Notch (9.98432e-01, -1.89913e+00, 9.98432e-01, -1.89913e+00, 9.96863e-01); int n = 0; double emgFiltered_bl; double emgFiltered_br; double emgFiltered_leg; double emg; double xmvc_value = 1e-11; int muscle; float sum = 0; 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 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*sin(0.1f*sinus_time.read()*2*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; } float EmgCalibration(double emgFiltered, float mvc_value, float rest_value) { float emgCalibrated; if (emgFiltered <= rest_value) { emgCalibrated = 0; } if (emgFiltered >= mvc_value) { emgCalibrated = 1; } else { emgCalibrated = (emgFiltered-rest_value)/(mvc_value-rest_value); } return emgCalibrated; } void emgsample() { emgFiltered_bl = highnotch.step(emg_bl.read()); emgFiltered_bl = fabs(emgFiltered_bl); emgFiltered_bl = low.step(emgFiltered_bl); emgFiltered_br = highnotch.step(emg_br.read()); emgFiltered_br = fabs(emgFiltered_br); emgFiltered_br = low.step(emgFiltered_br); emgFiltered_leg = highnotch.step(emg_leg.read()); emgFiltered_leg = fabs(emgFiltered_leg); emgFiltered_leg = low.step(emgFiltered_leg); } void rest() { if (muscle == 0) { emg = emgFiltered_bl; } if (muscle == 2) { emg = emgFiltered_br; } if (muscle == 4) { emg = emgFiltered_leg; } if (n < 50) { sum = sum + emg; n++; rest_timeout.attach(rest,0.01f); } if (n == 50) { sum = sum + emg; n++; if (muscle == 0) { rest_value_bl = float (sum/n); CurrentSubstate = mvc_biceps_left; SubstateChanged = true; led_red = 1; } if (muscle == 2) { rest_value_br = float (sum/n); CurrentSubstate = mvc_biceps_right; SubstateChanged = true; led_red = 1; } if (muscle == 4) { rest_value_leg = float (sum/n); CurrentSubstate = mvc_biceps_leg; SubstateChanged = true; led_red = 1; } n = 0; sum = 0; } } void mvc() { if (muscle == 1) { emg = emgFiltered_bl; } if (muscle == 3) { emg = emgFiltered_br; } if (muscle == 5) { emg = emgFiltered_leg; } if (emg >= xmvc_value) { xmvc_value = emg; } n++; if (n < 100) { mvc_timeout.attach(mvc,0.01f); } if (n == 100) { n = 0; if (muscle == 1) { mvc_value_bl = xmvc_value; CurrentSubstate = rest_biceps_right; SubstateChanged = true; led_red = 1; } if (muscle == 3) { mvc_value_br = xmvc_value; CurrentSubstate = rest_biceps_leg; SubstateChanged = true; led_red = 1; } if (muscle == 5) { mvc_value_leg = xmvc_value; CurrentState = vertical_movement; StateChanged = true; led_red = 1; } xmvc_value = 1e-11; led_red = 1; } } void WriteScope() { //scope.set(0,theta_1); // scope.set(1,CalculateError(theta_reference_1,theta_1)); // scope.set(2,theta_reference_1); 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.send(); } void SubstateTransition() { pressed_1 = false; pressed_2 = false; SubstateChanged = false; } 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(); muscle = 0; led_green = 0; if (pressed_1 || pressed_2) { led_green = 1; led_red = 0; rest(); } break; case mvc_biceps_left: SubstateTransition(); muscle = 1; led_blue = 0; if (pressed_1 || pressed_2) { led_blue = 1; led_red = 0; mvc(); } break; case rest_biceps_right: SubstateTransition(); muscle = 2; led_red = 0; led_green = 0; if (pressed_1 || pressed_2) { led_green = 1; rest(); } break; case mvc_biceps_right: SubstateTransition(); muscle = 3; led_red = 0; led_blue = 0; if (pressed_1 || pressed_2) { led_blue = 1; mvc(); } break; case rest_biceps_leg: SubstateTransition(); muscle = 4; led_green = 0; led_blue = 0; if (pressed_1 || pressed_2) { led_green = 1; led_blue = 1; led_red = 0; rest(); } break; case mvc_biceps_leg: SubstateTransition(); muscle = 5; led_red = 0; led_green = 0; led_blue = 0; if (pressed_1 || pressed_2) { led_green = 1; led_blue = 1; led_red = 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() { pressed_1 = false; pressed_2 = false; if (StateChanged) { 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"); 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.1); //TickerStateMachine.attach(StateMachine,1.00f); while(true) { StateMachine(); } }