Diff: main.cpp

Revision:

31:393a7ec1d396

Parent:

30:7a66951a0122

Child:

32:2596cc9156ec

diff -r 7a66951a0122 -r 393a7ec1d396 main.cpp
--- a/main.cpp	Mon Oct 29 19:45:36 2018 +0000
+++ b/main.cpp	Mon Oct 29 20:07:54 2018 +0000
@@ -29,31 +29,42 @@
 AnalogIn    potmeter1(A2);
 AnalogIn    potmeter2(A3);
 DigitalIn   button(D0);
-DigitalOut  led_R(LED_RED) = 1;
-DigitalOut  led_B(LED_BLUE) = 1;
-DigitalOut  led_G(LED_GREEN) = 1;
+DigitalOut  led_R(LED_RED);
+DigitalOut  led_B(LED_BLUE);
+DigitalOut  led_G(LED_GREEN);
 
 // tickers and timers
 Ticker loop_ticker;
 Timer state_timer;
 Timer emg_timer;
 
-enum States {failure, waiting, calib_emg, calib_enc,  operational, demo, homing}; //All possible robot states
+enum States {failure, waiting, calib_emg, calib_enc, operational, demo, homing}; //All possible robot states
 enum Emg_measures_states {not_in_calib_emg, calib_right_bicep, calib_right_tricep, calib_left_bicep, calib_left_tricep}; // States inside
 
 //Global variables/objects
 States current_state;
-Emg_measures_states curent_emg_calibration_state = not_in_calib_enc;
+Emg_measures_states current_emg_calibration_state = not_in_calib_emg;
 
-double des_vx, des_vy, x, y, q1, q2, qref1, qref2, e1, e2, u1, u2, raw_emg_0, process_emg_0, raw_emg_1, process_emg_1; //will be set by the motor_controller function
+double des_vx, des_vy, x, y, q1, q2, qref1, qref2, e1, e2, u1, u2; //will be set by the motor_controller function
 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;
 
+// Variables for emg
+double raw_emg_0, process_emg_0;
+double raw_emg_1, process_emg_1;
+double raw_emg_2, process_emg_2;
+double raw_emg_3, process_emg_3;
+
+
 // Variables defined for the homing state
 double q1_homing = 0, q2_homing = 0;
 double q1_dot_ref_homing, q2_dot_ref_homing;
 double beta = 5;
-double k = 2;
+double k_hom = 2;
+
+// For the state calib_enc
+double q1old;
+double q2old;
 
 // Meaning of process_emg_0 and such
 // - process_emg_0 is right biceps
@@ -97,7 +108,7 @@
             if (button.read()==true) 
             {
                 current_state = calib_enc; //the NEXT loop we will be in calib_enc state
-                state_changed == true;
+                state_changed = true;
             }
             
             break; //to avoid falling through to the next state, although this can sometimes be very useful.
@@ -119,10 +130,10 @@
                 q2old = 0;
             }
             
-            if q1-q1old == 0.0 && q2 - q2old < 0.0 && state_timer.read() > 5.0f
+            if (q1-q1old == 0.0 && q2 - q2old < 0.0 && state_timer.read() > 5.0f)
             {
                 current_state = calib_emg; //the NEXT loop we will be in calib_emg state
-                curent_emg_calibration_state = calib_right_bicep;
+                current_emg_calibration_state = calib_right_bicep;
                 state_changed = true;
                 
             }    
@@ -146,8 +157,7 @@
                 state_changed = false;
                 }
             
-            // calibrating right bicep
-            switch(curent_emg_calibration_state){
+            switch(current_emg_calibration_state){
                 case calib_right_bicep:
                     if(emg_timer < 5.0f){
                         if (process_emg_0 > right_bicep_max){
@@ -172,7 +182,7 @@
                             emg_timer.start();
                         }
                     break;
-                case calib_left_bicep:
+                                case calib_left_bicep:
                     if(emg_timer < 5.0f){
                         if (process_emg_2 > left_bicep_max){
                             left_bicep_max = process_emg_2;
@@ -200,6 +210,9 @@
                 default:
                     current_state = failure;
                     state_changed = true;
+            }
+            
+            break;
                     
         case homing: 
             if (state_changed == true){
@@ -208,21 +221,22 @@
                     qref1 = q1; //NOT SURE IF WE NEED THIS. I do not think so, but just to be sure.
                     qref2 = q2;
                 }
-                q1_dot_ref_homing = min(beta, k*(q1 - q1_homing));
-                q2_dot_ref_homing = min(beta, k*(q2 - q2_homing));
-                qref1 = q1_dot_ref_homing*T + q1;
-                qref2 = q2_dot_ref_homing*T + q2;
+            q1_dot_ref_homing = min(beta, k_hom*(q1 - q1_homing));
+            q2_dot_ref_homing = min(beta, k_hom*(q2 - q2_homing));
+            qref1 = q1_dot_ref_homing*T + q1;
+            qref2 = q2_dot_ref_homing*T + q2;
                 
-                // The value of 3.0 and 2*resolution can be changed
-                if (fabs(q1-q1_homing) <= 2*resolution && fabs(q2-q2_homing) <= 2 * resolution ){
-                        if (state_timer > 3.0f){
-                            current_state = operational;
-                            state_changed = true;
-                        }
+            // The value of 3.0 and 2*resolution can be changed
+            if (fabs(q1-q1_homing) <= 2*resolution && fabs(q2-q2_homing) <= 2 * resolution ){
+                    if (state_timer > 3.0f){
+                        current_state = operational;
+                        state_changed = true;
                     }
-                else{
-                    state_timer.reset();    
                 }
+            else{
+                state_timer.reset();    
+            }
+            break;
         
         case operational:       //interpreting emg-signals to move the end effector
             if (state_changed == true){