Diff: main.cpp

Revision:

25:45c131af2dba

Parent:

24:e87e4fcf6226

Child:

26:418f025a30c6

--- a/main.cpp	Thu Oct 31 18:53:30 2019 +0000
+++ b/main.cpp	Thu Oct 31 21:40:05 2019 +0000
@@ -49,6 +49,8 @@
 int ingedrukt = 0;
 int state_int;
 int previous_state_int;
+double motorvalue1;
+double motorvalue2;
 // Define the different states in which the robot can be
 enum States {MOTORS_OFF, EMG_CALIBRATION, START_GAME,
              DEMO_MODE, OPERATING_MODE, END_GAME
@@ -258,7 +260,7 @@
     getmeasuredposition(measured1, measured2);
 
     // Calculate the motor values
-    double motorvalue1, motorvalue2;
+    //double motorvalue1, motorvalue2;
     motorvalue1 = PID_controller1(reference1 - measured1);
     motorvalue2 = PID_controller2(reference2 - measured2);
     sendtomotor(motorvalue1, motorvalue2);
@@ -402,7 +404,7 @@
 void demo_mode()
 {
 
-    state_int = 10;
+    //state_int = 10;
 
     // 5 pre determined positions of the end effector to show it can move in straight lines
     xendeffector=-5;
@@ -435,8 +437,6 @@
     double y0,y1,y2,y3;
     measure_data(y0,y1,y2,y3);
     
-    y0 = 0;
-    y1 = 0;
     
     double threshold = 0.3;     // 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
@@ -455,7 +455,9 @@
     }
 
     //servo besturing
-
+    
+    
+    
     if(but4.read() == 0 && ingedrukt == 0) {
         for(int i=0; i<100; i++) {
             myservo = i/100.0;
@@ -470,17 +472,43 @@
     }
     
     if (but3.read() == 0) {
+        bool buttonss = true;
+        while (buttonss) {
+            if (but3.read() == 1) {
+                buttonss = false;
+            }
+        }
         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;
+    }
+    /*
+    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()
 {
-    wait(1);
+    /*
+    current_y = yendeffector;
+    current_x = xendeffector;
+    
+    while (abs(current_y-10.6159) >= 0.01 && abs(current_x-10.6159) >= 0.01) [
+        
+    }
+    */
+    xendeffector = 0.0;
+    yendeffector = 20.6159;
+    //motorvalue1 = 0;
+    //motorvalue2 = 0;
+    wait(0.5);
     measurecontrol.detach();    
     MyState = START_GAME;
 }