Reiko Randoja / Mbed 2 deprecated Telliskivi2plus

Telliskivi 2+

Dependencies:   CoilGun HumanInterface EthernetInterface ExternalIn LedOut Motor2 PCA9555 QED mbed-rtos mbed PinDetect

main.cpp@7:a9e98f642a89, 2013-11-04 (annotated)

Committer:
mlaane
Date:
Mon Nov 04 23:57:09 2013 +0000
Revision:
7:a9e98f642a89
Parent:
6:5619aff36840 
Code cleanup - untested

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Reiko 0:22a7683646d1 1 #include "mbed.h"
Reiko 0:22a7683646d1 2 #include "EthernetInterface.h"
Reiko 0:22a7683646d1 3 //#include "ADXL345_I2C.h"
Reiko 0:22a7683646d1 4 //#include "L3G4200D.h"
Reiko 0:22a7683646d1 5 //#include "HMC5883L.h"
Reiko 0:22a7683646d1 6 #include "PCA9555.h"
Reiko 0:22a7683646d1 7 #include "motor.h"
Reiko 0:22a7683646d1 8 #include "qed.h"
Reiko 0:22a7683646d1 9 #include "ledout.h"
Reiko 0:22a7683646d1 10 #include "externalin.h"
Reiko 0:22a7683646d1 11 #include "coilgun.h"
Reiko 1:79ac4e293661 12 #include "HumanInterface.h"
Reiko 0:22a7683646d1 13
Reiko 0:22a7683646d1 14 #define SERVER_PORT 8042
Reiko 0:22a7683646d1 15
Reiko 0:22a7683646d1 16 Ticker sensorUpdate;
Reiko 0:22a7683646d1 17
Reiko 0:22a7683646d1 18 DigitalOut led1(LED1);
Reiko 5:b30229c6f32b 19 DigitalOut led3(LED3);
Reiko 5:b30229c6f32b 20 DigitalOut led4(LED4);
Reiko 0:22a7683646d1 21
Reiko 0:22a7683646d1 22 Serial pc(USBTX, USBRX); // tx, rx
Reiko 0:22a7683646d1 23 //ADXL345_I2C accelerometer(p9, p10);
Reiko 0:22a7683646d1 24 //L3G4200D gyro(p9, p10);
Reiko 0:22a7683646d1 25 //HMC5883L compass(p9, p10);
Reiko 0:22a7683646d1 26
Reiko 0:22a7683646d1 27 PCA9555 ioExt(p9, p10, p8, 0x40);
Reiko 0:22a7683646d1 28
Reiko 0:22a7683646d1 29 //int readings[3] = {0, 0, 0};
Reiko 0:22a7683646d1 30 //int gyroData[3] = {0, 0, 0};
Reiko 0:22a7683646d1 31 //int16_t compassData[3] = {0, 0, 0};
Reiko 0:22a7683646d1 32
Reiko 0:22a7683646d1 33
Reiko 0:22a7683646d1 34 volatile int update = 0;
mlaane 6:5619aff36840 35
Reiko 1:79ac4e293661 36 volatile int stallChanged = 0;
Reiko 1:79ac4e293661 37
Reiko 1:79ac4e293661 38 bool failSafeEnabled = true;
Reiko 1:79ac4e293661 39 int failSafeCount = 0;
Reiko 1:79ac4e293661 40 int failSafeLimit = 60;
Reiko 1:79ac4e293661 41
Reiko 0:22a7683646d1 42
Reiko 5:b30229c6f32b 43 void executeCommand(char *buffer);
Reiko 0:22a7683646d1 44
Reiko 0:22a7683646d1 45 /*
Reiko 0:22a7683646d1 46 void updateSensors() {
Reiko 0:22a7683646d1 47 if (led1 == 1.0) {
Reiko 0:22a7683646d1 48 led1 = 0;
Reiko 0:22a7683646d1 49 } else {
Reiko 0:22a7683646d1 50 led1 = 1.0;
Reiko 0:22a7683646d1 51 }
Reiko 0:22a7683646d1 52 accelerometer.getOutput(readings);
Reiko 0:22a7683646d1 53 pc.printf("<acc:%i:%i:%i>\n", (int16_t)readings[0], (int16_t)readings[1], (int16_t)readings[2]);
Reiko 0:22a7683646d1 54
Reiko 0:22a7683646d1 55 gyro.read(gyroData);
Reiko 0:22a7683646d1 56 pc.printf("<gyro:%i:%i:%i>\n", (int16_t)gyroData[0], (int16_t)gyroData[1], (int16_t)gyroData[2]);
Reiko 0:22a7683646d1 57
Reiko 0:22a7683646d1 58 compass.getXYZ(compassData);
Reiko 0:22a7683646d1 59 pc.printf("<comps:%i:%i:%i>\n", compassData[0], compassData[1], compassData[2]);
Reiko 0:22a7683646d1 60 }*/
Reiko 0:22a7683646d1 61
Reiko 1:79ac4e293661 62 //motor order: FL, FR, RL, RR, DRIBBLER
Reiko 1:79ac4e293661 63 Motor motor4(p25, &ioExt, 0, 1, p5, p6); //RR - M1
Reiko 1:79ac4e293661 64 Motor motor2(p24, &ioExt, 2, 3, p7, p11); //FR - M2
Reiko 1:79ac4e293661 65 Motor motor5(p23, &ioExt, 4, 5, p12, p13); //DRIBBLER - M3
Reiko 1:79ac4e293661 66 Motor motor1(p22, &ioExt, 6, 7, p14, p15); //FL - M4
Reiko 1:79ac4e293661 67 Motor motor3(p21, &ioExt, 9, 8, p16, p17); //RL - M5
Reiko 0:22a7683646d1 68
Reiko 5:b30229c6f32b 69 Coilgun coilgun(p20, p19, p18);
Reiko 0:22a7683646d1 70
Reiko 5:b30229c6f32b 71 HumanInterface humanInterface(&ioExt, 15, 14, 13, 12, 11, p29, p30, p26);
Reiko 1:79ac4e293661 72
Reiko 0:22a7683646d1 73 void updateTick() {
Reiko 0:22a7683646d1 74 //led3 = 1;
Reiko 0:22a7683646d1 75 motor1.pid();
Reiko 0:22a7683646d1 76 motor2.pid();
Reiko 0:22a7683646d1 77 motor3.pid();
Reiko 0:22a7683646d1 78 motor4.pid();
Reiko 0:22a7683646d1 79 motor5.pid();
Reiko 0:22a7683646d1 80 //led3 = 0;
Reiko 0:22a7683646d1 81 update = 1;
Reiko 0:22a7683646d1 82 }
Reiko 0:22a7683646d1 83
Reiko 1:79ac4e293661 84 void stallChangedCallback() {
Reiko 1:79ac4e293661 85 stallChanged = 1;
Reiko 1:79ac4e293661 86 }
Reiko 1:79ac4e293661 87
Reiko 0:22a7683646d1 88 UDPSocket server;
Reiko 0:22a7683646d1 89 Endpoint client;
Reiko 0:22a7683646d1 90 char buffer[256];
Reiko 0:22a7683646d1 91 char sendBuffer[256];
Reiko 0:22a7683646d1 92 int charCounter = 0;
Reiko 0:22a7683646d1 93
Reiko 0:22a7683646d1 94 //void readSerial() {
Reiko 0:22a7683646d1 95
Reiko 0:22a7683646d1 96 //int n = pc.readable();
Reiko 0:22a7683646d1 97 //if (pc.readable()) {
Reiko 0:22a7683646d1 98
Reiko 0:22a7683646d1 99 //pc.printf("Received packet from: %s\n", client.get_address());
Reiko 0:22a7683646d1 100 //pc.printf("n: %d\n", n);
Reiko 0:22a7683646d1 101 //pc.scanf("%s", &buffer);
Reiko 0:22a7683646d1 102 //pc.printf("%c\n", pc.getc());
Reiko 0:22a7683646d1 103 //buffer[charCounter] = pc.getc();
Reiko 0:22a7683646d1 104 /*printf("%c\n", buffer[charCounter]);
Reiko 0:22a7683646d1 105 //pc.printf("%s\n", buffer);
Reiko 0:22a7683646d1 106 if (buffer[charCounter] == '\n') {
Reiko 0:22a7683646d1 107 buffer[charCounter] = '\0';
Reiko 0:22a7683646d1 108 executeCommand(buffer);
Reiko 0:22a7683646d1 109 charCounter = 0;
Reiko 0:22a7683646d1 110 } else {
Reiko 0:22a7683646d1 111 charCounter++;
Reiko 0:22a7683646d1 112 }*/
Reiko 0:22a7683646d1 113 //}
Reiko 0:22a7683646d1 114
Reiko 0:22a7683646d1 115 //}
Reiko 0:22a7683646d1 116
Reiko 0:22a7683646d1 117 const char *byte_to_binary(int x) {
Reiko 0:22a7683646d1 118 static char b[9];
Reiko 0:22a7683646d1 119 b[0] = '\0';
Reiko 0:22a7683646d1 120
Reiko 0:22a7683646d1 121 int z;
Reiko 0:22a7683646d1 122 for (z = 32768; z > 0; z >>= 1) {
Reiko 0:22a7683646d1 123 strcat(b, ((x & z) == z) ? "1" : "0");
Reiko 0:22a7683646d1 124 }
Reiko 0:22a7683646d1 125
Reiko 0:22a7683646d1 126 return b;
Reiko 0:22a7683646d1 127 }
Reiko 0:22a7683646d1 128
Reiko 0:22a7683646d1 129 int main (void) {
Reiko 0:22a7683646d1 130 pc.baud(115200);
Reiko 0:22a7683646d1 131 EthernetInterface eth;
Reiko 0:22a7683646d1 132 eth.init("192.168.4.1", "255.255.255.0", "192.168.4.8");
Reiko 0:22a7683646d1 133 eth.connect();
Reiko 0:22a7683646d1 134 printf("IP Address is %s\n", eth.getIPAddress());
Reiko 0:22a7683646d1 135
Reiko 0:22a7683646d1 136 server.bind(SERVER_PORT);
Reiko 0:22a7683646d1 137
Reiko 0:22a7683646d1 138 //pc.printf("Starting ADXL345 test...\n");
Reiko 0:22a7683646d1 139 //wait(.001);
Reiko 0:22a7683646d1 140 //pc.printf("Device ID is: 0x%02x\n", accelerometer.getDeviceID());
Reiko 0:22a7683646d1 141 //wait(.001);
Reiko 0:22a7683646d1 142
Reiko 0:22a7683646d1 143 // These are here to test whether any of the initialization fails. It will print the failure
Reiko 0:22a7683646d1 144 /*if (accelerometer.setPowerControl(0x00)){
Reiko 0:22a7683646d1 145 pc.printf("didn't intitialize power control\n");
Reiko 0:22a7683646d1 146 return 0;
Reiko 0:22a7683646d1 147 }*/
Reiko 0:22a7683646d1 148 //Full resolution, +/-16g, 4mg/LSB.
Reiko 0:22a7683646d1 149 /*wait(.001);
Reiko 0:22a7683646d1 150
Reiko 0:22a7683646d1 151 if(accelerometer.setDataFormatControl(0x0B)){
Reiko 0:22a7683646d1 152 pc.printf("didn't set data format\n");
Reiko 0:22a7683646d1 153 return 0;
Reiko 0:22a7683646d1 154 }
Reiko 0:22a7683646d1 155
Reiko 0:22a7683646d1 156 wait(.001);*/
Reiko 0:22a7683646d1 157
Reiko 0:22a7683646d1 158 //3.2kHz data rate.
Reiko 0:22a7683646d1 159 /*if(accelerometer.setDataRate(ADXL345_3200HZ)){
Reiko 0:22a7683646d1 160 pc.printf("didn't set data rate\n");
Reiko 0:22a7683646d1 161 return 0;
Reiko 0:22a7683646d1 162 }
Reiko 0:22a7683646d1 163
Reiko 0:22a7683646d1 164 wait(.001);*/
Reiko 0:22a7683646d1 165
Reiko 0:22a7683646d1 166 //Measurement mode.
Reiko 0:22a7683646d1 167
Reiko 0:22a7683646d1 168 /*if (accelerometer.setPowerControl(MeasurementMode)) {
Reiko 0:22a7683646d1 169 pc.printf("didn't set the power control to measurement\n");
Reiko 0:22a7683646d1 170 return 0;
Reiko 0:22a7683646d1 171 }
Reiko 0:22a7683646d1 172 */
Reiko 0:22a7683646d1 173
Reiko 0:22a7683646d1 174 server.set_blocking(false, 1);
Reiko 0:22a7683646d1 175
Reiko 0:22a7683646d1 176 //int ioExtBefore = ioExt.read();
Reiko 0:22a7683646d1 177 //pc.printf("ioExt: %s\n", byte_to_binary(ioExt.read()));
Reiko 5:b30229c6f32b 178 //pc.printf("ioExt: %x\n", ioExt.read());
Reiko 5:b30229c6f32b 179 ioExt.setDirection(0x0400);
Reiko 0:22a7683646d1 180 ioExt.write(0x0155);
Reiko 0:22a7683646d1 181 //int ioExtAfter = ioExt.read();
Reiko 5:b30229c6f32b 182 //pc.printf("ioExt: %x\n", ioExt.read());
Reiko 0:22a7683646d1 183 //pc.printf("ioExt: %s\n", byte_to_binary(ioExt.read()));
Reiko 0:22a7683646d1 184
Reiko 1:79ac4e293661 185 motor1.stallChange(&stallChangedCallback);
Reiko 1:79ac4e293661 186 motor2.stallChange(&stallChangedCallback);
Reiko 1:79ac4e293661 187 motor3.stallChange(&stallChangedCallback);
Reiko 1:79ac4e293661 188 motor4.stallChange(&stallChangedCallback);
Reiko 1:79ac4e293661 189 motor5.stallChange(&stallChangedCallback);
Reiko 0:22a7683646d1 190
Reiko 0:22a7683646d1 191
Reiko 0:22a7683646d1 192 //int charCount = sprintf(sendBuffer, "<ready>");
Reiko 0:22a7683646d1 193 //server.sendTo(client, sendBuffer, charCount);
Reiko 0:22a7683646d1 194
Reiko 0:22a7683646d1 195 sensorUpdate.attach(&updateTick, 1.0 / 60.0);
Reiko 0:22a7683646d1 196
Reiko 0:22a7683646d1 197 led1 = 1;
Reiko 0:22a7683646d1 198
Reiko 0:22a7683646d1 199 while (1) {
Reiko 0:22a7683646d1 200
Reiko 0:22a7683646d1 201 if (update) {
Reiko 0:22a7683646d1 202 update = 0;
Reiko 0:22a7683646d1 203 ioExt.writePins();
Reiko 0:22a7683646d1 204
Reiko 1:79ac4e293661 205 failSafeCount++;
Reiko 1:79ac4e293661 206 if (failSafeCount == failSafeLimit) {
Reiko 1:79ac4e293661 207 failSafeCount = 0;
Reiko 1:79ac4e293661 208 if (failSafeEnabled) {
Reiko 1:79ac4e293661 209 motor1.setSpeed(0);
Reiko 1:79ac4e293661 210 motor2.setSpeed(0);
Reiko 1:79ac4e293661 211 motor3.setSpeed(0);
Reiko 1:79ac4e293661 212 motor4.setSpeed(0);
Reiko 1:79ac4e293661 213 motor5.setSpeed(0);
Reiko 1:79ac4e293661 214 coilgun.discharge();
Reiko 1:79ac4e293661 215 }
mlaane 2:a3e6eceed838 216 if (!coilgun.isCharged) {
Reiko 1:79ac4e293661 217 server.sendTo(client, "<discharged>", 12);
Reiko 1:79ac4e293661 218 }
Reiko 1:79ac4e293661 219 }
Reiko 1:79ac4e293661 220
Reiko 0:22a7683646d1 221 //led3 = 1;
Reiko 0:22a7683646d1 222 //updateSensors();
Reiko 0:22a7683646d1 223 //pc.printf("update");
Reiko 0:22a7683646d1 224 /*motor1.pid();
Reiko 0:22a7683646d1 225 motor2.pid();
Reiko 0:22a7683646d1 226 motor3.pid();
Reiko 0:22a7683646d1 227 motor4.pid();
Reiko 0:22a7683646d1 228 motor5.pid();*/
Reiko 0:22a7683646d1 229
Reiko 0:22a7683646d1 230 //led3 = 0;
Reiko 0:22a7683646d1 231 //int charCount = sprintf(sendBuffer, "<speeds:%d:%d:%d:%d:%d>", motor1.getSpeed(), motor2.getSpeed(), motor3.getSpeed(), motor4.getSpeed(), motor5.getSpeed());
Reiko 0:22a7683646d1 232 //server.sendTo(client, sendBuffer, charCount);
Reiko 0:22a7683646d1 233 //pc.printf("<speeds:%d:%d:%d:%d:%d>\n", motor1.getSpeed(), motor2.getSpeed(), motor3.getSpeed(), motor4.getSpeed(), motor5.getSpeed());
Reiko 0:22a7683646d1 234 //redLed.toggle();
Reiko 5:b30229c6f32b 235
Reiko 5:b30229c6f32b 236 ioExt.setDirection(0x0400);
Reiko 5:b30229c6f32b 237
Reiko 0:22a7683646d1 238 led1 = !led1;
Reiko 0:22a7683646d1 239 }
Reiko 0:22a7683646d1 240
Reiko 1:79ac4e293661 241 if (stallChanged) {
Reiko 1:79ac4e293661 242 stallChanged = 0;
Reiko 5:b30229c6f32b 243 int charCount = sprintf(sendBuffer, "<stall:%d:%d:%d:%d:%d>",
Reiko 1:79ac4e293661 244 motor1.getStallLevel(),
Reiko 1:79ac4e293661 245 motor2.getStallLevel(),
Reiko 1:79ac4e293661 246 motor3.getStallLevel(),
Reiko 1:79ac4e293661 247 motor4.getStallLevel(),
Reiko 1:79ac4e293661 248 motor5.getStallLevel());
Reiko 1:79ac4e293661 249 server.sendTo(client, sendBuffer, charCount);
Reiko 5:b30229c6f32b 250 }
Reiko 1:79ac4e293661 251
Reiko 1:79ac4e293661 252
mlaane 6:5619aff36840 253 switch (humanInterface.getBallState()){
mlaane 6:5619aff36840 254 case -1:
mlaane 6:5619aff36840 255 //Ball lost
mlaane 6:5619aff36840 256 server.sendTo(client, "<ball:0>", 8);
mlaane 6:5619aff36840 257 break;
mlaane 6:5619aff36840 258 case 0:
mlaane 6:5619aff36840 259 //Nothing has changed..
mlaane 6:5619aff36840 260 break;
mlaane 6:5619aff36840 261 case 1:
mlaane 6:5619aff36840 262 //Ball found
mlaane 6:5619aff36840 263 server.sendTo(client, "<ball:1>", 8);
mlaane 6:5619aff36840 264 break;
mlaane 6:5619aff36840 265 default:
mlaane 6:5619aff36840 266 break;
Reiko 1:79ac4e293661 267 }
Reiko 1:79ac4e293661 268
mlaane 6:5619aff36840 269 if (humanInterface.isGoalChange()) {
Reiko 1:79ac4e293661 270 server.sendTo(client, "<toggle-side>", 13);
Reiko 1:79ac4e293661 271 }
Reiko 1:79ac4e293661 272
mlaane 6:5619aff36840 273 if (humanInterface.isStart()) {
Reiko 1:79ac4e293661 274 server.sendTo(client, "<toggle-go>", 11);
Reiko 0:22a7683646d1 275 }
Reiko 0:22a7683646d1 276
Reiko 0:22a7683646d1 277 //printf("\nWait for packet...\n");
Reiko 0:22a7683646d1 278
Reiko 0:22a7683646d1 279 int n = server.receiveFrom(client, buffer, sizeof(buffer));
Reiko 0:22a7683646d1 280
Reiko 0:22a7683646d1 281 if (n > 0) {
Reiko 0:22a7683646d1 282 //pc.printf("Received packet from: %s\n", client.get_address());
Reiko 0:22a7683646d1 283 //pc.printf("n: %d\n", n);
Reiko 0:22a7683646d1 284 buffer[n] = '\0';
Reiko 0:22a7683646d1 285 //server.sendTo(client, buffer, n);
Reiko 5:b30229c6f32b 286 executeCommand(buffer);
Reiko 0:22a7683646d1 287 //coilgun.kick(1000);
Reiko 0:22a7683646d1 288 //executeCommand((short*)buffer);
Reiko 0:22a7683646d1 289 }
Reiko 0:22a7683646d1 290
Reiko 0:22a7683646d1 291 //int n = pc.readable();
Reiko 0:22a7683646d1 292 /*if (pc.readable()) {
Reiko 0:22a7683646d1 293 //pc.printf("Received packet from: %s\n", client.get_address());
Reiko 0:22a7683646d1 294 //pc.printf("n: %d\n", n);
Reiko 0:22a7683646d1 295 //pc.scanf("%s", &buffer);
Reiko 0:22a7683646d1 296 buffer[charCounter] = pc.getc();
Reiko 0:22a7683646d1 297 //pc.printf("%c\n", buffer[charCounter]);
Reiko 0:22a7683646d1 298 //fflush(stdout);
Reiko 0:22a7683646d1 299 //fflush(stdin);
Reiko 0:22a7683646d1 300 //pc.printf("%s\n", buffer);
Reiko 0:22a7683646d1 301 if (buffer[charCounter] == '\n') {
Reiko 0:22a7683646d1 302 buffer[charCounter] = '\0';
Reiko 0:22a7683646d1 303 executeCommand(buffer);
Reiko 0:22a7683646d1 304 charCounter = 0;
Reiko 0:22a7683646d1 305 } else {
Reiko 0:22a7683646d1 306 charCounter++;
Reiko 0:22a7683646d1 307 }
Reiko 0:22a7683646d1 308
Reiko 0:22a7683646d1 309 //server.sendTo(client, buffer, n);
Reiko 0:22a7683646d1 310 }*/
Reiko 0:22a7683646d1 311 }
Reiko 0:22a7683646d1 312 }
Reiko 0:22a7683646d1 313
Reiko 5:b30229c6f32b 314 void executeCommand(char *buffer) {
Reiko 1:79ac4e293661 315 failSafeCount = 0;
Reiko 1:79ac4e293661 316
Reiko 0:22a7683646d1 317 pc.printf("%s\n", buffer);
Reiko 0:22a7683646d1 318 char *cmd;
Reiko 1:79ac4e293661 319 cmd = strtok(buffer, ":");
Reiko 0:22a7683646d1 320
Reiko 0:22a7683646d1 321 //pc.printf("%s\n", cmd);
Reiko 0:22a7683646d1 322
Reiko 0:22a7683646d1 323 if (strncmp(cmd, "speeds", 6) == 0) {
Reiko 1:79ac4e293661 324 motor1.setSpeed(atoi(strtok(NULL, ":")));
Reiko 1:79ac4e293661 325 motor2.setSpeed(atoi(strtok(NULL, ":")));
Reiko 1:79ac4e293661 326 motor3.setSpeed(atoi(strtok(NULL, ":")));
Reiko 1:79ac4e293661 327 motor4.setSpeed(atoi(strtok(NULL, ":")));
Reiko 1:79ac4e293661 328 motor5.setSpeed(atoi(strtok(NULL, ":")));
Reiko 1:79ac4e293661 329 int charCount = sprintf(sendBuffer, "<speeds:%d:%d:%d:%d:%d>",
Reiko 1:79ac4e293661 330 motor1.getSpeed(), motor2.getSpeed(), motor3.getSpeed(), motor4.getSpeed(), motor5.getSpeed());
Reiko 1:79ac4e293661 331 server.sendTo(client, sendBuffer, charCount);
Reiko 0:22a7683646d1 332 } else if (strncmp(cmd, "gs", 2) == 0) {
Reiko 1:79ac4e293661 333 int charCount = sprintf(sendBuffer, "<speeds:%d:%d:%d:%d:%d>",
Reiko 1:79ac4e293661 334 motor1.getSpeed(), motor2.getSpeed(), motor3.getSpeed(), motor4.getSpeed(), motor5.getSpeed());
Reiko 0:22a7683646d1 335 server.sendTo(client, sendBuffer, charCount);
Reiko 1:79ac4e293661 336 } else if (strncmp(cmd, "kick", 4) == 0) {
Reiko 1:79ac4e293661 337 pc.printf("kick\n");
Reiko 1:79ac4e293661 338 coilgun.kick(atoi(strtok(NULL, ":")));
Reiko 1:79ac4e293661 339 } else if (strncmp(cmd, "charge", 6) == 0) {
Reiko 1:79ac4e293661 340 pc.printf("charge\n");
Reiko 1:79ac4e293661 341 coilgun.charge();
Reiko 1:79ac4e293661 342 } else if (strncmp(cmd, "discharge", 9) == 0) {
Reiko 1:79ac4e293661 343 pc.printf("discharge\n");
Reiko 1:79ac4e293661 344 coilgun.discharge();
Reiko 0:22a7683646d1 345 } else if (strncmp(cmd, "k", 1) == 0) {
Reiko 0:22a7683646d1 346 pc.printf("kick\n");
Reiko 1:79ac4e293661 347 int length = atoi(strtok(NULL, ":"));
Reiko 0:22a7683646d1 348 coilgun.kick(length);
Reiko 0:22a7683646d1 349 } else if (strncmp(cmd, "c", 1) == 0) {
Reiko 0:22a7683646d1 350 pc.printf("charge\n");
Reiko 1:79ac4e293661 351 if (atoi(strtok(NULL, ":")) == 1) {
Reiko 1:79ac4e293661 352 coilgun.charge();
Reiko 1:79ac4e293661 353 } else {
Reiko 1:79ac4e293661 354 coilgun.chargeEnd();
Reiko 1:79ac4e293661 355 }
Reiko 0:22a7683646d1 356 } else if (strncmp(cmd, "d", 1) == 0) {
Reiko 0:22a7683646d1 357 pc.printf("discharge\n");
Reiko 0:22a7683646d1 358 coilgun.discharge();
Reiko 4:c5cf0676baca 359 } else if (strncmp(cmd, "reset", 5) == 0) {
Reiko 4:c5cf0676baca 360 motor1.setSpeed(0);
Reiko 4:c5cf0676baca 361 motor2.setSpeed(0);
Reiko 4:c5cf0676baca 362 motor3.setSpeed(0);
Reiko 4:c5cf0676baca 363 motor4.setSpeed(0);
Reiko 4:c5cf0676baca 364 motor5.setSpeed(0);
Reiko 4:c5cf0676baca 365 humanInterface.setGoal(HumanInterface::UNSET);
Reiko 4:c5cf0676baca 366 humanInterface.setError(false);
Reiko 5:b30229c6f32b 367 humanInterface.setGo(false);
Reiko 1:79ac4e293661 368 } else if (strncmp(cmd, "fs", 2) == 0) {
Reiko 1:79ac4e293661 369 failSafeEnabled = (bool)atoi(strtok(NULL, ":"));
Reiko 1:79ac4e293661 370 } else if (strncmp(cmd, "target", 6) == 0) {
Reiko 3:4ec313b1b314 371 int target = atoi(strtok(NULL, ":"));
Reiko 3:4ec313b1b314 372 if (target == 0) {
Reiko 3:4ec313b1b314 373 humanInterface.setGoal(HumanInterface::BLUE);
Reiko 3:4ec313b1b314 374 } else if (target == 1) {
Reiko 1:79ac4e293661 375 humanInterface.setGoal(HumanInterface::YELLOW);
Reiko 3:4ec313b1b314 376 } else if (target == 2) {
Reiko 3:4ec313b1b314 377 humanInterface.setGoal(HumanInterface::UNSET);
Reiko 1:79ac4e293661 378 }
Reiko 1:79ac4e293661 379 } else if (strncmp(cmd, "error", 5) == 0) {
Reiko 1:79ac4e293661 380 humanInterface.setError(atoi(strtok(NULL, ":")));
Reiko 5:b30229c6f32b 381 } else if (strncmp(cmd, "go", 2) == 0) {
Reiko 5:b30229c6f32b 382 humanInterface.setGo(atoi(strtok(NULL, ":")));
Reiko 1:79ac4e293661 383 }
Reiko 0:22a7683646d1 384 }