Reiko Randoja
/
ut_bbr
Basketball robot mainboard firmware
Diff: main.cpp
- Revision:
- 0:88887cfb2b04
diff -r 000000000000 -r 88887cfb2b04 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Mon Sep 10 15:24:08 2018 +0000 @@ -0,0 +1,194 @@ +#include "mbed.h" +#include "pins.h" +#include "motor.h" +#include "RGBLed.hpp" +#include "USBSerial.h" +#include "RFManager.h" + +USBSerial serial; + +Serial pc(USBTX, USBRX); + +RGBLed led1(LED1R, LED1G, LED1B); +RGBLed led2(LED2R, LED2G, LED2B); + +DigitalIn infrared(ADC0); + +Timeout kicker; + +static const int NUMBER_OF_MOTORS = 3; + +Motor motor0(&pc, M0_PWM, M0_DIR1, M0_DIR2, M0_FAULT, M0_ENCA, M0_ENCB); +Motor motor1(&pc, M1_PWM, M1_DIR1, M1_DIR2, M1_FAULT, M1_ENCA, M1_ENCB); +Motor motor2(&pc, M2_PWM, M2_DIR1, M2_DIR2, M2_FAULT, M2_ENCA, M2_ENCB); +//Motor motor3(&pc, M3_PWM, M3_DIR1, M3_DIR2, M3_FAULT, M3_ENCA, M3_ENCB); + +Motor * motors[NUMBER_OF_MOTORS] = { + &motor0, &motor1, &motor2/*, &motor3*/ +}; + +PwmOut m0(M0_PWM); +PwmOut m1(M1_PWM); +PwmOut m2(M2_PWM); +PwmOut m3(M3_PWM); +PwmOut pwm0(PWM0); +PwmOut pwm1(PWM1); + +RFManager rfModule(COMTX, COMRX); + +void serialInterrupt(); +void parseCommand(char *command); + +Ticker pidTicker; +int pidTickerCount = 0; +static const float PID_FREQ = 60; + +char buf[32]; +int serialCount = 0; +bool serialData = false; + +bool failSafeEnabled = true; +int ticksSinceCommand = 0; + +void pidTick() { + for (int i = 0; i < NUMBER_OF_MOTORS; i++) { + motors[i]->pidTick(); + } + + if (pidTickerCount++ % 25 == 0) { + led1.setBlue(!led1.getBlue()); + } + + // Fail-safe + if (failSafeEnabled) { + ticksSinceCommand++; + } + + if (ticksSinceCommand == 60) { + for (int i = 0; i < NUMBER_OF_MOTORS; ++i) { + motors[i]->setSpeed(0); + } + + m3.pulsewidth_us(100); + } +} + +int main() { + pidTicker.attach(pidTick, 1/PID_FREQ); + //serial.attach(&serialInterrupt); + + // Ball detector status + int infraredStatus = -1; + + // Dribbler motor + m3.period_us(20000); + m3.pulsewidth_us(100); + + while (1) { + if (rfModule.readable()) { + serial.printf("<ref:%s>\n", rfModule.read()); + } + + rfModule.update(); + + if (serial.readable()) { + buf[serialCount] = serial.getc(); + //serial.putc(buf[serialCount]); + + if (buf[serialCount] == '\n') { + parseCommand(buf); + serialCount = 0; + memset(buf, 0, 32); + } else { + serialCount++; + } + } + + /// INFRARED DETECTOR + int newInfraredStatus = infrared.read(); + + if (newInfraredStatus != infraredStatus) { + infraredStatus = newInfraredStatus; + serial.printf("<ball:%d>\n", newInfraredStatus); + led2.setGreen(infraredStatus); + } + } +} + +void parseCommand(char *buffer) { + ticksSinceCommand = 0; + + char *cmd = strtok(buffer, ":"); + + // buffer == "sd:14:16:10:30" + if (strncmp(cmd, "sd", 2) == 0) { + for (int i = 0; i < NUMBER_OF_MOTORS; ++i) { + motors[i]->setSpeed((int16_t) atoi(strtok(NULL, ":"))); + } + + /*serial.printf("<gs:%d:%d:%d:%d>\n", + motors[0]->getSpeed(), + motors[1]->getSpeed(), + motors[2]->getSpeed(), + motors[3]->getSpeed() + );*/ + + serial.printf("<gs:%d:%d:%d>\n", + motors[0]->getSpeed(), + motors[1]->getSpeed(), + motors[2]->getSpeed() + ); + } + + if (strncmp(cmd, "d", 1) == 0) { + /* + if (command[1] == '0') { + pwm1.pulsewidth_us(100); + } else if (command[1] == '1') { + pwm1.pulsewidth_us(268); + } else*/ { + m3.pulsewidth_us(atoi(buffer + 2)); + } + //pwm1.pulsewidth_us((int) atoi(command+1)); + //serial.printf("sending %d\n", (int) atoi(command+1)); + } + + if (strncmp(cmd, "gs", 2) == 0) { + /*serial.printf("<gs:%d:%d:%d:%d>\n", + motors[0]->getSpeed(), + motors[1]->getSpeed(), + motors[2]->getSpeed(), + motors[3]->getSpeed() + );*/ + + serial.printf("<gs:%d:%d:%d>\n", + motors[0]->getSpeed(), + motors[1]->getSpeed(), + motors[2]->getSpeed() + ); + } + + if (strncmp(cmd, "rf", 2) == 0) { + rfModule.send(buffer + 3); + } + + if (strncmp(cmd, "r", 1) == 0) { + led1.setRed(!led1.getRed()); + } + + if (strncmp(cmd, "g", 1) == 0) { + led1.setGreen(!led1.getGreen()); + } + + if (strncmp(cmd, "b", 1) == 0) { + led1.setBlue(!led1.getBlue()); + } + + if (strncmp(cmd, "gb", 2) == 0) { + serial.printf("<ball:%d>\n", infrared.read()); + } + + if (strncmp(cmd, "fs", 1) == 0) { + failSafeEnabled = buffer[3] != '0'; + } +}