Reiko Randoja / Motor

Motor

motor.cpp@3:94db629c0a83, 2013-09-14 (annotated)

Committer:
Reiko
Date:
Sat Sep 14 17:18:51 2013 +0000
Revision:
3:94db629c0a83
Parent:
2:3faf5dcde08f
Child:
5:99fa6dffea40
Changed floats to ints

Who changed what in which revision?

UserRevisionLine numberNew 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 3:94db629c0a83 9 pMulti = 4;
Reiko 3:94db629c0a83 10 iDiv = 8;
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 3:94db629c0a83 18 iMax = 256 * pidMulti;
Reiko 1:c97f8bcd6c0f 19
Reiko 1:c97f8bcd6c0f 20 currentSpeed = 0;
Reiko 1:c97f8bcd6c0f 21 }
Reiko 1:c97f8bcd6c0f 22
Reiko 3:94db629c0a83 23 void Motor::setPWM(int newPWM) {
Reiko 1:c97f8bcd6c0f 24 if (newPWM < 0) {
Reiko 3:94db629c0a83 25 pwm.pulsewidth_us(-1 * newPWM);
Reiko 3:94db629c0a83 26 //extIO->setPin(dir1);
Reiko 3:94db629c0a83 27 //extIO->clearPin(dir2);
Reiko 1:c97f8bcd6c0f 28 } else {
Reiko 3:94db629c0a83 29 pwm.pulsewidth_us(newPWM);
Reiko 3:94db629c0a83 30 //extIO->clearPin(dir1);
Reiko 3:94db629c0a83 31 //extIO->setPin(dir2);
Reiko 1:c97f8bcd6c0f 32 }
Reiko 1:c97f8bcd6c0f 33 }
Reiko 1:c97f8bcd6c0f 34
Reiko 1:c97f8bcd6c0f 35 int Motor::getSpeed() {
Reiko 1:c97f8bcd6c0f 36 return currentSpeed;
Reiko 1:c97f8bcd6c0f 37 }
Reiko 1:c97f8bcd6c0f 38
Reiko 1:c97f8bcd6c0f 39 int Motor::getDecoderCount() {
Reiko 1:c97f8bcd6c0f 40 currentSpeed = qed.read();
Reiko 1:c97f8bcd6c0f 41 return currentSpeed;
Reiko 1:c97f8bcd6c0f 42 }
Reiko 1:c97f8bcd6c0f 43
Reiko 1:c97f8bcd6c0f 44 void Motor::setSpeed(int newSpeed) {
Reiko 1:c97f8bcd6c0f 45 setPoint = newSpeed;
Reiko 2:3faf5dcde08f 46 if (newSpeed == 0) {
Reiko 2:3faf5dcde08f 47 resetPID();
Reiko 2:3faf5dcde08f 48 }
Reiko 1:c97f8bcd6c0f 49 }
Reiko 1:c97f8bcd6c0f 50
Reiko 3:94db629c0a83 51 void Motor::pid() {
Reiko 1:c97f8bcd6c0f 52 prevError = error;
Reiko 3:94db629c0a83 53 error = setPoint - getDecoderCount();
Reiko 2:3faf5dcde08f 54
Reiko 2:3faf5dcde08f 55 //float err = (float)error / 250.0;
Reiko 2:3faf5dcde08f 56 float minPwmValue = minPwm;
Reiko 1:c97f8bcd6c0f 57
Reiko 2:3faf5dcde08f 58 if (setPoint == 0) {
Reiko 3:94db629c0a83 59 minPwmValue = 0;
Reiko 2:3faf5dcde08f 60 } else if (setPoint < 0) {
Reiko 2:3faf5dcde08f 61 minPwmValue = -minPwm;
Reiko 2:3faf5dcde08f 62 }
Reiko 1:c97f8bcd6c0f 63
Reiko 3:94db629c0a83 64 I += error * pidMulti / iDiv;
Reiko 1:c97f8bcd6c0f 65 //constrain integral
Reiko 3:94db629c0a83 66 if (I < -iMax) I = -iMax;
Reiko 3:94db629c0a83 67 if (I > iMax) I = iMax;
Reiko 1:c97f8bcd6c0f 68
Reiko 2:3faf5dcde08f 69 //D = error - prevError;
Reiko 2:3faf5dcde08f 70
Reiko 2:3faf5dcde08f 71 //float newPWMvalue = minPwmValue + error * pMulti + I + dMulti * D;
Reiko 3:94db629c0a83 72 int newPWMvalue = minPwmValue + error * pMulti + I / pidMulti;
Reiko 2:3faf5dcde08f 73
Reiko 1:c97f8bcd6c0f 74 //constrain pwm
Reiko 3:94db629c0a83 75 if (newPWMvalue < -1000) newPWMvalue = -1000;
Reiko 3:94db629c0a83 76 if (newPWMvalue > 1000) newPWMvalue = 1000;
Reiko 1:c97f8bcd6c0f 77
Reiko 1:c97f8bcd6c0f 78 setPWM(newPWMvalue);
Reiko 2:3faf5dcde08f 79 }
Reiko 2:3faf5dcde08f 80
Reiko 2:3faf5dcde08f 81 void Motor::resetPID() {
Reiko 2:3faf5dcde08f 82 error = 0;
Reiko 2:3faf5dcde08f 83 prevError = 0;
Reiko 2:3faf5dcde08f 84 P = 0;
Reiko 2:3faf5dcde08f 85 I = 0;
Reiko 2:3faf5dcde08f 86 setPoint = 0;
Reiko 2:3faf5dcde08f 87 pwm = 0;
Reiko 1:c97f8bcd6c0f 88 }