Reiko Randoja / Mbed 2 deprecated Telliskivi2plus

Telliskivi 2+

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

main.cpp@6:5619aff36840, 2013-11-04 (annotated)

Committer:
mlaane
Date:
Mon Nov 04 21:23:42 2013 +0000
Revision:
6:5619aff36840
Parent:
5:b30229c6f32b
Child:
7:a9e98f642a89
Debounced buttons and ball sensor

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