Diff: main.cpp

Revision:

8:2afb66572fc4

Parent:

7:d4090f334ce2

Child:

9:8b2d6ec577e3

--- a/main.cpp	Mon Oct 29 13:19:43 2018 +0000
+++ b/main.cpp	Wed Oct 31 09:12:59 2018 +0000
@@ -5,7 +5,7 @@
 #include "math.h"
 
 #include "mbed.h"
-#include "HIDScope.h"
+//#include "HIDScope.h"
 #include "BiQuad.h"
 #include "BiQuad4.h"
 #include "FilterDesign.h"
@@ -39,8 +39,8 @@
 Timer       timer;                        // Timer for counting time in this state
 Ticker      WriteValues;            // Ticker to show values of velocity to screen
 Ticker      StateMachine;
-Ticker      sample_EMGtoHIDscope;   // Ticker to send the EMG signals to screen
-HIDScope    scope(4);               //Number of channels which needs to be send to the HIDScope
+//Ticker      sample_EMGtoHIDscope;   // Ticker to send the EMG signals to screen
+//HIDScope    scope(4);               //Number of channels which needs to be send to the HIDScope
 
 // Set up ProcessStateMachine
 enum states {WAITING, MOTOR_ANGLE_CLBRT, EMG_CLBRT, HOMING, WAITING4SIGNAL, MOVE_W_EMG, MOVE_W_DEMO, FAILURE_MODE};
@@ -103,11 +103,13 @@
     emg1_filtered = FilterDesign(emg1_raw.read());
     emg2_filtered = FilterDesign2(emg2_raw.read());
     
+    /**
     scope.set(0, emg1_raw.read());      // Raw EMG 1 send to scope 0
     scope.set(1, emg1_filtered);        // Filtered EMG 1 send to scope 1
     scope.set(2, emg2_raw.read());      // Raw EMG 2 send to scope 2
     scope.set(3, emg2_filtered);        // Filtered EMG 2 send to scope 3
     scope.send();                       // Send the data to the computer
+    */
 }
 // ---------------------------------------------------
 // --------STATEMACHINE------------------------------- 
@@ -121,7 +123,6 @@
         // In this state we do nothing, and wait for a command
         
         // Actions        
-        pc.printf("WAITING... \r\n");
         led_red = 0; led_green = 0; led_blue = 0;   // Colouring the led WHITE
         
         // State transition logic
@@ -161,7 +162,7 @@
             {
                 //pc.printf( "Tijd in deze staat = %f \n\r", time_in_state);           
                 //pc.printf( "Tijd tijdens actions loop (Waarde voor bepalen van snelheid)") = %f \n\r", tijd);
-                pc.printf("Change State, Motor calibration is ended. \n\r");
+                pc.printf("Motor calibration has ended. \n\r");
                 timer.stop();                              // Stop timer for this state
                 timer.reset();                             // Reset timer for this state
                 motor1_pwm.write(fabs(0.0));               // Send PWM values to motor
@@ -170,6 +171,7 @@
                 Encoder2.reset();
         
                 currentState = EMG_CLBRT;                   // Switch to next state (EMG_CLRBRT).
+                pc.printf("EMG calibration \r\n");
                 stateChanged = true;                       
            }
         if (Fail_button == 0)
@@ -185,7 +187,6 @@
         // measure the maximum EMG-signal, which can be used to scale 
         // the EMG-filter.
                 
-         pc.printf("EMG calibration \r\n");
          led_red = 1; led_green = 1; led_blue = 0;   // Colouring the led BLUE
                 
          // Requirements to move to the next state:
@@ -206,8 +207,8 @@
         case HOMING:
         // Description:
         // Robot moves to the home starting configuration
+        pc.printf("HOMING, moving to the home starting configuration. \r\n");
                     
-        pc.printf("HOMING, moving to the home starting configuration. \r\n");
         led_red = 0; led_green = 1; led_red = 0;                  // Colouring the led PURPLE
                 
         // Requirements to move to the next state:
@@ -228,7 +229,7 @@
         case WAITING4SIGNAL:
         // Description:
         // In this state the robot waits for an action to occur.
-                    
+        pc.printf("Press d to run the demo, Press e to move with EMG, Press c to re-calibrate. \r\n");            
         led_red = 0; led_green = 0; led_blue = 0;                // Colouring the led WHITE
                 
         // Requirements to move to the next state:
@@ -236,19 +237,22 @@
         // state (MOVE_W_DEMO, MOVE_W_EMG or SHUTDOWN)
                 
         // CANNOT USE KEYBOARD. SO LOOK FOR ALTERNATIVE
-        pc.printf("Press d to run the demo, Press e to move with EMG, Press c to re-calibrate. \r\n");
         c = pc.getc();
         if (c == 'd') 
         {
+            pc.printf("MOVE_W_DEMO, Running the demo \r\n");
             currentState = MOVE_W_DEMO;
+            
         }
         else if (c == 'e') 
         {
+            pc.printf("MOVE_W_EMG, Moving with use of EMG \r\n");
             currentState = MOVE_W_EMG;
         }
         else if (c == 'c') 
         {
-        currentState = MOTOR_ANGLE_CLBRT;
+            pc.printf("Starting Calibration\n\r");
+            currentState = MOTOR_ANGLE_CLBRT;
         }
         
         if (Fail_button == 0)
@@ -264,7 +268,6 @@
         // a square.
                     
         led_red = 1; led_green = 1; led_blue = 0;   // Colouring the led GREEN
-        pc.printf("MOVE_W_DEMO, Running the demo \r\n");
                 
         // Requirements to move to the next state:
         // When the home button or the failure button is pressed, we 
@@ -292,7 +295,6 @@
         // EMG-signals. 
                     
         led_red = 1; led_green = 0; led_blue = 1;   // Colouring the led GREEN
-        pc.printf("MOVE_W_EMG, Moving with use of EMG \r\n");
              
         if (emg1_filtered >= (threshold_EMG*EMG_calibrated_max_1)){
             need_to_move_1 = 1; // The robot does have to move
@@ -301,7 +303,7 @@
             need_to_move_1 = 0; // If the robot does not have to move
             }
 
-        if(emg2_filtered >= threshold*EMG_calibrated_max_2){
+        if(emg2_filtered >= threshold_EMG*EMG_calibrated_max_2){
             need_to_move_2 = 1;
             }
         else {
@@ -381,8 +383,15 @@
     led_blue = 1;
     
     pc.baud(115200);
+    
+    pc.printf("\r\n _______________ INSERT ROBOT NAME HERE! _______________ \r\n");
+    wait(0.5f);
+    pc.printf("WAITING... \r\n");
+    
     StateMachine.attach(&ProcessStateMachine, 0.005f);   // Run statemachine 200 times per second
+    /**
     sample_EMGtoHIDscope.attach(&sample, 0.02f);         // Display EMG values 50 times per second
+    */
     while (true) {
         
     }