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@8:06124632c3e4, 2014-09-02 (annotated)

Committer:
Reiko
Date:
Tue Sep 02 15:40:53 2014 +0000
Revision:
8:06124632c3e4
Parent:
7:a9e98f642a89
Child:
9:2d2030d989d8
Changes for testing new hardware

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