lololololol
Dependencies: mbed QEI HIDScope biquadFilter MODSERIAL FastPWM
Diff: main.cpp
- Revision:
- 33:1da600f06862
- Parent:
- 32:7355524d862f
- Child:
- 34:89a424fd37ce
diff -r 7355524d862f -r 1da600f06862 main.cpp --- a/main.cpp Wed Oct 30 18:17:00 2019 +0000 +++ b/main.cpp Thu Oct 31 10:05:31 2019 +0000 @@ -53,7 +53,7 @@ volatile float theta_ref1; volatile float theta_ref2; -float Ts = 0.01; +float Ts = 0.01f; float Kp; float Ki; float Kd; @@ -65,10 +65,8 @@ float torque_2; float x; float y; -volatile float EMGx_velocity=0.02; -volatile float EMGy_velocity=0; -char beweging; - +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); @@ -105,7 +103,7 @@ bool previous_value_emg_leg; bool current_value_emg_leg; - +volatile char command; // functies void ledred() { @@ -156,6 +154,36 @@ 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; + } +} + void Controller() { float K = 1; @@ -469,8 +497,11 @@ 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 = emg_calibration; + CurrentState = vertical_movement; StateChanged = true; } } @@ -571,7 +602,7 @@ else { EMGy_velocity = 0.0f; - pc.printf("beweging %f \n\r", EMGy_velocity); + //pc.printf("beweging %f \n\r", EMGy_velocity); } if ((pressed_1) || (pressed_2) /*|| (emg_switch(treshold_leg,EmgCalibration(emgFiltered_leg, mvc_value_leg, rest_value_leg)))*/) { // EMG gebaseerde threshold aanmaken @@ -584,22 +615,25 @@ void while_horizontal_movement() { // Do horizontal movement stuff - /* - if (emg_trigger(treshold_bl,EmgCalibration(emgFiltered_bl, mvc_value_bl, rest_value_bl))) { + + if ((emg_trigger(treshold_bl,EmgCalibration(emgFiltered_bl, mvc_value_bl, rest_value_bl))) || (get_command_a())){ EMGx_velocity = -0.02f; + pc.printf("beweging %f \n\r", EMGx_velocity); } - else if (emg_trigger(treshold_br,EmgCalibration(emgFiltered_br, mvc_value_br, rest_value_br))) { + if (emg_trigger(treshold_br,EmgCalibration(emgFiltered_br, mvc_value_br, rest_value_br)) || (get_command_d())) { EMGx_velocity = 0.02f; + pc.printf("beweging %f \n\r", EMGx_velocity); } - else { + if ((!(emg_trigger(treshold_bl,EmgCalibration(emgFiltered_bl, mvc_value_bl, rest_value_bl))) && !(emg_trigger(treshold_br,EmgCalibration(emgFiltered_br, mvc_value_br, rest_value_br)))) && (!get_command_a() && !get_command_d())) { EMGx_velocity = 0.0f; + pc.printf("beweging %f \n\r", EMGx_velocity); } - if ((pressed_1) || (pressed_2) || (emg_switch(treshold_leg,EmgCalibration(emgFiltered_leg, mvc_value_leg, rest_value_leg)))) { // EMG gebaseerde threshold aanmaken + if ((pressed_1) || (pressed_2) || (emg_switch(treshold_leg,EmgCalibration(emgFiltered_leg, mvc_value_leg, rest_value_leg))) || (get_command_s())) { // EMG gebaseerde threshold aanmaken CurrentState = vertical_movement; StateChanged = true; } - */ + /* if (beweging == 'a') { EMGx_velocity = -0.02f; pc.printf(" you pressed %c \n\r" , beweging); @@ -613,10 +647,10 @@ EMGx_velocity = 0.0f; } - if ((pressed_1) || (pressed_2) /*|| (emg_switch(treshold_leg,EmgCalibration(emgFiltered_leg, mvc_value_leg, rest_value_leg)))*/) { // EMG gebaseerde threshold aanmaken + 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; - } + }*/ } @@ -692,13 +726,6 @@ PWM_motor_2.period_ms(10); motor_control.attach(&MotorControl, Ts); write_scope.attach(&WriteScope, 0.01); - - void CalculateDirectionMotor(); - void MotorControl(); - - - TickerStateMachine.attach(StateMachine,1.00f); - beweging = pc.getc(); while(true) { StateMachine(); }