Reiko Randoja / Telliskivi2_2014

Telliskivi 2 2014

Dependencies:   DoubleCoilGun 4DGL-uLCD-SE ExternalIn HumanInterfaceT14 LedOut MCP3021 MODSERIAL Motor1Pwm1Dir PCA9555 PinDetect QED RgbLedPCA9555 WDT_K64F mbed-src

Fork of Telliskivi2plus by Reiko Randoja

main.cpp@5:b30229c6f32b, 2013-11-03 (annotated)

Committer:
Reiko
Date:
Sun Nov 03 11:46:19 2013 +0000
Revision:
5:b30229c6f32b
Parent:
4:c5cf0676baca
Child:
6:5619aff36840
Added go command, changed code to use new HumanInterface

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