Diff: main.cpp

Revision:

22:c6459c435069

Parent:

21:9de9e0a73e57

Child:

23:174f955e5c15

--- a/main.cpp	Tue Nov 03 11:59:37 2015 +0000
+++ b/main.cpp	Thu Nov 05 10:47:06 2015 +0000
@@ -4,7 +4,7 @@
 #include "pidControl.h"
 #include "PinDetect.h"
 #include "compute.h"
-
+ 
 /// Define Objects
 Ticker      sample_timer;
 Ticker      movement_timer;
@@ -18,57 +18,73 @@
 double      motorSpeed = 0.2;
 int         calibrated = 0;
 double      emg_out0, emg_out1, emg_out2;
-
+bool        demoing = false;
+int         demoInt = 0;
+ 
 void tick()
 {
+    if(demoing) {
+        double a,b;
+        if(demoInt == 0)
+            Point2Angles(-20, 18, a, b);
+        if(demoInt == 1)
+            Point2Angles(-20, 35, a, b);
+        if(demoInt == 2)
+            Point2Angles(20, 35, a, b);
+        if(demoInt == 3)
+            Point2Angles(20, 18, a, b);
+        demoInt++;
+        demoInt = demoInt%4;
+        rotate(a,b);
+        return;  
+    }
     double a,b,x,y;
-
+ 
     /** If the game has not started, don't do anything. */
     if(!started) return;
-
+ 
     /// Get the current rotation
     getCurrent(a,b);
-
+ 
     /// Remove the offset
     a = getOffset(1) - a;
     b = getOffset(2) - b;
-
+ 
     /// Calculate the new position using the EMG output and the current position
     bool pushing;
     newPos(emg_out0, emg_out1, emg_out2, a, b, x, y, pushing);
-
-    Angles2Point(x,y,a,b);
-    if (pushing) 
+   
+    if (pushing)
         motorSpeed = 1;
     else if(a < -20 || a > 20)
         motorSpeed = 0.14;
     else
         motorSpeed = 0.25;
-
+ 
     /// Rotate the motors
     if (pushing)
         push(x, y);
     else
         rotate(x,y);
-
+ 
 }
-
+ 
 /** Functions which get called by tickers */
 void goMotor()
 {
     go_motor = true;
 }
-
+ 
 void goTick()
 {
     go_tick = true;
 }
-
+ 
 void goSample()
 {
     go_sample = true;
 }
-
+ 
 /** Function called by pressing the SW3 Button */
 void calibrateAndStartStop()
 {
@@ -82,18 +98,24 @@
         go_motor = false;
         PID_stop();
         movement_timer.detach();
-    } else 
+    } else
         movement_timer.attach(&goMotor, 0.01f);
 }
-
+ 
 /** Function called by pressing the SW2 Button */
 void startRound()
 {
-    if(calibrated < 3) return;
-    sample_timer.attach(&goSample, 0.002);
-    calc_timer.attach(&goTick, 0.15);
+    if(calibrated < 3) {
+        calibrated = 4;
+        calc_timer.attach(&goTick, 2.0);
+        demoing = true;  
+    }
+    else {
+        sample_timer.attach(&goSample, 0.002);
+        calc_timer.attach(&goTick, 0.15);
+    }
 }
-
+ 
 /** Initialize the buttons */
 void init()
 {
@@ -101,12 +123,12 @@
     caliAndSS.mode(PullUp);
     caliAndSS.attach_deasserted(&calibrateAndStartStop);
     caliAndSS.setSampleFrequency();
-
+ 
     sRound.mode(PullUp);
     sRound.attach_deasserted(&startRound);
     sRound.setSampleFrequency();
 }
-
+ 
 /** The main function
     Initializes every library
     Starts the timers
@@ -118,14 +140,14 @@
     double a,b;
     IBC_init(a,b);
     PID_init();
-    
+   
     /** Enable buttons */
     init();
-
+ 
     movement_timer.attach(&goMotor, 0.01f);
-
+ 
     rotate(a,b);
-
+ 
     /** The main while-loop */
     while(1) {
         /// The motor timer
@@ -144,4 +166,4 @@
             tick();
         }
     }
-}
\ No newline at end of file
+}