Reiko Randoja / Motor2

Motor

Fork of Motor by Reiko Randoja

motor.cpp@2:3faf5dcde08f, 2013-09-14 (annotated)

Committer:
Reiko
Date:
Sat Sep 14 16:22:37 2013 +0000
Revision:
2:3faf5dcde08f
Parent:
1:c97f8bcd6c0f
Child:
3:94db629c0a83
Changed some variables to float

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