![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Basketball robot mainboard firmware
motor/motor.cpp@0:88887cfb2b04, 2018-09-10 (annotated)
- Committer:
- Reiko
- Date:
- Mon Sep 10 15:24:08 2018 +0000
- Revision:
- 0:88887cfb2b04
Mainboard firmware for basketball robot
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Reiko | 0:88887cfb2b04 | 1 | #include "motor.h" |
Reiko | 0:88887cfb2b04 | 2 | |
Reiko | 0:88887cfb2b04 | 3 | Motor::Motor(Serial *pc, PinName pwm, PinName dir1, PinName dir2, PinName fault, PinName encA, PinName encB) : |
Reiko | 0:88887cfb2b04 | 4 | _pwm(pwm), |
Reiko | 0:88887cfb2b04 | 5 | _dir1(dir1), |
Reiko | 0:88887cfb2b04 | 6 | _dir2(dir2), |
Reiko | 0:88887cfb2b04 | 7 | _fault(fault), |
Reiko | 0:88887cfb2b04 | 8 | _encA(encA), |
Reiko | 0:88887cfb2b04 | 9 | _encB(encB), |
Reiko | 0:88887cfb2b04 | 10 | _pc(pc) |
Reiko | 0:88887cfb2b04 | 11 | { |
Reiko | 0:88887cfb2b04 | 12 | _encA.mode(PullNone); |
Reiko | 0:88887cfb2b04 | 13 | _encB.mode(PullNone); |
Reiko | 0:88887cfb2b04 | 14 | |
Reiko | 0:88887cfb2b04 | 15 | ticks = 0; |
Reiko | 0:88887cfb2b04 | 16 | encNow = 0; |
Reiko | 0:88887cfb2b04 | 17 | encLast = 0; |
Reiko | 0:88887cfb2b04 | 18 | |
Reiko | 0:88887cfb2b04 | 19 | enc_last = 0; |
Reiko | 0:88887cfb2b04 | 20 | |
Reiko | 0:88887cfb2b04 | 21 | pid_on = 1; |
Reiko | 0:88887cfb2b04 | 22 | currentPWM = 0; |
Reiko | 0:88887cfb2b04 | 23 | |
Reiko | 0:88887cfb2b04 | 24 | encTicks = 0; |
Reiko | 0:88887cfb2b04 | 25 | motor_polarity = 0; |
Reiko | 0:88887cfb2b04 | 26 | |
Reiko | 0:88887cfb2b04 | 27 | pidSpeed = 0; |
Reiko | 0:88887cfb2b04 | 28 | pidError = 0; |
Reiko | 0:88887cfb2b04 | 29 | pidErrorPrev = 0; |
Reiko | 0:88887cfb2b04 | 30 | pidSetpoint = 0; |
Reiko | 0:88887cfb2b04 | 31 | |
Reiko | 0:88887cfb2b04 | 32 | i = 0; |
Reiko | 0:88887cfb2b04 | 33 | counter = 0; |
Reiko | 0:88887cfb2b04 | 34 | |
Reiko | 0:88887cfb2b04 | 35 | _pwm.period_us(PWM_PERIOD_US); |
Reiko | 0:88887cfb2b04 | 36 | |
Reiko | 0:88887cfb2b04 | 37 | _encA.rise(this, &Motor::encTick); |
Reiko | 0:88887cfb2b04 | 38 | _encA.fall(this, &Motor::encTick); |
Reiko | 0:88887cfb2b04 | 39 | _encB.rise(this, &Motor::encTick); |
Reiko | 0:88887cfb2b04 | 40 | _encB.fall(this, &Motor::encTick); |
Reiko | 0:88887cfb2b04 | 41 | |
Reiko | 0:88887cfb2b04 | 42 | //pidTicker.attach(this, &Motor::pidTick, 1/PID_FREQ); |
Reiko | 0:88887cfb2b04 | 43 | |
Reiko | 0:88887cfb2b04 | 44 | dir = 0; |
Reiko | 0:88887cfb2b04 | 45 | motor_polarity = 0; |
Reiko | 0:88887cfb2b04 | 46 | pgain = 0.02; |
Reiko | 0:88887cfb2b04 | 47 | igain = 0.005; |
Reiko | 0:88887cfb2b04 | 48 | dgain = 0; |
Reiko | 0:88887cfb2b04 | 49 | pwm_min = 25; |
Reiko | 0:88887cfb2b04 | 50 | intgrl = 0; |
Reiko | 0:88887cfb2b04 | 51 | //count = 0; |
Reiko | 0:88887cfb2b04 | 52 | speed = 0; |
Reiko | 0:88887cfb2b04 | 53 | err = 0; |
Reiko | 0:88887cfb2b04 | 54 | } |
Reiko | 0:88887cfb2b04 | 55 | |
Reiko | 0:88887cfb2b04 | 56 | void Motor::forward(float pwm) { |
Reiko | 0:88887cfb2b04 | 57 | if (dir) { |
Reiko | 0:88887cfb2b04 | 58 | _dir1 = 0; |
Reiko | 0:88887cfb2b04 | 59 | _dir2 = 1; |
Reiko | 0:88887cfb2b04 | 60 | } else { |
Reiko | 0:88887cfb2b04 | 61 | _dir1 = 1; |
Reiko | 0:88887cfb2b04 | 62 | _dir2 = 0; |
Reiko | 0:88887cfb2b04 | 63 | } |
Reiko | 0:88887cfb2b04 | 64 | |
Reiko | 0:88887cfb2b04 | 65 | _pwm = pwm; |
Reiko | 0:88887cfb2b04 | 66 | currentPWM = pwm; |
Reiko | 0:88887cfb2b04 | 67 | } |
Reiko | 0:88887cfb2b04 | 68 | |
Reiko | 0:88887cfb2b04 | 69 | void Motor::backward(float pwm) { |
Reiko | 0:88887cfb2b04 | 70 | if (dir) { |
Reiko | 0:88887cfb2b04 | 71 | _dir1 = 1; |
Reiko | 0:88887cfb2b04 | 72 | _dir2 = 0; |
Reiko | 0:88887cfb2b04 | 73 | } else { |
Reiko | 0:88887cfb2b04 | 74 | _dir1 = 0; |
Reiko | 0:88887cfb2b04 | 75 | _dir2 = 1; |
Reiko | 0:88887cfb2b04 | 76 | } |
Reiko | 0:88887cfb2b04 | 77 | |
Reiko | 0:88887cfb2b04 | 78 | _pwm = pwm; |
Reiko | 0:88887cfb2b04 | 79 | currentPWM = -pwm; |
Reiko | 0:88887cfb2b04 | 80 | } |
Reiko | 0:88887cfb2b04 | 81 | |
Reiko | 0:88887cfb2b04 | 82 | void Motor::pid2(int16_t encTicks) { |
Reiko | 0:88887cfb2b04 | 83 | speed = encTicks; |
Reiko | 0:88887cfb2b04 | 84 | |
Reiko | 0:88887cfb2b04 | 85 | if (!pid_on) return; |
Reiko | 0:88887cfb2b04 | 86 | pidError = pidSetpoint - speed; |
Reiko | 0:88887cfb2b04 | 87 | float p = pgain * pidError; |
Reiko | 0:88887cfb2b04 | 88 | i += igain * pidError; |
Reiko | 0:88887cfb2b04 | 89 | float d = dgain * (pidError - pidErrorPrev); |
Reiko | 0:88887cfb2b04 | 90 | pidSpeed = p + i + d; |
Reiko | 0:88887cfb2b04 | 91 | |
Reiko | 0:88887cfb2b04 | 92 | if (i > 1) { |
Reiko | 0:88887cfb2b04 | 93 | i = 1; |
Reiko | 0:88887cfb2b04 | 94 | } else if (i < -1) { |
Reiko | 0:88887cfb2b04 | 95 | i = -1; |
Reiko | 0:88887cfb2b04 | 96 | } |
Reiko | 0:88887cfb2b04 | 97 | |
Reiko | 0:88887cfb2b04 | 98 | pidErrorPrev = pidError; |
Reiko | 0:88887cfb2b04 | 99 | |
Reiko | 0:88887cfb2b04 | 100 | if (pidSpeed > 1) pidSpeed = 1; |
Reiko | 0:88887cfb2b04 | 101 | if (pidSpeed < -1) pidSpeed = -1; |
Reiko | 0:88887cfb2b04 | 102 | |
Reiko | 0:88887cfb2b04 | 103 | if (pidSpeed > 0) forward(pidSpeed); |
Reiko | 0:88887cfb2b04 | 104 | else backward(-pidSpeed); |
Reiko | 0:88887cfb2b04 | 105 | |
Reiko | 0:88887cfb2b04 | 106 | if (pidSetpoint == 0) { |
Reiko | 0:88887cfb2b04 | 107 | forward(0); |
Reiko | 0:88887cfb2b04 | 108 | pidError = 0; |
Reiko | 0:88887cfb2b04 | 109 | pidSpeed = 0; |
Reiko | 0:88887cfb2b04 | 110 | } |
Reiko | 0:88887cfb2b04 | 111 | } |
Reiko | 0:88887cfb2b04 | 112 | |
Reiko | 0:88887cfb2b04 | 113 | void Motor::reset_pid() { |
Reiko | 0:88887cfb2b04 | 114 | err = 0; |
Reiko | 0:88887cfb2b04 | 115 | err_prev = 0; |
Reiko | 0:88887cfb2b04 | 116 | i = 0; |
Reiko | 0:88887cfb2b04 | 117 | der = 0; |
Reiko | 0:88887cfb2b04 | 118 | sp = 0; |
Reiko | 0:88887cfb2b04 | 119 | sp_pid = 0; |
Reiko | 0:88887cfb2b04 | 120 | forward(0); |
Reiko | 0:88887cfb2b04 | 121 | } |
Reiko | 0:88887cfb2b04 | 122 | |
Reiko | 0:88887cfb2b04 | 123 | int16_t Motor::getSpeed() { |
Reiko | 0:88887cfb2b04 | 124 | return speed; |
Reiko | 0:88887cfb2b04 | 125 | } |
Reiko | 0:88887cfb2b04 | 126 | |
Reiko | 0:88887cfb2b04 | 127 | void Motor::setSpeed(int16_t speed) { |
Reiko | 0:88887cfb2b04 | 128 | sp_pid = speed; |
Reiko | 0:88887cfb2b04 | 129 | pidSetpoint = speed; |
Reiko | 0:88887cfb2b04 | 130 | if (sp_pid == 0) reset_pid(); |
Reiko | 0:88887cfb2b04 | 131 | fail_counter = 0; |
Reiko | 0:88887cfb2b04 | 132 | } |
Reiko | 0:88887cfb2b04 | 133 | |
Reiko | 0:88887cfb2b04 | 134 | void Motor::getPIDGain(char *gain) { |
Reiko | 0:88887cfb2b04 | 135 | sprintf(gain, "PID:%f,%f,%f", pgain, igain, dgain); |
Reiko | 0:88887cfb2b04 | 136 | } |
Reiko | 0:88887cfb2b04 | 137 | |
Reiko | 0:88887cfb2b04 | 138 | void Motor::pidTick() { |
Reiko | 0:88887cfb2b04 | 139 | pid2(ticks); |
Reiko | 0:88887cfb2b04 | 140 | ticks = 0; |
Reiko | 0:88887cfb2b04 | 141 | } |
Reiko | 0:88887cfb2b04 | 142 | |
Reiko | 0:88887cfb2b04 | 143 | void Motor::encTick() { |
Reiko | 0:88887cfb2b04 | 144 | uint8_t enc_dir; |
Reiko | 0:88887cfb2b04 | 145 | encNow = _encA.read() | (_encB.read() << 1); |
Reiko | 0:88887cfb2b04 | 146 | enc_dir = (encLast & 1) ^ ((encNow & 2) >> 1); |
Reiko | 0:88887cfb2b04 | 147 | encLast = encNow; |
Reiko | 0:88887cfb2b04 | 148 | |
Reiko | 0:88887cfb2b04 | 149 | if (enc_dir & 1) ticks++; |
Reiko | 0:88887cfb2b04 | 150 | else ticks--; |
Reiko | 0:88887cfb2b04 | 151 | } |