Motor
motor.cpp@6:37199f371bf8, 2013-09-16 (annotated)
- Committer:
- Reiko
- Date:
- Mon Sep 16 17:54:14 2013 +0000
- Revision:
- 6:37199f371bf8
- Parent:
- 5:99fa6dffea40
Merge
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Reiko | 1:c97f8bcd6c0f | 1 | #include "motor.h" |
Reiko | 1:c97f8bcd6c0f | 2 | |
Reiko | 1:c97f8bcd6c0f | 3 | Motor::Motor(PinName PWMpin, PCA9555 *ioExt, unsigned int dir1Pin, unsigned int dir2Pin, PinName encA, PinName encB) |
Reiko | 1:c97f8bcd6c0f | 4 | : pwm(PWMpin), extIO(ioExt), dir1(dir1Pin), dir2(dir2Pin), qed(encA, encB) { |
Reiko | 1:c97f8bcd6c0f | 5 | |
Reiko | 1:c97f8bcd6c0f | 6 | PwmOut pwm(PWMpin); |
Reiko | 2:3faf5dcde08f | 7 | pwm.period_ms(1); |
Reiko | 1:c97f8bcd6c0f | 8 | setPoint = 0; |
Reiko | 5:99fa6dffea40 | 9 | pMulti = 16; |
Reiko | 5:99fa6dffea40 | 10 | iDiv = 1; |
Reiko | 2:3faf5dcde08f | 11 | dMulti = 1; |
Reiko | 3:94db629c0a83 | 12 | error = 0; |
Reiko | 3:94db629c0a83 | 13 | prevError = 0; |
Reiko | 3:94db629c0a83 | 14 | P = 0; |
Reiko | 3:94db629c0a83 | 15 | I = 0; |
Reiko | 3:94db629c0a83 | 16 | minPwm = 100; |
Reiko | 3:94db629c0a83 | 17 | pidMulti = 128; |
Reiko | 5:99fa6dffea40 | 18 | iMax = 512 * pidMulti; |
Reiko | 1:c97f8bcd6c0f | 19 | |
Reiko | 1:c97f8bcd6c0f | 20 | currentSpeed = 0; |
Reiko | 5:99fa6dffea40 | 21 | |
Reiko | 5:99fa6dffea40 | 22 | currentPWM = 0; |
Reiko | 5:99fa6dffea40 | 23 | stallCounter = 0; |
Reiko | 5:99fa6dffea40 | 24 | stallCounterLimit = 60; |
Reiko | 1:c97f8bcd6c0f | 25 | } |
Reiko | 1:c97f8bcd6c0f | 26 | |
Reiko | 3:94db629c0a83 | 27 | void Motor::setPWM(int newPWM) { |
Reiko | 5:99fa6dffea40 | 28 | currentPWM = newPWM; |
Reiko | 1:c97f8bcd6c0f | 29 | if (newPWM < 0) { |
Reiko | 3:94db629c0a83 | 30 | pwm.pulsewidth_us(-1 * newPWM); |
Reiko | 5:99fa6dffea40 | 31 | extIO->setPin(dir1); |
Reiko | 5:99fa6dffea40 | 32 | extIO->clearPin(dir2); |
Reiko | 1:c97f8bcd6c0f | 33 | } else { |
Reiko | 3:94db629c0a83 | 34 | pwm.pulsewidth_us(newPWM); |
Reiko | 5:99fa6dffea40 | 35 | extIO->clearPin(dir1); |
Reiko | 5:99fa6dffea40 | 36 | extIO->setPin(dir2); |
Reiko | 1:c97f8bcd6c0f | 37 | } |
Reiko | 1:c97f8bcd6c0f | 38 | } |
Reiko | 1:c97f8bcd6c0f | 39 | |
Reiko | 1:c97f8bcd6c0f | 40 | int Motor::getSpeed() { |
Reiko | 1:c97f8bcd6c0f | 41 | return currentSpeed; |
Reiko | 1:c97f8bcd6c0f | 42 | } |
Reiko | 1:c97f8bcd6c0f | 43 | |
Reiko | 1:c97f8bcd6c0f | 44 | int Motor::getDecoderCount() { |
Reiko | 1:c97f8bcd6c0f | 45 | currentSpeed = qed.read(); |
Reiko | 1:c97f8bcd6c0f | 46 | return currentSpeed; |
Reiko | 1:c97f8bcd6c0f | 47 | } |
Reiko | 1:c97f8bcd6c0f | 48 | |
Reiko | 1:c97f8bcd6c0f | 49 | void Motor::setSpeed(int newSpeed) { |
Reiko | 1:c97f8bcd6c0f | 50 | setPoint = newSpeed; |
Reiko | 2:3faf5dcde08f | 51 | if (newSpeed == 0) { |
Reiko | 2:3faf5dcde08f | 52 | resetPID(); |
Reiko | 2:3faf5dcde08f | 53 | } |
Reiko | 1:c97f8bcd6c0f | 54 | } |
Reiko | 1:c97f8bcd6c0f | 55 | |
Reiko | 3:94db629c0a83 | 56 | void Motor::pid() { |
Reiko | 1:c97f8bcd6c0f | 57 | prevError = error; |
Reiko | 3:94db629c0a83 | 58 | error = setPoint - getDecoderCount(); |
Reiko | 2:3faf5dcde08f | 59 | |
Reiko | 2:3faf5dcde08f | 60 | //float err = (float)error / 250.0; |
Reiko | 2:3faf5dcde08f | 61 | float minPwmValue = minPwm; |
Reiko | 1:c97f8bcd6c0f | 62 | |
Reiko | 2:3faf5dcde08f | 63 | if (setPoint == 0) { |
Reiko | 3:94db629c0a83 | 64 | minPwmValue = 0; |
Reiko | 2:3faf5dcde08f | 65 | } else if (setPoint < 0) { |
Reiko | 2:3faf5dcde08f | 66 | minPwmValue = -minPwm; |
Reiko | 2:3faf5dcde08f | 67 | } |
Reiko | 1:c97f8bcd6c0f | 68 | |
Reiko | 3:94db629c0a83 | 69 | I += error * pidMulti / iDiv; |
Reiko | 1:c97f8bcd6c0f | 70 | //constrain integral |
Reiko | 3:94db629c0a83 | 71 | if (I < -iMax) I = -iMax; |
Reiko | 3:94db629c0a83 | 72 | if (I > iMax) I = iMax; |
Reiko | 1:c97f8bcd6c0f | 73 | |
Reiko | 2:3faf5dcde08f | 74 | //D = error - prevError; |
Reiko | 2:3faf5dcde08f | 75 | |
Reiko | 2:3faf5dcde08f | 76 | //float newPWMvalue = minPwmValue + error * pMulti + I + dMulti * D; |
Reiko | 3:94db629c0a83 | 77 | int newPWMvalue = minPwmValue + error * pMulti + I / pidMulti; |
Reiko | 2:3faf5dcde08f | 78 | |
Reiko | 1:c97f8bcd6c0f | 79 | //constrain pwm |
Reiko | 3:94db629c0a83 | 80 | if (newPWMvalue < -1000) newPWMvalue = -1000; |
Reiko | 3:94db629c0a83 | 81 | if (newPWMvalue > 1000) newPWMvalue = 1000; |
Reiko | 1:c97f8bcd6c0f | 82 | |
Reiko | 5:99fa6dffea40 | 83 | /*if ((currentSpeed < 5 && currentPWM == 1000 || currentSpeed > -5 && currentPWM == -1000) && stallCounter < stallCounterLimit) { |
Reiko | 5:99fa6dffea40 | 84 | stallCounter++; |
Reiko | 5:99fa6dffea40 | 85 | } else if (stallCounter > 0) { |
Reiko | 5:99fa6dffea40 | 86 | stallCounter++; |
Reiko | 5:99fa6dffea40 | 87 | } |
Reiko | 5:99fa6dffea40 | 88 | |
Reiko | 5:99fa6dffea40 | 89 | if (stallCounter == stallCounterLimit) { |
Reiko | 5:99fa6dffea40 | 90 | |
Reiko | 5:99fa6dffea40 | 91 | }*/ |
Reiko | 5:99fa6dffea40 | 92 | |
Reiko | 1:c97f8bcd6c0f | 93 | setPWM(newPWMvalue); |
Reiko | 2:3faf5dcde08f | 94 | } |
Reiko | 2:3faf5dcde08f | 95 | |
Reiko | 2:3faf5dcde08f | 96 | void Motor::resetPID() { |
Reiko | 2:3faf5dcde08f | 97 | error = 0; |
Reiko | 2:3faf5dcde08f | 98 | prevError = 0; |
Reiko | 2:3faf5dcde08f | 99 | P = 0; |
Reiko | 2:3faf5dcde08f | 100 | I = 0; |
Reiko | 2:3faf5dcde08f | 101 | setPoint = 0; |
Reiko | 5:99fa6dffea40 | 102 | setPWM(0); |
Reiko | 5:99fa6dffea40 | 103 | } |
Reiko | 5:99fa6dffea40 | 104 | |
Reiko | 5:99fa6dffea40 | 105 | void Motor::stallWarning(void (*function)(void)) { |
Reiko | 5:99fa6dffea40 | 106 | stallWarningCallback.attach(function); |
Reiko | 5:99fa6dffea40 | 107 | } |
Reiko | 5:99fa6dffea40 | 108 | |
Reiko | 5:99fa6dffea40 | 109 | void Motor::stallError(void (*function)(void)) { |
Reiko | 5:99fa6dffea40 | 110 | stallErrorCallback.attach(function); |
Reiko | 1:c97f8bcd6c0f | 111 | } |