Diff: main.cpp

Revision:

16:b977ba7ed362

Parent:

14:0a66bce6ee19

Child:

17:b1d8c215b7ea

--- a/main.cpp	Thu Oct 29 10:55:06 2015 +0000
+++ b/main.cpp	Thu Oct 29 17:13:29 2015 +0000
@@ -25,22 +25,22 @@
 void tick()
 {
     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;
-    
+
     // TODO: Remove debugging overrides
     if (debug_tick < 5) {
         emg_out0 = 0;
         emg_out1 = 0;
-        emg_out2 = 0;    
+        emg_out2 = 0;
     } else if (debug_tick < 10) {
         emg_out0 = 0;
         emg_out1 = 0;
@@ -82,86 +82,99 @@
         emg_out1 = 0;
         emg_out2 = 0;
     }
-    
+
     /*if (debug_tick < 10) {
         emg_out0 = 0;
         emg_out1 = 0;
-        emg_out2 = 0;   
+        emg_out2 = 0;
     } else if (debug_tick < 15) {
         emg_out0 = 1;
         emg_out1 = 0;
-        emg_out2 = 0;   
+        emg_out2 = 0;
     } else if (debug_tick < 25) {
         emg_out0 = 0;
         emg_out1 = 1;
-        emg_out2 = 0;   
+        emg_out2 = 0;
     } else if (debug_tick < 35) {
         emg_out0 = 0;
         emg_out1 = 0;
-        emg_out2 = 0;   
+        emg_out2 = 0;
     } else if (debug_tick < 45) {
         emg_out0 = 1;
         emg_out1 = 0;
-        emg_out2 = 0;   
+        emg_out2 = 0;
     } else if (debug_tick < 55) {
         emg_out0 = 0;
         emg_out1 = 0;
-        emg_out2 = 0;   
+        emg_out2 = 0;
     } else if (debug_tick < 65) {
         emg_out0 = 0;
         emg_out1 = 1;
-        emg_out2 = 0;   
+        emg_out2 = 0;
     } else {
-        emg_out0 = 0;    
+        emg_out0 = 0;
         emg_out1 = 0;
         emg_out2 = 0;
     }*/
- 
+
     // 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);
     // pc2.printf("O0: %f | O1: %f | O2: %f\r\n",emg_out0, emg_out1, emg_out2);
-    
+
     Angles2Point(x,y,a,b);
-    
+
     if (pushing)
         motorSpeed = 1;
-    else if(a < -20 || a > 20) 
+    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);
-    
+
     debug_tick++;
 }
 
 /* Functions which get called by tickers */
-void goMotor() { go_motor = true; }
+void goMotor()
+{
+    go_motor = true;
+}
 
-void goTick() { go_tick = true; }
+void goTick()
+{
+    go_tick = true;
+}
 
-void goSample() { go_sample = true; }
+void goSample()
+{
+    go_sample = true;
+}
 
 /* Function called by pressing the SW3 Button */
 void calibrateAndStartStop()
 {
-    if(calibrated < 3) {
+    if(calibrated < 4) {
+        pc2.printf("Calibrating %d\r\n", calibrated);
+        emg_cal(calibrated);
         calibrated++;
-        emg_cal(calibrated);
-        return;   
+        pc2.printf("Done\r\n");
+        return;
     }
     started = !started;
-    if(started)
-        movement_timer.attach(&goMotor, 0.01f);
-    else {
+    if(started) {
+        pc2.printf("Stopped\r\n");
         go_motor = false;
         PID_stop();
         movement_timer.detach();
+    } else {
+        pc2.printf("Starting\r\n");
+        movement_timer.attach(&goMotor, 0.01f);
     }
 }
 
@@ -195,19 +208,20 @@
 */
 int main()
 {
-    init();
-    
     /* Initialize libraries */
     double a,b;
     IBC_init(a,b);
     pc2.printf("(--- In Between Controller Initialized ---)\r\n");
-    
+
     PID_init();
     pc2.printf("(--- PID Controller Initialized ---)\r\n");
+    
+    /* Enable buttons */
+    init();
 
     movement_timer.attach(&goMotor, 0.01f);
 
-    rotate(a,b);
+    //rotate(a,b);
 
     /* The main while-loop */
     while(1) {
@@ -219,7 +233,7 @@
         // The sample timer
         if(go_sample) {
             go_sample = false;
-            sample(emg_out0, emg_out1, emg_out2);   
+            sample(emg_out0, emg_out1, emg_out2);
         }
         // The tick timer
         if(go_tick) {