Diff: main.cpp

Branch:

onefile

Revision:

24:2d7e11441eee

Parent:

23:3f5d30b4784d

diff -r 3f5d30b4784d -r 2d7e11441eee main.cpp
--- a/main.cpp	Mon Oct 05 23:10:31 2015 +0200
+++ b/main.cpp	Mon Oct 05 21:46:51 2015 +0000
@@ -4,11 +4,7 @@
 #include "PID.h"
 #include "EMG.h"
 
-//#define DEBUG // send debug data to HIDScope
-#ifdef DEBUG
-    #include "debug.h"
-    dubugInit();
-#endif
+
 
 bool motorEnable = false;
 
@@ -38,25 +34,35 @@
 float Kd2 = 1;
 
 float PIDinterval = 0.2;
-
-
+    Encoder encoder1(enc1A, enc1B, true);
+    Encoder encoder2(enc2A, enc2B, true);
+        PID PIDmotor1(Kp1, Ki1, Kd1, PIDinterval);
+    PID PIDmotor2(Kp2, Ki2, Kd2, PIDinterval);
+        PwmOut motor1(motor1PWMPin);
+    PwmOut motor2(motor2PWMPin);
+        AnalogIn pot1(pot1Pin);
+    
+void initPID(){
+    // create PID instances for motors
+        // PID pidname(input, output, setpoint, kp, ki, kd, direction)
 
-int main(){
-    setPins();
-    motorInit(); 
-    while (true) {
-        checkSwitches();
-        // readEMG();
-        motorControl();
-        // servoControl();
-    }
+    PIDmotor1.setSetPoint(motorSetSpeed1);
+    PIDmotor2.setSetPoint(motorSetSpeed2);
+
+    PIDmotor1.setProcessValue(motorSpeed1);
+    PIDmotor2.setProcessValue(motorSpeed2);
+    // set PID mode
+    PIDmotor1.setMode(1);
+    PIDmotor2.setMode(1);
+
+    // set limits for PID output to avoid integrator build up.
+    PIDmotor1.setOutputLimits(-1.0, 1.0);
+    PIDmotor2.setOutputLimits(-1.0, 1.0);
 }
 
-
 void motorInit(){
     // Initialze motors
-    PwmOut motor1(motor1PWMPin);
-    PwmOut motor2(motor2PWMPin);
+
 
     // Set motor direction pins.
     DigitalOut motor1Dir(motor1DirPin);
@@ -71,30 +77,11 @@
     motor2.period(1/pwm_frequency);
 
     // Initialize encoders (with speed calculation)
-    Encoder encoder1(enc1A, enc1B, true);
-    Encoder encoder2(enc2A, enc2B, true);
 
     initPID();
 }
 
-void initPID(){
-    // create PID instances for motors
-        // PID pidname(input, output, setpoint, kp, ki, kd, direction)
-    PID PIDmotor1(Kp1, Ki1, Kd1, PIDinterval);
-    PID PIDmotor2(Kp2, Ki2, Kd2, PIDinterval);
-    PIDmotor1.setSetPoint(motorSetSpeed1);
-    PIDmotor2.setSetPoint(motorSetSpeed2);
 
-    PIDmotor1.setProcessValue(motorSpeed1);
-    PIDmotor2.setProcessValue(motorSpeed2);
-    // set PID mode
-    PIDmotor1.setMode(1);
-    PIDmotor2.setMode(1);
-
-    // set limits for PID output to avoid integrator build up.
-    PIDmotor1.setOutputLimits(-1.0, 1.0);
-    PIDmotor2.setOutputLimits(-1.0, 1.0);
-}
 
 
 void motorControl(){
@@ -137,7 +124,7 @@
 
 void setPins(){
     // set input/output pins
-    AnalogIn pot1(pot1Pin);
+
 }
 
 
@@ -155,4 +142,17 @@
     
     // read killswitches
     
-    }
\ No newline at end of file
+    }
+
+int main(){
+    setPins();
+    motorInit(); 
+    while (true) {
+        checkSwitches();
+        // readEMG();
+        motorControl();
+        // servoControl();
+    }
+}
+
+