Diff: motor.cpp

Revision:

8:63a67086a1b5

Parent:

5:99fa6dffea40

Child:

9:bec5a728405f

--- a/motor.cpp	Tue Sep 17 22:52:10 2013 +0000
+++ b/motor.cpp	Thu Sep 19 13:08:56 2013 +0000
@@ -7,7 +7,7 @@
     pwm.period_ms(1);
     setPoint = 0;
     pMulti = 16;
-    iDiv = 1;
+    iDiv = 2;
     dMulti = 1;
     error = 0;
     prevError = 0;
@@ -15,13 +15,16 @@
     I = 0;
     minPwm = 100;
     pidMulti = 128;
-    iMax = 512 * pidMulti;
+    iMax = 1024 * pidMulti;
     
     currentSpeed = 0;
     
     currentPWM = 0;
-    stallCounter = 0;
-    stallCounterLimit = 60;
+    stallCount = 0;
+    prevStallCount = 0;
+    stallWarningLimit = 60;
+    stallErrorLimit = 300;
+    stallLevel = 0;
 }
 
 void Motor::setPWM(int newPWM) {
@@ -53,44 +56,66 @@
     }
 }
 
-void Motor::pid() {    
+void Motor::pid() {
     prevError = error;
     error = setPoint - getDecoderCount();
-    
-    //float err = (float)error / 250.0;
-    float minPwmValue = minPwm;
-    
-    if (setPoint == 0) {
-        minPwmValue = 0;
-    } else if (setPoint < 0) {
-        minPwmValue = -minPwm;
-    }
-    
-    I += error * pidMulti / iDiv;
-    //constrain integral
-    if (I < -iMax) I = -iMax;
-    if (I > iMax) I = iMax;
-    
-    //D = error - prevError;
+
+    if (stallLevel != 2) {
+        
+        //float err = (float)error / 250.0;
+        float minPwmValue = minPwm;
+        
+        if (setPoint == 0) {
+            minPwmValue = 0;
+        } else if (setPoint < 0) {
+            minPwmValue = -minPwm;
+        }
+        
+        I += error * pidMulti / iDiv;
+        //constrain integral
+        if (I < -iMax) I = -iMax;
+        if (I > iMax) I = iMax;
+        
+        //D = error - prevError;
+        
+        //float newPWMvalue = minPwmValue + error * pMulti + I + dMulti * D;
+        int newPWMvalue = minPwmValue + error * pMulti + I / pidMulti;
     
-    //float newPWMvalue = minPwmValue + error * pMulti + I + dMulti * D;
-    int newPWMvalue = minPwmValue + error * pMulti + I / pidMulti;
-
-    //constrain pwm
-    if (newPWMvalue < -1000) newPWMvalue = -1000;
-    if (newPWMvalue > 1000) newPWMvalue = 1000;
-    
-    /*if ((currentSpeed < 5 && currentPWM == 1000 || currentSpeed > -5 && currentPWM == -1000) && stallCounter < stallCounterLimit) {
-        stallCounter++;
-    } else if (stallCounter > 0) {
-        stallCounter++;
+        //constrain pwm
+        if (newPWMvalue < -1000) newPWMvalue = -1000;
+        if (newPWMvalue > 1000) newPWMvalue = 1000;    
+        
+        prevStallCount = stallCount;
+        if ((currentSpeed < 5 && currentPWM == 1000 || currentSpeed > -5 && currentPWM == -1000) && stallCount < stallErrorLimit) {
+            stallCount++;
+        } else if (stallCount > 0) {
+            stallCount--;
+        }
+        
+        setPWM(newPWMvalue);
+        
+        if ((stallCount == stallWarningLimit - 1) && (prevStallCount == stallWarningLimit)) {
+            stallLevel = 0;
+            stallEndCallback.call();
+            stallChangeCallback.call();
+        } else if ((stallCount == stallWarningLimit) && (prevStallCount == stallWarningLimit - 1)) {
+            stallLevel = 1;
+            stallWarningCallback.call();
+            stallChangeCallback.call();
+        } else if (stallCount == stallErrorLimit) {
+            stallLevel = 2;
+            stallErrorCallback.call();
+            stallChangeCallback.call();
+            resetPID();
+        }
+    } else {
+        stallCount--;
+        if (stallCount == 0) {
+            stallLevel = 0;
+            stallEndCallback.call();
+            stallChangeCallback.call();
+        }
     }
-    
-    if (stallCounter == stallCounterLimit) {
-        
-    }*/
-    
-    setPWM(newPWMvalue);
 }
 
 void Motor::resetPID() {
@@ -102,6 +127,18 @@
     setPWM(0);
 }
 
+int Motor::getStallLevel() {
+    return stallLevel;
+}
+
+void Motor::stallChange(void (*function)(void)) { 
+    stallChangeCallback.attach(function);
+}
+
+void Motor::stallEnd(void (*function)(void)) { 
+    stallEndCallback.attach(function);
+}
+
 void Motor::stallWarning(void (*function)(void)) { 
     stallWarningCallback.attach(function);
 }