motors

Fork of motor by Karl Oskar Lember

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers motor.cpp Source File

motor.cpp

00001 #include "motor.h"
00002 
00003 Motor::Motor() {
00004     
00005 }
00006 
00007 Motor::Motor(USBSerial *pc, PwmOut *pwm, DigitalOut *dir1, DigitalOut *dir2, DigitalIn *fault) {
00008     _pc = pc;
00009     _pwm = pwm;
00010     _dir1 = dir1;
00011     _dir2 = dir2;
00012     _fault = fault;
00013 
00014     enc_last = 0;
00015 
00016     pid_on = 1;
00017     currentPWM = 0;
00018 
00019     encTicks = 0;
00020     motor_polarity = 0;
00021 
00022     pidSpeed0 = 0;
00023     pidSpeed1 = 0;
00024     pidError0 = 0;
00025     pidError1 = 0;
00026     pidError2 = 0;
00027     pidSetpoint = 0;
00028 
00029     _pwm->period_us(PWM_PERIOD_US);
00030 }
00031 
00032 void Motor::forward(float pwm) {
00033     if (dir) {
00034         *_dir1 = 0;
00035         *_dir2 = 1;
00036     } else {
00037         *_dir1 = 1;
00038         *_dir2 = 0;
00039     }
00040 
00041     *_pwm = pwm;
00042     currentPWM = pwm;
00043 }
00044 
00045 void Motor::backward(float pwm) {
00046     if (dir) {
00047         *_dir1 = 1;
00048         *_dir2 = 0;
00049     } else {
00050         *_dir1 = 0;
00051         *_dir2 = 1;
00052     }
00053 
00054     *_pwm = pwm;
00055     currentPWM = -pwm;
00056 }
00057 
00058 void Motor::pid2(int16_t encTicks) {
00059     speed = encTicks;
00060     float pidDiv = 100.0;
00061 
00062     if (!pid_on) return;
00063     pidError2 = pidError1;
00064     pidError1 = pidError0;
00065     pidError0 = pidSetpoint - speed;
00066     pidSpeed1 = pidSpeed0;
00067     float p = pgain / pidDiv * (pidError0 - pidError1);
00068     float i = igain / pidDiv * (pidError0 + pidError1) / 2.0;
00069     float d = dgain / pidDiv * (pidError0 - 2.0 * pidError1 + pidError2);
00070     pidSpeed0 = pidSpeed1 + p + i + d;
00071 
00072 
00073     if (pidSpeed0 > 255) pidSpeed0 = 255;
00074     if (pidSpeed0 < -255) pidSpeed0 = -255;
00075 
00076     if (pidSpeed0 > 0) forward((pidSpeed0) / 255.0);
00077     else backward((pidSpeed0) / -255.0);
00078 
00079     if (pidSetpoint == 0) {
00080         forward(0);
00081         pidError0 = 0;
00082         pidError1 = 0;
00083         pidError2 = 0;
00084         pidSpeed0 = 0;
00085         pidSpeed1 = 0;
00086     }
00087 }
00088 
00089 void Motor::pid(int16_t encTicks) {
00090     speed = encTicks;
00091     encTicks = 0;
00092 
00093     if (!pid_on) return;
00094 
00095     err_prev = err;
00096     err = sp_pid - speed;
00097 
00098     if (stallLevel != 2) {
00099         intgrl += (err * pid_multi) / igain;
00100 
00101         //constrain integral
00102         if (intgrl < -imax) intgrl = -imax;
00103         if (intgrl > imax) intgrl = imax;
00104 
00105         if (sp == 0) pwmmin = 0;
00106         else if (sp < 0) pwmmin = -pwm_min;
00107         else pwmmin = pwm_min;
00108 
00109         pwm = pwmmin + err*pgain + intgrl/pid_multi;
00110         //constrain pwm
00111         if (pwm < -255) pwm = -255;
00112         if (pwm > 255) pwm = 255;
00113 
00114         prevStallCount = stallCount;
00115         if (((speed < 5 && currentPWM == 255) || (speed > -5 && currentPWM == -255)) && stallCount < stallErrorLimit) {
00116             stallCount++;
00117         } else if (stallCount > 0) {
00118             stallCount--;
00119         }
00120 
00121         _pc->printf("pwm:%d\n", pwm);
00122 
00123         if (pwm < 0) {
00124             pwm *= -1;
00125             backward(pwm/255.0);
00126         } else {
00127             forward(pwm/255.0);
00128         }
00129 
00130         if ((stallCount == stallWarningLimit - 1) && (prevStallCount == stallWarningLimit)) {
00131             stallLevel = 0;
00132             stallChanged = 1;
00133         } else if ((stallCount == stallWarningLimit) && (prevStallCount == stallWarningLimit - 1)) {
00134             stallLevel = 1;
00135             stallChanged = 1;
00136         } else if (stallCount == stallErrorLimit) {
00137             stallLevel = 2;
00138             stallChanged = 1;
00139             reset_pid();
00140         }
00141     } else {
00142         stallCount--;
00143         if (stallCount == 0) {
00144             stallLevel = 0;
00145             stallChanged = 1;
00146         }
00147     }
00148 
00149     if ((fail_counter == 100) && failsafe) {
00150         sp_pid = 0;
00151         reset_pid();
00152         forward(0);
00153     }
00154 
00155     if (stallChanged) {
00156         _pc->printf("<stall:%d>\n", stallLevel);
00157         stallChanged = 0;
00158     }
00159 
00160     if ((speed < 10) && (pwm > 250)) {
00161         stall_counter++;
00162     } else {
00163         stall_counter = 0;
00164     }
00165 
00166     fail_counter++;
00167 
00168     if ((fail_counter == 100) && failsafe) {
00169         sp_pid = 0;
00170         reset_pid();
00171         forward(0);
00172     }
00173 }
00174 
00175 void Motor::reset_pid() {
00176     err = 0;
00177     err_prev = 0;
00178     intgrl = 0;
00179     der = 0;
00180     sp = 0;
00181     sp_pid = 0;
00182     forward(0);
00183 }
00184 
00185 
00186 void Motor::setup() {
00187     pid_on = 0;
00188     forward(50);
00189     encTicks = 0;
00190     wait_ms(500);
00191 
00192     if (speed < 0) {
00193         motor_polarity ^= 1;
00194     }
00195 
00196     reset_pid();
00197     forward(0);
00198     pid_on = 1;
00199 }
00200 
00201 void Motor::init() {
00202     dir = 0;
00203     motor_polarity = 0;
00204     pgain = 50;
00205     igain = 10;
00206     dgain = 0;
00207     pid_multi = 32;
00208     imax = 255*pid_multi;
00209     err_max = 4000;
00210     pwm_min = 25;
00211     intgrl = 0;
00212     //count = 0;
00213     speed = 50;
00214     err = 0;
00215 }
00216 
00217 int16_t Motor::getSpeed() {
00218     return speed;
00219 }
00220 
00221 void Motor::setSpeed(int16_t speed) {
00222     sp_pid = speed;
00223     pidSetpoint = speed;
00224     if (sp_pid == 0) reset_pid();
00225     fail_counter = 0;
00226 }
00227 
00228 void Motor::getPIDGain(char *gain) {
00229     sprintf(gain, "PID:%d,%d,%d", pgain, igain, dgain);
00230 }