Diff: actuators.cpp

Revision:

4:80e2280058ed

Parent:

3:47c76be6d402

Child:

5:73bfad06b775

diff -r 47c76be6d402 -r 80e2280058ed actuators.cpp
--- a/actuators.cpp	Mon Oct 05 16:01:21 2015 +0200
+++ b/actuators.cpp	Mon Oct 05 16:15:25 2015 +0200
@@ -17,11 +17,15 @@
     motor1.period(1/pwm_frequency);
     motor2.period(1/pwm_frequency);
 
-    // Initialize encoders
-    Encoder encoder1(enc1A, enc1B);
-    Encoder encoder2(enc2A, enc2B);
+    // Initialize encoders (with speed calculation)
+    Encoder encoder1(enc1A, enc1B, true);
+    Encoder encoder2(enc2A, enc2B, true);
 
-    // create PID instances for motors
+    initPID();
+}
+
+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);
@@ -31,9 +35,9 @@
     PIDmotor2.SetMode(AUTOMATIC);
 
     // set limits for PID output to avoid integrator build up.
-    PIDmotor1.SetOutputLimits(-1, 1);
-    PIDmotor2.SetOutputLimits(-1, 1);
-    }
+    PIDmotor1.SetOutputLimits(-1.0, 1.0);
+    PIDmotor2.SetOutputLimits(-1.0, 1.0);
+}
 
 
 void motorControl(){
@@ -44,13 +48,10 @@
         motor2Pos = encoder2.getPosition();
 
         // check if motor's are within rotational boundarys
-    // calculate  encoder speeds
-        motorSpeed1=(motor1Pos-prevMotor1Pos)/(time.read()-prevTime);
-        motorSpeed2=(motor2Pos-prevMotor2Pos)/(time.read()-prevTime);
+    // get  encoder speeds
+        motorSpeed1 = encoder1.getSpeed();
+        motorSpeed2 = encoder2.getSpeed();
 
-        // store current positions and time
-        prevMotor1Pos = motor1Pos;
-        prevMotor2Pos = motor2Pos;
         prevTime = time.read();
     // translate to x/y speed
     // compute new PID parameters using setpoint speeds and x/y speeds
@@ -75,12 +76,4 @@
         // calculate x y translation of endpoint
         // find new x and y speed.
     
-    }
-    
-void pumpControl(){
-    if (pumpButton == HIGH){
-        // write pumpPin High 
-        }else{
-        // write pumpPin Low    
-        }
-    }
\ No newline at end of file
+}