Reiko Randoja / Motor2

Motor

Fork of Motor by Reiko Randoja

motor.cpp@1:c97f8bcd6c0f, 2013-08-31 (annotated)

Committer:
Reiko
Date:
Sat Aug 31 15:58:46 2013 +0000
Revision:
1:c97f8bcd6c0f
Parent:
0:5cafacc2a607
Child:
2:3faf5dcde08f
Added proper speed changing methods, added initial PI controller

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 1:c97f8bcd6c0f 7 pwm.period_ms(5);
Reiko 1:c97f8bcd6c0f 8 setPoint = 0;
Reiko 1:c97f8bcd6c0f 9 pMulti = 0.2;
Reiko 1:c97f8bcd6c0f 10 iMulti = 0.01;
Reiko 1:c97f8bcd6c0f 11 error = 0;
Reiko 1:c97f8bcd6c0f 12 prevError = 0;
Reiko 1:c97f8bcd6c0f 13 P = 0.0;
Reiko 1:c97f8bcd6c0f 14 I = 0.0;
Reiko 1:c97f8bcd6c0f 15
Reiko 1:c97f8bcd6c0f 16 currentSpeed = 0;
Reiko 1:c97f8bcd6c0f 17 }
Reiko 1:c97f8bcd6c0f 18
Reiko 1:c97f8bcd6c0f 19 void Motor::setPWM(float newPWM) {
Reiko 1:c97f8bcd6c0f 20 if (newPWM < 0) {
Reiko 1:c97f8bcd6c0f 21 pwm = -1 * newPWM;
Reiko 1:c97f8bcd6c0f 22 extIO->setPin(dir1);
Reiko 1:c97f8bcd6c0f 23 extIO->clearPin(dir2);
Reiko 1:c97f8bcd6c0f 24 } else {
Reiko 1:c97f8bcd6c0f 25 pwm = newPWM;
Reiko 1:c97f8bcd6c0f 26 extIO->clearPin(dir1);
Reiko 1:c97f8bcd6c0f 27 extIO->setPin(dir2);
Reiko 1:c97f8bcd6c0f 28 }
Reiko 1:c97f8bcd6c0f 29 }
Reiko 1:c97f8bcd6c0f 30
Reiko 1:c97f8bcd6c0f 31 int Motor::getSpeed() {
Reiko 1:c97f8bcd6c0f 32 return currentSpeed;
Reiko 1:c97f8bcd6c0f 33 }
Reiko 1:c97f8bcd6c0f 34
Reiko 1:c97f8bcd6c0f 35 int Motor::getDecoderCount() {
Reiko 1:c97f8bcd6c0f 36 currentSpeed = qed.read();
Reiko 1:c97f8bcd6c0f 37 return currentSpeed;
Reiko 1:c97f8bcd6c0f 38 }
Reiko 1:c97f8bcd6c0f 39
Reiko 1:c97f8bcd6c0f 40 void Motor::setSpeed(int newSpeed) {
Reiko 1:c97f8bcd6c0f 41 setPoint = newSpeed;
Reiko 1:c97f8bcd6c0f 42 }
Reiko 1:c97f8bcd6c0f 43
Reiko 1:c97f8bcd6c0f 44 void Motor::pid() {
Reiko 1:c97f8bcd6c0f 45 /*int setPoint = 0;
Reiko 1:c97f8bcd6c0f 46 float pMulti = 0.1;
Reiko 1:c97f8bcd6c0f 47 float iMulti = 0.001;
Reiko 1:c97f8bcd6c0f 48 int error = 0;
Reiko 1:c97f8bcd6c0f 49 int prevError = 0;
Reiko 1:c97f8bcd6c0f 50 float P = 0.0;
Reiko 1:c97f8bcd6c0f 51 float I = 0.0;*/
Reiko 1:c97f8bcd6c0f 52
Reiko 1:c97f8bcd6c0f 53 prevError = error;
Reiko 1:c97f8bcd6c0f 54 error = setPoint - getDecoderCount();
Reiko 1:c97f8bcd6c0f 55
Reiko 1:c97f8bcd6c0f 56 float err = (float)error / 250.0;
Reiko 1:c97f8bcd6c0f 57
Reiko 1:c97f8bcd6c0f 58 I += err * iMulti;
Reiko 1:c97f8bcd6c0f 59 //constrain integral
Reiko 1:c97f8bcd6c0f 60 if (I < -1.0) I = -1.0;
Reiko 1:c97f8bcd6c0f 61 if (I > 1.0) I = 1.0;
Reiko 1:c97f8bcd6c0f 62
Reiko 1:c97f8bcd6c0f 63 float newPWMvalue = err * pMulti + I;
Reiko 1:c97f8bcd6c0f 64 //constrain pwm
Reiko 1:c97f8bcd6c0f 65 if (newPWMvalue < -1.0) newPWMvalue = -1.0;
Reiko 1:c97f8bcd6c0f 66 if (newPWMvalue > 1.0) newPWMvalue = 1.0;
Reiko 1:c97f8bcd6c0f 67
Reiko 1:c97f8bcd6c0f 68 setPWM(newPWMvalue);
Reiko 1:c97f8bcd6c0f 69 }