Diff: main.cpp

Revision:

20:7f1997276ce2

Parent:

19:2d9421b0316a

--- a/main.cpp	Wed Oct 31 15:33:40 2018 +0000
+++ b/main.cpp	Wed Oct 31 16:01:14 2018 +0000
@@ -43,7 +43,7 @@
 const double r_big = 590.0; //maximum radius of the moving space
 const double r_small = 162.0; //minimum radius of the moving space
 const double r_top = 250.0; //radius of the top portion of the moving space
-double v=1.0; //moving speed of setpoint (dependant on the waiting time)
+double v=0.02; //moving speed of setpoint (dependant on the waiting time)
 volatile int sx;//value of the button and store as switch
 volatile int sy;//value of the button and store as switch
 int dirx = 1; //determine the direction of the setpoint placement
@@ -100,8 +100,8 @@
 
 int need_to_move_1;                     // Does the robot needs to move in the first direction?
 int need_to_move_2;                     // Does the robot needs to move in the second direction?
-double EMG_calibrated_max_1 = 2.00000;  // Maximum value of the first EMG signal found in the calibration state.
-double EMG_calibrated_max_2 = 2.00000;  // Maximum value of the second EMG signal found in the calibration state.
+double EMG_calibrated_max_1 = 0.00000;  // Maximum value of the first EMG signal found in the calibration state.
+double EMG_calibrated_max_2 = 0.00000;  // Maximum value of the second EMG signal found in the calibration state.
 double emg1_cal = 0.00000;              //measured value of the first emg
 double emg2_cal = 0.00000;              //measured value of the second emg
 
@@ -308,7 +308,7 @@
             
             // Actions        
             led_red = 0; led_green = 0; led_blue = 0;   // Colouring the led WHITE
-            
+            pc.printf("I am WAITING");
             // State transition logic
             if (button_clbrt_home == 0) 
             {
@@ -330,6 +330,7 @@
              
             // Actions       
             led_red = 1; led_green = 0; led_blue = 0;   // Colouring the led TURQUOISE
+            pc.printf("Motor angle calibrating....");
             timer.start();                              //Start timer to get time in the state "MOTOR_ANGLE_CLRBRT" 
             if (stateChanged)
                 {
@@ -355,7 +356,7 @@
                     Encoder2.reset();
             
                     currentState = EMG_CLBRT;                   // Switch to next state (EMG_CLRBRT).
-                    pc.printf("EMG calibration \r\n");
+                    pc.printf("Go to EMG calibration \r\n");
                     stateChanged = true;                       
                }
             if (Fail_button == 0)
@@ -370,7 +371,8 @@
             // to flex his/her muscles as hard as possible, in order to 
             // measure the maximum EMG-signal, which can be used to scale 
             // the EMG-filter.
-            led_red = 1; led_green = 1; led_blue = 0;   // Colouring the led BLUE      
+            led_red = 1; led_green = 1; led_blue = 0;   // Colouring the led BLUE 
+            pc.printf("EMG is calibrating");
             for (int i = 0; i <= 10; i++) //10 measuring points
             {      
             if (emg1_cal > EMG_calibrated_max_1){
@@ -379,11 +381,12 @@
             if (emg2_cal > EMG_calibrated_max_2){
                 EMG_calibrated_max_2 = emg2_cal;}
                 
-            //pc.printf("EMG1 = %f, EMG2 = %f \r\n",EMG_calibrated_max_1,EMG_calibrated_max_2);
+            pc.printf("EMG1 = %f, EMG2 = %f \r\n",EMG_calibrated_max_1,EMG_calibrated_max_2);
             wait(0.5f);
             }
             
-             currentState = HOMING;
+            currentState = HOMING;
+            pc.printf("Calibreren klaar, ik ga weer naar HOMING");
             if (Fail_button == 0)
             {
                 currentState = FAILURE_MODE;
@@ -394,7 +397,7 @@
         case HOMING:
             // Description:
             // Robot moves to the home starting configuration
-            pc.printf("HOMING \r\n");
+            pc.printf("HOMING... \r\n");
                         
             led_red = 0; led_green = 1; led_red = 0;                  // Colouring the led PURPLE
                     
@@ -417,7 +420,7 @@
             // In this state the robot waits for an action to occur.
             
             led_red = 0; led_green = 0; led_blue = 0;                // Colouring the led WHITE
-                    
+            pc.printf("I am waiting for a signal...");
             // Requirements to move to the next state:
             // If a certain button is pressed we move to the corresponding 
             // state (MOVE_W_DEMO, MOVE_W_EMG or SHUTDOWN)
@@ -428,13 +431,13 @@
                 stateChanged = true;
                 pc.printf("Starting Calibration \n\r");
             }
-            else if (button_Demo == 1) 
+            else if (button_Demo == 0) 
             {
                 currentState = MOVE_W_DEMO;
                 pc.printf("DEMO \r\n");
                 wait(1.0f);
             }
-            else if (button_Emg == 1) 
+            else if (button_Emg == 0) 
             {
                 currentState = MOVE_W_EMG;
                 pc.printf("EMG \r\n");
@@ -454,7 +457,7 @@
         // a square.
                     
         led_red = 1; led_green = 1; led_blue = 0;   // Colouring the led GREEN
-                
+        pc.printf("I am moving with a demo");
         // Requirements to move to the next state:
         // When the home button or the failure button is pressed, we 
         // will the move to the corresponding state.
@@ -482,7 +485,7 @@
             // EMG-signals. 
                         
             led_red = 1; led_green = 0; led_blue = 1;   // Colouring the led GREEN
-                 
+            pc.printf("I am moving with EMG signals...");
             if (emg1_filtered >= (threshold_EMG*EMG_calibrated_max_1)){
                 need_to_move_1 = 1; // The robot does have to move
                 }
@@ -499,6 +502,7 @@
             
             setpointx = EMG1On(need_to_move_1);         // Determine setpoints
             setpointy = EMG2On(need_to_move_2);
+            pc.printf("X moet %f worden en Y moet %f worden",setpointx, setpointy);
             motoraansturing();
             
             // Requirements to move to the next state: