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@1:79ac4e293661, 2013-09-19 (annotated)

Committer:
Reiko
Date:
Thu Sep 19 13:18:02 2013 +0000
Revision:
1:79ac4e293661
Parent:
0:22a7683646d1
Child:
2:a3e6eceed838
Added new commands, stall detection code and HumanInterface library code

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 0:22a7683646d1 20
Reiko 0:22a7683646d1 21 Serial pc(USBTX, USBRX); // tx, rx
Reiko 0:22a7683646d1 22 //ADXL345_I2C accelerometer(p9, p10);
Reiko 0:22a7683646d1 23 //L3G4200D gyro(p9, p10);
Reiko 0:22a7683646d1 24 //HMC5883L compass(p9, p10);
Reiko 0:22a7683646d1 25
Reiko 0:22a7683646d1 26 PCA9555 ioExt(p9, p10, p8, 0x40);
Reiko 0:22a7683646d1 27
Reiko 0:22a7683646d1 28 //int readings[3] = {0, 0, 0};
Reiko 0:22a7683646d1 29 //int gyroData[3] = {0, 0, 0};
Reiko 0:22a7683646d1 30 //int16_t compassData[3] = {0, 0, 0};
Reiko 0:22a7683646d1 31
Reiko 0:22a7683646d1 32 //DigitalOut led3(LED3);
Reiko 0:22a7683646d1 33
Reiko 0:22a7683646d1 34 volatile int update = 0;
Reiko 0:22a7683646d1 35 volatile int extInChanged = 0;
Reiko 1:79ac4e293661 36 volatile int ballChanged = 0;
Reiko 1:79ac4e293661 37 volatile int goalButtonReleased = 0;
Reiko 1:79ac4e293661 38 volatile int startButtonReleased = 0;
Reiko 1:79ac4e293661 39 volatile int stallChanged = 0;
Reiko 1:79ac4e293661 40
Reiko 1:79ac4e293661 41 bool failSafeEnabled = true;
Reiko 1:79ac4e293661 42 int failSafeCount = 0;
Reiko 1:79ac4e293661 43 int failSafeLimit = 60;
Reiko 1:79ac4e293661 44
Reiko 1:79ac4e293661 45 bool isCharged = false; //has charge been sent after discharge
Reiko 0:22a7683646d1 46
Reiko 0:22a7683646d1 47 void executeCommand(short *cmd);
Reiko 0:22a7683646d1 48 void executeCommandOld(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 1:79ac4e293661 74 //LedOut redLed(&ioExt, 13);
Reiko 1:79ac4e293661 75 //LedOut blueLed(&ioExt, 12);
Reiko 1:79ac4e293661 76 //LedOut yellowLed(&ioExt, 11);
Reiko 0:22a7683646d1 77
Reiko 1:79ac4e293661 78 Coilgun coilgun(p20, p19, p18);
Reiko 0:22a7683646d1 79 //CoilGun coilgun(LED3, LED4, p18);
Reiko 0:22a7683646d1 80 //Timeout kickFinish;
Reiko 0:22a7683646d1 81
Reiko 1:79ac4e293661 82 HumanInterface humanInterface(&pc, &ioExt, 11, 12, 13, 14, 15, 10);
Reiko 1:79ac4e293661 83
Reiko 0:22a7683646d1 84 void updateTick() {
Reiko 0:22a7683646d1 85 //led3 = 1;
Reiko 0:22a7683646d1 86 motor1.pid();
Reiko 0:22a7683646d1 87 motor2.pid();
Reiko 0:22a7683646d1 88 motor3.pid();
Reiko 0:22a7683646d1 89 motor4.pid();
Reiko 0:22a7683646d1 90 motor5.pid();
Reiko 0:22a7683646d1 91 //led3 = 0;
Reiko 0:22a7683646d1 92 update = 1;
Reiko 0:22a7683646d1 93 }
Reiko 0:22a7683646d1 94
Reiko 0:22a7683646d1 95 void extChangedCallback() {
Reiko 0:22a7683646d1 96 extInChanged = 1;
Reiko 0:22a7683646d1 97 }
Reiko 0:22a7683646d1 98
Reiko 1:79ac4e293661 99 void ballChangedCallback() {
Reiko 1:79ac4e293661 100 ballChanged = 1;
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 DigitalOut led2(LED2);
Reiko 0:22a7683646d1 126
Reiko 0:22a7683646d1 127 //void readSerial() {
Reiko 0:22a7683646d1 128
Reiko 0:22a7683646d1 129 //int n = pc.readable();
Reiko 0:22a7683646d1 130 //if (pc.readable()) {
Reiko 0:22a7683646d1 131
Reiko 0:22a7683646d1 132 //pc.printf("Received packet from: %s\n", client.get_address());
Reiko 0:22a7683646d1 133 //pc.printf("n: %d\n", n);
Reiko 0:22a7683646d1 134 //pc.scanf("%s", &buffer);
Reiko 0:22a7683646d1 135 //pc.printf("%c\n", pc.getc());
Reiko 0:22a7683646d1 136 //buffer[charCounter] = pc.getc();
Reiko 0:22a7683646d1 137 //led2 = !led2;
Reiko 0:22a7683646d1 138 /*printf("%c\n", buffer[charCounter]);
Reiko 0:22a7683646d1 139 //pc.printf("%s\n", buffer);
Reiko 0:22a7683646d1 140 if (buffer[charCounter] == '\n') {
Reiko 0:22a7683646d1 141 buffer[charCounter] = '\0';
Reiko 0:22a7683646d1 142 executeCommand(buffer);
Reiko 0:22a7683646d1 143 charCounter = 0;
Reiko 0:22a7683646d1 144 } else {
Reiko 0:22a7683646d1 145 charCounter++;
Reiko 0:22a7683646d1 146 }*/
Reiko 0:22a7683646d1 147 //}
Reiko 0:22a7683646d1 148
Reiko 0:22a7683646d1 149 //}
Reiko 0:22a7683646d1 150
Reiko 0:22a7683646d1 151 const char *byte_to_binary(int x) {
Reiko 0:22a7683646d1 152 static char b[9];
Reiko 0:22a7683646d1 153 b[0] = '\0';
Reiko 0:22a7683646d1 154
Reiko 0:22a7683646d1 155 int z;
Reiko 0:22a7683646d1 156 for (z = 32768; z > 0; z >>= 1) {
Reiko 0:22a7683646d1 157 strcat(b, ((x & z) == z) ? "1" : "0");
Reiko 0:22a7683646d1 158 }
Reiko 0:22a7683646d1 159
Reiko 0:22a7683646d1 160 return b;
Reiko 0:22a7683646d1 161 }
Reiko 0:22a7683646d1 162
Reiko 0:22a7683646d1 163 int main (void) {
Reiko 0:22a7683646d1 164 pc.baud(115200);
Reiko 0:22a7683646d1 165 EthernetInterface eth;
Reiko 0:22a7683646d1 166 //eth.init(); //Use DHCP
Reiko 0:22a7683646d1 167 eth.init("192.168.4.1", "255.255.255.0", "192.168.4.8");
Reiko 0:22a7683646d1 168 eth.connect();
Reiko 0:22a7683646d1 169 printf("IP Address is %s\n", eth.getIPAddress());
Reiko 0:22a7683646d1 170
Reiko 0:22a7683646d1 171 server.bind(SERVER_PORT);
Reiko 0:22a7683646d1 172
Reiko 0:22a7683646d1 173 //pc.printf("Starting ADXL345 test...\n");
Reiko 0:22a7683646d1 174 //wait(.001);
Reiko 0:22a7683646d1 175 //pc.printf("Device ID is: 0x%02x\n", accelerometer.getDeviceID());
Reiko 0:22a7683646d1 176 //wait(.001);
Reiko 0:22a7683646d1 177
Reiko 0:22a7683646d1 178 // These are here to test whether any of the initialization fails. It will print the failure
Reiko 0:22a7683646d1 179 /*if (accelerometer.setPowerControl(0x00)){
Reiko 0:22a7683646d1 180 pc.printf("didn't intitialize power control\n");
Reiko 0:22a7683646d1 181 return 0;
Reiko 0:22a7683646d1 182 }*/
Reiko 0:22a7683646d1 183 //Full resolution, +/-16g, 4mg/LSB.
Reiko 0:22a7683646d1 184 /*wait(.001);
Reiko 0:22a7683646d1 185
Reiko 0:22a7683646d1 186 if(accelerometer.setDataFormatControl(0x0B)){
Reiko 0:22a7683646d1 187 pc.printf("didn't set data format\n");
Reiko 0:22a7683646d1 188 return 0;
Reiko 0:22a7683646d1 189 }
Reiko 0:22a7683646d1 190
Reiko 0:22a7683646d1 191 wait(.001);*/
Reiko 0:22a7683646d1 192
Reiko 0:22a7683646d1 193 //3.2kHz data rate.
Reiko 0:22a7683646d1 194 /*if(accelerometer.setDataRate(ADXL345_3200HZ)){
Reiko 0:22a7683646d1 195 pc.printf("didn't set data rate\n");
Reiko 0:22a7683646d1 196 return 0;
Reiko 0:22a7683646d1 197 }
Reiko 0:22a7683646d1 198
Reiko 0:22a7683646d1 199 wait(.001);*/
Reiko 0:22a7683646d1 200
Reiko 0:22a7683646d1 201 //Measurement mode.
Reiko 0:22a7683646d1 202
Reiko 0:22a7683646d1 203 /*if (accelerometer.setPowerControl(MeasurementMode)) {
Reiko 0:22a7683646d1 204 pc.printf("didn't set the power control to measurement\n");
Reiko 0:22a7683646d1 205 return 0;
Reiko 0:22a7683646d1 206 }
Reiko 0:22a7683646d1 207 */
Reiko 0:22a7683646d1 208
Reiko 0:22a7683646d1 209 server.set_blocking(false, 1);
Reiko 0:22a7683646d1 210
Reiko 0:22a7683646d1 211 //int ioExtBefore = ioExt.read();
Reiko 0:22a7683646d1 212 //pc.printf("ioExt: %s\n", byte_to_binary(ioExt.read()));
Reiko 0:22a7683646d1 213 pc.printf("ioExt: %x\n", ioExt.read());
Reiko 0:22a7683646d1 214 ioExt.setDirection(0xC400);
Reiko 0:22a7683646d1 215 ioExt.write(0x0155);
Reiko 0:22a7683646d1 216 //int ioExtAfter = ioExt.read();
Reiko 0:22a7683646d1 217 pc.printf("ioExt: %x\n", ioExt.read());
Reiko 0:22a7683646d1 218 //pc.printf("ioExt: %s\n", byte_to_binary(ioExt.read()));
Reiko 0:22a7683646d1 219
Reiko 1:79ac4e293661 220 //ballSens.change(&extChangedCallback);
Reiko 1:79ac4e293661 221 //button1.change(&extChangedCallback);
Reiko 1:79ac4e293661 222 //button2.change(&extChangedCallback);
Reiko 1:79ac4e293661 223 //humanInterface.change(&extChangedCallback);
Reiko 1:79ac4e293661 224 humanInterface.ballChange(&ballChangedCallback);
Reiko 1:79ac4e293661 225 humanInterface.goalRise(&goalRiseCallback);
Reiko 1:79ac4e293661 226 humanInterface.startRise(&startRiseCallback);
Reiko 1:79ac4e293661 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 isCharged = false;
Reiko 1:79ac4e293661 271 }
Reiko 1:79ac4e293661 272 if (!isCharged) {
Reiko 1:79ac4e293661 273 //int charCount = sprintf(sendBuffer, "<speeds:%d:%d:%d:%d:%d>");
Reiko 1:79ac4e293661 274 server.sendTo(client, "<discharged>", 12);
Reiko 1:79ac4e293661 275 }
Reiko 1:79ac4e293661 276 }
Reiko 1:79ac4e293661 277
Reiko 0:22a7683646d1 278 //led3 = 1;
Reiko 0:22a7683646d1 279 //updateSensors();
Reiko 0:22a7683646d1 280 //pc.printf("update");
Reiko 0:22a7683646d1 281 /*motor1.pid();
Reiko 0:22a7683646d1 282 motor2.pid();
Reiko 0:22a7683646d1 283 motor3.pid();
Reiko 0:22a7683646d1 284 motor4.pid();
Reiko 0:22a7683646d1 285 motor5.pid();*/
Reiko 0:22a7683646d1 286
Reiko 0:22a7683646d1 287 //led3 = 0;
Reiko 0:22a7683646d1 288 //int charCount = sprintf(sendBuffer, "<speeds:%d:%d:%d:%d:%d>", motor1.getSpeed(), motor2.getSpeed(), motor3.getSpeed(), motor4.getSpeed(), motor5.getSpeed());
Reiko 0:22a7683646d1 289 //server.sendTo(client, sendBuffer, charCount);
Reiko 0:22a7683646d1 290 //pc.printf("<speeds:%d:%d:%d:%d:%d>\n", motor1.getSpeed(), motor2.getSpeed(), motor3.getSpeed(), motor4.getSpeed(), motor5.getSpeed());
Reiko 0:22a7683646d1 291 //redLed.toggle();
Reiko 0:22a7683646d1 292 led1 = !led1;
Reiko 0:22a7683646d1 293 }
Reiko 0:22a7683646d1 294
Reiko 1:79ac4e293661 295 if (stallChanged) {
Reiko 1:79ac4e293661 296 stallChanged = 0;
Reiko 1:79ac4e293661 297 int charCount = sprintf(sendBuffer, "<stall:%d:%d:%d:%d:%d>\n",
Reiko 1:79ac4e293661 298 motor1.getStallLevel(),
Reiko 1:79ac4e293661 299 motor2.getStallLevel(),
Reiko 1:79ac4e293661 300 motor3.getStallLevel(),
Reiko 1:79ac4e293661 301 motor4.getStallLevel(),
Reiko 1:79ac4e293661 302 motor5.getStallLevel());
Reiko 1:79ac4e293661 303 server.sendTo(client, sendBuffer, charCount);
Reiko 1:79ac4e293661 304 }
Reiko 1:79ac4e293661 305
Reiko 1:79ac4e293661 306 /*if (extInChanged) {
Reiko 1:79ac4e293661 307 extInChanged = 0;
Reiko 1:79ac4e293661 308 pc.printf("extChanged\n");
Reiko 1:79ac4e293661 309 //bool ballState = ballSens.read();
Reiko 1:79ac4e293661 310 //bool button1State = button1.read();
Reiko 1:79ac4e293661 311 //bool button2State = button2.read();
Reiko 0:22a7683646d1 312 if (button1State) {
Reiko 0:22a7683646d1 313 led1 = 1;
Reiko 0:22a7683646d1 314 } else {
Reiko 0:22a7683646d1 315 led1 = 0;
Reiko 0:22a7683646d1 316 }
Reiko 0:22a7683646d1 317 pc.printf("<ball:%d>\n", ballState ? 1 : 0);
Reiko 0:22a7683646d1 318 pc.printf("<btn1:%d>\n", button1State ? 1 : 0);
Reiko 1:79ac4e293661 319 pc.printf("<btn2:%d>\n", button2State ? 1 : 0);
Reiko 1:79ac4e293661 320 }*/
Reiko 1:79ac4e293661 321
Reiko 1:79ac4e293661 322 if (ballChanged) {
Reiko 1:79ac4e293661 323 ballChanged = 0;
Reiko 1:79ac4e293661 324 int charCount = sprintf(sendBuffer, "<ball:%d>", humanInterface.getBallState());
Reiko 1:79ac4e293661 325 server.sendTo(client, sendBuffer, charCount);
Reiko 1:79ac4e293661 326 }
Reiko 1:79ac4e293661 327
Reiko 1:79ac4e293661 328 if (goalButtonReleased) {
Reiko 1:79ac4e293661 329 goalButtonReleased = 0;
Reiko 1:79ac4e293661 330 server.sendTo(client, "<toggle-side>", 13);
Reiko 1:79ac4e293661 331 }
Reiko 1:79ac4e293661 332
Reiko 1:79ac4e293661 333 if (startButtonReleased) {
Reiko 1:79ac4e293661 334 startButtonReleased = 0;
Reiko 1:79ac4e293661 335 server.sendTo(client, "<toggle-go>", 11);
Reiko 0:22a7683646d1 336 }
Reiko 0:22a7683646d1 337
Reiko 0:22a7683646d1 338 //printf("\nWait for packet...\n");
Reiko 0:22a7683646d1 339
Reiko 0:22a7683646d1 340 int n = server.receiveFrom(client, buffer, sizeof(buffer));
Reiko 0:22a7683646d1 341
Reiko 0:22a7683646d1 342 if (n > 0) {
Reiko 0:22a7683646d1 343 //pc.printf("Received packet from: %s\n", client.get_address());
Reiko 0:22a7683646d1 344 //pc.printf("n: %d\n", n);
Reiko 0:22a7683646d1 345 buffer[n] = '\0';
Reiko 0:22a7683646d1 346 //server.sendTo(client, buffer, n);
Reiko 0:22a7683646d1 347 executeCommandOld(buffer);
Reiko 0:22a7683646d1 348 //coilgun.kick(1000);
Reiko 0:22a7683646d1 349 //executeCommand((short*)buffer);
Reiko 0:22a7683646d1 350 }
Reiko 0:22a7683646d1 351
Reiko 0:22a7683646d1 352 //int n = pc.readable();
Reiko 0:22a7683646d1 353 /*if (pc.readable()) {
Reiko 0:22a7683646d1 354 //pc.printf("Received packet from: %s\n", client.get_address());
Reiko 0:22a7683646d1 355 //pc.printf("n: %d\n", n);
Reiko 0:22a7683646d1 356 //pc.scanf("%s", &buffer);
Reiko 0:22a7683646d1 357 buffer[charCounter] = pc.getc();
Reiko 0:22a7683646d1 358 //pc.printf("%c\n", buffer[charCounter]);
Reiko 0:22a7683646d1 359 //fflush(stdout);
Reiko 0:22a7683646d1 360 //fflush(stdin);
Reiko 0:22a7683646d1 361 //pc.printf("%s\n", buffer);
Reiko 0:22a7683646d1 362 if (buffer[charCounter] == '\n') {
Reiko 0:22a7683646d1 363 buffer[charCounter] = '\0';
Reiko 0:22a7683646d1 364 executeCommand(buffer);
Reiko 0:22a7683646d1 365 charCounter = 0;
Reiko 0:22a7683646d1 366 } else {
Reiko 0:22a7683646d1 367 charCounter++;
Reiko 0:22a7683646d1 368 }
Reiko 0:22a7683646d1 369
Reiko 0:22a7683646d1 370 //server.sendTo(client, buffer, n);
Reiko 0:22a7683646d1 371 }*/
Reiko 0:22a7683646d1 372 }
Reiko 0:22a7683646d1 373 }
Reiko 0:22a7683646d1 374
Reiko 0:22a7683646d1 375 void executeCommand(short *cmd) {
Reiko 0:22a7683646d1 376 if (cmd[0] == 1) {
Reiko 0:22a7683646d1 377 motor1.setSpeed(cmd[1]);
Reiko 0:22a7683646d1 378 motor2.setSpeed(cmd[2]);
Reiko 0:22a7683646d1 379 motor3.setSpeed(cmd[3]);
Reiko 0:22a7683646d1 380 motor4.setSpeed(cmd[4]);
Reiko 0:22a7683646d1 381 motor5.setSpeed(cmd[5]);
Reiko 0:22a7683646d1 382 } else if (cmd[0] == 2) {
Reiko 0:22a7683646d1 383 int charCount = sprintf(sendBuffer, "<speeds:%d:%d:%d:%d:%d>", motor1.getSpeed(), motor2.getSpeed(), motor3.getSpeed(), motor4.getSpeed(), motor5.getSpeed());
Reiko 0:22a7683646d1 384 //pc.printf("%d\n", sizeof(sendBuffer));
Reiko 0:22a7683646d1 385 //pc.printf("sendTo %s\n", client.get_address());
Reiko 0:22a7683646d1 386 server.sendTo(client, sendBuffer, charCount);
Reiko 0:22a7683646d1 387 led2 = !led2;
Reiko 0:22a7683646d1 388 } else if (cmd[0] == 3) {
Reiko 0:22a7683646d1 389 pc.printf("kick\n");
Reiko 0:22a7683646d1 390 coilgun.kick(cmd[1]);
Reiko 0:22a7683646d1 391 } else if (cmd[0] == 4) {
Reiko 0:22a7683646d1 392 pc.printf("charge\n");
Reiko 1:79ac4e293661 393 if (cmd[1] == 1) {
Reiko 1:79ac4e293661 394 coilgun.charge();
Reiko 1:79ac4e293661 395 } else {
Reiko 1:79ac4e293661 396 coilgun.chargeEnd();
Reiko 1:79ac4e293661 397 }
Reiko 0:22a7683646d1 398 }
Reiko 0:22a7683646d1 399 }
Reiko 0:22a7683646d1 400
Reiko 0:22a7683646d1 401 void executeCommandOld(char *buffer) {
Reiko 1:79ac4e293661 402 failSafeCount = 0;
Reiko 1:79ac4e293661 403
Reiko 0:22a7683646d1 404 pc.printf("%s\n", buffer);
Reiko 0:22a7683646d1 405 char *cmd;
Reiko 1:79ac4e293661 406 cmd = strtok(buffer, ":");
Reiko 0:22a7683646d1 407
Reiko 0:22a7683646d1 408 //pc.printf("%s\n", cmd);
Reiko 0:22a7683646d1 409
Reiko 0:22a7683646d1 410 if (strncmp(cmd, "speeds", 6) == 0) {
Reiko 1:79ac4e293661 411 motor1.setSpeed(atoi(strtok(NULL, ":")));
Reiko 1:79ac4e293661 412 motor2.setSpeed(atoi(strtok(NULL, ":")));
Reiko 1:79ac4e293661 413 motor3.setSpeed(atoi(strtok(NULL, ":")));
Reiko 1:79ac4e293661 414 motor4.setSpeed(atoi(strtok(NULL, ":")));
Reiko 1:79ac4e293661 415 motor5.setSpeed(atoi(strtok(NULL, ":")));
Reiko 1:79ac4e293661 416 int charCount = sprintf(sendBuffer, "<speeds:%d:%d:%d:%d:%d>",
Reiko 1:79ac4e293661 417 motor1.getSpeed(), motor2.getSpeed(), motor3.getSpeed(), motor4.getSpeed(), motor5.getSpeed());
Reiko 1:79ac4e293661 418 server.sendTo(client, sendBuffer, charCount);
Reiko 0:22a7683646d1 419 } else if (strncmp(cmd, "gs", 2) == 0) {
Reiko 1:79ac4e293661 420 int charCount = sprintf(sendBuffer, "<speeds:%d:%d:%d:%d:%d>",
Reiko 1:79ac4e293661 421 motor1.getSpeed(), motor2.getSpeed(), motor3.getSpeed(), motor4.getSpeed(), motor5.getSpeed());
Reiko 0:22a7683646d1 422 server.sendTo(client, sendBuffer, charCount);
Reiko 0:22a7683646d1 423 led2 = !led2;
Reiko 1:79ac4e293661 424 } else if (strncmp(cmd, "kick", 4) == 0) {
Reiko 1:79ac4e293661 425 pc.printf("kick\n");
Reiko 1:79ac4e293661 426 coilgun.kick(atoi(strtok(NULL, ":")));
Reiko 1:79ac4e293661 427 } else if (strncmp(cmd, "charge", 6) == 0) {
Reiko 1:79ac4e293661 428 pc.printf("charge\n");
Reiko 1:79ac4e293661 429 coilgun.charge();
Reiko 1:79ac4e293661 430 isCharged = true;
Reiko 1:79ac4e293661 431 } else if (strncmp(cmd, "discharge", 9) == 0) {
Reiko 1:79ac4e293661 432 pc.printf("discharge\n");
Reiko 1:79ac4e293661 433 coilgun.discharge();
Reiko 1:79ac4e293661 434 isCharged = false;
Reiko 0:22a7683646d1 435 } else if (strncmp(cmd, "k", 1) == 0) {
Reiko 0:22a7683646d1 436 pc.printf("kick\n");
Reiko 1:79ac4e293661 437 int length = atoi(strtok(NULL, ":"));
Reiko 0:22a7683646d1 438 coilgun.kick(length);
Reiko 0:22a7683646d1 439 } else if (strncmp(cmd, "c", 1) == 0) {
Reiko 0:22a7683646d1 440 pc.printf("charge\n");
Reiko 1:79ac4e293661 441 if (atoi(strtok(NULL, ":")) == 1) {
Reiko 1:79ac4e293661 442 coilgun.charge();
Reiko 1:79ac4e293661 443 isCharged = true;
Reiko 1:79ac4e293661 444 } else {
Reiko 1:79ac4e293661 445 coilgun.chargeEnd();
Reiko 1:79ac4e293661 446 }
Reiko 0:22a7683646d1 447 } else if (strncmp(cmd, "d", 1) == 0) {
Reiko 0:22a7683646d1 448 pc.printf("discharge\n");
Reiko 0:22a7683646d1 449 coilgun.discharge();
Reiko 1:79ac4e293661 450 isCharged = false;
Reiko 1:79ac4e293661 451 } else if (strncmp(cmd, "fs", 2) == 0) {
Reiko 1:79ac4e293661 452 failSafeEnabled = (bool)atoi(strtok(NULL, ":"));
Reiko 1:79ac4e293661 453 } else if (strncmp(cmd, "target", 6) == 0) {
Reiko 1:79ac4e293661 454 if (atoi(strtok(NULL, ":")) == 1) {
Reiko 1:79ac4e293661 455 humanInterface.setGoal(HumanInterface::YELLOW);
Reiko 1:79ac4e293661 456 } else {
Reiko 1:79ac4e293661 457 humanInterface.setGoal(HumanInterface::BLUE);
Reiko 1:79ac4e293661 458 }
Reiko 1:79ac4e293661 459 } else if (strncmp(cmd, "error", 5) == 0) {
Reiko 1:79ac4e293661 460 humanInterface.setError(atoi(strtok(NULL, ":")));
Reiko 1:79ac4e293661 461 }
Reiko 0:22a7683646d1 462 }