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@3:4ec313b1b314, 2013-09-22 (annotated)

Committer:
Reiko
Date:
Sun Sep 22 07:27:21 2013 +0000
Revision:
3:4ec313b1b314
Parent:
2:a3e6eceed838
Child:
4:c5cf0676baca
Added target:2 command for unsetting goal LEDs

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