Diff: main.cpp

Revision:

69:bfefdfb04c29

Parent:

68:2879967ebb25

Parent:

67:edc3c644d316

Child:

70:c4a019e3830d

--- a/main.cpp	Tue Nov 05 10:01:56 2019 +0000
+++ b/main.cpp	Tue Nov 05 10:24:09 2019 +0000
@@ -31,10 +31,10 @@
 DigitalOut      led_b(LED_BLUE);
 
 // Analog EMG inputs
-AnalogIn emg1_in (A0); // Right biceps -> x axis
-AnalogIn emg2_in (A1); // Left biceps  -> y axis
-AnalogIn emg3_in (A2); // Third muscle -> TBD
-AnalogIn emg4_in (A3); // Third muscle -> TBD
+AnalogIn emg1_in (A0); // Right biceps  -> x axis
+AnalogIn emg2_in (A1); // Left biceps   -> y axis
+AnalogIn emg3_in (A2); // Right tibia   -> x direction
+AnalogIn emg4_in (A3); // Left tibia    -> y direction
 
 // Analog Potmeter inputs
 AnalogIn potmeter1 (A4);
@@ -891,8 +891,8 @@
     // Do stuff until end condition is met
     EMGOperationFunc();
 
-    Vx = emg1_out * (10.0 + 10.0 * potmeter1.read() ) * emg1_dir;
-    Vy = emg2_out * (10.0 + 10.0 * potmeter2.read() ) * emg2_dir;
+    Vx = emg1_out * (10.0f + 10.0f * potmeter1.read() ) * emg1_dir;
+    Vy = emg2_out * (10.0f + 10.0f * potmeter2.read() ) * emg2_dir;
 
     PID_controller();
     motorControl();
@@ -907,11 +907,9 @@
         operation_state_changed = true;
     } else if ( button2_pressed == true ) {
         button2_pressed = false;
-        //operation_curr_state = operation_finish; // To move to finished operation mode (not used yet)
-        operation_curr_state = operation_wait; // TEMPORARY
+        operation_curr_state = operation_wait;
         operation_state_changed = true;
-        global_curr_state = global_wait; // TEMPORARY move directly to global wait state
-        global_state_changed = true; // TEMPORARY move directly to global wait state
+        exit_operation = true;
     }
 }
 
@@ -927,8 +925,8 @@
     // Do stuff until end condition is met
     EMGOperationFunc();
 
-    Vx = emg1_out * (10.0 + 10.0 * potmeter1.read() ) * emg1_dir;
-    Vy = emg2_out * (10.0 + 10.0 * potmeter2.read() ) * emg2_dir;
+    Vx = emg1_out * (10.0f + 10.0f * potmeter1.read() ) * emg1_dir;
+    Vy = emg2_out * (10.0f + 10.0f * potmeter2.read() ) * emg2_dir;
 
     PID_controller();
     motorControl();
@@ -974,6 +972,11 @@
     }
 
     // Do nothing until end condition is met
+    PID_controller();
+    motorControl();
+    RKI();
+    
+    motorKillPower();
 
     // State transition guard
     if ( button1_pressed == true ) { // demo_XY
@@ -997,6 +1000,10 @@
         demo_state_changed = false;
         set_led_color('c'); // Set LED to motor calibration (CYAN)
     }
+    
+    PID_controller();
+    motorControl();
+    RKI();
 
     // Do stuff until end condition is met
     motor_state_machine();
@@ -1018,6 +1025,7 @@
         timerStateChange.start();
     }
 
+
     Vx = 5.0; // Move in the positive x direction
     Vy = 5.0; // Move in the positive y direction
 
@@ -1030,7 +1038,7 @@
     scope.send();
 
     // State transition guard
-    if ( timerStateChange.read() >= 3.0f ) { // demo_wait
+    if ( timerStateChange.read() >= 5.0f ) { // demo_wait
         button1_pressed = false;
         demo_curr_state = demo_XY;
         demo_state_changed = true;
@@ -1049,7 +1057,6 @@
 
     if ( t >= 0.0f && t < 3.0f ) {          // With this code the endpoint will make a square every 12 seconds
         Vx = 5.0;
-        Vy = 0.0;
     } else if ( t >= 3.0f && t < 6.0f ) {
         Vx = 0.0;
         Vy = 5.0;
@@ -1195,6 +1202,8 @@
     }
 
     // Do nothing until end condition is met
+    motorKillPower();
+
 
     // State transition guard
     if ( switch2_pressed == true ) { // DEMO MODE
@@ -1268,6 +1277,9 @@
     // Entry function
     if ( global_state_changed == true ) {
         global_state_changed = false;
+        
+        button1.fall( &button1Press );
+        button2.fall( &button2Press );
 
         emg_sampleNow = true; // Enable signal sampling in sampleSignals()
         emg_calibrateNow = false; // Disable calibration functionality in sampleSignals()
@@ -1290,6 +1302,10 @@
         global_curr_state = global_wait;
         global_state_changed = true;
         set_led_color('o'); // Disable operation led
+        emg_sampleNow = false; // Enable signal sampling in sampleSignals()
+        emg_calibrateNow = false; // Disable calibration functionality in sampleSignals()
+        emg_cal_done = false;
+        motor_cal_done = false;
     }
 }
 /*