Reiko Randoja / Mbed 2 deprecated ut_bbr_2018

Firmware for UT Robotex 2018 basketball robot

Dependencies:   mbed USBDevice

main.cpp@4:81cb68f1bcbd, 2019-11-11 (annotated)

Committer:
Reiko
Date:
Mon Nov 11 19:19:43 2019 +0000
Revision:
4:81cb68f1bcbd
Parent:
3:2f12dac1bcdf 
Change default thrower (motor 3) pulsewidth_us from 800 to 100

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 2:7affec1c81cb 7 #define BUF_SIZE 32
Reiko 2:7affec1c81cb 8
Reiko 0:ef6268629f0b 9 USBSerial serial;
Reiko 0:ef6268629f0b 10
Reiko 0:ef6268629f0b 11 Serial pc(USBTX, USBRX);
Reiko 0:ef6268629f0b 12
Reiko 0:ef6268629f0b 13 DigitalOut led1(LED_1);
Reiko 0:ef6268629f0b 14 DigitalOut led2(LED_2);
Reiko 0:ef6268629f0b 15 DigitalOut led3(LED_3);
Reiko 0:ef6268629f0b 16
Reiko 0:ef6268629f0b 17 static const int NUMBER_OF_MOTORS = 3;
Reiko 0:ef6268629f0b 18
Reiko 0:ef6268629f0b 19 PwmOut m3(M3_PWM);
Reiko 0:ef6268629f0b 20
Reiko 0:ef6268629f0b 21 Motor motor0(M0_PWM, M0_DIR1, M0_DIR2, M0_ENCA, M0_ENCB);
Reiko 0:ef6268629f0b 22 Motor motor1(M1_PWM, M1_DIR1, M1_DIR2, M1_ENCA, M1_ENCB);
Reiko 0:ef6268629f0b 23 Motor motor2(M2_PWM, M2_DIR1, M2_DIR2, M2_ENCA, M2_ENCB);
Reiko 0:ef6268629f0b 24 //Motor motor3(M3_PWM, M3_DIR1, M3_DIR2, M3_ENCA, M3_ENCB);
Reiko 0:ef6268629f0b 25
Reiko 0:ef6268629f0b 26 Motor * motors[NUMBER_OF_MOTORS] = {
Reiko 0:ef6268629f0b 27 &motor0, &motor1, &motor2
Reiko 0:ef6268629f0b 28 };
Reiko 0:ef6268629f0b 29
Reiko 4:81cb68f1bcbd 30 DigitalOut servo(ISO_PWM5);
Reiko 4:81cb68f1bcbd 31
Reiko 0:ef6268629f0b 32 RFManager rfModule(COM1_TX, COM1_RX);
Reiko 0:ef6268629f0b 33
Reiko 0:ef6268629f0b 34 void parseCommand(char *command);
Reiko 0:ef6268629f0b 35
Reiko 0:ef6268629f0b 36 Ticker pidTicker;
Reiko 0:ef6268629f0b 37 unsigned int pidTickerCount = 0;
Reiko 0:ef6268629f0b 38 static const float PID_FREQ = 60;
Reiko 0:ef6268629f0b 39
Reiko 2:7affec1c81cb 40 char buf[BUF_SIZE];
Reiko 0:ef6268629f0b 41 int serialCount = 0;
Reiko 0:ef6268629f0b 42 bool serialData = false;
Reiko 0:ef6268629f0b 43
Reiko 0:ef6268629f0b 44 bool failSafeEnabled = true;
Reiko 0:ef6268629f0b 45 int failSafeCount = 0;
Reiko 0:ef6268629f0b 46 int failSafeLimit = 60;
Reiko 0:ef6268629f0b 47
Reiko 1:a286bf92d291 48 Ticker softPWMTicker;
Reiko 1:a286bf92d291 49 Timeout softPWMTimeout;
Reiko 1:a286bf92d291 50 int currentServoPosition = 0;
Reiko 1:a286bf92d291 51
Reiko 0:ef6268629f0b 52 void pidTick() {
Reiko 0:ef6268629f0b 53 for (int i = 0; i < NUMBER_OF_MOTORS; i++) {
Reiko 0:ef6268629f0b 54 motors[i]->pid();
Reiko 0:ef6268629f0b 55 }
Reiko 0:ef6268629f0b 56
Reiko 0:ef6268629f0b 57 if (pidTickerCount++ % 60 == 0) {
Reiko 0:ef6268629f0b 58 led1 = !led1;
Reiko 0:ef6268629f0b 59 }
Reiko 0:ef6268629f0b 60
Reiko 0:ef6268629f0b 61 failSafeCount++;
Reiko 0:ef6268629f0b 62
Reiko 0:ef6268629f0b 63 if (failSafeCount == failSafeLimit) {
Reiko 0:ef6268629f0b 64 failSafeCount = 0;
Reiko 0:ef6268629f0b 65
Reiko 0:ef6268629f0b 66 if (failSafeEnabled) {
Reiko 0:ef6268629f0b 67 for (int i = 0; i < NUMBER_OF_MOTORS; ++i) {
Reiko 0:ef6268629f0b 68 motors[i]->setSpeed(0);
Reiko 0:ef6268629f0b 69 }
Reiko 0:ef6268629f0b 70
Reiko 4:81cb68f1bcbd 71 m3.pulsewidth_us(100);
Reiko 0:ef6268629f0b 72 }
Reiko 0:ef6268629f0b 73 }
Reiko 0:ef6268629f0b 74 }
Reiko 0:ef6268629f0b 75
Reiko 1:a286bf92d291 76 void softPWMEndPulse() {
Reiko 1:a286bf92d291 77 servo = 0;
Reiko 1:a286bf92d291 78 }
Reiko 1:a286bf92d291 79
Reiko 1:a286bf92d291 80 void softPWMTick() {
Reiko 1:a286bf92d291 81 if(currentServoPosition <= 0) {
Reiko 1:a286bf92d291 82 servo = 0 ;
Reiko 1:a286bf92d291 83 } else {
Reiko 1:a286bf92d291 84 servo = 1;
Reiko 1:a286bf92d291 85 softPWMTimeout.attach_us(softPWMEndPulse, currentServoPosition);
Reiko 1:a286bf92d291 86 }
Reiko 1:a286bf92d291 87 }
Reiko 1:a286bf92d291 88
Reiko 0:ef6268629f0b 89 int main() {
Reiko 4:81cb68f1bcbd 90 pc.baud(115200);
Reiko 4:81cb68f1bcbd 91
Reiko 0:ef6268629f0b 92 pidTicker.attach(pidTick, 1 / PID_FREQ);
Reiko 0:ef6268629f0b 93
Reiko 4:81cb68f1bcbd 94 m3.pulsewidth_us(100);
Reiko 1:a286bf92d291 95 //servo.pulsewidth_us(0);
Reiko 1:a286bf92d291 96
Reiko 1:a286bf92d291 97 // TGY-180D (KC2462 controller) has problems with higher frequency PWM.
Reiko 1:a286bf92d291 98 // 50Hz works, but would like to use higher frequency for motors.
Reiko 4:81cb68f1bcbd 99 // Software PWM seems to be good enough.
Reiko 1:a286bf92d291 100 softPWMTicker.attach_us(softPWMTick, 20000);
Reiko 0:ef6268629f0b 101
Reiko 0:ef6268629f0b 102 while (1) {
Reiko 0:ef6268629f0b 103 if (rfModule.readable()) {
Reiko 0:ef6268629f0b 104 serial.printf("<ref:%s>\n", rfModule.read());
Reiko 0:ef6268629f0b 105 }
Reiko 0:ef6268629f0b 106
Reiko 0:ef6268629f0b 107 rfModule.update();
Reiko 4:81cb68f1bcbd 108
Reiko 2:7affec1c81cb 109 while (serial.readable()) {
Reiko 2:7affec1c81cb 110 char c = serial.getc();
Reiko 2:7affec1c81cb 111
Reiko 2:7affec1c81cb 112 buf[serialCount] = c;
Reiko 0:ef6268629f0b 113
Reiko 2:7affec1c81cb 114 if (c == '\n') {
Reiko 0:ef6268629f0b 115 parseCommand(buf);
Reiko 0:ef6268629f0b 116 serialCount = 0;
Reiko 2:7affec1c81cb 117 memset(buf, 0, BUF_SIZE);
Reiko 0:ef6268629f0b 118 } else {
Reiko 0:ef6268629f0b 119 serialCount++;
Reiko 2:7affec1c81cb 120
Reiko 2:7affec1c81cb 121 if (serialCount == BUF_SIZE) {
Reiko 2:7affec1c81cb 122 serialCount = 0;
Reiko 2:7affec1c81cb 123 }
Reiko 0:ef6268629f0b 124 }
Reiko 0:ef6268629f0b 125 }
Reiko 0:ef6268629f0b 126 }
Reiko 0:ef6268629f0b 127 }
Reiko 0:ef6268629f0b 128
Reiko 0:ef6268629f0b 129 void parseCommand(char *buffer) {
Reiko 0:ef6268629f0b 130 failSafeCount = 0;
Reiko 0:ef6268629f0b 131
Reiko 0:ef6268629f0b 132 char *cmd = strtok(buffer, ":");
Reiko 0:ef6268629f0b 133
Reiko 0:ef6268629f0b 134 // buffer == "sd:14:16:10:30"
Reiko 0:ef6268629f0b 135 if (strncmp(cmd, "sd", 2) == 0) {
Reiko 0:ef6268629f0b 136 for (int i = 0; i < NUMBER_OF_MOTORS; ++i) {
Reiko 0:ef6268629f0b 137 motors[i]->setSpeed(atoi(strtok(NULL, ":")));
Reiko 0:ef6268629f0b 138 }
Reiko 0:ef6268629f0b 139
Reiko 0:ef6268629f0b 140 serial.printf("<gs:%d:%d:%d>\n",
Reiko 0:ef6268629f0b 141 motors[0]->getSpeed(),
Reiko 0:ef6268629f0b 142 motors[1]->getSpeed(),
Reiko 0:ef6268629f0b 143 motors[2]->getSpeed()
Reiko 0:ef6268629f0b 144 );
Reiko 1:a286bf92d291 145
Reiko 1:a286bf92d291 146 } else if (strncmp(cmd, "d", 1) == 0) {
Reiko 0:ef6268629f0b 147 int pulsewidth = atoi(buffer + 2);
Reiko 0:ef6268629f0b 148
Reiko 3:2f12dac1bcdf 149 /*if (pulsewidth < 800) {
Reiko 0:ef6268629f0b 150 pulsewidth = 800;
Reiko 0:ef6268629f0b 151 } else if (pulsewidth > 2100) {
Reiko 1:a286bf92d291 152 pulsewidth = 2100;
Reiko 3:2f12dac1bcdf 153 }*/
Reiko 0:ef6268629f0b 154
Reiko 1:a286bf92d291 155 m3.pulsewidth_us(pulsewidth);
Reiko 1:a286bf92d291 156
Reiko 1:a286bf92d291 157 } else if (strncmp(cmd, "sv", 2) == 0) {
Reiko 1:a286bf92d291 158 currentServoPosition = atoi(buffer + 3);
Reiko 1:a286bf92d291 159
Reiko 1:a286bf92d291 160 if (currentServoPosition < 700) {
Reiko 1:a286bf92d291 161 currentServoPosition = 0;
Reiko 1:a286bf92d291 162 } else if (currentServoPosition > 2300) {
Reiko 1:a286bf92d291 163 currentServoPosition = 2300;
Reiko 1:a286bf92d291 164 }
Reiko 1:a286bf92d291 165
Reiko 1:a286bf92d291 166 //servo.pulsewidth_us(currentServoPosition);
Reiko 1:a286bf92d291 167
Reiko 1:a286bf92d291 168 } else if (strncmp(cmd, "gs", 2) == 0) {
Reiko 0:ef6268629f0b 169 serial.printf("<gs:%d:%d:%d>\n",
Reiko 0:ef6268629f0b 170 motors[0]->getSpeed(),
Reiko 0:ef6268629f0b 171 motors[1]->getSpeed(),
Reiko 0:ef6268629f0b 172 motors[2]->getSpeed()
Reiko 0:ef6268629f0b 173 );
Reiko 1:a286bf92d291 174 } else if (strncmp(cmd, "rf", 2) == 0) {
Reiko 0:ef6268629f0b 175 rfModule.send(buffer + 3);
Reiko 1:a286bf92d291 176
Reiko 1:a286bf92d291 177 } else if (strncmp(cmd, "fs", 1) == 0) {
Reiko 0:ef6268629f0b 178 failSafeEnabled = buffer[3] != '0';
Reiko 0:ef6268629f0b 179 }
Reiko 0:ef6268629f0b 180 }