Diff: main.cpp

Revision:

5:5339a7be9fb7

Parent:

4:e021dacffa24

Child:

6:4f8760baa448

--- a/main.cpp	Tue Oct 20 09:57:07 2015 +0000
+++ b/main.cpp	Tue Oct 20 11:39:08 2015 +0000
@@ -2,29 +2,98 @@
 #include "emg.h"
 #include "inbetweencontroller.h"
 #include "pidControl.h"
+#include "PinDetect.h"
 
 // Define Objects
+Serial      pc2(USBTX, USBRX);
 Ticker      sample_timer;
+PinDetect   cali(SW2);
+PinDetect   start(SW3);
+int         counter = 0;
+bool        started = false;
+bool        calibrating = false;
 
-void tick() {
+double generateSample() {
+    counter++;
+    if(counter < 100)
+        return 0.0;
+    else if(counter < 2250)
+        return 0.1;
+    else if(counter < 3750)
+        return 0.0;
+    else if(counter < 5050)
+        return 0.0;
+    else if(counter < 6750)
+        return 0.0;
+    else if(counter < 8250)
+        return 0.0;
+    return 0.0;
+}
+
+void tick() {  
+    if(!started) return;
     // EMG Reader
-    double output0 = sample(0);
-    double output1 = sample(1);
-    double output2 = sample(2);
+    double output0 = generateSample();
+    pc2.printf("OUT: %f\r\nCOUNTER: %d\r\n",output0,counter);
+    double output1 = 0.0;
+    double output2 = 0.0;
     
     // In Between Controller
     double lastA, lastB, nextA, nextB;
     getCurrent(lastA, lastB);
-    newPos(output0, output2, output2, lastA, lastB, nextA, nextB);
-    
+    newPos(output0, output1, output2, lastA, lastB, nextA, nextB);
+    pc2.printf("CURRENT: %f rad | %f rad\r\nNEW: %f rad | %f rad\r\n",lastA, lastB, nextA, nextB);
     // Rotate the motors
     move(nextA,nextB);
 }
 
+void startAll() {
+    started = !started;
+}
+
+void calibrate() {
+    calibrating = false;   
+}
+
+void init() {
+    // Set the shutup and start buttons
+    start.mode(PullUp);
+    start.attach_deasserted(&startAll);
+    start.setSampleFrequency();
+    
+    cali.mode(PullUp);
+    cali.attach_deasserted(&calibrate);
+    cali.setSampleFrequency();
+    
+    pc2.printf("Buttons done\r\nStarting Initialization...\r\n\r\nMoving Motor #1...\r\n");
+    
+    double a,b;
+    calibrating = true;
+    while(calibrating) {
+        a+=0.00002;
+        move(a,b);
+    }
+    
+    pc2.printf("Calibrated Motor #1 (%f rad)\r\nMoving Motor #2...\r\n",a);
+    
+    calibrating = true;
+    while(calibrating) {
+        b+=0.00002;
+        move(a,b);
+    }
+    
+    pc2.printf("Calibrated Motor #2 (%f rad)\r\nFinished Initializarion.\r\n",b);
+    
+    IBC_init(a,b);
+}
+
 int main()
 {
+    // Initialize libraries
+    PID_init();
+    
     init();
-    initialize();
+    
     // Attach a timer to the function 'Sample' which fires every 0.002 seconds.
     sample_timer.attach(&tick, 0.002);