23:00
Dependencies: biquadFilter MODSERIAL QEI Servo mbed
Fork of StateMachine_EMg_RKI_PID_MOTOR_DEMO_CLICK by
Diff: main.cpp
- Revision:
- 13:0e25698dce40
- Parent:
- 12:99e29b8d4155
- Child:
- 14:fb5d8064830d
diff -r 99e29b8d4155 -r 0e25698dce40 main.cpp --- a/main.cpp Thu Nov 01 17:51:37 2018 +0000 +++ b/main.cpp Thu Nov 01 18:56:23 2018 +0000 @@ -196,7 +196,7 @@ led1=!led1; if(emg1_filtered>temp_highest_emg1) { temp_highest_emg1= emg1_filtered; - pc.printf("Temp1 = %f \r\n",temp_highest_emg1); + //pc.printf("Temp1 = %f \r\n",temp_highest_emg1); } } if(timer_calibration>10 && timer_calibration<15) { @@ -208,7 +208,7 @@ led2=!led2; if(emg2_filtered>temp_highest_emg2) { temp_highest_emg2= emg2_filtered; - pc.printf("Temp2 = %f \r\n",temp_highest_emg2); + //pc.printf("Temp2 = %f \r\n",temp_highest_emg2); } } if(timer_calibration>25 && timer_calibration<30) { @@ -220,7 +220,7 @@ led3=!led3; if(emg3_filtered>temp_highest_emg3) { temp_highest_emg3= emg3_filtered; - pc.printf("Temp3 = %f \r\n",temp_highest_emg3); + //pc.printf("Temp3 = %f \r\n",temp_highest_emg3); } } if(timer_calibration>40 && timer_calibration<45) { @@ -233,7 +233,7 @@ led3=!led3; if(emg4_filtered>temp_highest_emg4) { temp_highest_emg4= emg4_filtered; - pc.printf("Temp4 = %f \r\n",temp_highest_emg4); + //pc.printf("Temp4 = %f \r\n",temp_highest_emg4); } } led1=1; @@ -290,17 +290,7 @@ */ } -//Activate ticker for Movement state, filtering and Threshold checking -void movement_ticker_activator() -{ - sample_ticker.attach(&emgsample, ts); - threshold_check_ticker.attach(&threshold_check, ts); -} -void movement_ticker_deactivator() -{ - sample_ticker.detach(); - threshold_check_ticker.detach(); -} + // ~~~~~~~~~~~~~~ROBOT KINEMATICS ~~~~~~~~~~~~~~~~~~ @@ -450,6 +440,9 @@ // CONTROLLING THE MOTOR void Motor_mover() { + float px = positionx(bicepsR,bicepsL); // EMG: +x, -x + float py = positiony(tricepsL,tricepsL); // EMG: +y, -y + double motor_position = encoder1.getPulses(); //output in counts double reference_rotation = hoek1(px, py); double error = reference_rotation - motor_position*(2*PI)/8400; @@ -471,7 +464,22 @@ } - +//Activate ticker for Movement state, filtering and Threshold checking +void emg_sample_ticker(){ + sample_ticker.attach(&emgsample, ts); + //sample_ticker.attach(&Motor_mover, ts); + + } +void movement_ticker_activator() +{ + sample_ticker.attach(&emgsample, ts); + sample_ticker.attach(&threshold_check, ts); +} +void movement_ticker_deactivator() +{ + sample_ticker.detach(); + //sample_ticker_ticker.detach(); +} //-------------------- STATE MACHINE -------------------------- enum states {MOTORS_OFF,CALIBRATION,HOMING,DEMO,MOVEMENT,CLICK}; @@ -480,6 +488,7 @@ void ProcessStateMachine(void) { + switch (currentState) { case MOTORS_OFF: // Actions @@ -518,9 +527,8 @@ timer_calibration.reset(); timer_calibration.start(); - sample_ticker.attach(&emgsample, ts); + CalibrationEMG(); - sample_ticker.detach(); timer_calibration.stop(); @@ -572,7 +580,7 @@ led1 = 0; led2 = 1; led3 = 0; - wait (1); + //wait (1); stateChanged = false; } @@ -697,6 +705,7 @@ int main() { + //BiQuad Chain add highp1.add( &highp1_1 ).add( &highp1_2 ); notch1.add( ¬ch1_1 ).add( ¬ch1_2 ); @@ -720,11 +729,13 @@ led3 = 1; pwmpin1.period_us(60); // setup motor - ref_rot.attach(Motor_mover, 0.01f);// HAS TO GO TO STATE MACHINE - movement_ticker_activator(); + //ref_rot.attach(Motor_mover, 0.01f);// HAS TO GO TO STATE MACHINE + //movement_ticker_activator(); + emg_sample_ticker(); while (true) { - //ProcessStateMachine(); - + ProcessStateMachine(); + +/* if (button2 == false) { wait(0.01f);