Reiko Randoja / Motor

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?

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 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 }