Reiko Randoja / Mbed 2 deprecated ut_bbr_2018

Firmware for UT Robotex 2018 basketball robot

Dependencies:   mbed USBDevice

main.cpp@3:2f12dac1bcdf, 2019-09-17 (annotated)

Committer:
Reiko
Date:
Tue Sep 17 05:12:24 2019 +0000
Revision:
3:2f12dac1bcdf
Parent:
2:7affec1c81cb
Child:
4:81cb68f1bcbd
Remove thrower motor pulsewidth limits

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 1:a286bf92d291 20 DigitalOut servo(ISO_PWM5);
Reiko 0:ef6268629f0b 21
Reiko 0:ef6268629f0b 22 Motor motor0(M0_PWM, M0_DIR1, M0_DIR2, M0_ENCA, M0_ENCB);
Reiko 0:ef6268629f0b 23 Motor motor1(M1_PWM, M1_DIR1, M1_DIR2, M1_ENCA, M1_ENCB);
Reiko 0:ef6268629f0b 24 Motor motor2(M2_PWM, M2_DIR1, M2_DIR2, M2_ENCA, M2_ENCB);
Reiko 0:ef6268629f0b 25 //Motor motor3(M3_PWM, M3_DIR1, M3_DIR2, M3_ENCA, M3_ENCB);
Reiko 0:ef6268629f0b 26
Reiko 0:ef6268629f0b 27 Motor * motors[NUMBER_OF_MOTORS] = {
Reiko 0:ef6268629f0b 28 &motor0, &motor1, &motor2
Reiko 0:ef6268629f0b 29 };
Reiko 0:ef6268629f0b 30
Reiko 0:ef6268629f0b 31 RFManager rfModule(COM1_TX, COM1_RX);
Reiko 0:ef6268629f0b 32
Reiko 0:ef6268629f0b 33 void serialInterrupt();
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 0:ef6268629f0b 71 m3.pulsewidth_us(800);
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 0:ef6268629f0b 90 pidTicker.attach(pidTick, 1 / PID_FREQ);
Reiko 0:ef6268629f0b 91 //serial.attach(&serialInterrupt);
Reiko 0:ef6268629f0b 92
Reiko 1:a286bf92d291 93 //servo.period_us(20000);
Reiko 1:a286bf92d291 94
Reiko 0:ef6268629f0b 95 m3.pulsewidth_us(800);
Reiko 1:a286bf92d291 96 //servo.pulsewidth_us(0);
Reiko 1:a286bf92d291 97
Reiko 1:a286bf92d291 98 // TGY-180D (KC2462 controller) has problems with higher frequency PWM.
Reiko 1:a286bf92d291 99 // 50Hz works, but would like to use higher frequency for motors.
Reiko 1:a286bf92d291 100 // Software PWM seems to be good enough.
Reiko 1:a286bf92d291 101
Reiko 1:a286bf92d291 102 softPWMTicker.attach_us(softPWMTick, 20000);
Reiko 0:ef6268629f0b 103
Reiko 0:ef6268629f0b 104 while (1) {
Reiko 0:ef6268629f0b 105 if (rfModule.readable()) {
Reiko 0:ef6268629f0b 106 serial.printf("<ref:%s>\n", rfModule.read());
Reiko 0:ef6268629f0b 107 }
Reiko 0:ef6268629f0b 108
Reiko 0:ef6268629f0b 109 rfModule.update();
Reiko 2:7affec1c81cb 110
Reiko 2:7affec1c81cb 111 while (serial.readable()) {
Reiko 2:7affec1c81cb 112 char c = serial.getc();
Reiko 2:7affec1c81cb 113
Reiko 2:7affec1c81cb 114 buf[serialCount] = c;
Reiko 0:ef6268629f0b 115
Reiko 2:7affec1c81cb 116 if (c == '\n') {
Reiko 0:ef6268629f0b 117 parseCommand(buf);
Reiko 0:ef6268629f0b 118 serialCount = 0;
Reiko 2:7affec1c81cb 119 memset(buf, 0, BUF_SIZE);
Reiko 0:ef6268629f0b 120 } else {
Reiko 0:ef6268629f0b 121 serialCount++;
Reiko 2:7affec1c81cb 122
Reiko 2:7affec1c81cb 123 if (serialCount == BUF_SIZE) {
Reiko 2:7affec1c81cb 124 serialCount = 0;
Reiko 2:7affec1c81cb 125 }
Reiko 0:ef6268629f0b 126 }
Reiko 0:ef6268629f0b 127 }
Reiko 0:ef6268629f0b 128 }
Reiko 0:ef6268629f0b 129 }
Reiko 0:ef6268629f0b 130
Reiko 0:ef6268629f0b 131 void parseCommand(char *buffer) {
Reiko 0:ef6268629f0b 132 failSafeCount = 0;
Reiko 0:ef6268629f0b 133
Reiko 0:ef6268629f0b 134 char *cmd = strtok(buffer, ":");
Reiko 0:ef6268629f0b 135
Reiko 0:ef6268629f0b 136 // buffer == "sd:14:16:10:30"
Reiko 0:ef6268629f0b 137 if (strncmp(cmd, "sd", 2) == 0) {
Reiko 0:ef6268629f0b 138 for (int i = 0; i < NUMBER_OF_MOTORS; ++i) {
Reiko 0:ef6268629f0b 139 motors[i]->setSpeed(atoi(strtok(NULL, ":")));
Reiko 0:ef6268629f0b 140 }
Reiko 0:ef6268629f0b 141
Reiko 0:ef6268629f0b 142 serial.printf("<gs:%d:%d:%d>\n",
Reiko 0:ef6268629f0b 143 motors[0]->getSpeed(),
Reiko 0:ef6268629f0b 144 motors[1]->getSpeed(),
Reiko 0:ef6268629f0b 145 motors[2]->getSpeed()
Reiko 0:ef6268629f0b 146 );
Reiko 1:a286bf92d291 147
Reiko 1:a286bf92d291 148 } else if (strncmp(cmd, "d", 1) == 0) {
Reiko 0:ef6268629f0b 149 int pulsewidth = atoi(buffer + 2);
Reiko 0:ef6268629f0b 150
Reiko 3:2f12dac1bcdf 151 /*if (pulsewidth < 800) {
Reiko 0:ef6268629f0b 152 pulsewidth = 800;
Reiko 0:ef6268629f0b 153 } else if (pulsewidth > 2100) {
Reiko 1:a286bf92d291 154 pulsewidth = 2100;
Reiko 3:2f12dac1bcdf 155 }*/
Reiko 0:ef6268629f0b 156
Reiko 1:a286bf92d291 157 m3.pulsewidth_us(pulsewidth);
Reiko 1:a286bf92d291 158
Reiko 1:a286bf92d291 159 } else if (strncmp(cmd, "sv", 2) == 0) {
Reiko 1:a286bf92d291 160 currentServoPosition = atoi(buffer + 3);
Reiko 1:a286bf92d291 161
Reiko 1:a286bf92d291 162 if (currentServoPosition < 700) {
Reiko 1:a286bf92d291 163 currentServoPosition = 0;
Reiko 1:a286bf92d291 164 } else if (currentServoPosition > 2300) {
Reiko 1:a286bf92d291 165 currentServoPosition = 2300;
Reiko 1:a286bf92d291 166 }
Reiko 1:a286bf92d291 167
Reiko 1:a286bf92d291 168 //servo.pulsewidth_us(currentServoPosition);
Reiko 1:a286bf92d291 169
Reiko 1:a286bf92d291 170 } else if (strncmp(cmd, "gs", 2) == 0) {
Reiko 0:ef6268629f0b 171 serial.printf("<gs:%d:%d:%d>\n",
Reiko 0:ef6268629f0b 172 motors[0]->getSpeed(),
Reiko 0:ef6268629f0b 173 motors[1]->getSpeed(),
Reiko 0:ef6268629f0b 174 motors[2]->getSpeed()
Reiko 0:ef6268629f0b 175 );
Reiko 1:a286bf92d291 176 } else if (strncmp(cmd, "rf", 2) == 0) {
Reiko 0:ef6268629f0b 177 rfModule.send(buffer + 3);
Reiko 1:a286bf92d291 178
Reiko 1:a286bf92d291 179 } else if (strncmp(cmd, "fs", 1) == 0) {
Reiko 0:ef6268629f0b 180 failSafeEnabled = buffer[3] != '0';
Reiko 0:ef6268629f0b 181 }
Reiko 0:ef6268629f0b 182 }