Kaarel Adamson / motor

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Fork of motor by Karl Oskar Lember

motor.cpp@1:2911b1a7b611, 2016-11-15 (annotated)

Committer:
adamsonk
Date:
Tue Nov 15 14:08:11 2016 +0000
Revision:
1:2911b1a7b611
Parent:
0:910b46b1e4ae 
15.11 t??tav variant

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kolibakter 0:910b46b1e4ae 1 #include "motor.h"
kolibakter 0:910b46b1e4ae 2
kolibakter 0:910b46b1e4ae 3 Motor::Motor() {
kolibakter 0:910b46b1e4ae 4
kolibakter 0:910b46b1e4ae 5 }
kolibakter 0:910b46b1e4ae 6
kolibakter 0:910b46b1e4ae 7 Motor::Motor(USBSerial *pc, PwmOut *pwm, DigitalOut *dir1, DigitalOut *dir2, DigitalIn *fault) {
kolibakter 0:910b46b1e4ae 8 _pc = pc;
kolibakter 0:910b46b1e4ae 9 _pwm = pwm;
kolibakter 0:910b46b1e4ae 10 _dir1 = dir1;
kolibakter 0:910b46b1e4ae 11 _dir2 = dir2;
kolibakter 0:910b46b1e4ae 12 _fault = fault;
kolibakter 0:910b46b1e4ae 13
kolibakter 0:910b46b1e4ae 14 enc_last = 0;
kolibakter 0:910b46b1e4ae 15
kolibakter 0:910b46b1e4ae 16 pid_on = 1;
kolibakter 0:910b46b1e4ae 17 currentPWM = 0;
kolibakter 0:910b46b1e4ae 18
kolibakter 0:910b46b1e4ae 19 encTicks = 0;
kolibakter 0:910b46b1e4ae 20 motor_polarity = 0;
kolibakter 0:910b46b1e4ae 21
kolibakter 0:910b46b1e4ae 22 pidSpeed0 = 0;
kolibakter 0:910b46b1e4ae 23 pidSpeed1 = 0;
kolibakter 0:910b46b1e4ae 24 pidError0 = 0;
kolibakter 0:910b46b1e4ae 25 pidError1 = 0;
kolibakter 0:910b46b1e4ae 26 pidError2 = 0;
kolibakter 0:910b46b1e4ae 27 pidSetpoint = 0;
kolibakter 0:910b46b1e4ae 28
kolibakter 0:910b46b1e4ae 29 _pwm->period_us(PWM_PERIOD_US);
kolibakter 0:910b46b1e4ae 30 }
kolibakter 0:910b46b1e4ae 31
kolibakter 0:910b46b1e4ae 32 void Motor::forward(float pwm) {
kolibakter 0:910b46b1e4ae 33 if (dir) {
kolibakter 0:910b46b1e4ae 34 *_dir1 = 0;
kolibakter 0:910b46b1e4ae 35 *_dir2 = 1;
kolibakter 0:910b46b1e4ae 36 } else {
kolibakter 0:910b46b1e4ae 37 *_dir1 = 1;
kolibakter 0:910b46b1e4ae 38 *_dir2 = 0;
kolibakter 0:910b46b1e4ae 39 }
kolibakter 0:910b46b1e4ae 40
kolibakter 0:910b46b1e4ae 41 *_pwm = pwm;
kolibakter 0:910b46b1e4ae 42 currentPWM = pwm;
kolibakter 0:910b46b1e4ae 43 }
kolibakter 0:910b46b1e4ae 44
kolibakter 0:910b46b1e4ae 45 void Motor::backward(float pwm) {
kolibakter 0:910b46b1e4ae 46 if (dir) {
kolibakter 0:910b46b1e4ae 47 *_dir1 = 1;
kolibakter 0:910b46b1e4ae 48 *_dir2 = 0;
kolibakter 0:910b46b1e4ae 49 } else {
kolibakter 0:910b46b1e4ae 50 *_dir1 = 0;
kolibakter 0:910b46b1e4ae 51 *_dir2 = 1;
kolibakter 0:910b46b1e4ae 52 }
kolibakter 0:910b46b1e4ae 53
kolibakter 0:910b46b1e4ae 54 *_pwm = pwm;
kolibakter 0:910b46b1e4ae 55 currentPWM = -pwm;
kolibakter 0:910b46b1e4ae 56 }
kolibakter 0:910b46b1e4ae 57
kolibakter 0:910b46b1e4ae 58 void Motor::pid2(int16_t encTicks) {
kolibakter 0:910b46b1e4ae 59 speed = encTicks;
kolibakter 0:910b46b1e4ae 60 float pidDiv = 100.0;
kolibakter 0:910b46b1e4ae 61
kolibakter 0:910b46b1e4ae 62 if (!pid_on) return;
kolibakter 0:910b46b1e4ae 63 pidError2 = pidError1;
kolibakter 0:910b46b1e4ae 64 pidError1 = pidError0;
kolibakter 0:910b46b1e4ae 65 pidError0 = pidSetpoint - speed;
kolibakter 0:910b46b1e4ae 66 pidSpeed1 = pidSpeed0;
kolibakter 0:910b46b1e4ae 67 float p = pgain / pidDiv * (pidError0 - pidError1);
kolibakter 0:910b46b1e4ae 68 float i = igain / pidDiv * (pidError0 + pidError1) / 2.0;
kolibakter 0:910b46b1e4ae 69 float d = dgain / pidDiv * (pidError0 - 2.0 * pidError1 + pidError2);
kolibakter 0:910b46b1e4ae 70 pidSpeed0 = pidSpeed1 + p + i + d;
kolibakter 0:910b46b1e4ae 71
kolibakter 0:910b46b1e4ae 72
kolibakter 0:910b46b1e4ae 73 if (pidSpeed0 > 255) pidSpeed0 = 255;
kolibakter 0:910b46b1e4ae 74 if (pidSpeed0 < -255) pidSpeed0 = -255;
kolibakter 0:910b46b1e4ae 75
kolibakter 0:910b46b1e4ae 76 if (pidSpeed0 > 0) forward((pidSpeed0) / 255.0);
kolibakter 0:910b46b1e4ae 77 else backward((pidSpeed0) / -255.0);
kolibakter 0:910b46b1e4ae 78
kolibakter 0:910b46b1e4ae 79 if (pidSetpoint == 0) {
kolibakter 0:910b46b1e4ae 80 forward(0);
kolibakter 0:910b46b1e4ae 81 pidError0 = 0;
kolibakter 0:910b46b1e4ae 82 pidError1 = 0;
kolibakter 0:910b46b1e4ae 83 pidError2 = 0;
kolibakter 0:910b46b1e4ae 84 pidSpeed0 = 0;
kolibakter 0:910b46b1e4ae 85 pidSpeed1 = 0;
kolibakter 0:910b46b1e4ae 86 }
kolibakter 0:910b46b1e4ae 87 }
kolibakter 0:910b46b1e4ae 88
kolibakter 0:910b46b1e4ae 89 void Motor::pid(int16_t encTicks) {
kolibakter 0:910b46b1e4ae 90 speed = encTicks;
kolibakter 0:910b46b1e4ae 91 encTicks = 0;
kolibakter 0:910b46b1e4ae 92
kolibakter 0:910b46b1e4ae 93 if (!pid_on) return;
kolibakter 0:910b46b1e4ae 94
kolibakter 0:910b46b1e4ae 95 err_prev = err;
kolibakter 0:910b46b1e4ae 96 err = sp_pid - speed;
kolibakter 0:910b46b1e4ae 97
kolibakter 0:910b46b1e4ae 98 if (stallLevel != 2) {
kolibakter 0:910b46b1e4ae 99 intgrl += (err * pid_multi) / igain;
kolibakter 0:910b46b1e4ae 100
kolibakter 0:910b46b1e4ae 101 //constrain integral
kolibakter 0:910b46b1e4ae 102 if (intgrl < -imax) intgrl = -imax;
kolibakter 0:910b46b1e4ae 103 if (intgrl > imax) intgrl = imax;
kolibakter 0:910b46b1e4ae 104
kolibakter 0:910b46b1e4ae 105 if (sp == 0) pwmmin = 0;
kolibakter 0:910b46b1e4ae 106 else if (sp < 0) pwmmin = -pwm_min;
kolibakter 0:910b46b1e4ae 107 else pwmmin = pwm_min;
kolibakter 0:910b46b1e4ae 108
kolibakter 0:910b46b1e4ae 109 pwm = pwmmin + err*pgain + intgrl/pid_multi;
kolibakter 0:910b46b1e4ae 110 //constrain pwm
kolibakter 0:910b46b1e4ae 111 if (pwm < -255) pwm = -255;
kolibakter 0:910b46b1e4ae 112 if (pwm > 255) pwm = 255;
kolibakter 0:910b46b1e4ae 113
kolibakter 0:910b46b1e4ae 114 prevStallCount = stallCount;
kolibakter 0:910b46b1e4ae 115 if (((speed < 5 && currentPWM == 255) || (speed > -5 && currentPWM == -255)) && stallCount < stallErrorLimit) {
kolibakter 0:910b46b1e4ae 116 stallCount++;
kolibakter 0:910b46b1e4ae 117 } else if (stallCount > 0) {
kolibakter 0:910b46b1e4ae 118 stallCount--;
kolibakter 0:910b46b1e4ae 119 }
kolibakter 0:910b46b1e4ae 120
kolibakter 0:910b46b1e4ae 121 _pc->printf("pwm:%d\n", pwm);
kolibakter 0:910b46b1e4ae 122
kolibakter 0:910b46b1e4ae 123 if (pwm < 0) {
kolibakter 0:910b46b1e4ae 124 pwm *= -1;
kolibakter 0:910b46b1e4ae 125 backward(pwm/255.0);
kolibakter 0:910b46b1e4ae 126 } else {
kolibakter 0:910b46b1e4ae 127 forward(pwm/255.0);
kolibakter 0:910b46b1e4ae 128 }
kolibakter 0:910b46b1e4ae 129
kolibakter 0:910b46b1e4ae 130 if ((stallCount == stallWarningLimit - 1) && (prevStallCount == stallWarningLimit)) {
kolibakter 0:910b46b1e4ae 131 stallLevel = 0;
kolibakter 0:910b46b1e4ae 132 stallChanged = 1;
kolibakter 0:910b46b1e4ae 133 } else if ((stallCount == stallWarningLimit) && (prevStallCount == stallWarningLimit - 1)) {
kolibakter 0:910b46b1e4ae 134 stallLevel = 1;
kolibakter 0:910b46b1e4ae 135 stallChanged = 1;
kolibakter 0:910b46b1e4ae 136 } else if (stallCount == stallErrorLimit) {
kolibakter 0:910b46b1e4ae 137 stallLevel = 2;
kolibakter 0:910b46b1e4ae 138 stallChanged = 1;
kolibakter 0:910b46b1e4ae 139 reset_pid();
kolibakter 0:910b46b1e4ae 140 }
kolibakter 0:910b46b1e4ae 141 } else {
kolibakter 0:910b46b1e4ae 142 stallCount--;
kolibakter 0:910b46b1e4ae 143 if (stallCount == 0) {
kolibakter 0:910b46b1e4ae 144 stallLevel = 0;
kolibakter 0:910b46b1e4ae 145 stallChanged = 1;
kolibakter 0:910b46b1e4ae 146 }
kolibakter 0:910b46b1e4ae 147 }
kolibakter 0:910b46b1e4ae 148
kolibakter 0:910b46b1e4ae 149 if ((fail_counter == 100) && failsafe) {
kolibakter 0:910b46b1e4ae 150 sp_pid = 0;
kolibakter 0:910b46b1e4ae 151 reset_pid();
kolibakter 0:910b46b1e4ae 152 forward(0);
kolibakter 0:910b46b1e4ae 153 }
kolibakter 0:910b46b1e4ae 154
kolibakter 0:910b46b1e4ae 155 if (stallChanged) {
kolibakter 0:910b46b1e4ae 156 _pc->printf("<stall:%d>\n", stallLevel);
kolibakter 0:910b46b1e4ae 157 stallChanged = 0;
kolibakter 0:910b46b1e4ae 158 }
kolibakter 0:910b46b1e4ae 159
kolibakter 0:910b46b1e4ae 160 if ((speed < 10) && (pwm > 250)) {
kolibakter 0:910b46b1e4ae 161 stall_counter++;
kolibakter 0:910b46b1e4ae 162 } else {
kolibakter 0:910b46b1e4ae 163 stall_counter = 0;
kolibakter 0:910b46b1e4ae 164 }
kolibakter 0:910b46b1e4ae 165
kolibakter 0:910b46b1e4ae 166 fail_counter++;
kolibakter 0:910b46b1e4ae 167
kolibakter 0:910b46b1e4ae 168 if ((fail_counter == 100) && failsafe) {
kolibakter 0:910b46b1e4ae 169 sp_pid = 0;
kolibakter 0:910b46b1e4ae 170 reset_pid();
kolibakter 0:910b46b1e4ae 171 forward(0);
kolibakter 0:910b46b1e4ae 172 }
kolibakter 0:910b46b1e4ae 173 }
kolibakter 0:910b46b1e4ae 174
kolibakter 0:910b46b1e4ae 175 void Motor::reset_pid() {
kolibakter 0:910b46b1e4ae 176 err = 0;
kolibakter 0:910b46b1e4ae 177 err_prev = 0;
kolibakter 0:910b46b1e4ae 178 intgrl = 0;
kolibakter 0:910b46b1e4ae 179 der = 0;
kolibakter 0:910b46b1e4ae 180 sp = 0;
kolibakter 0:910b46b1e4ae 181 sp_pid = 0;
kolibakter 0:910b46b1e4ae 182 forward(0);
kolibakter 0:910b46b1e4ae 183 }
kolibakter 0:910b46b1e4ae 184
kolibakter 0:910b46b1e4ae 185
kolibakter 0:910b46b1e4ae 186 void Motor::setup() {
kolibakter 0:910b46b1e4ae 187 pid_on = 0;
kolibakter 0:910b46b1e4ae 188 forward(50);
kolibakter 0:910b46b1e4ae 189 encTicks = 0;
kolibakter 0:910b46b1e4ae 190 wait_ms(500);
kolibakter 0:910b46b1e4ae 191
kolibakter 0:910b46b1e4ae 192 if (speed < 0) {
kolibakter 0:910b46b1e4ae 193 motor_polarity ^= 1;
kolibakter 0:910b46b1e4ae 194 }
kolibakter 0:910b46b1e4ae 195
kolibakter 0:910b46b1e4ae 196 reset_pid();
kolibakter 0:910b46b1e4ae 197 forward(0);
kolibakter 0:910b46b1e4ae 198 pid_on = 1;
kolibakter 0:910b46b1e4ae 199 }
kolibakter 0:910b46b1e4ae 200
kolibakter 0:910b46b1e4ae 201 void Motor::init() {
kolibakter 0:910b46b1e4ae 202 dir = 0;
kolibakter 0:910b46b1e4ae 203 motor_polarity = 0;
kolibakter 0:910b46b1e4ae 204 pgain = 50;
kolibakter 0:910b46b1e4ae 205 igain = 10;
kolibakter 0:910b46b1e4ae 206 dgain = 0;
kolibakter 0:910b46b1e4ae 207 pid_multi = 32;
kolibakter 0:910b46b1e4ae 208 imax = 255*pid_multi;
kolibakter 0:910b46b1e4ae 209 err_max = 4000;
kolibakter 0:910b46b1e4ae 210 pwm_min = 25;
kolibakter 0:910b46b1e4ae 211 intgrl = 0;
kolibakter 0:910b46b1e4ae 212 //count = 0;
adamsonk 1:2911b1a7b611 213 speed = 50;
kolibakter 0:910b46b1e4ae 214 err = 0;
kolibakter 0:910b46b1e4ae 215 }
kolibakter 0:910b46b1e4ae 216
kolibakter 0:910b46b1e4ae 217 int16_t Motor::getSpeed() {
kolibakter 0:910b46b1e4ae 218 return speed;
kolibakter 0:910b46b1e4ae 219 }
kolibakter 0:910b46b1e4ae 220
kolibakter 0:910b46b1e4ae 221 void Motor::setSpeed(int16_t speed) {
kolibakter 0:910b46b1e4ae 222 sp_pid = speed;
kolibakter 0:910b46b1e4ae 223 pidSetpoint = speed;
kolibakter 0:910b46b1e4ae 224 if (sp_pid == 0) reset_pid();
kolibakter 0:910b46b1e4ae 225 fail_counter = 0;
kolibakter 0:910b46b1e4ae 226 }
kolibakter 0:910b46b1e4ae 227
kolibakter 0:910b46b1e4ae 228 void Motor::getPIDGain(char *gain) {
kolibakter 0:910b46b1e4ae 229 sprintf(gain, "PID:%d,%d,%d", pgain, igain, dgain);
kolibakter 0:910b46b1e4ae 230 }