Diff: actuators.cpp

Revision:

50:b0cf07ca53cf

Parent:

48:3cf1eaf34926

Child:

51:1e6334b993a3

--- a/actuators.cpp	Thu Oct 08 16:26:39 2015 +0200
+++ b/actuators.cpp	Fri Oct 09 10:49:56 2015 +0200
@@ -34,12 +34,11 @@
     float Ki2 = 0; 
     float Kd2 = 0;
 
-    float PIDinterval = 0.1;
-
     float prevMotor2Pos = 0;
     float prevMotor1Pos = 0;
     float prevTime = 0;
-    float curTime = 0;
+    float now = 0;
+    float timechange;
 
     // Create object instances
     // Initialze motors
@@ -55,8 +54,8 @@
     DigitalOut motor2Dir(motor2DirPin);  
 
     // create PID instances
-    PID PIDmotor1(Kp1, Ki1, Kd1, PIDinterval);
-    PID PIDmotor2(Kp2, Ki2, Kd2, PIDinterval);
+    PID PIDmotor1(Kp1, Ki1, Kd1, motorCall); // motorCall is period motorControll() gets called
+    PID PIDmotor2(Kp2, Ki2, Kd2, motorCall);
 
     Servo servo(servoPin);
     Timer t;  
@@ -101,10 +100,11 @@
 
         // check if motor's are within rotational boundarys
     // get  encoder speeds in deg/sec
-        curTime = t.read();
-        motorSpeed1 = (motor1Pos - prevMotor1Pos)/(curTime-prevTime);
-        motorSpeed2 = (motor2Pos - prevMotor2Pos)/(curTime-prevTime);
-        prevTime = curTime;
+        now = t.read();
+        timechange = (now - prevTime);
+        motorSpeed1 = (motor1Pos - prevMotor1Pos)/timechange;
+        motorSpeed2 = (motor2Pos - prevMotor2Pos)/timechange;
+        prevTime = now;
         prevMotor1Pos = motor1Pos;
         prevMotor2Pos = motor2Pos;
         
@@ -131,12 +131,12 @@
     motorPWM1 = PIDmotor1.compute();
     motorPWM2 = PIDmotor2.compute();
 // write new values to motor's
-    if (motorSetSpeed1 > 0 ){ // CCW rotation 
+    if (motorSetSpeed1 >= 0 ){ // CCW rotation 
         direction1 = false;
     }else{
         direction1 = true; // CW rotation
     }
-    if (motorSetSpeed2 > 0 ){ // CCW rotation 
+    if (motorSetSpeed2 >= 0 ){ // CCW rotation 
         direction2 = false;
     }else{
         direction2 = true; // CW rotation