Diff: main.cpp

Revision:

20:ebeb18494f3e

Parent:

19:7c356ec0fae0

Child:

21:9de9e0a73e57

--- a/main.cpp	Fri Oct 30 10:41:56 2015 +0000
+++ b/main.cpp	Tue Nov 03 10:16:46 2015 +0000
@@ -6,7 +6,6 @@
 #include "compute.h"
 
 // Define Objects
-Serial      pc2(USBTX, USBRX);
 Ticker      sample_timer;
 Ticker      movement_timer;
 Ticker      calc_timer;
@@ -18,10 +17,8 @@
 bool        go_sample = false;
 double      motorSpeed = 0.2;
 int         calibrated = 0;
-
 double      emg_out0, emg_out1, emg_out2;
 
-int debug_tick = 0;
 void tick()
 {
     double a,b,x,y;
@@ -36,23 +33,13 @@
     a = getOffset(1) - a;
     b = getOffset(2) - b;
 
-    // TODO: Remove debugging overrides
-    if (debug_tick < 1) {
-        emg_out0 = 0;
-        emg_out1 = 0;
-        emg_out2 = 0;
-    }
-    debug_tick++;
-
     // 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;
-        //pc2.printf("PUSHING: %f, %f\r\n",a,b);
-    }
     else if(a < -20 || a > 20)
         motorSpeed = 0.14;
     else
@@ -86,22 +73,17 @@
 void calibrateAndStartStop()
 {
     if(calibrated < 4) {
-        pc2.printf("Calibrating %d\r\n", calibrated);
         emg_cal(calibrated);
         calibrated++;
-        pc2.printf("Done\r\n");
         return;
     }
     started = !started;
     if(!started) {
-        pc2.printf("Stopped\r\n");
         go_motor = false;
         PID_stop();
         movement_timer.detach();
-    } else {
-        pc2.printf("Starting\r\n");
+    } else 
         movement_timer.attach(&goMotor, 0.01f);
-    }
 }
 
 /* Function called by pressing the SW2 Button */
@@ -123,8 +105,6 @@
     sRound.mode(PullUp);
     sRound.attach_deasserted(&startRound);
     sRound.setSampleFrequency();
-
-    pc2.printf("Buttons done\r\n\r\n");
 }
 
 /* The main function
@@ -137,10 +117,7 @@
     /* 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();