Diff: motor.cpp

Revision:

3:94db629c0a83

Parent:

2:3faf5dcde08f

Child:

5:99fa6dffea40

--- a/motor.cpp	Sat Sep 14 16:22:37 2013 +0000
+++ b/motor.cpp	Sat Sep 14 17:18:51 2013 +0000
@@ -6,27 +6,29 @@
     PwmOut pwm(PWMpin);
     pwm.period_ms(1);
     setPoint = 0;
-    pMulti = 2.0;
-    iMulti = 0.01;
+    pMulti = 4;
+    iDiv = 8;
     dMulti = 1;
-    error = 0.0;
-    prevError = 0.0;
-    P = 0.0;
-    I = 0.0;
-    minPwm = 0.1;
+    error = 0;
+    prevError = 0;
+    P = 0;
+    I = 0;
+    minPwm = 100;
+    pidMulti = 128;
+    iMax = 256 * pidMulti;
     
     currentSpeed = 0;
 }
 
-void Motor::setPWM(float newPWM) {
+void Motor::setPWM(int newPWM) {
     if (newPWM < 0) {
-        pwm = -1 * newPWM;
-        extIO->setPin(dir1);
-        extIO->clearPin(dir2);
+        pwm.pulsewidth_us(-1 * newPWM);
+        //extIO->setPin(dir1);
+        //extIO->clearPin(dir2);
     } else {
-        pwm = newPWM;
-        extIO->clearPin(dir1);
-        extIO->setPin(dir2);
+        pwm.pulsewidth_us(newPWM);
+        //extIO->clearPin(dir1);
+        //extIO->setPin(dir2);
     }    
 }
 
@@ -46,40 +48,32 @@
     }
 }
 
-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;*/
-    
+void Motor::pid() {    
     prevError = error;
-    error = (float)(setPoint - getDecoderCount()) / 250.0;
+    error = setPoint - getDecoderCount();
     
     //float err = (float)error / 250.0;
     float minPwmValue = minPwm;
     
     if (setPoint == 0) {
-        minPwmValue = 0.0;
+        minPwmValue = 0;
     } else if (setPoint < 0) {
         minPwmValue = -minPwm;
     }
     
-    I += error * iMulti;
+    I += error * pidMulti / iDiv;
     //constrain integral
-    if (I < -1.0) I = -1.0;
-    if (I > 1.0) I = 1.0;
+    if (I < -iMax) I = -iMax;
+    if (I > iMax) I = iMax;
     
     //D = error - prevError;
     
     //float newPWMvalue = minPwmValue + error * pMulti + I + dMulti * D;
-    float newPWMvalue = minPwmValue + error * pMulti + I;
+    int newPWMvalue = minPwmValue + error * pMulti + I / pidMulti;
 
     //constrain pwm
-    if (newPWMvalue < -1.0) newPWMvalue = -1.0;
-    if (newPWMvalue > 1.0) newPWMvalue = 1.0;
+    if (newPWMvalue < -1000) newPWMvalue = -1000;
+    if (newPWMvalue > 1000) newPWMvalue = 1000;
     
     setPWM(newPWMvalue);
 }