Diff: actuators.cpp

Revision:

1:80f098c05d4b

Parent:

0:525558a26464

Child:

2:95ba9f6f0128

--- a/actuators.cpp	Mon Sep 28 13:54:34 2015 +0000
+++ b/actuators.cpp	Thu Oct 01 15:09:24 2015 +0200
@@ -1,17 +1,34 @@
 // functions for controlling the motors
 
 void motorInit(){
-    
+    PwmOut motor1(motorpin1);
+    PwmOut motor2(motorpin2);
+
+    // create PID instances for motors
+    // PID pidname(input, output, setpoint, kp, ki, kd, direction)
+    PID PIDmotor1(&motorSpeed1, &motorPWM1, &motorSetSpeed1, Kp1, Ki1, Kd1, DIRECT);
+    PID PIDmotor2(&motorSpeed2, &motorPWM2, &motorSetSpeed2, Kp2, Ki2, Kd2, DIRECT);
+
+    // set PID mode
+    PIDmotor1.SetMode(AUTOMATIC);
+    PIDmotor2.SetMode(AUTOMATIC);
+
+    PIDmotor1.SetOutputLimits(-1f, 1f);
+    PIDmotor2.SetOutputLimits(-1f, 1f);
     }
 
 
 void motorControl(){
-    if(motorEnable == HIGH){  // only run motors if switch is enabled
+    if(motorEnable){  // only run motors if switch is enabled
+
     // get encoder positions
         // check if motor's are within rotational boundarys
     // calculate  encoder speeds
     // translate to x/y speed
+        
     // compute new PID parameters using setpoint speeds and x/y speeds
+        PIDmotor1.compute();
+        PIDmotor2.compute();
     // write new values to motor's
         
     }else{