Diff: actuators.cpp

Revision:

26:0a9e4147a31a

Parent:

25:874675516927

Child:

28:40a931dfe840

--- a/actuators.cpp	Tue Oct 06 14:24:40 2015 +0000
+++ b/actuators.cpp	Tue Oct 06 16:37:07 2015 +0200
@@ -5,7 +5,7 @@
 #include "encoder.h"
 #include "HIDScope.h"
 // functions for controlling the motors
-bool motorEnable = false;
+bool motorEnable = true;
 
 bool direction1 = false; // CCW is false(positive rotation), CW is true (neg rotation)
 bool direction2 = false;
@@ -35,29 +35,26 @@
 float PIDinterval = 0.2;
 
 
+// Create object instances
+// Initialze motors
+PwmOut motor1(motor1PWMPin);
+PwmOut motor2(motor2PWMPin);
+
+// Initialize encoders
 Encoder encoder1(enc1A, enc1B, true);
 Encoder encoder2(enc2A, enc2B, true);
 
-    PwmOut motor1(motor1PWMPin);
-    PwmOut motor2(motor2PWMPin);
-     
-         DigitalOut motor1Dir(motor1DirPin);
-    DigitalOut motor2Dir(motor2DirPin);  
-        PID PIDmotor1(Kp1, Ki1, Kd1, PIDinterval);
-    PID PIDmotor2(Kp2, Ki2, Kd2, PIDinterval);
+// Set direction pins     
+DigitalOut motor1Dir(motor1DirPin);
+DigitalOut motor2Dir(motor2DirPin);  
+
+// create PID instances
+PID PIDmotor1(Kp1, Ki1, Kd1, PIDinterval);
+PID PIDmotor2(Kp2, Ki2, Kd2, PIDinterval);
     
     
 void motorInit(){
-    // Initialze motors
-
-
-    // Initialize encoders (with speed calculation)
-
-
-        // Set motor direction pins.
-
     
-    // Set initial direction
     motor1Dir.write(direction1);
     motor2Dir.write(direction2);
 
@@ -65,16 +62,6 @@
     motor1.period(1/pwm_frequency);
     motor2.period(1/pwm_frequency);
 
-
-
-    initPID();
-}
-
-void initPID(){
-    // create PID instances for motors
-        // PID pidname(input, output, setpoint, kp, ki, kd, direction)
-
-    
     PIDmotor1.setSetPoint(motorSetSpeed1);
     PIDmotor2.setSetPoint(motorSetSpeed2);
 
@@ -87,8 +74,10 @@
     // set limits for PID output to avoid integrator build up.
     PIDmotor1.setOutputLimits(-1.0, 1.0);
     PIDmotor2.setOutputLimits(-1.0, 1.0);
+
 }
 
+
 void motorControl(){
     if(motorEnable){  // only run motors if switch is enabled