Reiko Randoja / Mbed 2 deprecated ut_bbr_2018

Firmware for UT Robotex 2018 basketball robot

Dependencies:   mbed USBDevice

main.cpp@0:ef6268629f0b, 2018-09-28 (annotated)

Committer:
Reiko
Date:
Fri Sep 28 10:46:57 2018 +0000
Revision:
0:ef6268629f0b
Child:
1:a286bf92d291
Firmware for UT Robotex 2018 basketball robot

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Reiko 0:ef6268629f0b 1 #include "mbed.h"
Reiko 0:ef6268629f0b 2 #include "pins.h"
Reiko 0:ef6268629f0b 3 #include "motor.h"
Reiko 0:ef6268629f0b 4 #include "USBSerial.h"
Reiko 0:ef6268629f0b 5 #include "RFManager.h"
Reiko 0:ef6268629f0b 6
Reiko 0:ef6268629f0b 7 USBSerial serial;
Reiko 0:ef6268629f0b 8
Reiko 0:ef6268629f0b 9 Serial pc(USBTX, USBRX);
Reiko 0:ef6268629f0b 10
Reiko 0:ef6268629f0b 11 DigitalOut led1(LED_1);
Reiko 0:ef6268629f0b 12 DigitalOut led2(LED_2);
Reiko 0:ef6268629f0b 13 DigitalOut led3(LED_3);
Reiko 0:ef6268629f0b 14
Reiko 0:ef6268629f0b 15 static const int NUMBER_OF_MOTORS = 3;
Reiko 0:ef6268629f0b 16
Reiko 0:ef6268629f0b 17 PwmOut m3(M3_PWM);
Reiko 0:ef6268629f0b 18
Reiko 0:ef6268629f0b 19 Motor motor0(M0_PWM, M0_DIR1, M0_DIR2, M0_ENCA, M0_ENCB);
Reiko 0:ef6268629f0b 20 Motor motor1(M1_PWM, M1_DIR1, M1_DIR2, M1_ENCA, M1_ENCB);
Reiko 0:ef6268629f0b 21 Motor motor2(M2_PWM, M2_DIR1, M2_DIR2, M2_ENCA, M2_ENCB);
Reiko 0:ef6268629f0b 22 //Motor motor3(M3_PWM, M3_DIR1, M3_DIR2, M3_ENCA, M3_ENCB);
Reiko 0:ef6268629f0b 23
Reiko 0:ef6268629f0b 24 Motor * motors[NUMBER_OF_MOTORS] = {
Reiko 0:ef6268629f0b 25 &motor0, &motor1, &motor2
Reiko 0:ef6268629f0b 26 };
Reiko 0:ef6268629f0b 27
Reiko 0:ef6268629f0b 28 RFManager rfModule(COM1_TX, COM1_RX);
Reiko 0:ef6268629f0b 29
Reiko 0:ef6268629f0b 30 void serialInterrupt();
Reiko 0:ef6268629f0b 31 void parseCommand(char *command);
Reiko 0:ef6268629f0b 32
Reiko 0:ef6268629f0b 33 Ticker pidTicker;
Reiko 0:ef6268629f0b 34 unsigned int pidTickerCount = 0;
Reiko 0:ef6268629f0b 35 static const float PID_FREQ = 60;
Reiko 0:ef6268629f0b 36
Reiko 0:ef6268629f0b 37 char buf[32];
Reiko 0:ef6268629f0b 38 int serialCount = 0;
Reiko 0:ef6268629f0b 39 bool serialData = false;
Reiko 0:ef6268629f0b 40
Reiko 0:ef6268629f0b 41 bool failSafeEnabled = true;
Reiko 0:ef6268629f0b 42 int failSafeCount = 0;
Reiko 0:ef6268629f0b 43 int failSafeLimit = 60;
Reiko 0:ef6268629f0b 44
Reiko 0:ef6268629f0b 45 void pidTick() {
Reiko 0:ef6268629f0b 46 for (int i = 0; i < NUMBER_OF_MOTORS; i++) {
Reiko 0:ef6268629f0b 47 motors[i]->pid();
Reiko 0:ef6268629f0b 48 }
Reiko 0:ef6268629f0b 49
Reiko 0:ef6268629f0b 50 if (pidTickerCount++ % 60 == 0) {
Reiko 0:ef6268629f0b 51 led1 = !led1;
Reiko 0:ef6268629f0b 52 }
Reiko 0:ef6268629f0b 53
Reiko 0:ef6268629f0b 54 failSafeCount++;
Reiko 0:ef6268629f0b 55
Reiko 0:ef6268629f0b 56 if (failSafeCount == failSafeLimit) {
Reiko 0:ef6268629f0b 57 failSafeCount = 0;
Reiko 0:ef6268629f0b 58
Reiko 0:ef6268629f0b 59 if (failSafeEnabled) {
Reiko 0:ef6268629f0b 60 for (int i = 0; i < NUMBER_OF_MOTORS; ++i) {
Reiko 0:ef6268629f0b 61 motors[i]->setSpeed(0);
Reiko 0:ef6268629f0b 62 }
Reiko 0:ef6268629f0b 63
Reiko 0:ef6268629f0b 64 m3.pulsewidth_us(800);
Reiko 0:ef6268629f0b 65 }
Reiko 0:ef6268629f0b 66 }
Reiko 0:ef6268629f0b 67 }
Reiko 0:ef6268629f0b 68
Reiko 0:ef6268629f0b 69 int main() {
Reiko 0:ef6268629f0b 70 pidTicker.attach(pidTick, 1 / PID_FREQ);
Reiko 0:ef6268629f0b 71 //serial.attach(&serialInterrupt);
Reiko 0:ef6268629f0b 72
Reiko 0:ef6268629f0b 73 // Dribbler motor
Reiko 0:ef6268629f0b 74 //m3.period_us(4000);
Reiko 0:ef6268629f0b 75 m3.pulsewidth_us(800);
Reiko 0:ef6268629f0b 76
Reiko 0:ef6268629f0b 77 while (1) {
Reiko 0:ef6268629f0b 78 if (rfModule.readable()) {
Reiko 0:ef6268629f0b 79 serial.printf("<ref:%s>\n", rfModule.read());
Reiko 0:ef6268629f0b 80 }
Reiko 0:ef6268629f0b 81
Reiko 0:ef6268629f0b 82 rfModule.update();
Reiko 0:ef6268629f0b 83
Reiko 0:ef6268629f0b 84 if (serial.readable()) {
Reiko 0:ef6268629f0b 85 buf[serialCount] = serial.getc();
Reiko 0:ef6268629f0b 86
Reiko 0:ef6268629f0b 87 if (buf[serialCount] == '\n') {
Reiko 0:ef6268629f0b 88 parseCommand(buf);
Reiko 0:ef6268629f0b 89 serialCount = 0;
Reiko 0:ef6268629f0b 90 memset(buf, 0, 32);
Reiko 0:ef6268629f0b 91 } else {
Reiko 0:ef6268629f0b 92 serialCount++;
Reiko 0:ef6268629f0b 93 }
Reiko 0:ef6268629f0b 94 }
Reiko 0:ef6268629f0b 95 }
Reiko 0:ef6268629f0b 96 }
Reiko 0:ef6268629f0b 97
Reiko 0:ef6268629f0b 98 void parseCommand(char *buffer) {
Reiko 0:ef6268629f0b 99 failSafeCount = 0;
Reiko 0:ef6268629f0b 100
Reiko 0:ef6268629f0b 101 char *cmd = strtok(buffer, ":");
Reiko 0:ef6268629f0b 102
Reiko 0:ef6268629f0b 103 // buffer == "sd:14:16:10:30"
Reiko 0:ef6268629f0b 104 if (strncmp(cmd, "sd", 2) == 0) {
Reiko 0:ef6268629f0b 105 for (int i = 0; i < NUMBER_OF_MOTORS; ++i) {
Reiko 0:ef6268629f0b 106 motors[i]->setSpeed(atoi(strtok(NULL, ":")));
Reiko 0:ef6268629f0b 107 }
Reiko 0:ef6268629f0b 108
Reiko 0:ef6268629f0b 109 serial.printf("<gs:%d:%d:%d>\n",
Reiko 0:ef6268629f0b 110 motors[0]->getSpeed(),
Reiko 0:ef6268629f0b 111 motors[1]->getSpeed(),
Reiko 0:ef6268629f0b 112 motors[2]->getSpeed()
Reiko 0:ef6268629f0b 113 );
Reiko 0:ef6268629f0b 114 }
Reiko 0:ef6268629f0b 115
Reiko 0:ef6268629f0b 116 if (strncmp(cmd, "d", 1) == 0) {
Reiko 0:ef6268629f0b 117 int pulsewidth = atoi(buffer + 2);
Reiko 0:ef6268629f0b 118
Reiko 0:ef6268629f0b 119 if (pulsewidth < 800) {
Reiko 0:ef6268629f0b 120 pulsewidth = 800;
Reiko 0:ef6268629f0b 121 } else if (pulsewidth > 2100) {
Reiko 0:ef6268629f0b 122 pulsewidth = 2100;
Reiko 0:ef6268629f0b 123 }
Reiko 0:ef6268629f0b 124
Reiko 0:ef6268629f0b 125 m3.pulsewidth_us(pulsewidth);
Reiko 0:ef6268629f0b 126 }
Reiko 0:ef6268629f0b 127
Reiko 0:ef6268629f0b 128 if (strncmp(cmd, "gs", 2) == 0) {
Reiko 0:ef6268629f0b 129 serial.printf("<gs:%d:%d:%d>\n",
Reiko 0:ef6268629f0b 130 motors[0]->getSpeed(),
Reiko 0:ef6268629f0b 131 motors[1]->getSpeed(),
Reiko 0:ef6268629f0b 132 motors[2]->getSpeed()
Reiko 0:ef6268629f0b 133 );
Reiko 0:ef6268629f0b 134 }
Reiko 0:ef6268629f0b 135
Reiko 0:ef6268629f0b 136 if (strncmp(cmd, "rf", 2) == 0) {
Reiko 0:ef6268629f0b 137 rfModule.send(buffer + 3);
Reiko 0:ef6268629f0b 138 }
Reiko 0:ef6268629f0b 139
Reiko 0:ef6268629f0b 140 if (strncmp(cmd, "fs", 1) == 0) {
Reiko 0:ef6268629f0b 141 failSafeEnabled = buffer[3] != '0';
Reiko 0:ef6268629f0b 142 }
Reiko 0:ef6268629f0b 143 }