Gaston Gabriël
23:00
Dependencies: biquadFilter MODSERIAL QEI Servo mbed
Fork of
StateMachine_EMg_RKI_PID_MOTOR_DEMO_CLICK
by 
Gaston Gabriël
Diff: main.cpp
- Revision:
- 7:88fa84742b3c
- Parent:
- 6:f55ab7e38a7f
- Child:
- 8:ec3ea0623620
diff -r f55ab7e38a7f -r 88fa84742b3c main.cpp
--- a/main.cpp Thu Nov 01 15:40:37 2018 +0000
+++ b/main.cpp Thu Nov 01 16:31:57 2018 +0000
@@ -9,7 +9,7 @@
DigitalOut gpo(D0);
-DigitalIn button2(SW3);
+DigitalIn button2(SW3);
DigitalIn button1(SW2); //or SW2
DigitalOut led1(LED_GREEN);
@@ -34,464 +34,431 @@
volatile double emg1_filtered, emg2_filtered, emg3_filtered, emg4_filtered;
int i = 0;
-void emgsample(){
+void emgsample()
+{
//All EMG signal through Highpass
double emgread1 = emg1.read();
double emgread2 = emg2.read();
double emgread3 = emg3.read();
double emgread4 = emg4.read();
-
+
double emg1_highpassed = highp1.step(emgread1);
double emg2_highpassed = highp2.step(emgread2);
double emg3_highpassed = highp3.step(emgread3);
double emg4_highpassed = highp4.step(emgread4);
-
+
//All EMG highpassed through Notch
double emg1_notched = notch1.step(emg1_highpassed);
double emg2_notched = notch2.step(emg2_highpassed);
double emg3_notched = notch3.step(emg3_highpassed);
double emg4_notched = notch4.step(emg4_highpassed);
-
+
//All EMG notched rectify
double emg1_abs = abs(emg1_notched);
double emg2_abs = abs(emg2_notched);
double emg3_abs = abs(emg3_notched);
double emg4_abs = abs(emg4_notched);
-
+
//All EMG abs into lowpass
emg1_filtered = lowp1.step(emg1_abs);
emg2_filtered = lowp2.step(emg2_abs);
emg3_filtered = lowp3.step(emg3_abs);
emg4_filtered = lowp4.step(emg4_abs);
-
-
- }
-
+
+
+}
+
//Define doubles for calibration and ticker
- double ts = 0.001; //tijdsstap
- double calibration_time = 55; //time EMG calibration should take
-
- volatile double temp_highest_emg1 = 0; //highest detected value right biceps
- volatile double temp_highest_emg2 = 0;
- volatile double temp_highest_emg3 = 0;
- volatile double temp_highest_emg4 = 0;
-
- //Doubles for calculation threshold
- double biceps_p_t = 0.4; //set threshold at percentage of highest value
- double triceps_p_t = 0.5; //set threshold at percentage of highest value
- volatile double threshold1;
- volatile double threshold2;
- volatile double threshold3;
- volatile double threshold4;
-
- void CalibrationEMG()
- {
- //static float samples = calibration_time/ts;
- while(timer_calibration<55){
- if(timer_calibration>0 && timer_calibration<10)
- {
- led1=!led1;
- if(emg1_filtered>temp_highest_emg1)
- {
- temp_highest_emg1= emg1_filtered;
- pc.printf("Temp1 = %f \r\n",temp_highest_emg1);
- }
- }
- if(timer_calibration>10 && timer_calibration<15)
- {
- led1=0;
- led2=0;
- led3=0;
- }
- if(timer_calibration>15 && timer_calibration<25)
- {
- led2=!led2;
- if(emg2_filtered>temp_highest_emg2)
- {
- temp_highest_emg2= emg2_filtered;
- pc.printf("Temp2 = %f \r\n",temp_highest_emg2);
- }
- }
- if(timer_calibration>25 && timer_calibration<30)
- {
- led1=0;
- led2=0;
- led3=0;
- }
- if(timer_calibration>30 && timer_calibration<40)
- {
- led3=!led3;
- if(emg3_filtered>temp_highest_emg3)
- {
- temp_highest_emg3= emg3_filtered;
- pc.printf("Temp3 = %f \r\n",temp_highest_emg3);
- }
- }
- if(timer_calibration>40 && timer_calibration<45)
- {
- led1=0;
- led2=0;
- led3=0;
- }
- if(timer_calibration>45 && timer_calibration<55)
- {
- led2=!led2;
- led3=!led3;
- if(emg4_filtered>temp_highest_emg4)
- {
- temp_highest_emg4= emg4_filtered;
- pc.printf("Temp4 = %f \r\n",temp_highest_emg4);
- }
- }
- led1=1;
- led2=1;
- led3=1;
-
+double ts = 0.001; //tijdsstap
+double calibration_time = 55; //time EMG calibration should take
+
+volatile double temp_highest_emg1 = 0; //highest detected value right biceps
+volatile double temp_highest_emg2 = 0;
+volatile double temp_highest_emg3 = 0;
+volatile double temp_highest_emg4 = 0;
+
+//Doubles for calculation threshold
+double biceps_p_t = 0.4; //set threshold at percentage of highest value
+double triceps_p_t = 0.5; //set threshold at percentage of highest value
+volatile double threshold1;
+volatile double threshold2;
+volatile double threshold3;
+volatile double threshold4;
+
+void CalibrationEMG()
+{
+ //static float samples = calibration_time/ts;
+ while(timer_calibration<55) {
+ if(timer_calibration>0 && timer_calibration<10) {
+ led1=!led1;
+ if(emg1_filtered>temp_highest_emg1) {
+ temp_highest_emg1= emg1_filtered;
+ pc.printf("Temp1 = %f \r\n",temp_highest_emg1);
+ }
+ }
+ if(timer_calibration>10 && timer_calibration<15) {
+ led1=0;
+ led2=0;
+ led3=0;
+ }
+ if(timer_calibration>15 && timer_calibration<25) {
+ led2=!led2;
+ if(emg2_filtered>temp_highest_emg2) {
+ temp_highest_emg2= emg2_filtered;
+ pc.printf("Temp2 = %f \r\n",temp_highest_emg2);
+ }
+ }
+ if(timer_calibration>25 && timer_calibration<30) {
+ led1=0;
+ led2=0;
+ led3=0;
+ }
+ if(timer_calibration>30 && timer_calibration<40) {
+ led3=!led3;
+ if(emg3_filtered>temp_highest_emg3) {
+ temp_highest_emg3= emg3_filtered;
+ pc.printf("Temp3 = %f \r\n",temp_highest_emg3);
+ }
+ }
+ if(timer_calibration>40 && timer_calibration<45) {
+ led1=0;
+ led2=0;
+ led3=0;
+ }
+ if(timer_calibration>45 && timer_calibration<55) {
+ led2=!led2;
+ led3=!led3;
+ if(emg4_filtered>temp_highest_emg4) {
+ temp_highest_emg4= emg4_filtered;
+ pc.printf("Temp4 = %f \r\n",temp_highest_emg4);
+ }
+ }
+ led1=1;
+ led2=1;
+ led3=1;
+
}
-
+
pc.printf("Highest value right biceps= %f \r\n", temp_highest_emg1);
pc.printf("Highest value right triceps= %f \r\n", temp_highest_emg2);
pc.printf("Highest value left biceps= %f \r\n", temp_highest_emg3);
pc.printf("Highest value left triceps= %f \r\n", temp_highest_emg4);
-
-
+
+
threshold1 = temp_highest_emg1*biceps_p_t; //Right Biceps
threshold2 = temp_highest_emg2*triceps_p_t; //Right Triceps
threshold3 = temp_highest_emg3*biceps_p_t; //Left Biceps
- threshold4 = temp_highest_emg4*triceps_p_t; //Left Triceps
+ threshold4 = temp_highest_emg4*triceps_p_t; //Left Triceps
}
//Check if emg_filtered has reached their threshold
- int bicepsR;
- int tricepsR;
- int bicepsL;
- int tricepsL;
+int bicepsR;
+int tricepsR;
+int bicepsL;
+int tricepsL;
-void threshold_check(){
-
+void threshold_check()
+{
+
//EMG1 threshold check
- if(emg1_filtered>threshold1){
+ if(emg1_filtered>threshold1) {
bicepsR = 1;
- }
- else{
+ } else {
bicepsR= 0;
- }
+ }
//EMG2 threshold check
- if(emg2_filtered>threshold2){
+ if(emg2_filtered>threshold2) {
tricepsR = 1;
- }
- else{
+ } else {
tricepsR= 0;
- }
+ }
//EMG3 threshold check
- if(emg3_filtered>threshold3){
+ if(emg3_filtered>threshold3) {
bicepsL = 1;
- }
- else{
+ } else {
bicepsL= 0;
- }
+ }
//EMG4 threshold check
- if(emg4_filtered>threshold4){
+ if(emg4_filtered>threshold4) {
tricepsL = 1;
- }
- else{
+ } else {
tricepsL= 0;
- }
-
- /*
+ }
+
+ /*
pc.printf("Biceps Right = %i", bicepsR);
- pc.printf("Triceps Right = %i",tricepsR);
+ pc.printf("Triceps Right = %i",tricepsR);
pc.printf("Biceps Left = %i", bicepsL);
pc.printf("Triceps Left = %i", tricepsL);
*/
-
-
+
+
}
//Activate ticker for Movement state, filtering and Threshold checking
-void movement_ticker_activator(){
+void movement_ticker_activator()
+{
sample_ticker.attach(&emgsample, ts);
threshold_check_ticker.attach(&threshold_check, ts);
- }
-void movement_ticker_deactivator(){
+}
+void movement_ticker_deactivator()
+{
sample_ticker.detach();
threshold_check_ticker.detach();
- }
-
-enum states {MOTORS_OFF,CALIBRATION,HOMING,DEMO,MOVEMENT,CLICK};
+}
+
+enum states {MOTORS_OFF,CALIBRATION,HOMING,DEMO,MOVEMENT,CLICK};
states currentState = MOTORS_OFF; //Chosen startingposition for states
bool stateChanged = true; // Make sure the initialization of first state is executed
void ProcessStateMachine(void)
{
- switch (currentState)
- {
- case MOTORS_OFF:
- // Actions
- if (stateChanged)
- {
- // state initialization: rood
- led1 = 1;
- led2 = 0;
- led3 = 1;
- wait (1);
- stateChanged = false;
- }
-
- // State transition logic: Als button 1 word ingedrukt --> calibratie, anders motor uithouden
- if (!button1)
- {
- currentState = CALIBRATION;
- stateChanged = true;
- }
- else if (!button2)
- {
- currentState = HOMING ;
- stateChanged = true;
- }
- else
- {
- currentState = MOTORS_OFF;
- stateChanged = true;
- }
-
- break;
-
- case CALIBRATION:
- // Actions
- if (stateChanged)
- {
- // state initialization: oranje
- temp_highest_emg1 = 0; //highest detected value right biceps
- temp_highest_emg2 = 0;
- temp_highest_emg3 = 0;
- temp_highest_emg4 = 0;
-
- timer_calibration.reset();
- timer_calibration.start();
-
- sample_ticker.attach(&emgsample, ts);
- CalibrationEMG();
- sample_ticker.detach();
- timer_calibration.stop();
-
-
- stateChanged = false;
- }
-
- // State transition logic: automatisch terug naar motors off.
+ switch (currentState) {
+ case MOTORS_OFF:
+ // Actions
+ if (stateChanged) {
+ // state initialization: rood
+ led1 = 1;
+ led2 = 0;
+ led3 = 1;
+ wait (1);
+ stateChanged = false;
+ }
+
+ // State transition logic: Als button 1 word ingedrukt --> calibratie, anders motor uithouden
+ if (!button1) {
+ currentState = CALIBRATION;
+ stateChanged = true;
+ } else if (!button2) {
+ currentState = HOMING ;
+ stateChanged = true;
+ } else {
+ currentState = MOTORS_OFF;
+ stateChanged = true;
+ }
+
+ break;
+
+ case CALIBRATION:
+ // Actions
+ if (stateChanged) {
+ // state initialization: oranje
+ temp_highest_emg1 = 0; //highest detected value right biceps
+ temp_highest_emg2 = 0;
+ temp_highest_emg3 = 0;
+ temp_highest_emg4 = 0;
+
+ timer_calibration.reset();
+ timer_calibration.start();
+
+ sample_ticker.attach(&emgsample, ts);
+ CalibrationEMG();
+ sample_ticker.detach();
+ timer_calibration.stop();
- currentState = MOTORS_OFF;
- stateChanged = true;
- break;
-
- case HOMING:
- // Actions
- if (stateChanged)
- {
- // state initialization: green
- t.reset();
- t.start();
- led1 = 0;
- led2 = 1;
- led3 = 1;
- wait (1);
-
- stateChanged = false;
- }
-
- // State transition logic: naar DEMO (button1), naar MOVEMENT(button2)
- if (!button1)
- {
- currentState = DEMO;
- stateChanged = true;
- }
- else if (!button2)
- {
- currentState = MOVEMENT ;
- stateChanged = true;
- }
- else if (t>300)
- {
- t.stop();
- t.reset();
- currentState = MOTORS_OFF ;
- stateChanged = true;
- }
- else
- {
- currentState = HOMING ;
- stateChanged = true;
- }
- break;
-
+
+ stateChanged = false;
+ }
+
+ // State transition logic: automatisch terug naar motors off.
+
+ currentState = MOTORS_OFF;
+ stateChanged = true;
+ break;
+
+ case HOMING:
+ // Actions
+ if (stateChanged) {
+ // state initialization: green
+ t.reset();
+ t.start();
+ led1 = 0;
+ led2 = 1;
+ led3 = 1;
+ wait (1);
+
+ stateChanged = false;
+ }
+
+ // State transition logic: naar DEMO (button1), naar MOVEMENT(button2)
+ if (!button1) {
+ currentState = DEMO;
+ stateChanged = true;
+ } else if (!button2) {
+ currentState = MOVEMENT ;
+ stateChanged = true;
+ } else if (t>300) {
+ t.stop();
+ t.reset();
+ currentState = MOTORS_OFF ;
+ stateChanged = true;
+ } else {
+ currentState = HOMING ;
+ stateChanged = true;
+ }
+ break;
+
case DEMO:
- // Actions
- if (stateChanged)
- {
- // state initialization: light blue
- led1 = 0;
- led2 = 1;
- led3 = 0;
- wait (1);
-
- stateChanged = false;
- }
-
- // State transition logic: automatisch terug naar HOMING
- currentState = HOMING;
- stateChanged = true;
- break;
-
- case MOVEMENT:
- // Actions
- if (stateChanged)
- {
- // state initialization: purple
- t.reset();
- t.start();
-
- led1 = 1;
- led2 = 0;
- led3 = 0;
- wait(2);
-
- movement_ticker_activator();
-
- led1 = 0;
- led2 = 0;
- led3 = 0;
- wait(2);
-
-
- stateChanged = false;
- }
-
- // State transition logic: naar CLICK (button1), naar MOTORS_OFF(button2) anders naar MOVEMENT
- if (!button1)
- {
- movement_ticker_deactivator();
- currentState = CLICK;
- stateChanged = true;
- }
- else if (!button2)
- {
- movement_ticker_deactivator();
- currentState = MOTORS_OFF ;
- stateChanged = true;
- }
- else if (t>300)
- {
- movement_ticker_deactivator();
- t.stop();
- t.reset();
- currentState = HOMING ;
- stateChanged = true;
- }
- else
- {
- //For every muscle a different colour if threshold is passed
- if(bicepsR==1){
- led1 = 0;
- led2 = 1;
- led3 = 1;
- }
- else if (bicepsR==0 && tricepsR==0 && bicepsL==0 && tricepsL==0 ){
- led1 = 1;
- led2 = 1;
- led3 = 1;
- }
- if(tricepsR==1){
- led1 = 1;
- led2 = 0;
- led3 = 1;
- }
- else if (bicepsR==0 && tricepsR==0 && bicepsL==0 && tricepsL==0 ){
- led1 = 1;
- led2 = 1;
- led3 = 1;
- }
- if(bicepsL==1){
- led1 = 1;
- led2 = 1;
- led3 = 0;
- }
- else if (bicepsR==0 && tricepsR==0 && bicepsL==0 && tricepsL==0 ){
- led1 = 1;
- led2 = 1;
- led3 = 1;
- }
- if(tricepsL==1){
- led1 = 1;
- led2 = 0;
- led3 = 0;
- }
- else if (bicepsR==0 && tricepsR==0 && bicepsL==0 && tricepsL==0 ){
- led1 = 1;
- led2 = 1;
- led3 = 1;
- }
- currentState = MOVEMENT ;
- stateChanged = false;
- }
-
- break;
-
+ // Actions
+ if (stateChanged) {
+ // state initialization: light blue
+ led1 = 0;
+ led2 = 1;
+ led3 = 0;
+ wait (1);
+
+ stateChanged = false;
+ }
+
+ // State transition logic: automatisch terug naar HOMING
+ currentState = HOMING;
+ stateChanged = true;
+ break;
+
+ case MOVEMENT:
+ // Actions
+ if (stateChanged) {
+ // state initialization: purple
+ //t.reset();
+ //t.start();
+
+ led1 = 1;
+ led2 = 0;
+ led3 = 0;
+ wait(2);
+
+ movement_ticker_activator();
+
+ led1 = 0;
+ led2 = 0;
+ led3 = 0;
+ wait(2);
+
+
+ stateChanged = false;
+ }
+
+ // State transition logic: naar CLICK (button1), naar MOTORS_OFF(button2) anders naar MOVEMENT
+ if (!button1) {
+ movement_ticker_deactivator();
+ currentState = CLICK;
+ stateChanged = true;
+ } else if (!button2) {
+ movement_ticker_deactivator();
+ currentState = MOTORS_OFF ;
+ stateChanged = true;
+ } else if (bicepsR==0 && tricepsR==0 && bicepsL==0 && tricepsL==0) { //this check if person is idle for more than 300seconds
+ t.start();
+ } else if (bicepsR==1 || tricepsR==1 || bicepsL==1 || tricepsL==1) {
+ t.stop();
+ t.reset();
+ }
+
+ if(t>20) {
+ movement_ticker_deactivator();
+ t.stop();
+ t.reset();
+ currentState = HOMING ;
+ stateChanged = true;
+ }
+ // here ends the idle checking mode
+ else {
+ //For every muscle a different colour if threshold is passed
+ if(bicepsR==1) {
+ led1 = 0;
+ led2 = 1;
+ led3 = 1;
+ } else if (bicepsR==0 && tricepsR==0 && bicepsL==0 && tricepsL==0 ) {
+ led1 = 1;
+ led2 = 1;
+ led3 = 1;
+ }
+ if(tricepsR==1) {
+ led1 = 1;
+ led2 = 0;
+ led3 = 1;
+ } else if (bicepsR==0 && tricepsR==0 && bicepsL==0 && tricepsL==0 ) {
+ led1 = 1;
+ led2 = 1;
+ led3 = 1;
+ }
+ if(bicepsL==1) {
+ led1 = 1;
+ led2 = 1;
+ led3 = 0;
+ } else if (bicepsR==0 && tricepsR==0 && bicepsL==0 && tricepsL==0 ) {
+ led1 = 1;
+ led2 = 1;
+ led3 = 1;
+ }
+ if(tricepsL==1) {
+ led1 = 1;
+ led2 = 0;
+ led3 = 0;
+ } else if (bicepsR==0 && tricepsR==0 && bicepsL==0 && tricepsL==0 ) {
+ led1 = 1;
+ led2 = 1;
+ led3 = 1;
+ }
+ currentState = MOVEMENT ;
+ stateChanged = false;
+ }
+
+ break;
+
case CLICK:
- // Actions
- if (stateChanged)
- {
- // state initialization: blue
- led1 = 1;
- led2 = 1;
- led3 = 0;
- wait (1);
-
- stateChanged = false;
- }
-
- // State transition logic: automatisch terug naar MOVEMENT.
+ // Actions
+ if (stateChanged) {
+ // state initialization: blue
+ led1 = 1;
+ led2 = 1;
+ led3 = 0;
+ wait (1);
- currentState = MOVEMENT;
- stateChanged = true;
- break;
-
+ stateChanged = false;
+ }
+
+ // State transition logic: automatisch terug naar MOVEMENT.
+
+ currentState = MOVEMENT;
+ stateChanged = true;
+ break;
+
}
}
-
+
int main()
{
//BiQuad Chain add
highp1.add( &highp1_1 ).add( &highp1_2 );
notch1.add( ¬ch1_1 ).add( ¬ch1_2 );
lowp1.add( &lowp1_1 ).add(&lowp1_2);
-
+
highp2.add( &highp2_1 ).add( &highp2_2 );
notch2.add( ¬ch2_1 ).add( ¬ch2_2 );
- lowp2.add( &lowp2_1 ).add(&lowp2_2);
-
+ lowp2.add( &lowp2_1 ).add(&lowp2_2);
+
highp3.add( &highp3_1 ).add( &highp3_2 );
notch3.add( ¬ch3_1 ).add( ¬ch3_2 );
lowp3.add( &lowp3_1 ).add(&lowp3_2);
-
+
highp4.add( &highp4_1 ).add( &highp4_2 );
notch4.add( ¬ch4_1 ).add( ¬ch4_2 );
lowp4.add( &lowp4_1 ).add(&lowp4_2);
-
+
pc.baud(115200);
led1 = 1;
led2 = 1;
led3 = 1;
-
- while (true)
- {
- ProcessStateMachine();
-
+
+ while (true) {
+ ProcessStateMachine();
+
}
-
+
}