Diff: motor.cpp

Revision:

5:99fa6dffea40

Parent:

3:94db629c0a83

Child:

8:63a67086a1b5

--- a/motor.cpp	Sat Sep 14 17:18:51 2013 +0000
+++ b/motor.cpp	Mon Sep 16 17:53:13 2013 +0000
@@ -6,8 +6,8 @@
     PwmOut pwm(PWMpin);
     pwm.period_ms(1);
     setPoint = 0;
-    pMulti = 4;
-    iDiv = 8;
+    pMulti = 16;
+    iDiv = 1;
     dMulti = 1;
     error = 0;
     prevError = 0;
@@ -15,20 +15,25 @@
     I = 0;
     minPwm = 100;
     pidMulti = 128;
-    iMax = 256 * pidMulti;
+    iMax = 512 * pidMulti;
     
     currentSpeed = 0;
+    
+    currentPWM = 0;
+    stallCounter = 0;
+    stallCounterLimit = 60;
 }
 
 void Motor::setPWM(int newPWM) {
+    currentPWM = newPWM;
     if (newPWM < 0) {
         pwm.pulsewidth_us(-1 * newPWM);
-        //extIO->setPin(dir1);
-        //extIO->clearPin(dir2);
+        extIO->setPin(dir1);
+        extIO->clearPin(dir2);
     } else {
         pwm.pulsewidth_us(newPWM);
-        //extIO->clearPin(dir1);
-        //extIO->setPin(dir2);
+        extIO->clearPin(dir1);
+        extIO->setPin(dir2);
     }    
 }
 
@@ -75,6 +80,16 @@
     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++;
+    }
+    
+    if (stallCounter == stallCounterLimit) {
+        
+    }*/
+    
     setPWM(newPWMvalue);
 }
 
@@ -84,5 +99,13 @@
     P = 0;
     I = 0;
     setPoint = 0;
-    pwm = 0;
+    setPWM(0);
+}
+
+void Motor::stallWarning(void (*function)(void)) { 
+    stallWarningCallback.attach(function);
+}
+
+void Motor::stallError(void (*function)(void)) { 
+    stallErrorCallback.attach(function);
 }
\ No newline at end of file