Diff: motor.cpp

Revision:

9:bec5a728405f

Parent:

8:63a67086a1b5

Child:

10:b0e6051a6109

--- a/motor.cpp	Thu Sep 19 13:08:56 2013 +0000
+++ b/motor.cpp	Sun Sep 22 07:24:15 2013 +0000
@@ -3,11 +3,12 @@
 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) {
  
+    pwmPeriod = 2500;
     PwmOut pwm(PWMpin);
-    pwm.period_ms(1);
+    pwm.period_us(pwmPeriod);
     setPoint = 0;
-    pMulti = 16;
-    iDiv = 2;
+    pMulti = 32;
+    iDiv = 1;
     dMulti = 1;
     error = 0;
     prevError = 0;
@@ -15,7 +16,7 @@
     I = 0;
     minPwm = 100;
     pidMulti = 128;
-    iMax = 1024 * pidMulti;
+    iMax = 4096 * pidMulti;
     
     currentSpeed = 0;
     
@@ -82,11 +83,11 @@
         int newPWMvalue = minPwmValue + error * pMulti + I / pidMulti;
     
         //constrain pwm
-        if (newPWMvalue < -1000) newPWMvalue = -1000;
-        if (newPWMvalue > 1000) newPWMvalue = 1000;    
+        if (newPWMvalue < -pwmPeriod) newPWMvalue = -pwmPeriod;
+        if (newPWMvalue > pwmPeriod) newPWMvalue = pwmPeriod;    
         
         prevStallCount = stallCount;
-        if ((currentSpeed < 5 && currentPWM == 1000 || currentSpeed > -5 && currentPWM == -1000) && stallCount < stallErrorLimit) {
+        if ((currentSpeed < 5 && currentPWM == pwmPeriod || currentSpeed > -5 && currentPWM == -pwmPeriod) && stallCount < stallErrorLimit) {
             stallCount++;
         } else if (stallCount > 0) {
             stallCount--;