Diff: motor.cpp

Revision:

2:3faf5dcde08f

Parent:

1:c97f8bcd6c0f

Child:

3:94db629c0a83

--- a/motor.cpp	Sat Aug 31 15:58:46 2013 +0000
+++ b/motor.cpp	Sat Sep 14 16:22:37 2013 +0000
@@ -4,14 +4,16 @@
     : pwm(PWMpin), extIO(ioExt), dir1(dir1Pin), dir2(dir2Pin), qed(encA, encB) {
  
     PwmOut pwm(PWMpin);
-    pwm.period_ms(5);
+    pwm.period_ms(1);
     setPoint = 0;
-    pMulti = 0.2;
+    pMulti = 2.0;
     iMulti = 0.01;
-    error = 0;
-    prevError = 0;
+    dMulti = 1;
+    error = 0.0;
+    prevError = 0.0;
     P = 0.0;
     I = 0.0;
+    minPwm = 0.1;
     
     currentSpeed = 0;
 }
@@ -39,6 +41,9 @@
 
 void Motor::setSpeed(int newSpeed) {
     setPoint = newSpeed;
+    if (newSpeed == 0) {
+        resetPID();
+    }
 }
 
 void Motor::pid() {
@@ -51,19 +56,39 @@
     float I = 0.0;*/
     
     prevError = error;
-    error = setPoint - getDecoderCount();
+    error = (float)(setPoint - getDecoderCount()) / 250.0;
+    
+    //float err = (float)error / 250.0;
+    float minPwmValue = minPwm;
     
-    float err = (float)error / 250.0;
+    if (setPoint == 0) {
+        minPwmValue = 0.0;
+    } else if (setPoint < 0) {
+        minPwmValue = -minPwm;
+    }
     
-    I += err * iMulti;
+    I += error * iMulti;
     //constrain integral
     if (I < -1.0) I = -1.0;
     if (I > 1.0) I = 1.0;
     
-    float newPWMvalue = err * pMulti + I;
+    //D = error - prevError;
+    
+    //float newPWMvalue = minPwmValue + error * pMulti + I + dMulti * D;
+    float newPWMvalue = minPwmValue + error * pMulti + I;
+
     //constrain pwm
     if (newPWMvalue < -1.0) newPWMvalue = -1.0;
     if (newPWMvalue > 1.0) newPWMvalue = 1.0;
     
     setPWM(newPWMvalue);
+}
+
+void Motor::resetPID() {
+    error = 0;
+    prevError = 0;
+    P = 0;
+    I = 0;
+    setPoint = 0;
+    pwm = 0;
 }
\ No newline at end of file