Diff: main.cpp

Revision:

9:70cab4bf38a5

Parent:

8:7814ba9f801e

Child:

10:3cee5adfbd72

--- a/main.cpp	Thu Oct 22 14:46:51 2015 +0000
+++ b/main.cpp	Mon Oct 26 12:14:22 2015 +0000
@@ -12,66 +12,42 @@
 Ticker      calc_timer;
 PinDetect   cali(SW2);
 PinDetect   start(SW3);
-int         counter = 0;
 bool        started = true;
-bool        calibrating = false;
 bool        go_motor = true;
 bool        go_tick = false;
 bool        go_sample = false;
 
 double      emg_out0, emg_out1, emg_out2;
 
-double generateSample()
-{
-    counter++;
-    if(counter < 10)
-        return 0.0;
-    else if(counter < 50)
-        return 0.1;
-    else if(counter < 100)
-        return 0.2;
-    else if(counter < 150)
-        return 0.3;
-    else if(counter < 200)
-        return 0.4;
-    else if(counter < 250)
-        return 0.5;
-    return 0.0;
-}
-
 void tick()
 {
+    double a,b,x,y;
+    
+    /* If the game has not started, don't do anything. */
     if(!started) return;
-    // EMG Reader
     
-    double a,b;
+    // Get the current rotation
+    getCurrent(a,b);
     
-    // In Between Controller
-    getCurrent(a,b);
-    double x,y;
+    // Remove the offset
     a = getOffset(1) - a;
     b = getOffset(2) - b;
-    //if(output2 == 1) newPos(0,0,1,a,b,x,y);
+ 
+    // 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);
+    // Rotate the motors
     rotate(x,y);
 }
 
-void goMotor()
-{
-    go_motor = true;
-}
+/* Functions which get called by tickers */
+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 startAll()
 {
     started = !started;
@@ -84,39 +60,14 @@
     }
 }
 
+/* Function called by pressing the SW2 Button */
 void calibrate()
 {
     sample_timer.attach(&goSample, 0.002);
     calc_timer.attach(&goTick, 0.5);
-    
-    /*if(ding >= 1 && ding <= 2) {
-        getCurrent(a,b);
-        pc2.printf("CUR A: %d (%f) | CUR B: %d (%f)\r\n",rad2deg(a),a,rad2deg(b),b);
-        double x,y;
-        a = getOffset(1) - a;
-        b = getOffset(2) - b;
-        newPos(0,1,0,a,b,x,y);
-        pc2.printf("CUR A: %d (%f) | CUR B: %d (%f)\r\n",rad2deg(a),a,rad2deg(b),b);
-        pc2.printf("NEW A: %d (%f) | NEW B: %d (%f)\r\n",rad2deg(x),x,rad2deg(y),y);
-        //Point2Angles(-10,18,a,b);
-        rotate(x,y);
-        if(ding == 6) ding = 0;
-    }
-    if(ding == 3) {  
-        getCurrent(a,b);
-        pc2.printf("CUR A: %d (%f) | CUR B: %d (%f)\r\n",rad2deg(a),a,rad2deg(b),b);
-        double x,y;
-        a = getOffset(1) - a;
-        b = getOffset(2) - b;
-        newPos(0,0,1,a,b,x,y);
-        pc2.printf("CUR A: %d (%f) | CUR B: %d (%f)\r\n",rad2deg(a),a,rad2deg(b),b);
-        pc2.printf("NEW A: %d (%f) | NEW B: %d (%f)\r\n",rad2deg(x),x,rad2deg(y),y);
-        //Point2Angles(-10,18,a,b);
-        rotate(x,y);
-        ding = 0;
-    }*/
 }
 
+/* Initialize the buttons */
 void init()
 {
     // Set the shutup and start buttons
@@ -128,36 +79,43 @@
     cali.attach_deasserted(&calibrate);
     cali.setSampleFrequency();
 
-    pc2.printf("Buttons done\r\nStarting Initialization...\r\n\r\n");
+    pc2.printf("Buttons done\r\n\r\n");
 }
 
+/* The main function
+    Initializes every library
+    Starts the timers
+    Has a while-loop which calls functions on a timer
+*/
 int main()
 {
     init();
-    pc2.printf("STARTED!");
-    // Initialize libraries
+    
+    /* Initialize libraries */
     double a,b;
     IBC_init(a,b);
-    PID_init(a,b);
-
-    //movement_timer.attach(&moveTick, 0.01f);
-
-    pc2.printf("A: %f | B: %f\r\n",a,b);
+    pc2.printf("(--- In Between Controller Initialized ---)\r\n");
+    
+    PID_init();
+    pc2.printf("(--- PID Controller Initialized ---)\r\n");
 
     movement_timer.attach(&goMotor, 0.01f);
-    //sample_timer.attach(&goTick, 1f);
 
     rotate(a,b);
 
+    /* The main while-loop */
     while(1) {
+        // The motor timer
         if(go_motor) {
             go_motor = false;
             moveTick();
         }
+        // The sample timer
         if(go_sample) {
             go_sample = false;
             sample(emg_out0, emg_out1, emg_out2);   
         }
+        // The tick timer
         if(go_tick) {
             go_tick = false;
             tick();