Diff: motor.cpp

Revision:

1:c97f8bcd6c0f

Parent:

0:5cafacc2a607

Child:

2:3faf5dcde08f

--- a/motor.cpp	Mon Jul 29 20:25:33 2013 +0000
+++ b/motor.cpp	Sat Aug 31 15:58:46 2013 +0000
@@ -0,0 +1,69 @@
+#include "motor.h"
+
+Motor::Motor(PinName PWMpin, PCA9555 *ioExt, unsigned int dir1Pin, unsigned int dir2Pin, PinName encA, PinName encB)
+    : pwm(PWMpin), extIO(ioExt), dir1(dir1Pin), dir2(dir2Pin), qed(encA, encB) {
+ 
+    PwmOut pwm(PWMpin);
+    pwm.period_ms(5);
+    setPoint = 0;
+    pMulti = 0.2;
+    iMulti = 0.01;
+    error = 0;
+    prevError = 0;
+    P = 0.0;
+    I = 0.0;
+    
+    currentSpeed = 0;
+}
+
+void Motor::setPWM(float newPWM) {
+    if (newPWM < 0) {
+        pwm = -1 * newPWM;
+        extIO->setPin(dir1);
+        extIO->clearPin(dir2);
+    } else {
+        pwm = newPWM;
+        extIO->clearPin(dir1);
+        extIO->setPin(dir2);
+    }    
+}
+
+int Motor::getSpeed() {
+    return currentSpeed;
+}
+
+int Motor::getDecoderCount() {
+    currentSpeed = qed.read();
+    return currentSpeed;
+}
+
+void Motor::setSpeed(int newSpeed) {
+    setPoint = newSpeed;
+}
+
+void Motor::pid() {
+    /*int setPoint = 0;
+    float pMulti = 0.1;
+    float iMulti = 0.001;
+    int error = 0;
+    int prevError = 0;
+    float P = 0.0;
+    float I = 0.0;*/
+    
+    prevError = error;
+    error = setPoint - getDecoderCount();
+    
+    float err = (float)error / 250.0;
+    
+    I += err * iMulti;
+    //constrain integral
+    if (I < -1.0) I = -1.0;
+    if (I > 1.0) I = 1.0;
+    
+    float newPWMvalue = err * pMulti + I;
+    //constrain pwm
+    if (newPWMvalue < -1.0) newPWMvalue = -1.0;
+    if (newPWMvalue > 1.0) newPWMvalue = 1.0;
+    
+    setPWM(newPWMvalue);
+}
\ No newline at end of file