Diff: main.cpp

Revision:

11:46a859e526ca

Parent:

10:3cee5adfbd72

Child:

14:0a66bce6ee19

Child:

15:59d7fffd070a

--- a/main.cpp	Tue Oct 27 11:43:55 2015 +0000
+++ b/main.cpp	Wed Oct 28 13:40:35 2015 +0000
@@ -16,10 +16,11 @@
 bool        go_motor = true;
 bool        go_tick = false;
 bool        go_sample = false;
-double      motorSpeed = 0.05;
+double      motorSpeed = 0.2;
 
 double      emg_out0, emg_out1, emg_out2;
 
+int debug_tick = 0;
 void tick()
 {
     double a,b,x,y;
@@ -33,20 +34,109 @@
     // 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;    
+    } else if (debug_tick < 10) {
+        emg_out0 = 0;
+        emg_out1 = 0;
+        emg_out2 = 1;
+    } else if (debug_tick < 20) {
+        emg_out0 = 0;
+        emg_out1 = 0;
+        emg_out2 = 0;
+    } else if (debug_tick < 25) {
+        emg_out0 = 1;
+        emg_out1 = 0;
+        emg_out2 = 0;
+    } else if (debug_tick < 30) {
+        emg_out0 = 0;
+        emg_out1 = 0;
+        emg_out2 = 0;
+    } else if (debug_tick < 40) {
+        emg_out0 = 0;
+        emg_out1 = 0;
+        emg_out2 = 1;
+    } else if (debug_tick < 45) {
+        emg_out0 = 0;
+        emg_out1 = 0;
+        emg_out2 = 0;
+    } else if (debug_tick < 55) {
+        emg_out0 = 0;
+        emg_out1 = 1;
+        emg_out2 = 0;
+    } else if (debug_tick < 60) {
+        emg_out0 = 0;
+        emg_out1 = 0;
+        emg_out2 = 0;
+    } else if (debug_tick < 70) {
+        emg_out0 = 0;
+        emg_out1 = 0;
+        emg_out2 = 1;
+    } else {
+        emg_out0 = 0;
+        emg_out1 = 0;
+        emg_out2 = 0;
+    }
+    
+    /*if (debug_tick < 10) {
+        emg_out0 = 0;
+        emg_out1 = 0;
+        emg_out2 = 0;   
+    } else if (debug_tick < 15) {
+        emg_out0 = 1;
+        emg_out1 = 0;
+        emg_out2 = 0;   
+    } else if (debug_tick < 25) {
+        emg_out0 = 0;
+        emg_out1 = 1;
+        emg_out2 = 0;   
+    } else if (debug_tick < 35) {
+        emg_out0 = 0;
+        emg_out1 = 0;
+        emg_out2 = 0;   
+    } else if (debug_tick < 45) {
+        emg_out0 = 1;
+        emg_out1 = 0;
+        emg_out2 = 0;   
+    } else if (debug_tick < 55) {
+        emg_out0 = 0;
+        emg_out1 = 0;
+        emg_out2 = 0;   
+    } else if (debug_tick < 65) {
+        emg_out0 = 0;
+        emg_out1 = 1;
+        emg_out2 = 0;   
+    } else {
+        emg_out0 = 0;    
+        emg_out1 = 0;
+        emg_out2 = 0;
+    }*/
  
     // Calculate the new position using the EMG output and the current position
-    newPos(emg_out0, emg_out1, 0, a, b, x, y);
-    pc2.printf("O0: %f | O1: %f | O2: %f\r\n",emg_out0, emg_out1, emg_out2);
+    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(a < -20 || a > 20) 
-        motorSpeed = 0.03;
+    if (pushing)
+        motorSpeed = 1;
+    else if(a < -20 || a > 20) 
+        motorSpeed = 0.14;
     else
-        motorSpeed = 0.05;
+        motorSpeed = 0.25;
     
     // Rotate the motors
-    rotate(x,y);
+    if (pushing)
+        push(x, y);
+    else
+        rotate(x,y);
+    
+    debug_tick++;
 }
 
 /* Functions which get called by tickers */