![](/media/cache/group/9e3cc099b3b04bca937a1cca1da81b19.jpg.50x50_q85.jpg)
De hele robot in 1 keer bam
Dependencies: mbed QEI Servo HIDScope biquadFilter MODSERIAL FastPWM
Diff: main.cpp
- Revision:
- 41:8e8141f355af
- Parent:
- 40:c6dffb676350
- Child:
- 42:2937ad8f1032
--- a/main.cpp Wed Oct 30 19:37:41 2019 +0000 +++ b/main.cpp Wed Oct 30 20:44:56 2019 +0000 @@ -447,7 +447,7 @@ */ // Set global constant values for EMG reading & analysis -const double Tcal = 10.0f; // Calibration duration (s) +const double Tcal = 7.5f; // Calibration duration (s) // Initialize variables for EMG reading & analysis double emg1; @@ -460,7 +460,7 @@ double emg1_norm; //delete vector<double> emg1_cal; int emg1_cal_size; //delete -int emg1_dir = 1; +double emg1_dir = 1.0; double emg1_out_prev; double emg1_dt; //delete double emg1_dt_prev; @@ -476,7 +476,7 @@ double emg2_norm;//delete vector<double> emg2_cal; int emg2_cal_size;//delete -int emg2_dir = 1; +double emg2_dir = 1.0; double emg3; double emg3_env; @@ -599,6 +599,8 @@ // Set HIDScope outputs scope.set(0, emg1 ); scope.set(1, emg1_env ); + scope.set(2, emg2 ); + scope.set(3, emg2_env ); //scope.set(2, emg2_env ); //scope.set(3, emg3_env ); scope.send(); @@ -655,15 +657,19 @@ //button1.fall( &toggleEMG1Dir ); // Change to state MVC calibration on button1 press //button2.fall( &toggleEMG2Dir ); // Change to state rest calibration on button2 press - emg_cal_done = true; // Let the global substate machine know that EMG calibration is finished } // This state only runs its entry functions ONCE and then exits the EMG substate machine // State transition guard - if ( false ) { // EMG substate machine is terminated after running this state once, so there is no transition to next EMG substate + if ( true ) { // EMG substate machine is terminated directly after running this state once emg_curr_state = emg_wait; // Set next state emg_state_changed = true; // Enable entry function + emg_cal_done = true; // Let the global substate machine know that EMG calibration is finished + + // Enable buttons again + button1.fall( &button1Press ); + button2.fall( &button2Press ); } } @@ -836,13 +842,7 @@ motor_state_machine(); // State transition guard - if ( emg_cal_done == true ) { // OPERATION MODE - motor_cal_done = true; - global_curr_state = global_operation; - global_state_changed = true; - } else if ( button2_pressed == true ) { // WAIT MODE - button2_pressed = false; - motor_cal_done = true; + if ( motor_cal_done == true ) { // WAIT MODE global_curr_state = global_wait; global_state_changed = true; } @@ -866,6 +866,16 @@ emg1_out_prev = emg1_out; // Set previous emg_out signal emg1_dt_prev = emg1_dt; // Set previous emg_out_dt signal + + if (button1_pressed) { + button1_pressed = false; + emg1_dir = emg1_dir * -1.0; + } + + if (button2_pressed) { + button2_pressed = false; + emg2_dir = emg2_dir * -1.0; + } // Set normalized EMG output signal (CAN BE MOVED TO EXTERNAL FUNCTION BECAUSE IT IS REPEATED 3 TIMES) if ( emg1_norm < emg1_th ) { // If below threshold, emg_out = 0 (ignored) @@ -879,8 +889,7 @@ } emg1_dt = (emg1_out - emg1_out_prev) / Ts; // Calculate derivative of filtered normalized output signal emg1_dtdt = (emg1_dt - emg1_dt_prev) / Ts; // Calculate acceleration of filtered normalized output signal - emg1_out = emg1_out * emg1_dir; // Set direction of EMG output - Vx = emg1_out; + Vx = emg1_out * 15.0f * emg1_dir; // Idem for emg2 if ( emg2_norm < emg2_th ) { @@ -890,8 +899,7 @@ } else { emg2_out = rescale(emg2_norm, 0, 1, emg2_th, 1); } - emg2_out = emg2_out * emg2_dir; // Set direction of EMG output - Vy = emg2_out; + Vy = emg2_out * 15.0f * emg2_dir; // Idem for emg3 if ( emg3_norm < emg3_th ) { @@ -917,9 +925,9 @@ // Set HIDScope outputs scope.set(0, emg1 ); - scope.set(1, emg1_out ); - scope.set(2, emg1_dt ); - scope.set(3, emg1_dtdt ); + scope.set(1, Vx ); + scope.set(2, emg2 ); + scope.set(3, Vy ); //scope.set(2, emg2_out ); //scope.set(3, emg3_out ); scope.send(); @@ -1056,7 +1064,8 @@ while(true) { pc.printf("Global state: %i EMG substate: %i Motor substate: %i\r\n", global_curr_state, emg_curr_state, motor_curr_state); - + pc.printf("EMG1 direction: %f EMG2 direction: %f \r\n", emg1_dir, emg2_dir); + pc.printf("Vx: %f Vy: %f \r\n", Vx, Vy); pc.printf("q1: %f q2: %f \r\n",q1*rad2deg,q2*rad2deg); pc.printf("Xe: %f Ye: %f\r\n",xe,ye); wait(0.5f);