Diff: actuators.cpp

Revision:

52:2ac9dee099ce

Parent:

51:1e6334b993a3

Child:

54:c14c3bc48b8a

--- a/actuators.cpp	Fri Oct 09 13:32:29 2015 +0200
+++ b/actuators.cpp	Fri Oct 09 13:42:41 2015 +0200
@@ -14,16 +14,16 @@
     float motor1Pos = 0;
     float motor2Pos = 0;
 
-    float motorSpeed1 = 0;
-    float motorSpeed2 = 0;
+    float motor1Speed = 0;
+    float motor2Speed = 0;
 
-    float motorSetSpeed1 = 0;
-    float motorSetSpeed2 = 0;
+    float motor1SetSpeed = 0;
+    float motor2SetSpeed = 0;
 
     float servoPos = 0;
 
-    float motorPWM1 = 0;
-    float motorPWM2 = 0;
+    float motor1PWM = 0;
+    float motor2PWM = 0;
 
     // Set PID values
     float Kp1 = 1; 
@@ -34,8 +34,8 @@
     float Ki2 = 0; 
     float Kd2 = 0;
 
-    float prevMotor2Pos = 0;
-    float prevMotor1Pos = 0;
+    float motor1PrevPos = 0;
+    float motor2PrevPos = 0;
     float prevTime = 0;
     float now = 0;
     float timechange;
@@ -54,8 +54,8 @@
     DigitalOut motor2Dir(motor2DirPin);  
 
     // create PID instances
-    PID PIDmotor1(Kp1, Ki1, Kd1, motorCall); // motorCall is period motorControll() gets called
-    PID PIDmotor2(Kp2, Ki2, Kd2, motorCall);
+    PID motor1PID(&motor1Speed, &motor1PWM, &motor1SetSpeed, Kp1, Ki1, Kd1);
+    PID motor2PID(&motor2Speed, &motor2PWM, &motor2SetSpeed, Kp2, Ki2, Kd2);
 
     Servo servo(servoPin);
     Timer t;  
@@ -69,21 +69,9 @@
     motor1.period(1/pwm_frequency);
     motor2.period(1/pwm_frequency);
 
-    PIDmotor1.setSetPoint(&abs(motorSetSpeed1));
-    PIDmotor2.setSetPoint(&abs(motorSetSpeed2));
-
-    PIDmotor1.setProcessValue(&abs(motorSpeed1));
-    PIDmotor2.setProcessValue(&abs(motorSpeed2));
-
-    // set limits for PID output to avoid integrator build up.
-    PIDmotor1.setInputLimits(0, 300);
-    PIDmotor2.setInputLimits(0, 300);
-    PIDmotor1.setOutputLimits(0, 1);
-    PIDmotor2.setOutputLimits(0, 1);
-    
     // Turn PID on
-    PIDmotor1.setMode(1);
-    PIDmotor2.setMode(1);
+    motor1PID.SetMode(1);
+    motor2PID.SetMode(1);
     
     // start the timer
     t.start();
@@ -102,11 +90,11 @@
     // get  encoder speeds in deg/sec
         now = t.read();
         timechange = (now - prevTime);
-        motorSpeed1 = (motor1Pos - prevMotor1Pos)/timechange;
-        motorSpeed2 = (motor2Pos - prevMotor2Pos)/timechange;
+        motor1Speed = (motor1Pos - motor1PrevPos)/timechange;
+        motor2Speed = (motor2Pos - motor2PrevPos)/timechange;
         prevTime = now;
-        prevMotor1Pos = motor1Pos;
-        prevMotor2Pos = motor2Pos;
+        motor1PrevPos = motor1Pos;
+        motor2PrevPos = motor2Pos;
         
     // calculate motor setpoint speed in deg/sec from setpoint x/y speed
 
@@ -122,21 +110,15 @@
 }
 
 void writeMotors(){
-    // PIDmotor1.setSetPoint(abs(motorSetSpeed1));
-    // PIDmotor2.setSetPoint(abs(motorSetSpeed2));
-
-    // PIDmotor1.setProcessValue(abs(motorSpeed1));
-    // PIDmotor2.setProcessValue(abs(motorSpeed2));
-
-    motorPWM1 = PIDmotor1.compute();
-    motorPWM2 = PIDmotor2.compute();
+    motor1PID.compute(); // calculate PID outputs, output changes automatically
+    motor2PID.compute();
 // write new values to motor's
-    if (motorSetSpeed1 >= 0 ){ // CCW rotation 
+    if (motor1PWM >= 0 ){ // CCW rotation 
         direction1 = false;
     }else{
         direction1 = true; // CW rotation
     }
-    if (motorSetSpeed2 >= 0 ){ // CCW rotation 
+    if (motor2PWM >= 0 ){ // CCW rotation 
         direction2 = false;
     }else{
         direction2 = true; // CW rotation
@@ -144,8 +126,8 @@
     motor1Dir.write(direction1);
     motor2Dir.write(direction2);
     
-    motor1.write(abs(motorPWM1));
-    motor2.write(abs(motorPWM2));
+    motor1.write(abs(motor1PWM));
+    motor2.write(abs(motor2PWM));
 }
 
 void servoControl(){