De hele robot in 1 keer bam
Dependencies: mbed QEI Servo HIDScope biquadFilter MODSERIAL FastPWM
Diff: main.cpp
- Revision:
- 45:d09040915cfe
- Parent:
- 44:342af9b3c1a0
- Child:
- 46:8a8fa8e602a1
diff -r 342af9b3c1a0 -r d09040915cfe main.cpp --- a/main.cpp Thu Oct 31 13:37:25 2019 +0000 +++ b/main.cpp Thu Oct 31 14:14:23 2019 +0000 @@ -33,6 +33,8 @@ AnalogIn emg1_in (A0); // Right biceps -> x axis AnalogIn emg2_in (A1); // Left biceps -> y axis AnalogIn emg3_in (A2); // Third muscle -> TBD +AnalogIn emg4_in (A3); // Third muscle -> TBD + // Encoder inputs DigitalIn encA1(D9); @@ -197,10 +199,6 @@ vector<double> emg1_cal; int emg1_cal_size; //delete double emg1_dir = 1.0; -double emg1_out_prev; -double emg1_dt; //delete -double emg1_dt_prev; -double emg1_dtdt; //delete double emg2; double emg2_env; @@ -223,8 +221,16 @@ double emg3_out; double emg3_norm;//delete vector<double> emg3_cal; -int emg3_cal_size;//delete -int emg3_dir = 1; + +double emg4; +double emg4_env; +double emg4_MVC; +double emg4_rest; +double emg4_factor;//delete +double emg4_th; +double emg4_out; +double emg4_norm;//delete +vector<double> emg4_cal; /* ------------------------------ EMG FILTERS ------------------------------ @@ -234,9 +240,11 @@ BiQuad bq1_notch( 0.995636295063941, -1.89829218816065, 0.995636295063941, 1, -1.89829218816065, 0.991272590127882); // b01 b11 b21 a01 a11 a21 BiQuad bq2_notch = bq1_notch; BiQuad bq3_notch = bq1_notch; +BiQuad bq4_notch = bq1_notch; BiQuadChain bqc1_notch; BiQuadChain bqc2_notch; BiQuadChain bqc3_notch; +BiQuadChain bqc4_notch; // Highpass biquad filter coefficients (butter 4th order @10Hz cutoff) from MATLAB BiQuad bq1_H1(0.922946103200875, -1.84589220640175, 0.922946103200875, 1, -1.88920703055163, 0.892769008131025); // b01 b11 b21 a01 a11 a21 @@ -245,9 +253,12 @@ BiQuad bq2_H2 = bq1_H2; BiQuad bq3_H1 = bq1_H1; BiQuad bq3_H2 = bq1_H2; +BiQuad bq4_H1 = bq1_H1; +BiQuad bq4_H2 = bq1_H2; BiQuadChain bqc1_high; BiQuadChain bqc2_high; BiQuadChain bqc3_high; +BiQuadChain bqc4_high; // Lowpass biquad filter coefficients (butter 4th order @5Hz cutoff) from MATLAB: BiQuad bq1_L1(5.32116245737504e-08, 1.06423249147501e-07, 5.32116245737504e-08, 1, -1.94396715039462, 0.944882378004138); // b01 b11 b21 a01 a11 a21 @@ -256,9 +267,12 @@ BiQuad bq2_L2 = bq1_L2; BiQuad bq3_L1 = bq1_L1; BiQuad bq3_L2 = bq1_L2; +BiQuad bq4_L1 = bq1_L1; +BiQuad bq4_L2 = bq1_L2; BiQuadChain bqc1_low; BiQuadChain bqc2_low; BiQuadChain bqc3_low; +BiQuadChain bqc4_low; // Function to check filter stability bool checkBQChainStable() @@ -283,19 +297,8 @@ emg1_norm = emg1_env * emg1_factor; // Normalize current EMG signal with calibrated factor emg2_norm = emg2_env * emg2_factor; // Idem emg3_norm = emg3_env * emg3_factor; // Idem - - emg1_out_prev = emg1_out; // Set previous emg_out signal - emg1_dt_prev = emg1_dt; // Set previous emg_out_dt signal + emg4_norm = emg4_env * emg4_factor; // Idem - 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) @@ -307,8 +310,6 @@ // Outputs are scaled to range [0, 1] emg1_out = rescale(emg1_norm, 0, 1, emg1_th, 1); } - 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 // Idem for emg2 if ( emg2_norm < emg2_th ) { @@ -321,11 +322,15 @@ // Idem for emg3 if ( emg3_norm < emg3_th ) { - emg3_out = 0.0; - } else if ( emg3_norm > 1.0f ) { - emg3_out = 1.0; + emg1_dir = 1.0; } else { - emg3_out = rescale(emg3_norm, 0, 1, emg3_th, 1); + emg1_dir = -1.0; + } + + if ( emg4_norm < emg4_th ) { + emg2_dir = 1.0; + } else { + emg2_dir = -1.0; } } /* @@ -384,14 +389,15 @@ emg1_cal.reserve(Fs * Tcal); // Initialize vector lengths to prevent memory overflow emg2_cal.reserve(Fs * Tcal); // Idem emg3_cal.reserve(Fs * Tcal); // Idem + emg4_cal.reserve(Fs * Tcal); // Idem } // Do stuff until end condition is met // Set HIDScope outputs scope.set(0, emg1 ); - scope.set(1, emg1_env ); - scope.set(2, emg2 ); - scope.set(3, emg2_env ); + scope.set(1, emg2 ); + scope.set(2, emg3 ); + scope.set(3, emg4 ); //scope.set(2, emg2_env ); //scope.set(3, emg3_env ); scope.send(); @@ -408,19 +414,21 @@ emg1_MVC = getMax(emg1_cal); // Store max value of MVC globally emg2_MVC = getMax(emg2_cal); emg3_MVC = getMax(emg3_cal); - + emg4_MVC = getMax(emg4_cal); emg_MVC_cal_done = true; // Set up transition to EMG operation mode break; case emg_cal_rest: // In case of rest calibration emg1_rest = getMean(emg1_cal); // Store mean of EMG in rest globally emg2_rest = getMean(emg2_cal); emg3_rest = getMean(emg3_cal); + emg4_rest = getMean(emg4_cal); emg_rest_cal_done = true; // Set up transition to EMG operation mode break; } vector<double>().swap(emg1_cal); // Empty vector to prevent memory overflow vector<double>().swap(emg2_cal); vector<double>().swap(emg3_cal); +vector<double>().swap(emg4_cal); emg_curr_state = emg_wait; // Set next substate emg_state_changed = true; // Enable substate entry function @@ -442,12 +450,8 @@ emg2_th = emg2_rest * emg2_factor + margin_percentage/100; // Set normalized rest threshold emg3_factor = 1 / emg3_MVC; // Factor to normalize MVC emg3_th = emg3_rest * emg3_factor + margin_percentage/100; // Set normalized rest threshold - - - // ------- TO DO: MAKE SURE THESE BUTTONS DO NOT BOUNCE (e.g. with button1.rise() ) ------ - //button1.fall( &toggleEMG1Dir ); // Change to state MVC calibration on button1 press - //button2.fall( &toggleEMG2Dir ); // Change to state rest calibration on button2 press - + emg4_factor = 1 / emg4_MVC; // Factor to normalize MVC + emg4_th = emg4_rest * emg4_factor + margin_percentage/100; // Set normalized rest threshold } // This state only runs its entry functions ONCE and then exits the EMG substate machine @@ -811,10 +815,17 @@ } // State transition guard - if ( switch2_pressed == true ) { - switch2_pressed = false; + if ( button1_pressed == true ) { + button1_pressed = false; operation_curr_state = operation_movement; operation_state_changed = true; + } else if ( button2_pressed == true ) { + button2_pressed = false; + //operation_curr_state = operation_finish; // To move to finished operation mode (not used yet) + operation_curr_state = operation_wait; // TEMPORARY + operation_state_changed = true; + global_curr_state = global_wait; // TEMPORARY move directly to global wait state + global_state_changed = true; // TEMPORARY move directly to global wait state } } @@ -843,14 +854,21 @@ } // State transition guard - if ( switch2_pressed == true ) { - switch2_pressed = false; + if ( button1_pressed == true ) { + button1_pressed = false; operation_curr_state = operation_wait; operation_state_changed = true; + } else if ( button2_pressed == true ) { + button2_pressed = false; + //operation_curr_state = operation_finish; // To move to finished operation mode (not used yet) + operation_curr_state = operation_wait; // TEMPORARY + operation_state_changed = true; + global_curr_state = global_wait; // TEMPORARY move directly to global wait state + global_state_changed = true; // TEMPORARY move directly to global wait state } } -void do_operation_finish() +void do_operation_finish() // NOT USED YET { // Entry function if ( operation_state_changed == true ) { @@ -872,8 +890,8 @@ // Function to move to starting position // State transition guard - if ( switch2_pressed == true ) { - switch2_pressed = false; + if ( button2_pressed == true ) { + button2_pressed = false; operation_curr_state = operation_wait; operation_state_changed = true; @@ -1230,6 +1248,7 @@ emg1 = emg1_in.read(); emg2 = emg2_in.read(); emg3 = emg3_in.read(); + emg4 = emg4_in.read(); double emg1_n = bqc1_notch.step( emg1 ); // Filter notch double emg1_hp = bqc1_high.step( emg1_n ); // Filter highpass @@ -1245,14 +1264,17 @@ double emg3_hp = bqc3_high.step( emg3_n ); // Filter highpass double emg3_rectify = fabs( emg3_hp ); // Rectify emg3_env = bqc3_low.step( emg3_rectify ); // Filter lowpass (completes envelope) + + double emg4_n = bqc4_notch.step( emg4 ); // Filter notch + double emg4_hp = bqc4_high.step( emg4_n ); // Filter highpass + double emg4_rectify = fabs( emg4_hp ); // Rectify + emg4_env = bqc4_low.step( emg4_rectify ); // Filter lowpass (completes envelope) if (emg_calibrateNow == true) { // Only add values to EMG vectors if calibration flag is true emg1_cal.push_back(emg1_env); // Add values to calibration vector - // emg1_cal_size = emg1_cal.size(); // Used for debugging emg2_cal.push_back(emg2_env); // Add values to calibration vector - // emg2_cal_size = emg1_cal.size(); // Used for debugging emg3_cal.push_back(emg3_env); // Add values to calibration vector - // emg3_cal_size = emg1_cal.size(); // Used for debugging + emg4_cal.push_back(emg4_env); // Add values to calibration vector } } @@ -1299,6 +1321,10 @@ bqc3_notch.add( &bq3_notch ); bqc3_high.add( &bq3_H1 ).add( &bq3_H2 ); bqc3_low.add( &bq3_L1 ).add( &bq3_L2 ); + + bqc4_notch.add( &bq4_notch ); + bqc4_high.add( &bq4_H1 ).add( &bq4_H2 ); + bqc4_low.add( &bq4_L1 ).add( &bq4_L2 ); // ---------- Attach buttons ---------- button1.fall( &button1Press ); @@ -1316,11 +1342,10 @@ led_r = 1; 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("Global state: %i EMG state: %i Motor state: %i Operation state: %i Demo state: %i\r\n", global_curr_state, emg_curr_state, motor_curr_state, operation_curr_state, demo_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); + //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); } } \ No newline at end of file