Reiko Randoja / Mbed 2 deprecated ut_bbr

Basketball robot mainboard firmware

Dependencies:   USBDevice mbed

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?

UserRevisionLine numberNew 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 }