Diff: main.cpp

Revision:

40:e217056584be

Parent:

39:5db36ce5e620

Child:

41:e9d6fdf02074

--- a/main.cpp	Wed Oct 31 11:06:29 2018 +0000
+++ b/main.cpp	Wed Oct 31 14:24:58 2018 +0000
@@ -29,6 +29,7 @@
 AnalogIn    potmeter1(A2);
 AnalogIn    potmeter2(A3);
 DigitalIn   button(D0);
+DigitalIn   emgstop(SW2);
 DigitalOut  led_R(LED_RED);
 DigitalOut  led_B(LED_BLUE);
 DigitalOut  led_G(LED_GREEN);
@@ -54,6 +55,7 @@
 double u1 = 0, u2= 0;
 double vxmax = 1.0, vymax = 1.0;
 double right_bicep_max = 0.0, right_tricep_max = 0.0, left_bicep_max= 0.0, left_tricep_max = 0.0;
+double scaling_right_bicep = 1.0, scaling_right_tricep = 1.0, scaling_left_bicep = 1.0, scaling_left_tricep = 1.0;
 
 // Variables for emg
 double raw_emg_0, process_emg_0;
@@ -176,53 +178,69 @@
             
             switch(current_emg_calibration_state){
                 case calib_right_bicep:
+                    led_R = 0;
+                    led_G = 1;
+                    led_B = 1;
                     if(emg_timer < 5.0f){
                         if (process_emg_0 > right_bicep_max){
                             right_bicep_max = process_emg_0;
                             }
                         }
-                    else if (process_emg_0 < 0.1*right_bicep_max){
+                    else if ((process_emg_0 < 0.1*right_bicep_max) || (emgstop.read() == false)){
+                            scaling_right_bicep = 1.0/ right_bicep_max;
                             current_emg_calibration_state = calib_right_tricep;
                             emg_timer.reset();
                             emg_timer.start();
                         }
                     break;
                 case calib_right_tricep:
+                    led_R = 1;
+                    led_G = 1;
+                    led_B = 0;
                     if(emg_timer < 5.0f){
                             if (process_emg_1 > right_tricep_max){
                                 right_tricep_max = process_emg_1;
                                 }
                             }
-                    else if (process_emg_1 < 0.1*right_tricep_max){
+                    else if ((process_emg_1 < 0.1*right_tricep_max) || (emgstop.read() == false)){
+                            scaling_right_tricep = 1.0/ right_tricep_max;
                             current_emg_calibration_state = calib_left_bicep;
                             emg_timer.reset();
                             emg_timer.start();
                         }
                     break;
-                                case calib_left_bicep:
+                case calib_left_bicep:
+                    led_R = 0;
+                    led_G = 1;
+                    led_B = 1;
                     if(emg_timer < 5.0f){
                         if (process_emg_2 > left_bicep_max){
                             left_bicep_max = process_emg_2;
                             }
                         }
-                    else if (process_emg_2 < 0.1*left_bicep_max){
+                    else if ((process_emg_2 < 0.1*left_bicep_max) || (emgstop.read() == false)){
+                            scaling_left_bicep = 1.0/ left_bicep_max;
                             current_emg_calibration_state = calib_left_tricep;
                             emg_timer.reset();
                             emg_timer.start();
                         }
                     break;
                 case calib_left_tricep:
-                        if(emg_timer < 5.0f){
-                            if (process_emg_3 > left_tricep_max){
-                                left_tricep_max = process_emg_3;
-                                }
-                            }
-                        else if (process_emg_3 < 0.1*left_tricep_max){
-                            current_emg_calibration_state = not_in_calib_emg;
-                            current_state = homing;
-                            state_changed = true;
-                            emg_timer.reset();
-                        }
+                    led_R = 1;
+                    led_G = 1;
+                    led_B = 0;
+                    if(emg_timer < 5.0f){
+                       if (process_emg_3 > left_tricep_max){
+                           left_tricep_max = process_emg_3;
+                       }
+                    }
+                    else if ((process_emg_3 < 0.1*left_tricep_max) || (emgstop.read() == false)){
+                      scaling_left_tricep = 1.0/ left_tricep_max;
+                      current_emg_calibration_state = not_in_calib_emg;
+                      current_state = homing;
+                      state_changed = true;
+                      emg_timer.reset();
+                    }
                     break;
                 default:
                     current_state = failure;
@@ -259,18 +277,34 @@
             if (state_changed == true){
                     state_changed = false;
             }
-                       
+            
+            // normalization 
+            double x_norm = process_emg_0 * scaling_right_bicep - process_emg_1 * scaling_right_tricep;
+            double y_norm = process_emg_2 * scaling_left_bicep - process_emg_3 * scaling_left_tricep;
+                            
             // here we have to determine the desired velocity based on the processed emg signals and calibration
-            if (process_emg_0 >= 0.16) { des_vx = vxmax; }
-            else if(process_emg_0 >= 0.09) { des_vx = vxmax * 0.66; }
-            else if(process_emg_0 >= 0.02) { des_vx = vxmax * 0.33; }
+            // x velocity
+            if (x_norm >= 0.8) { des_vx = 0.4; }
+            else if(x_norm >= 0.6) { des_vx = 0.3; }
+            else if(x_norm >= 0.4) { des_vx = 0.2; }
+            else if(x_norm >= 0.2) { des_vx = 0.1; } 
+            else if(x_norm <= -0.8) { des_vx = -0.4; }
+            else if(x_norm <= -0.6) { des_vx = -0.3; }
+            else if(x_norm <= -0.4) { des_vx = -0.2; }
+            else if(x_norm <= -0.2) { des_vx = -0.1; }
             else { des_vx = 0; }
             
-            if (process_emg_1 >= 0.16) { des_vy = vymax; }
-            else if(process_emg_1 >= 0.09) { des_vy = vymax * 0.66; }
-            else if(process_emg_1 >= 0.02) { des_vy = vymax * 0.33; }
+            // y velocity
+            if (y_norm >= 0.8) { des_vy = 0.4; }
+            else if(y_norm >= 0.6) { des_vy = 0.3; }
+            else if(y_norm >= 0.4) { des_vy = 0.2; }
+            else if(y_norm >= 0.2) { des_vy = 0.1; } 
+            else if(y_norm <= -0.8) { des_vy = -0.4; }
+            else if(y_norm <= -0.6) { des_vy = -0.3; }
+            else if(y_norm <= -0.4) { des_vy = -0.2; }
+            else if(y_norm <= -0.2) { des_vy = -0.1; }
             else { des_vy = 0; }
-            
+                        
             if (button.read() == true && button_suppressed == false ) {
                 current_state = demo; 
                 state_changed = true;