Motor
Fork of Motor by
Diff: motor.cpp
- Revision:
- 2:3faf5dcde08f
- Parent:
- 1:c97f8bcd6c0f
- Child:
- 3:94db629c0a83
diff -r c97f8bcd6c0f -r 3faf5dcde08f motor.cpp --- 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