Diff: main.cpp

Revision:

29:d1e8eb135e6c

Parent:

28:d952367fc0fc

Child:

30:7a66951a0122

diff -r d952367fc0fc -r d1e8eb135e6c main.cpp
--- a/main.cpp	Mon Oct 29 15:12:24 2018 +0000
+++ b/main.cpp	Mon Oct 29 15:20:50 2018 +0000
@@ -8,7 +8,7 @@
 #include "processing_chain_emg.h"
 
 //Define objects
-MODSERIAL pc(USBTX, USBRX);
+MODSERIAL   pc(USBTX, USBRX);
 HIDScope    scope(2);
 
 // emg inputs
@@ -29,14 +29,21 @@
 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;
 
 // tickers and timers
 Ticker loop_ticker;
 Timer state_timer;
 Timer emg_timer;
 
+<<<<<<< local
+enum States {failure, waiting, calib_emg, homing, calib_enc,  operational, demo}; //All possible robot states
+=======
 enum States {failure, waiting, calib_enc, calib_emg, operational, demo}; //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
+>>>>>>> other
 
 //Global variables/objects
 States current_state;
@@ -69,6 +76,9 @@
     processing_chain_emg(raw_emg_0, raw_emg_1, process_emg_0, process_emg_1);  // processes the emg signals
 }
 
+void homing() {
+    PID_controller(0.5*3.1415926535f-q1,3.1415926535f-q2,u1,u2,T)
+    
 void output_all() {
     motor1_pwm = fabs(u1);
     motor1_dir = u1 > 0.0f;
@@ -85,24 +95,44 @@
             if (button.read()==true) 
             {
                 current_state = calib_enc; //the NEXT loop we will be in calib_enc state
-                // no state_changed action?
+                state_changed == true;
             }
+            
             break; //to avoid falling through to the next state, although this can sometimes be very useful.
         
         case calib_enc:
+        
             if (state_changed==true) 
             {
                 state_timer.reset();
                 state_timer.start();
                 state_changed = false;
+                n = 0;
+                led_G = 0;
+                led_B = 1;
+                led_R = 1;
+                u1 = 0.55f; //a low PWM value to move the motors slowly (0.0 to 0.45 don’t do much due to friction)
+                u2 = 0.55f;
+                q1old = 0; 
+                q2old = 0;
             }
-            u1 = 0.55f; //a low PWM value to move the motors slowly (0.0 to 0.45 don’t do much due to friction)
-            // fabs(motor1.velocity()) < 0.1f && 
-            if (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;
                 state_changed = true;
+                
+            }    
+            q1old = q1;
+            q2old = q2;
+            
+            n++;
+            if (n%1000 == 0)
+            {
+                led_G = !led_G;
             }
+            
             break;
             
         case calib_emg:     //calibrate emg-signals
@@ -186,6 +216,7 @@
             
             if (button.read() == true) { current_state = demo; }
             
+            
             break;
             
         case demo: //moving according to a specified trajectory
@@ -197,6 +228,9 @@
         case failure: //no way to get out
             u1 = 0.0f;
             u2 = 0.0f;
+            led_R = 0;
+            led_G = 1;
+            led_B = 1;
             break;
     }
 }