Diff: main.cpp

Revision:

15:59d7fffd070a

Parent:

11:46a859e526ca

--- a/main.cpp	Wed Oct 28 13:46:56 2015 +0000
+++ b/main.cpp	Thu Oct 29 17:11:07 2015 +0000
@@ -16,7 +16,9 @@
 bool        go_motor = true;
 bool        go_tick = false;
 bool        go_sample = false;
+bool        go_cali = false;
 double      motorSpeed = 0.2;
+int         calibrated = 0;
 
 double      emg_out0, emg_out1, emg_out2;
 
@@ -36,7 +38,7 @@
     b = getOffset(2) - b;
     
     // TODO: Remove debugging overrides
-    if (debug_tick < 5) {
+    /*if (debug_tick < 5) {
         emg_out0 = 0;
         emg_out1 = 0;
         emg_out2 = 0;    
@@ -80,25 +82,25 @@
         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) {
+    } else if (debug_tick < 25) {
         emg_out0 = 1;
         emg_out1 = 0;
         emg_out2 = 0;   
-    } else if (debug_tick < 25) {
+    } else if (debug_tick < 40) {
         emg_out0 = 0;
         emg_out1 = 1;
         emg_out2 = 0;   
-    } else if (debug_tick < 35) {
+    } else if (debug_tick < 45) {
         emg_out0 = 0;
         emg_out1 = 0;
         emg_out2 = 0;   
-    } else if (debug_tick < 45) {
+    } else if (debug_tick < 50) {
         emg_out0 = 1;
         emg_out1 = 0;
         emg_out2 = 0;   
@@ -146,9 +148,19 @@
 
 void goSample() { go_sample = true; }
 
+void goCali() { go_cali = true; }
+
 /* Function called by pressing the SW3 Button */
 void startAll()
 {
+    pc2.printf("PUSH\r\n");
+    if(calibrated < 4) {
+        pc2.printf("CALI: %d\r\n",calibrated);
+        emg_cal(calibrated);
+        calibrated++;
+        pc2.printf("DONE\r\n");
+        return;
+    }
     started = !started;
     if(started)
         movement_timer.attach(&goMotor, 0.01f);
@@ -171,7 +183,7 @@
 {
     // Set the shutup and start buttons
     start.mode(PullUp);
-    start.attach_deasserted(&startAll);
+    start.attach_deasserted(&goCali);
     start.setSampleFrequency();
 
     cali.mode(PullUp);
@@ -219,5 +231,9 @@
             go_tick = false;
             tick();
         }
+        if(go_cali) {
+            go_cali = false;
+            startAll();
+        }
     }
 }
\ No newline at end of file