Diff: main.cpp

Revision:

105:663b73bb2f81

Parent:

104:750d7e13137d

diff -r 750d7e13137d -r 663b73bb2f81 main.cpp
--- a/main.cpp	Fri Oct 23 12:17:29 2015 +0000
+++ b/main.cpp	Mon Oct 26 11:25:25 2015 +0000
@@ -14,37 +14,28 @@
 #include "debug.h"
 #include "emg.h"
 
-Ticker switchesTick, debugTick, motorTick, EMGTick, safetyTick, servoTick;
-volatile bool switches_go=false, debug_go=false, motor_go=false, emg_go=false, safety_go=false, servo_go=false;
+Ticker switchesTick, debugTick, motorTick, EMGTick, safetyTick;
+volatile bool switches_go=false, debug_go=false, motor_go=false, emg_go=false, safety_go=false;
 
 void switches_activate(){switches_go=true;};
 void debug_activate(){debug_go=true;};
 void motor_activate(){motor_go=true;};
 void emg_activate(){emg_go=true;};
 void safety_activate(){safety_go=true;};
-void servo_activate(){servo_go=true;};
 
-const double    emgCall     = 0.005;        // Set EMG sampling period
-const double    motorCall   = 0.005;        // Set motor control period global so it can be used for speed.
-const double    servoCall   = 0.025;         // Set servo control speed  
-const int       sample      = 0;            // Constant for mode switching for program readability
-const int       normalize   = 1;            // Constant for mode switching for program readability
-bool            mode        = sample;    // Set program mode
-
+double motorCall = 0.01; // set motor frequency global so it can be used for speed.
+const int sample = 0;  // Constant for mode switching for program readability
+const int normalize = 1; // Constant for mode switching for program readability
+bool mode = normalize; // Set program mode
 
 int main(){
-    wait_ms(2000);
-    motorsEnable = true;
-    //motorInit();    
+    motorInit();    
 //    calibrateMotors(); // start motor calibration
-    blueLed.write(1),redLed.write(1),greenLed.write(1);
-    servo.pulsewidth(0.0015); // Set servo to zero position
-    
-    //EMGTick.attach(&emg_activate, emgCall);
-    //switchesTick.attach(&switches_activate, 0.02f);
-    //debugTick.attach(&debug_activate, 0.03f);
+
+    EMGTick.attach(&emg_activate, 0.005f);
+    switchesTick.attach(&switches_activate, 0.02f);
+    debugTick.attach(&debug_activate, 0.03f);
     motorTick.attach(&motor_activate, motorCall);
-    servoTick.attach(&servo_activate, servoCall);
     safetyTick.attach(&safety_activate, 0.001f);
 
     while (true) {
@@ -61,14 +52,6 @@
                 emg_go=false;
                 readEMG();
             }
-            if(motor_go){
-                motor_go=false;
-                motorControl();
-            }
-            if(servo_go){
-                servo_go=false;
-                servoControl();
-            }                        
             if(switches_go){
                 switches_go=false;
                 checkSwitches();
@@ -77,6 +60,10 @@
                 debug_go=false;
                 debugProcess();
             }
+            if(motor_go){
+                motor_go=false;
+                motorControl();
+            }
         }
     }
 }
\ No newline at end of file