Diff: main.cpp

Revision:

26:418f025a30c6

Parent:

25:45c131af2dba

Child:

27:e704fdc41e87

--- a/main.cpp	Thu Oct 31 21:40:05 2019 +0000
+++ b/main.cpp	Fri Nov 01 17:18:39 2019 +0000
@@ -14,6 +14,9 @@
 Servo myservo(D13);     // To control the servo motor
 DigitalIn but3(SW2);    // To go to the next state or to choose one of two game modes when in state START_GAME
 DigitalIn but4(SW3);    // To choose one of two game modes when in state START_GAME or to move the gripper
+DigitalOut ledr(LED_RED);
+DigitalOut ledg(LED_GREEN);
+DigitalOut ledb(LED_BLUE);
 
 AnalogIn S0(A0);
 AnalogIn S1(A1);
@@ -101,6 +104,9 @@
 
 int main()
 {
+    ledr = 1;
+    ledg = 1;
+    ledb = 1;
     pc.baud(115200);
     pc.printf("Starting...\r\n\r\n");
 
@@ -362,18 +368,23 @@
 
     measure_data(y0, y1, y2, y3);   // Calculate RMS values
 
-    double duration = 15.0;                 // Duration of the emgcalibration function, in this case 10 seconds
+    double duration = 20.0;                 // Duration of the emgcalibration function, in this case 10 seconds
     int rounds = (duration / timeinterval); // Determine the amount of times this function has to run to run for the duration time
     // rounds is an integer so the value of duration / timeinterval is floored
 
     static int counter = 0;             // Counter which keeps track of the amount of times the function has executed
     if (counter >= rounds) {
         MyState = START_GAME;    // If counter is larger than rounds, change MyState to the next state
+        ledb = 1;
         measurecontrol.detach();
     } else {
         counter++;  // Else increase counter by 1
     }
-
+    
+    int duration_led = 0.1 / timeinterval;
+    if (counter % duration_led == 0) {
+        ledb = !ledb;
+    }
 }
 
 void startgame()
@@ -404,8 +415,6 @@
 void demo_mode()
 {
 
-    //state_int = 10;
-
     // 5 pre determined positions of the end effector to show it can move in straight lines
     xendeffector=-5;
     yendeffector=10.6159;
@@ -434,11 +443,45 @@
 
 void operating_mode()
 {
+    // green turns on and off while running this function
+    static int counter = 0;
+    int duration_led = 0.1 / timeinterval;
+    
+    if (counter % duration_led == 0) {
+        ledg = !ledg;
+        counter = 0;
+    }
+    counter++;
+    
+    
     double y0,y1,y2,y3;
     measure_data(y0,y1,y2,y3);
     
+    //servo besturing
     
-    double threshold = 0.3;     // When the analysed signal is above this threshold, the position of the end effector is changed
+    if(but4.read() == 0 && ingedrukt == 0) {
+        for(int i=0; i<100; i++) {
+            myservo = i/100.0;
+        }
+        ingedrukt = 1;
+        y0 = 0;
+        y1 = 0;
+        y2 = 0;
+        y3 = 0;
+    }
+    else if(but4.read() == 0 && ingedrukt == 1) {
+        for(int i=100; i>0; i--) {
+            myservo = i/100.0;
+        }
+        y0 = 0;
+        y1 = 0;
+        y2 = 0;
+        y3 = 0;
+        ingedrukt = 0;
+    }
+    
+    
+    double threshold = 0.4;     // When the analysed signal is above this threshold, the position of the end effector is changed
     double dr = 0.01;            // The change in position with each step
     
     if(y0 > threshold && xendeffector < 16) {
@@ -447,29 +490,13 @@
     else if(y1 > threshold && xendeffector > -16) {
         xendeffector=xendeffector-dr;
     }
-    if(y2 > threshold && yendeffector < 28) {
+    if(y2 > threshold+0.2 && yendeffector < 28) {
         yendeffector=yendeffector+dr;
     }
-    else if(y3 > threshold && yendeffector > 8) {
+    else if(y3 > threshold+0.2 && yendeffector > 8) {
         yendeffector=yendeffector-dr;
     }
 
-    //servo besturing
-    
-    
-    
-    if(but4.read() == 0 && ingedrukt == 0) {
-        for(int i=0; i<100; i++) {
-            myservo = i/100.0;
-        }
-        ingedrukt = 1;
-    } else if(but4.read() == 0 && ingedrukt == 1) {
-        for(int i=100; i>0; i--) {
-            myservo = i/100.0;
-        }
-        
-        ingedrukt = 0;
-    }
     
     if (but3.read() == 0) {
         bool buttonss = true;
@@ -480,35 +507,26 @@
         }
         pc.printf("The game has ended, will move the end effector to (0,0), put the motors off and will now return to the state START_GAME");
         MyState = END_GAME;
-        //xendeffector=0.0;
-        //yendeffector=10.6159;
+        measurecontrol.detach();
     }
-    /*
-    if (but3.read() == 0) {
-        pc.printf("The game has ended, will move the end effector to (0,10.6159), put the motors off and will now return to the state START_GAME");
-        MyState = END_GAME;
-        xendeffector=0.0;
-        yendeffector=10.6159;
-    }
-    */
+
     measureandcontrol();
 }
 
 void endgame()
 {
-    /*
-    current_y = yendeffector;
-    current_x = xendeffector;
+    measurecontrol.attach(measureandcontrol,timeinterval);
     
-    while (abs(current_y-10.6159) >= 0.01 && abs(current_x-10.6159) >= 0.01) [
-        
+    double tempx = xendeffector;
+    double tempy = yendeffector-10.6159;
+    double steps = 20;
+    
+    for (int i = 0; i < steps; i++) {
+        xendeffector = xendeffector - tempx/steps;
+        yendeffector = yendeffector - tempy/steps;
+        wait(0.1);
     }
-    */
-    xendeffector = 0.0;
-    yendeffector = 20.6159;
-    //motorvalue1 = 0;
-    //motorvalue2 = 0;
-    wait(0.5);
+
     measurecontrol.detach();    
     MyState = START_GAME;
 }
@@ -520,7 +538,9 @@
     switch (MyState) {
         case MOTORS_OFF :
             pc.printf("\r\nState: Motors turned off\r\n");
+            ledb = 0;       // blue led turns on when you enter MOTORS_OFF state
             motorsoff();
+            ledb = 1;       // blue led turns off when you exit MOTORS_OFF state
             break;
 
         case EMG_CALIBRATION :
@@ -531,23 +551,35 @@
 
         case START_GAME :
             pc.printf("\r\nState: Start game\r\n");
+            ledr = 0;       // red led is on when you enter START_GAME state
+            ledb = 0;       // blue led is on when you enter START_GAME state
+            ledg = 0;       // green led is on when you enter START_GAME state
             startgame();
             break;
 
         case DEMO_MODE :
             pc.printf("\r\nState: Demo mode\r\n");
             measurecontrol.attach(measureandcontrol,timeinterval);
+            ledr = 1;       // red led is on when you enter START_GAME state
+            ledb = 0;       // blue led is on when you enter START_GAME state
+            ledg = 0;       // green led is on when you enter START_GAME state
             demo_mode();
             measurecontrol.detach();
             break;
 
         case OPERATING_MODE :
             pc.printf("\r\nState: Operating mode\r\n");
+            ledr = 1;       // red led is off when you enter OPERATING_MODE state
+            ledb = 1;       // blue led is off when you enter OPERATING_MODE state
+            ledg = 0;       // green led is on when you enter OPERATING_MODE state
             measurecontrol.attach(operating_mode,timeinterval);
             break;
 
         case END_GAME :
             pc.printf("\r\nState: End of the game\r\n");
+            ledr = 0;       // red led is on when you enter OPERATING_MODE state
+            ledb = 1;       // blue led is off when you enter OPERATING_MODE state
+            ledg = 1;       // green led is off when you enter OPERATING_MODE state
             endgame();
             break;