mag niet van hendrik D:
Dependencies: mbed MatrixMath QEI HIDScope Matrix biquadFilter MODSERIAL FastPWM
Diff: main.cpp
- Revision:
- 24:a9ec9b836fd9
- Parent:
- 23:78898ddfb103
- Child:
- 25:832b26afbe0b
--- a/main.cpp Mon Oct 21 10:14:44 2019 +0000 +++ b/main.cpp Mon Oct 21 15:11:14 2019 +0000 @@ -8,24 +8,39 @@ // 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; -bool demo = false; -bool emg = false; -bool next = false; +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); @@ -40,6 +55,31 @@ 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; +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) { @@ -49,17 +89,14 @@ float Controller(float theta_error, bool motor) { - if (motor == false) - { + 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 - { + } else { float K = 1; float ti = 0.1; float td = 10; @@ -70,46 +107,38 @@ 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() +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 WriteScope() -{ - scope.set(0,theta_1); - scope.set(1,CalculateError(theta_reference_1,theta_1)); - scope.set(2,theta_reference_1); - scope.send(); -} - 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() +void MotorControl() { ReadEncoder(); theta_reference_1 = 360*sin(0.1f*sinus_time.read()*2*3.14f); // voor test, moet weg in eindscript @@ -120,13 +149,163 @@ void go_next_1() { - demo = !demo; + pressed_1 = !pressed_1; } void go_next_2() { - emg = !emg; - next = emg; + 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_value = float (sum/n); + rest_timeout.attach(rest,0.01f); + } + if (n == 50) + { + sum = sum + emg; + n++; + rest_value = float (sum/n); + n = 0; + sum = 0; + if (muscle == 0) + { + rest_value_bl = rest_value; + CurrentSubstate = mvc_biceps_left; + SubstateChanged = true; + led_red = 1; + } + if (muscle == 2) + { + rest_value_br = rest_value; + CurrentSubstate = mvc_biceps_right; + SubstateChanged = true; + led_red = 1; + } + if (muscle == 4) + { + rest_value_leg = rest_value; + CurrentSubstate = mvc_biceps_leg; + SubstateChanged = true; + led_red = 1; + } + } +} + +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() @@ -139,13 +318,11 @@ void while_motor_calibration() { // Do motor calibration stuff - if (demo) // bool aanmaken voor demo (switch oid aanmaken) - { + if (pressed_1) { // bool aanmaken voor demo (switch oid aanmaken) CurrentState = demo_mode; StateChanged = true; } - if (emg) // bool aanmaken voor EMG (switch oid aanmaken) - { + if (pressed_2) { // bool aanmaken voor EMG (switch oid aanmaken) CurrentState = emg_calibration; StateChanged = true; } @@ -154,12 +331,7 @@ void while_demo_mode() { // Do demo mode stuff - if (!demo) // bool aanmaken voor demo (switch oid) - { - emg = true; - } - if (emg) // bool aanmaken voor EMG (switch oid aanmaken) - { + if (pressed_1 || pressed_2) { CurrentState = emg_calibration; StateChanged = true; } @@ -168,18 +340,87 @@ void while_emg_calibration() { // Do emg calibration stuff - if (!emg) // bool aanmaken voor EMG (switch) - { - CurrentState = vertical_movement; - StateChanged = true; + 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 (next) // EMG gebaseerde threshold aanmaken - { + if (pressed_1 || pressed_2) { // EMG gebaseerde threshold aanmaken CurrentState = horizontal_movement; StateChanged = true; } @@ -188,8 +429,7 @@ void while_horizontal_movement() { // Do horizontal movement stuff - if (!next) // EMG gebaseerde threshold aanmaken - { + if (pressed_1 || pressed_2) { // EMG gebaseerde threshold aanmaken CurrentState = vertical_movement; StateChanged = true; } @@ -197,30 +437,25 @@ void StateTransition() { - if (StateChanged) - { - if (CurrentState == start) - { + pressed_1 = false; + pressed_2 = false; + if (StateChanged) { + if (CurrentState == start) { pc.printf("Initiating start.\n\r"); } - if (CurrentState == motor_calibration) - { + if (CurrentState == motor_calibration) { pc.printf("Initiating motor_calibration.\n\r"); } - if (CurrentState == demo_mode) - { + if (CurrentState == demo_mode) { pc.printf("Initiating demo_mode.\n\r"); } - if (CurrentState == emg_calibration) - { + if (CurrentState == emg_calibration) { pc.printf("Initiating emg_calibration.\n\r"); } - if (CurrentState == vertical_movement) - { + if (CurrentState == vertical_movement) { pc.printf("Initiating vertical_movement.\n\r"); } - if (CurrentState == horizontal_movement) - { + if (CurrentState == horizontal_movement) { pc.printf("Initiating horizontal_movement.\n\r"); } StateChanged = false; @@ -229,8 +464,7 @@ void StateMachine() { - switch(CurrentState) - { + switch(CurrentState) { case start: StateTransition(); while_start(); @@ -261,7 +495,8 @@ } // main -int main(){ +int main() +{ pc.baud(115200); pc.printf("Hello World!\n\r"); button_1.fall(go_next_1);