Reiko Randoja
/
ut_bbr
Basketball robot mainboard firmware
Diff: motor/motor.cpp
- Revision:
- 0:88887cfb2b04
diff -r 000000000000 -r 88887cfb2b04 motor/motor.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/motor/motor.cpp Mon Sep 10 15:24:08 2018 +0000 @@ -0,0 +1,151 @@ +#include "motor.h" + +Motor::Motor(Serial *pc, PinName pwm, PinName dir1, PinName dir2, PinName fault, PinName encA, PinName encB) : + _pwm(pwm), + _dir1(dir1), + _dir2(dir2), + _fault(fault), + _encA(encA), + _encB(encB), + _pc(pc) +{ + _encA.mode(PullNone); + _encB.mode(PullNone); + + ticks = 0; + encNow = 0; + encLast = 0; + + enc_last = 0; + + pid_on = 1; + currentPWM = 0; + + encTicks = 0; + motor_polarity = 0; + + pidSpeed = 0; + pidError = 0; + pidErrorPrev = 0; + pidSetpoint = 0; + + i = 0; + counter = 0; + + _pwm.period_us(PWM_PERIOD_US); + + _encA.rise(this, &Motor::encTick); + _encA.fall(this, &Motor::encTick); + _encB.rise(this, &Motor::encTick); + _encB.fall(this, &Motor::encTick); + + //pidTicker.attach(this, &Motor::pidTick, 1/PID_FREQ); + + dir = 0; + motor_polarity = 0; + pgain = 0.02; + igain = 0.005; + dgain = 0; + pwm_min = 25; + intgrl = 0; + //count = 0; + speed = 0; + err = 0; +} + +void Motor::forward(float pwm) { + if (dir) { + _dir1 = 0; + _dir2 = 1; + } else { + _dir1 = 1; + _dir2 = 0; + } + + _pwm = pwm; + currentPWM = pwm; +} + +void Motor::backward(float pwm) { + if (dir) { + _dir1 = 1; + _dir2 = 0; + } else { + _dir1 = 0; + _dir2 = 1; + } + + _pwm = pwm; + currentPWM = -pwm; +} + +void Motor::pid2(int16_t encTicks) { + speed = encTicks; + + if (!pid_on) return; + pidError = pidSetpoint - speed; + float p = pgain * pidError; + i += igain * pidError; + float d = dgain * (pidError - pidErrorPrev); + pidSpeed = p + i + d; + + if (i > 1) { + i = 1; + } else if (i < -1) { + i = -1; + } + + pidErrorPrev = pidError; + + if (pidSpeed > 1) pidSpeed = 1; + if (pidSpeed < -1) pidSpeed = -1; + + if (pidSpeed > 0) forward(pidSpeed); + else backward(-pidSpeed); + + if (pidSetpoint == 0) { + forward(0); + pidError = 0; + pidSpeed = 0; + } +} + +void Motor::reset_pid() { + err = 0; + err_prev = 0; + i = 0; + der = 0; + sp = 0; + sp_pid = 0; + forward(0); +} + +int16_t Motor::getSpeed() { + return speed; +} + +void Motor::setSpeed(int16_t speed) { + sp_pid = speed; + pidSetpoint = speed; + if (sp_pid == 0) reset_pid(); + fail_counter = 0; +} + +void Motor::getPIDGain(char *gain) { + sprintf(gain, "PID:%f,%f,%f", pgain, igain, dgain); +} + +void Motor::pidTick() { + pid2(ticks); + ticks = 0; +} + +void Motor::encTick() { + uint8_t enc_dir; + encNow = _encA.read() | (_encB.read() << 1); + enc_dir = (encLast & 1) ^ ((encNow & 2) >> 1); + encLast = encNow; + + if (enc_dir & 1) ticks++; + else ticks--; +}