Diff: main.cpp

Revision:

41:8e8141f355af

Parent:

40:c6dffb676350

Child:

42:2937ad8f1032

--- a/main.cpp	Wed Oct 30 19:37:41 2019 +0000
+++ b/main.cpp	Wed Oct 30 20:44:56 2019 +0000
@@ -447,7 +447,7 @@
 */
 
 // Set global constant values for EMG reading & analysis
-const double Tcal = 10.0f; // Calibration duration (s)
+const double Tcal = 7.5f; // Calibration duration (s)
 
 // Initialize variables for EMG reading & analysis
 double emg1;
@@ -460,7 +460,7 @@
 double emg1_norm; //delete
 vector<double> emg1_cal;
 int emg1_cal_size; //delete
-int emg1_dir = 1;
+double emg1_dir = 1.0;
 double emg1_out_prev;
 double emg1_dt; //delete
 double emg1_dt_prev;
@@ -476,7 +476,7 @@
 double emg2_norm;//delete
 vector<double> emg2_cal;
 int emg2_cal_size;//delete
-int emg2_dir = 1;
+double emg2_dir = 1.0;
 
 double emg3;
 double emg3_env;
@@ -599,6 +599,8 @@
     // Set HIDScope outputs
     scope.set(0, emg1 );
     scope.set(1, emg1_env );
+    scope.set(2, emg2 );
+    scope.set(3, emg2_env );
     //scope.set(2, emg2_env );
     //scope.set(3, emg3_env );
     scope.send();
@@ -655,15 +657,19 @@
         //button1.fall( &toggleEMG1Dir ); // Change to state MVC calibration on button1 press
         //button2.fall( &toggleEMG2Dir ); // Change to state rest calibration on button2 press
 
-        emg_cal_done = true; // Let the global substate machine know that EMG calibration is finished
     }
 
     // This state only runs its entry functions ONCE and then exits the EMG substate machine
 
     // State transition guard
-    if ( false ) { // EMG substate machine is terminated after running this state once, so there is no transition to next EMG substate
+    if ( true ) { // EMG substate machine is terminated directly after running this state once
         emg_curr_state = emg_wait; // Set next state
         emg_state_changed = true; // Enable entry function
+        emg_cal_done = true; // Let the global substate machine know that EMG calibration is finished
+        
+        // Enable buttons again
+        button1.fall( &button1Press );
+        button2.fall( &button2Press );
     }
 }
 
@@ -836,13 +842,7 @@
     motor_state_machine();
 
     // State transition guard
-    if ( emg_cal_done == true ) { // OPERATION MODE
-        motor_cal_done = true;
-        global_curr_state = global_operation;
-        global_state_changed = true;
-    } else if ( button2_pressed == true ) { // WAIT MODE
-        button2_pressed = false;
-        motor_cal_done = true;
+    if ( motor_cal_done == true ) { // WAIT MODE
         global_curr_state = global_wait;
         global_state_changed = true;
     }
@@ -866,6 +866,16 @@
 
     emg1_out_prev = emg1_out; // Set previous emg_out signal
     emg1_dt_prev = emg1_dt; // Set previous emg_out_dt signal
+    
+    if (button1_pressed) {
+        button1_pressed = false;
+        emg1_dir = emg1_dir * -1.0;
+    }
+    
+    if (button2_pressed) {
+        button2_pressed = false;
+        emg2_dir = emg2_dir * -1.0;
+    }
 
     // Set normalized EMG output signal (CAN BE MOVED TO EXTERNAL FUNCTION BECAUSE IT IS REPEATED 3 TIMES)
     if ( emg1_norm < emg1_th ) { // If below threshold, emg_out = 0 (ignored)
@@ -879,8 +889,7 @@
     }
     emg1_dt = (emg1_out - emg1_out_prev) / Ts; // Calculate derivative of filtered normalized output signal
     emg1_dtdt = (emg1_dt - emg1_dt_prev) / Ts; // Calculate acceleration of filtered normalized output signal
-    emg1_out = emg1_out * emg1_dir; // Set direction of EMG output
-    Vx = emg1_out;
+    Vx = emg1_out * 15.0f * emg1_dir;
 
     // Idem for emg2
     if ( emg2_norm < emg2_th ) {
@@ -890,8 +899,7 @@
     } else {
         emg2_out = rescale(emg2_norm, 0, 1, emg2_th, 1);
     }
-    emg2_out = emg2_out * emg2_dir; // Set direction of EMG output
-    Vy = emg2_out;
+    Vy = emg2_out * 15.0f * emg2_dir;
 
     // Idem for emg3
     if ( emg3_norm < emg3_th ) {
@@ -917,9 +925,9 @@
 
     // Set HIDScope outputs
     scope.set(0, emg1 );
-    scope.set(1, emg1_out );
-    scope.set(2, emg1_dt );
-    scope.set(3, emg1_dtdt );
+    scope.set(1, Vx );
+    scope.set(2, emg2 );
+    scope.set(3, Vy );
     //scope.set(2, emg2_out );
     //scope.set(3, emg3_out );
     scope.send();
@@ -1056,7 +1064,8 @@
 
     while(true) {
         pc.printf("Global state: %i EMG substate: %i Motor substate: %i\r\n", global_curr_state, emg_curr_state, motor_curr_state);
-
+        pc.printf("EMG1 direction: %f EMG2 direction: %f \r\n", emg1_dir, emg2_dir);
+        pc.printf("Vx: %f Vy: %f \r\n", Vx, Vy);
         pc.printf("q1: %f q2: %f \r\n",q1*rad2deg,q2*rad2deg);
         pc.printf("Xe: %f Ye: %f\r\n",xe,ye);
         wait(0.5f);