Reiko Randoja / Mbed 2 deprecated ut_bbr

Basketball robot mainboard firmware

Dependencies:   USBDevice mbed

main.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 "mbed.h"
Reiko 0:88887cfb2b04 2 #include "pins.h"
Reiko 0:88887cfb2b04 3 #include "motor.h"
Reiko 0:88887cfb2b04 4 #include "RGBLed.hpp"
Reiko 0:88887cfb2b04 5 #include "USBSerial.h"
Reiko 0:88887cfb2b04 6 #include "RFManager.h"
Reiko 0:88887cfb2b04 7
Reiko 0:88887cfb2b04 8 USBSerial serial;
Reiko 0:88887cfb2b04 9
Reiko 0:88887cfb2b04 10 Serial pc(USBTX, USBRX);
Reiko 0:88887cfb2b04 11
Reiko 0:88887cfb2b04 12 RGBLed led1(LED1R, LED1G, LED1B);
Reiko 0:88887cfb2b04 13 RGBLed led2(LED2R, LED2G, LED2B);
Reiko 0:88887cfb2b04 14
Reiko 0:88887cfb2b04 15 DigitalIn infrared(ADC0);
Reiko 0:88887cfb2b04 16
Reiko 0:88887cfb2b04 17 Timeout kicker;
Reiko 0:88887cfb2b04 18
Reiko 0:88887cfb2b04 19 static const int NUMBER_OF_MOTORS = 3;
Reiko 0:88887cfb2b04 20
Reiko 0:88887cfb2b04 21 Motor motor0(&pc, M0_PWM, M0_DIR1, M0_DIR2, M0_FAULT, M0_ENCA, M0_ENCB);
Reiko 0:88887cfb2b04 22 Motor motor1(&pc, M1_PWM, M1_DIR1, M1_DIR2, M1_FAULT, M1_ENCA, M1_ENCB);
Reiko 0:88887cfb2b04 23 Motor motor2(&pc, M2_PWM, M2_DIR1, M2_DIR2, M2_FAULT, M2_ENCA, M2_ENCB);
Reiko 0:88887cfb2b04 24 //Motor motor3(&pc, M3_PWM, M3_DIR1, M3_DIR2, M3_FAULT, M3_ENCA, M3_ENCB);
Reiko 0:88887cfb2b04 25
Reiko 0:88887cfb2b04 26 Motor * motors[NUMBER_OF_MOTORS] = {
Reiko 0:88887cfb2b04 27 &motor0, &motor1, &motor2/*, &motor3*/
Reiko 0:88887cfb2b04 28 };
Reiko 0:88887cfb2b04 29
Reiko 0:88887cfb2b04 30 PwmOut m0(M0_PWM);
Reiko 0:88887cfb2b04 31 PwmOut m1(M1_PWM);
Reiko 0:88887cfb2b04 32 PwmOut m2(M2_PWM);
Reiko 0:88887cfb2b04 33 PwmOut m3(M3_PWM);
Reiko 0:88887cfb2b04 34 PwmOut pwm0(PWM0);
Reiko 0:88887cfb2b04 35 PwmOut pwm1(PWM1);
Reiko 0:88887cfb2b04 36
Reiko 0:88887cfb2b04 37 RFManager rfModule(COMTX, COMRX);
Reiko 0:88887cfb2b04 38
Reiko 0:88887cfb2b04 39 void serialInterrupt();
Reiko 0:88887cfb2b04 40 void parseCommand(char *command);
Reiko 0:88887cfb2b04 41
Reiko 0:88887cfb2b04 42 Ticker pidTicker;
Reiko 0:88887cfb2b04 43 int pidTickerCount = 0;
Reiko 0:88887cfb2b04 44 static const float PID_FREQ = 60;
Reiko 0:88887cfb2b04 45
Reiko 0:88887cfb2b04 46 char buf[32];
Reiko 0:88887cfb2b04 47 int serialCount = 0;
Reiko 0:88887cfb2b04 48 bool serialData = false;
Reiko 0:88887cfb2b04 49
Reiko 0:88887cfb2b04 50 bool failSafeEnabled = true;
Reiko 0:88887cfb2b04 51 int ticksSinceCommand = 0;
Reiko 0:88887cfb2b04 52
Reiko 0:88887cfb2b04 53 void pidTick() {
Reiko 0:88887cfb2b04 54 for (int i = 0; i < NUMBER_OF_MOTORS; i++) {
Reiko 0:88887cfb2b04 55 motors[i]->pidTick();
Reiko 0:88887cfb2b04 56 }
Reiko 0:88887cfb2b04 57
Reiko 0:88887cfb2b04 58 if (pidTickerCount++ % 25 == 0) {
Reiko 0:88887cfb2b04 59 led1.setBlue(!led1.getBlue());
Reiko 0:88887cfb2b04 60 }
Reiko 0:88887cfb2b04 61
Reiko 0:88887cfb2b04 62 // Fail-safe
Reiko 0:88887cfb2b04 63 if (failSafeEnabled) {
Reiko 0:88887cfb2b04 64 ticksSinceCommand++;
Reiko 0:88887cfb2b04 65 }
Reiko 0:88887cfb2b04 66
Reiko 0:88887cfb2b04 67 if (ticksSinceCommand == 60) {
Reiko 0:88887cfb2b04 68 for (int i = 0; i < NUMBER_OF_MOTORS; ++i) {
Reiko 0:88887cfb2b04 69 motors[i]->setSpeed(0);
Reiko 0:88887cfb2b04 70 }
Reiko 0:88887cfb2b04 71
Reiko 0:88887cfb2b04 72 m3.pulsewidth_us(100);
Reiko 0:88887cfb2b04 73 }
Reiko 0:88887cfb2b04 74 }
Reiko 0:88887cfb2b04 75
Reiko 0:88887cfb2b04 76 int main() {
Reiko 0:88887cfb2b04 77 pidTicker.attach(pidTick, 1/PID_FREQ);
Reiko 0:88887cfb2b04 78 //serial.attach(&serialInterrupt);
Reiko 0:88887cfb2b04 79
Reiko 0:88887cfb2b04 80 // Ball detector status
Reiko 0:88887cfb2b04 81 int infraredStatus = -1;
Reiko 0:88887cfb2b04 82
Reiko 0:88887cfb2b04 83 // Dribbler motor
Reiko 0:88887cfb2b04 84 m3.period_us(20000);
Reiko 0:88887cfb2b04 85 m3.pulsewidth_us(100);
Reiko 0:88887cfb2b04 86
Reiko 0:88887cfb2b04 87 while (1) {
Reiko 0:88887cfb2b04 88 if (rfModule.readable()) {
Reiko 0:88887cfb2b04 89 serial.printf("<ref:%s>\n", rfModule.read());
Reiko 0:88887cfb2b04 90 }
Reiko 0:88887cfb2b04 91
Reiko 0:88887cfb2b04 92 rfModule.update();
Reiko 0:88887cfb2b04 93
Reiko 0:88887cfb2b04 94 if (serial.readable()) {
Reiko 0:88887cfb2b04 95 buf[serialCount] = serial.getc();
Reiko 0:88887cfb2b04 96 //serial.putc(buf[serialCount]);
Reiko 0:88887cfb2b04 97
Reiko 0:88887cfb2b04 98 if (buf[serialCount] == '\n') {
Reiko 0:88887cfb2b04 99 parseCommand(buf);
Reiko 0:88887cfb2b04 100 serialCount = 0;
Reiko 0:88887cfb2b04 101 memset(buf, 0, 32);
Reiko 0:88887cfb2b04 102 } else {
Reiko 0:88887cfb2b04 103 serialCount++;
Reiko 0:88887cfb2b04 104 }
Reiko 0:88887cfb2b04 105 }
Reiko 0:88887cfb2b04 106
Reiko 0:88887cfb2b04 107 /// INFRARED DETECTOR
Reiko 0:88887cfb2b04 108 int newInfraredStatus = infrared.read();
Reiko 0:88887cfb2b04 109
Reiko 0:88887cfb2b04 110 if (newInfraredStatus != infraredStatus) {
Reiko 0:88887cfb2b04 111 infraredStatus = newInfraredStatus;
Reiko 0:88887cfb2b04 112 serial.printf("<ball:%d>\n", newInfraredStatus);
Reiko 0:88887cfb2b04 113 led2.setGreen(infraredStatus);
Reiko 0:88887cfb2b04 114 }
Reiko 0:88887cfb2b04 115 }
Reiko 0:88887cfb2b04 116 }
Reiko 0:88887cfb2b04 117
Reiko 0:88887cfb2b04 118 void parseCommand(char *buffer) {
Reiko 0:88887cfb2b04 119 ticksSinceCommand = 0;
Reiko 0:88887cfb2b04 120
Reiko 0:88887cfb2b04 121 char *cmd = strtok(buffer, ":");
Reiko 0:88887cfb2b04 122
Reiko 0:88887cfb2b04 123 // buffer == "sd:14:16:10:30"
Reiko 0:88887cfb2b04 124 if (strncmp(cmd, "sd", 2) == 0) {
Reiko 0:88887cfb2b04 125 for (int i = 0; i < NUMBER_OF_MOTORS; ++i) {
Reiko 0:88887cfb2b04 126 motors[i]->setSpeed((int16_t) atoi(strtok(NULL, ":")));
Reiko 0:88887cfb2b04 127 }
Reiko 0:88887cfb2b04 128
Reiko 0:88887cfb2b04 129 /*serial.printf("<gs:%d:%d:%d:%d>\n",
Reiko 0:88887cfb2b04 130 motors[0]->getSpeed(),
Reiko 0:88887cfb2b04 131 motors[1]->getSpeed(),
Reiko 0:88887cfb2b04 132 motors[2]->getSpeed(),
Reiko 0:88887cfb2b04 133 motors[3]->getSpeed()
Reiko 0:88887cfb2b04 134 );*/
Reiko 0:88887cfb2b04 135
Reiko 0:88887cfb2b04 136 serial.printf("<gs:%d:%d:%d>\n",
Reiko 0:88887cfb2b04 137 motors[0]->getSpeed(),
Reiko 0:88887cfb2b04 138 motors[1]->getSpeed(),
Reiko 0:88887cfb2b04 139 motors[2]->getSpeed()
Reiko 0:88887cfb2b04 140 );
Reiko 0:88887cfb2b04 141 }
Reiko 0:88887cfb2b04 142
Reiko 0:88887cfb2b04 143 if (strncmp(cmd, "d", 1) == 0) {
Reiko 0:88887cfb2b04 144 /*
Reiko 0:88887cfb2b04 145 if (command[1] == '0') {
Reiko 0:88887cfb2b04 146 pwm1.pulsewidth_us(100);
Reiko 0:88887cfb2b04 147 } else if (command[1] == '1') {
Reiko 0:88887cfb2b04 148 pwm1.pulsewidth_us(268);
Reiko 0:88887cfb2b04 149 } else*/ {
Reiko 0:88887cfb2b04 150 m3.pulsewidth_us(atoi(buffer + 2));
Reiko 0:88887cfb2b04 151 }
Reiko 0:88887cfb2b04 152 //pwm1.pulsewidth_us((int) atoi(command+1));
Reiko 0:88887cfb2b04 153 //serial.printf("sending %d\n", (int) atoi(command+1));
Reiko 0:88887cfb2b04 154 }
Reiko 0:88887cfb2b04 155
Reiko 0:88887cfb2b04 156 if (strncmp(cmd, "gs", 2) == 0) {
Reiko 0:88887cfb2b04 157 /*serial.printf("<gs:%d:%d:%d:%d>\n",
Reiko 0:88887cfb2b04 158 motors[0]->getSpeed(),
Reiko 0:88887cfb2b04 159 motors[1]->getSpeed(),
Reiko 0:88887cfb2b04 160 motors[2]->getSpeed(),
Reiko 0:88887cfb2b04 161 motors[3]->getSpeed()
Reiko 0:88887cfb2b04 162 );*/
Reiko 0:88887cfb2b04 163
Reiko 0:88887cfb2b04 164 serial.printf("<gs:%d:%d:%d>\n",
Reiko 0:88887cfb2b04 165 motors[0]->getSpeed(),
Reiko 0:88887cfb2b04 166 motors[1]->getSpeed(),
Reiko 0:88887cfb2b04 167 motors[2]->getSpeed()
Reiko 0:88887cfb2b04 168 );
Reiko 0:88887cfb2b04 169 }
Reiko 0:88887cfb2b04 170
Reiko 0:88887cfb2b04 171 if (strncmp(cmd, "rf", 2) == 0) {
Reiko 0:88887cfb2b04 172 rfModule.send(buffer + 3);
Reiko 0:88887cfb2b04 173 }
Reiko 0:88887cfb2b04 174
Reiko 0:88887cfb2b04 175 if (strncmp(cmd, "r", 1) == 0) {
Reiko 0:88887cfb2b04 176 led1.setRed(!led1.getRed());
Reiko 0:88887cfb2b04 177 }
Reiko 0:88887cfb2b04 178
Reiko 0:88887cfb2b04 179 if (strncmp(cmd, "g", 1) == 0) {
Reiko 0:88887cfb2b04 180 led1.setGreen(!led1.getGreen());
Reiko 0:88887cfb2b04 181 }
Reiko 0:88887cfb2b04 182
Reiko 0:88887cfb2b04 183 if (strncmp(cmd, "b", 1) == 0) {
Reiko 0:88887cfb2b04 184 led1.setBlue(!led1.getBlue());
Reiko 0:88887cfb2b04 185 }
Reiko 0:88887cfb2b04 186
Reiko 0:88887cfb2b04 187 if (strncmp(cmd, "gb", 2) == 0) {
Reiko 0:88887cfb2b04 188 serial.printf("<ball:%d>\n", infrared.read());
Reiko 0:88887cfb2b04 189 }
Reiko 0:88887cfb2b04 190
Reiko 0:88887cfb2b04 191 if (strncmp(cmd, "fs", 1) == 0) {
Reiko 0:88887cfb2b04 192 failSafeEnabled = buffer[3] != '0';
Reiko 0:88887cfb2b04 193 }
Reiko 0:88887cfb2b04 194 }