Diff: actuators.cpp

Revision:

14:0c0d1bfd94ea

Parent:

13:4837b36b9a68

Child:

19:e89eae07dece

--- a/actuators.cpp	Mon Oct 05 17:24:14 2015 +0000
+++ b/actuators.cpp	Mon Oct 05 19:39:00 2015 +0200
@@ -9,27 +9,27 @@
 bool direction1 = false; // CCW is false(positive rotation), CW is true (neg rotation)
 bool direction2 = false;
 
-double motor1Pos = 0;
-double motor2Pos = 0;
+float motor1Pos = 0;
+float motor2Pos = 0;
 
-double motorSpeed1 = 0;
-double motorSpeed2 = 0;
+float motorSpeed1 = 0;
+float motorSpeed2 = 0;
 
-double motorSetSpeed1 = 0;
-double motorSetSpeed2 = 0;
+float motorSetSpeed1 = 0;
+float motorSetSpeed2 = 0;
 
 
-double motorPWM1 = 0;
-double motorPWM2 = 0;
+float motorPWM1 = 0;
+float motorPWM2 = 0;
 
 // Set PID values
-double Kp1 = 1; 
-double Ki1 = 1; 
-double Kd1 = 1;
+float Kp1 = 1; 
+float Ki1 = 1; 
+float Kd1 = 1;
 
-double Kp2 = 1; 
-double Ki2 = 1; 
-double Kd2 = 1;
+float Kp2 = 1; 
+float Ki2 = 1; 
+float Kd2 = 1;
 
 void motorInit(){
     // Initialze motors
@@ -58,9 +58,13 @@
 void initPID(){
     // 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);
+    PID PIDmotor1(Kp1, Ki1, Kd1);
+    PID PIDmotor2(Kp2, Ki2, Kd2);
+    PIDmotor1.setSetPoint(motorSetSpeed1);
+    PIDmotor2.setSetPoint(motorSetSpeed2);
 
+    PIDmotor1.setProccessValue(motorSpeed1);
+    PIDmotor2.setProccessValue(motorSpeed2);
     // set PID mode
     PIDmotor1.SetMode(AUTOMATIC);
     PIDmotor2.SetMode(AUTOMATIC);
@@ -85,8 +89,8 @@
 
     // translate to x/y speed
     // compute new PID parameters using setpoint speeds and x/y speeds
-        PIDmotor1.compute();
-        PIDmotor2.compute();
+        motorPWM1 = PIDmotor1.compute();
+        motorPWM2 = PIDmotor2.compute();
     // translate to motor rotation speed
     // write new values to motor's
         if (motorPWM1 > 0 ){ // CCW rotation (unitcircle convetion)