Diff: main.cpp

Revision:

51:7fe2659a1fcb

Parent:

50:86bad994f08f

Child:

52:fd35025574ca

--- a/main.cpp	Thu Oct 31 19:05:31 2019 +0000
+++ b/main.cpp	Thu Oct 31 20:43:28 2019 +0000
@@ -57,6 +57,7 @@
 ------------------------------ INITIALIZE TICKERS, TIMERS & TIMEOUTS ------------------------------
 */
 Ticker tickGlobal; // Set global ticker
+Ticker tickLED; // LED ticker
 Timer timerCalibration; // Set EMG Calibration timer
 
 /*
@@ -81,8 +82,8 @@
 bool emg_rest_cal_done = false; // Internal rest calibration flag
 
 // Motor Substate variables
-enum Motor_States { motor_wait, motor_encoder_set, motor_finish, motor_movement }; // Define motor substates
-Motor_States motor_curr_state = motor_wait; // Initialize motor substate variable
+enum Motor_States { motor_encoder_set, motor_finish }; // Define motor substates
+Motor_States motor_curr_state = motor_encoder_set; // Initialize motor substate variable
 bool motor_state_changed = true; // Enable entry functions
 bool motor_encoder_cal_done = false; // Internal encoder calibration flag
 
@@ -91,6 +92,7 @@
 Operation_States operation_curr_state = operation_wait; // Initialize operation substate variable
 bool operation_state_changed = true; // Enable entry functions
 bool operation_showcard = false; // Internal flag to toggle servo position
+bool exit_operation = false;
 
 // Demo Substate variables
 enum Demo_States { demo_wait, demo_motor_cal, demo_XY }; // Define demo substates
@@ -102,11 +104,93 @@
 bool button2_pressed = false;
 bool switch2_pressed = false;
 
+// LED states
+enum LED_Colors { off, red, green, blue, purple, yellow, cyan, white }; // Define possible LED colors
+LED_Colors curr_led_color; // Initialize LED color variable
+bool led_color_changed = true; // Enable LED entry functions
+
 // Global constants
 const double Fs = 500.0;
 const double Ts = 1/Fs;
 
 /*
+------------------------------ LED COLOR FUNCTIONS ------------------------------
+*/
+// Only called once when color is changed
+void set_led_color(char color)
+{
+    switch(color) {
+        case 'o':
+            curr_led_color = off;
+            break;
+        case 'r':
+            curr_led_color = red;
+            break;
+        case 'b':
+            curr_led_color = blue;
+            break;
+        case 'g':
+            curr_led_color = green;
+            break;
+        case 'y':
+            curr_led_color = yellow;
+            break;
+        case 'p':
+            curr_led_color = purple;
+            break;
+        case 'c':
+            curr_led_color = cyan;
+            break;
+        case 'w':
+            curr_led_color = white;
+            break;
+        
+    }
+    led_color_changed = true;
+}
+
+// Run constantly
+void disp_led_color()
+{
+    if (led_color_changed == true) {
+        led_color_changed = false;
+        led_g = 1;
+        led_b = 1;
+        led_r = 1;
+    }
+    switch(curr_led_color) {
+        case off:
+            break;
+        case red:
+            led_r = !led_r;
+            break;
+        case blue:
+            led_b = !led_b;
+            break;
+        case green:
+            led_g = !led_g;
+            break;
+        case yellow:
+            led_r = !led_r;
+            led_g = !led_g;
+            break;
+        case purple:
+            led_r = !led_r;
+            led_b = !led_b;
+            break;
+        case cyan:
+            led_b = !led_b;
+            led_g = !led_g;
+            break;
+        case white:
+            led_r = !led_r;
+            led_g = !led_g;
+            led_b = !led_b;
+            break;
+    }
+}
+
+/*
 ------------------------------ HELPER FUNCTIONS ------------------------------
 */
 // Empty placeholder function, needs to be deleted at end of project
@@ -328,7 +412,7 @@
     } else {
         emg1_dir = -1.0;
     }
-    
+
     if ( emg4_norm < emg4_th ) {
         emg2_dir = 1.0;
     } else {
@@ -381,7 +465,7 @@
     // Entry functions
     if ( emg_state_changed == true ) {
         emg_state_changed = false; // Disable entry functions
-        led_b = 0; // Turn on calibration led
+        set_led_color('b'); // Turn on calibration led (BLUE)
 
         timerCalibration.reset();
         timerCalibration.start(); // Sets up timer to stop calibration after Tcal seconds
@@ -408,7 +492,7 @@
     if ( timerCalibration.read() >= Tcal ) { // After interval Tcal the calibration step is finished
         emg_sampleNow = false; // Disable signal sampling in sampleSignals()
         emg_calibrateNow = false; // Disable calibration sampling
-        led_b = 1; // Turn off calibration led
+        set_led_color('o'); // Turn off calibration led (BLUE)
 
         // Extract EMG scale data from calibration
         switch( emg_curr_state ) {
@@ -430,7 +514,7 @@
         vector<double>().swap(emg1_cal); // Empty vector to prevent memory overflow
         vector<double>().swap(emg2_cal);
         vector<double>().swap(emg3_cal);
-vector<double>().swap(emg4_cal);
+        vector<double>().swap(emg4_cal);
 
         emg_curr_state = emg_wait; // Set next substate
         emg_state_changed = true; // Enable substate entry function
@@ -695,14 +779,13 @@
 }
 
 void changeservo()
-{    
+{
     butt1= extButton1.read();
-    if (butt1==1){
-    myservo.SetPosition(2000);
+    if (butt1==1) {
+        myservo.SetPosition(2000);
+    } else {
+        myservo.SetPosition(1000);
     }
-    else {
-    myservo.SetPosition(1000);
-        }    
 }
 
 void motorKillPower()
@@ -716,25 +799,6 @@
 /*
 ------------------------------ MOTOR SUBSTATE FUNCTIONS ------------------------------
 */
-
-void do_motor_wait()
-{
-    // Entry function
-    if ( motor_state_changed == true ) {
-        motor_state_changed = false;
-    }
-
-    PID_controller();
-    motorControl();
-
-    // State transition guard
-    if ( button2_pressed ) {
-        button2_pressed = false;
-        motor_curr_state = motor_encoder_set;           //Beginnen met calibratie
-        motor_state_changed = true;
-    }
-}
-
 void do_motor_encoder_set()
 {
     // Entry function
@@ -772,7 +836,7 @@
     if ( button2_pressed ) {
         button2_pressed = false;
         motor_cal_done = true;
-        motor_curr_state = motor_wait;
+        motor_curr_state = motor_encoder_set;
         motor_state_changed = true;
     }
 }
@@ -873,43 +937,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_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
-    }
-}
-
-void do_operation_finish() // NOT USED YET
-{
-    // Entry function
-    if ( operation_state_changed == true ) {
-        operation_state_changed = false;
-        emg_sampleNow = false; // Enable signal sampling in sampleSignals()
-        emg_calibrateNow = false; // Disable calibration functionality in sampleSignals()
-    }
-
-    // Do stuff until end condition is met
-    EMGOperationFunc();
-
-    Vx = emg1_out * 15.0f * emg1_dir;
-    Vy = emg2_out * 15.0f * emg2_dir;
-
-    PID_controller();
-    motorControl();
-    RKI();
-
-    // Function to move to starting position
-
-    // State transition guard
-    if ( button2_pressed == true ) {
-        button2_pressed = false;
         operation_curr_state = operation_wait;
         operation_state_changed = true;
-
-        global_curr_state = global_wait;
-        global_state_changed = true;
+        exit_operation = true;
     }
 }
 
@@ -942,9 +972,6 @@
 void motor_state_machine()
 {
     switch(motor_curr_state) {
-        case motor_wait:
-            do_motor_wait();
-            break;
         case motor_encoder_set:
             do_motor_encoder_set();
             break;
@@ -967,9 +994,6 @@
         case operation_movement:
             do_operation_movement();
             break;
-        case operation_finish:
-            do_operation_finish();
-            break;
     }
 }
 
@@ -1111,7 +1135,7 @@
     // Entry function
     if ( global_state_changed == true ) {
         global_state_changed = false;
-        
+
         if ( motor_cal_done == true ) {
             demo_curr_state = demo_wait;
         } else if (motor_cal_done == false ) {
@@ -1122,7 +1146,7 @@
 
     // Do stuff until end condition is met
     demo_state_machine();
-    
+
     scope.set(0, emg1 );
     scope.set(1, Vx );
     scope.set(2, emg2 );
@@ -1174,6 +1198,7 @@
     // Entry function
     if ( global_state_changed == true ) {
         global_state_changed = false;
+        set_led_color('p'); // Set EMG calibration LED (PURPLE)
     }
 
     // Run EMG state machine until emg_cal_done flag is true
@@ -1183,6 +1208,7 @@
     if ( emg_cal_done == true ) { // WAIT MODE
         global_curr_state = global_wait;
         global_state_changed = true;
+        set_led_color('o'); // Disable EMG calibration LED
     }
 }
 
@@ -1192,6 +1218,7 @@
     // Entry function
     if ( global_state_changed == true ) {
         global_state_changed = false;
+        set_led_color('c'); // Set motor calibration LED (CYAN)
     }
 
     // Do stuff until end condition is met
@@ -1201,6 +1228,7 @@
     if ( motor_cal_done == true ) { // WAIT MODE
         global_curr_state = global_wait;
         global_state_changed = true;
+        set_led_color('o'); // Disable motor calibration LED
     }
 }
 
@@ -1213,6 +1241,7 @@
 
         emg_sampleNow = true; // Enable signal sampling in sampleSignals()
         emg_calibrateNow = false; // Disable calibration functionality in sampleSignals()
+        set_led_color('g'); // Set operation led (GREEN)
     }
 
     // Do stuff until end condition is met
@@ -1225,10 +1254,9 @@
     scope.set(3, Vy );
     scope.send();
 
-    led_g = !led_g;
-
     // State transition guard
-    if ( false ) { // Always stay in operation mode (can be changed)
+    if ( exit_operation == true ) {
+        exit_operation = false;
         global_curr_state = global_wait;
         global_state_changed = true;
     }
@@ -1286,7 +1314,7 @@
         double emg3_hp = bqc3_high.step( emg3_n );       // Filter highpass
         double emg3_rectify = fabs( emg3_hp );           // Rectify
         emg3_env = bqc3_low.step( emg3_rectify ); // Filter lowpass (completes envelope)
-        
+
         double emg4_n = bqc4_notch.step( emg4 );         // Filter notch
         double emg4_hp = bqc4_high.step( emg4_n );       // Filter highpass
         double emg4_rectify = fabs( emg4_hp );           // Rectify
@@ -1317,8 +1345,6 @@
     sampleSignals();
     global_state_machine();
     changeservo();
-    // controller();
-    // outputToMotors();
 }
 
 /*
@@ -1329,11 +1355,12 @@
     pc.baud(115200); // MODSERIAL rate
     pc.printf("Starting\r\n");
 
-    global_curr_state = global_wait; // Start off in EMG Wait state
+    global_curr_state = global_wait; // Start off in global wait state
     tickGlobal.attach( &tickGlobalFunc, Ts ); // Start global ticker
+    tickLED.attach ( &disp_led_color, 0.25f ); // Start LED ticker
 
-    // ---------- Enable Servo ----------    
-    myservo.Enable(1000,20000);
+    // ---------- Enable Servo ----------
+    myservo.Enable(1000, 20000);
 
     // ---------- Attach filters ----------
     bqc1_notch.add( &bq1_notch );
@@ -1347,7 +1374,7 @@
     bqc3_notch.add( &bq3_notch );
     bqc3_high.add( &bq3_H1 ).add( &bq3_H2 );
     bqc3_low.add( &bq3_L1 ).add( &bq3_L2 );
-    
+
     bqc4_notch.add( &bq4_notch );
     bqc4_high.add( &bq4_H1 ).add( &bq4_H2 );
     bqc4_low.add( &bq4_L1 ).add( &bq4_L2 );
@@ -1357,16 +1384,14 @@
     button2.fall( &button2Press );
     switch2.fall( &switch2Press );
     switch3.fall( &switch3Press );
-    extButton1.mode(PullUp);
+    extButton1.mode( PullUp ); // To make sure the servo works
 
     // ---------- Attach PWM ----------
     motor1Power.period(PWM_period);
     motor2Power.period(PWM_period);
 
     // ---------- Turn OFF LEDs ----------
-    led_b = 1;
-    led_g = 1;
-    led_r = 1;
+    set_led_color('o');
 
     while(true) {
         pc.printf("Global state: %i EMG state: %i Motor state: %i Operation state: %i Demo state: %i\r\n", global_curr_state, emg_curr_state, motor_curr_state, operation_curr_state, demo_curr_state);
@@ -1374,7 +1399,7 @@
         pc.printf("q1: %f q2: %f \r\n",q1*rad2deg,q2*rad2deg);
         pc.printf("Motor1ref: %f Motor1angle: %f\r\n",motor1_ref*rad2deg/5.5f,motor1_angle*rad2deg/5.5f);
         pc.printf("Motor2ref: %f Motor2angle: %f\r\n",motor2_ref*rad2deg/2.75f,motor2_angle*rad2deg/2.75f);
-        
+
         //pc.printf("Xe: %f Ye: %f\r\n",xe,ye);
         wait(0.5f);
     }