Reiko Randoja / Mbed 2 deprecated ut_bbr_2018

Firmware for UT Robotex 2018 basketball robot

Dependencies:   mbed USBDevice

Motor/motor.cpp@4:81cb68f1bcbd, 2019-11-11 (annotated)

Committer:
Reiko
Date:
Mon Nov 11 19:19:43 2019 +0000
Revision:
4:81cb68f1bcbd
Parent:
0:ef6268629f0b 
Change default thrower (motor 3) pulsewidth_us from 800 to 100

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Reiko 0:ef6268629f0b 1 #include "motor.h"
Reiko 0:ef6268629f0b 2
Reiko 0:ef6268629f0b 3 Motor::Motor(PinName PWMpin, PinName dir1Pin, PinName dir2Pin, PinName encA, PinName encB)
Reiko 0:ef6268629f0b 4 : pwm(PWMpin), dir1(dir1Pin), dir2(dir2Pin), qed(encA, encB) {
Reiko 0:ef6268629f0b 5
Reiko 0:ef6268629f0b 6 pwmPeriod = 4000;
Reiko 0:ef6268629f0b 7 PwmOut pwm(PWMpin);
Reiko 0:ef6268629f0b 8 pwm.period_us(pwmPeriod);
Reiko 0:ef6268629f0b 9 setPoint = 0;
Reiko 0:ef6268629f0b 10 pMulti = 64;
Reiko 0:ef6268629f0b 11 iMulti = 2;
Reiko 0:ef6268629f0b 12 dMulti = 1;
Reiko 0:ef6268629f0b 13 error = 0;
Reiko 0:ef6268629f0b 14 prevError = 0;
Reiko 0:ef6268629f0b 15 P = 0;
Reiko 0:ef6268629f0b 16 I = 0;
Reiko 0:ef6268629f0b 17 minPwm = 100;
Reiko 0:ef6268629f0b 18 pidMulti = 256;
Reiko 0:ef6268629f0b 19 iMax = 4096 * pidMulti;
Reiko 0:ef6268629f0b 20
Reiko 0:ef6268629f0b 21 currentSpeed = 0;
Reiko 0:ef6268629f0b 22
Reiko 0:ef6268629f0b 23 currentPWM = 0;
Reiko 0:ef6268629f0b 24 stallCount = 0;
Reiko 0:ef6268629f0b 25 prevStallCount = 0;
Reiko 0:ef6268629f0b 26 stallWarningLimit = 60;
Reiko 0:ef6268629f0b 27 stallErrorLimit = 300;
Reiko 0:ef6268629f0b 28 stallLevel = 0;
Reiko 0:ef6268629f0b 29 }
Reiko 0:ef6268629f0b 30
Reiko 0:ef6268629f0b 31 void Motor::setPWM(int newPWM) {
Reiko 0:ef6268629f0b 32 currentPWM = newPWM;
Reiko 0:ef6268629f0b 33 if (newPWM < 0) {
Reiko 0:ef6268629f0b 34 pwm.pulsewidth_us(-1 * newPWM);
Reiko 0:ef6268629f0b 35 dir1 = 0;
Reiko 0:ef6268629f0b 36 dir2 = 1;
Reiko 0:ef6268629f0b 37 } else {
Reiko 0:ef6268629f0b 38 pwm.pulsewidth_us(newPWM);
Reiko 0:ef6268629f0b 39 dir1 = 1;
Reiko 0:ef6268629f0b 40 dir2 = 0;
Reiko 0:ef6268629f0b 41 }
Reiko 0:ef6268629f0b 42 }
Reiko 0:ef6268629f0b 43
Reiko 0:ef6268629f0b 44 int Motor::getSpeed() {
Reiko 0:ef6268629f0b 45 return currentSpeed;
Reiko 0:ef6268629f0b 46 }
Reiko 0:ef6268629f0b 47
Reiko 0:ef6268629f0b 48 int Motor::getDecoderCount() {
Reiko 0:ef6268629f0b 49 currentSpeed = qed.read();
Reiko 0:ef6268629f0b 50 return currentSpeed;
Reiko 0:ef6268629f0b 51 }
Reiko 0:ef6268629f0b 52
Reiko 0:ef6268629f0b 53 void Motor::setSpeed(int newSpeed) {
Reiko 0:ef6268629f0b 54 setPoint = newSpeed;
Reiko 0:ef6268629f0b 55
Reiko 0:ef6268629f0b 56 if (newSpeed == 0) {
Reiko 0:ef6268629f0b 57 resetPID();
Reiko 0:ef6268629f0b 58 }
Reiko 0:ef6268629f0b 59 }
Reiko 0:ef6268629f0b 60
Reiko 0:ef6268629f0b 61 void Motor::pid() {
Reiko 0:ef6268629f0b 62 prevError = error;
Reiko 0:ef6268629f0b 63 error = setPoint - getDecoderCount();
Reiko 0:ef6268629f0b 64
Reiko 0:ef6268629f0b 65 if (stallLevel != 2) {
Reiko 0:ef6268629f0b 66 float minPwmValue = minPwm;
Reiko 0:ef6268629f0b 67
Reiko 0:ef6268629f0b 68 if (setPoint == 0) {
Reiko 0:ef6268629f0b 69 minPwmValue = 0;
Reiko 0:ef6268629f0b 70 } else if (setPoint < 0) {
Reiko 0:ef6268629f0b 71 minPwmValue = -minPwm;
Reiko 0:ef6268629f0b 72 }
Reiko 0:ef6268629f0b 73
Reiko 0:ef6268629f0b 74 I += error * pidMulti * iMulti;
Reiko 0:ef6268629f0b 75 //constrain integral
Reiko 0:ef6268629f0b 76 if (I < -iMax) I = -iMax;
Reiko 0:ef6268629f0b 77 if (I > iMax) I = iMax;
Reiko 0:ef6268629f0b 78
Reiko 0:ef6268629f0b 79 //D = error - prevError;
Reiko 0:ef6268629f0b 80
Reiko 0:ef6268629f0b 81 int newPWMvalue = minPwmValue + error * pMulti + I / pidMulti;
Reiko 0:ef6268629f0b 82
Reiko 0:ef6268629f0b 83 //constrain pwm
Reiko 0:ef6268629f0b 84 if (newPWMvalue < -pwmPeriod) newPWMvalue = -pwmPeriod;
Reiko 0:ef6268629f0b 85 if (newPWMvalue > pwmPeriod) newPWMvalue = pwmPeriod;
Reiko 0:ef6268629f0b 86
Reiko 0:ef6268629f0b 87 prevStallCount = stallCount;
Reiko 0:ef6268629f0b 88 if ((currentSpeed < 5 && currentPWM == pwmPeriod || currentSpeed > -5 && currentPWM == -pwmPeriod) && stallCount < stallErrorLimit) {
Reiko 0:ef6268629f0b 89 stallCount++;
Reiko 0:ef6268629f0b 90 } else if (stallCount > 0) {
Reiko 0:ef6268629f0b 91 stallCount--;
Reiko 0:ef6268629f0b 92 }
Reiko 0:ef6268629f0b 93
Reiko 0:ef6268629f0b 94 setPWM(newPWMvalue);
Reiko 0:ef6268629f0b 95
Reiko 0:ef6268629f0b 96 if ((stallCount == stallWarningLimit - 1) && (prevStallCount == stallWarningLimit)) {
Reiko 0:ef6268629f0b 97 stallLevel = 0;
Reiko 0:ef6268629f0b 98 stallEndCallback.call();
Reiko 0:ef6268629f0b 99 stallChangeCallback.call();
Reiko 0:ef6268629f0b 100 } else if ((stallCount == stallWarningLimit) && (prevStallCount == stallWarningLimit - 1)) {
Reiko 0:ef6268629f0b 101 stallLevel = 1;
Reiko 0:ef6268629f0b 102 stallWarningCallback.call();
Reiko 0:ef6268629f0b 103 stallChangeCallback.call();
Reiko 0:ef6268629f0b 104 } else if (stallCount == stallErrorLimit) {
Reiko 0:ef6268629f0b 105 stallLevel = 2;
Reiko 0:ef6268629f0b 106 stallErrorCallback.call();
Reiko 0:ef6268629f0b 107 stallChangeCallback.call();
Reiko 0:ef6268629f0b 108 resetPID();
Reiko 0:ef6268629f0b 109 }
Reiko 0:ef6268629f0b 110 } else {
Reiko 0:ef6268629f0b 111 stallCount--;
Reiko 0:ef6268629f0b 112 if (stallCount == 0) {
Reiko 0:ef6268629f0b 113 stallLevel = 0;
Reiko 0:ef6268629f0b 114 stallEndCallback.call();
Reiko 0:ef6268629f0b 115 stallChangeCallback.call();
Reiko 0:ef6268629f0b 116 }
Reiko 0:ef6268629f0b 117 }
Reiko 0:ef6268629f0b 118 }
Reiko 0:ef6268629f0b 119
Reiko 0:ef6268629f0b 120 void Motor::resetPID() {
Reiko 0:ef6268629f0b 121 error = 0;
Reiko 0:ef6268629f0b 122 prevError = 0;
Reiko 0:ef6268629f0b 123 P = 0;
Reiko 0:ef6268629f0b 124 I = 0;
Reiko 0:ef6268629f0b 125 setPoint = 0;
Reiko 0:ef6268629f0b 126 setPWM(0);
Reiko 0:ef6268629f0b 127 }
Reiko 0:ef6268629f0b 128
Reiko 0:ef6268629f0b 129 int Motor::getStallLevel() {
Reiko 0:ef6268629f0b 130 return stallLevel;
Reiko 0:ef6268629f0b 131 }
Reiko 0:ef6268629f0b 132
Reiko 0:ef6268629f0b 133 void Motor::stallChange(void (*function)(void)) {
Reiko 0:ef6268629f0b 134 stallChangeCallback.attach(function);
Reiko 0:ef6268629f0b 135 }
Reiko 0:ef6268629f0b 136
Reiko 0:ef6268629f0b 137 void Motor::stallEnd(void (*function)(void)) {
Reiko 0:ef6268629f0b 138 stallEndCallback.attach(function);
Reiko 0:ef6268629f0b 139 }
Reiko 0:ef6268629f0b 140
Reiko 0:ef6268629f0b 141 void Motor::stallWarning(void (*function)(void)) {
Reiko 0:ef6268629f0b 142 stallWarningCallback.attach(function);
Reiko 0:ef6268629f0b 143 }
Reiko 0:ef6268629f0b 144
Reiko 0:ef6268629f0b 145 void Motor::stallError(void (*function)(void)) {
Reiko 0:ef6268629f0b 146 stallErrorCallback.attach(function);
Reiko 0:ef6268629f0b 147 }