Diff: actuators.cpp

Revision:

25:874675516927

Parent:

22:c562b9a4176d

Child:

26:0a9e4147a31a

--- a/actuators.cpp	Mon Oct 05 21:27:52 2015 +0200
+++ b/actuators.cpp	Tue Oct 06 14:24:40 2015 +0000
@@ -3,6 +3,7 @@
 #include "mbed.h"
 #include "config.h"
 #include "encoder.h"
+#include "HIDScope.h"
 // functions for controlling the motors
 bool motorEnable = false;
 
@@ -33,15 +34,29 @@
 
 float PIDinterval = 0.2;
 
-void motorInit(){
-    // Initialze motors
+
+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);
+    
+    
+void motorInit(){
+    // Initialze motors
 
-    // Set motor direction pins.
-    DigitalOut motor1Dir(motor1DirPin);
-    DigitalOut motor2Dir(motor2DirPin);
+
+    // Initialize encoders (with speed calculation)
+
 
+        // Set motor direction pins.
+
+    
     // Set initial direction
     motor1Dir.write(direction1);
     motor2Dir.write(direction2);
@@ -50,9 +65,7 @@
     motor1.period(1/pwm_frequency);
     motor2.period(1/pwm_frequency);
 
-    // Initialize encoders (with speed calculation)
-    Encoder encoder1(enc1A, enc1B, true);
-    Encoder encoder2(enc2A, enc2B, true);
+
 
     initPID();
 }
@@ -60,8 +73,8 @@
 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);
 
@@ -76,7 +89,6 @@
     PIDmotor2.setOutputLimits(-1.0, 1.0);
 }
 
-
 void motorControl(){
     if(motorEnable){  // only run motors if switch is enabled
 
@@ -122,4 +134,5 @@
         // calculate x y translation of endpoint
         // find new x and y speed.
     
-}
\ No newline at end of file
+}
+