Diff: main.cpp

Revision:

42:bcd496523c66

Parent:

38:0f824c45f717

Parent:

41:e9d6fdf02074

Child:

43:8e2ea92fee01

--- a/main.cpp	Wed Oct 31 15:33:20 2018 +0000
+++ b/main.cpp	Wed Oct 31 16:18:40 2018 +0000
@@ -25,6 +25,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);
@@ -52,6 +53,7 @@
 double u1, u2;
 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;
@@ -97,7 +99,7 @@
     forwardkinematics_function(q1,q2,x,y);  
     raw_emg_0 = emg0.read(); //sample analog voltages (all sampling theory applies, you might get aliasing etc.)
     raw_emg_1 = emg1.read();
-    processing_chain_emg(raw_emg_0, raw_emg_1, process_emg_0, process_emg_1);  // processes the emg signals
+    processing_chain_emg(raw_emg_0, raw_emg_1, raw_emg_2, raw_emg_3, process_emg_0, process_emg_1, process_emg_2, process_emg_3);  // processes the emg signals
 }
 
 void output_all() {
@@ -182,56 +184,79 @@
                 emg_timer.start();
                 state_changed = false;
                 }
-            
+            // first the led will be purple, during this time the right biceps should be maximally contracted
+            // after five seconds and when the right biceps are relaxed, it switches to a blue led
+            // when the blue led is active the right triceps should be maximally contracted
+            // then again it switches to purple for left biceps and after that blue for left triceps
+            // after this, the emg signals are calibrated
             switch(current_emg_calibration_state){
                 case calib_right_bicep:
+                    led_R = 0;
+                    led_G = 1;
+                    led_B = 0;
                     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 = 0;
                     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 = operational;
+                      state_changed = true;
+                      emg_timer.reset();
+                      led_R = 1;
+                      led_G = 1;
+                      led_B = 1;
+                    }
                     break;
                 default:
                     current_state = failure;
@@ -267,18 +292,33 @@
         case operational:       //interpreting emg-signals to move the end effector
             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 ) {
@@ -288,7 +328,7 @@
                 
                 make_button_active.attach(&unsuppressing_button,0.5);
 
-                } 
+            } 
             
             break;
             
@@ -296,7 +336,29 @@
             // We want to draw a square. Hence, first move to a certain point and then start moving a square.
             if (state_changed == true){
                     state_changed = false;
-                }
+                    state_timer.reset();
+                    state_timer.start();
+            //        des_vx = 0.1;           // first we move to the right
+            //        des_vy = 0.0;
+            }
+            
+            //static double new_vx, new_vy;
+                        
+            //if(state_timer > 3.0f){         // after waiting an extra second we start on another side of the square
+            //    state_timer.reset();
+            //    state_timer.start();
+            //    des_vx = new_vx;
+            //    des_vy = new_vy;
+            //}
+            //else if(state_timer > 2.0f){                          // we have a velocity of 0.1 m/s for 2 seconds, so we make a square of 20 by 20 cm
+            //    if(des_vx > 0){new_vx = 0.0; new_vy = 0.1;}       // here we check which side of the square we were on
+            //    else if(des_vy > 0){new_vx = -0.1; new_vy = 0.0;}
+            //    else if(des_vx < 0){new_vx = 0.0; new_vy = -0.1;}
+            //    else if(des_vy < 0){new_vx = 0.1; new_vy = 0.0;}
+            //    des_vx = 0;
+            //    des_vy = 0;
+            //}
+       
                 
             if (button.read() == true && button_suppressed == false ) {
                 current_state = operational;
@@ -345,8 +407,10 @@
     current_state = waiting; //we start in state ‘waiting’ and current_state can be accessed by all functions
     u1 = 0.0f; //initial output to motors is 0.
     u2 = 0.0f;
-    bqc0.add(&bq0high).add(&bq0notch);        // filter cascade for emg
-    bqc1.add(&bq1high).add(&bq1notch);        // filter cascade for emg
+    bqc0.add(&bq0high).add(&bq0notch).add(&bq0notch2).add(&bq0notch3).add(&bq0notch4);        // filter cascade for emg 0
+    bqc1.add(&bq1high).add(&bq1notch).add(&bq1notch2).add(&bq1notch3).add(&bq1notch4);        // filter cascade for emg 1
+    bqc2.add(&bq2high).add(&bq2notch).add(&bq2notch2).add(&bq2notch3).add(&bq2notch4);        // filter cascade for emg 2
+    bqc3.add(&bq3high).add(&bq3notch).add(&bq3notch2).add(&bq3notch3).add(&bq3notch4);        // filter cascade for emg 3
     loop_ticker.attach(&loop_function, T); //Run the function loop_function 1000 times per second
     led_R = 1;
     led_B = 1;