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@0:22a7683646d1, 2013-09-18 (annotated)

Committer:
Reiko
Date:
Wed Sep 18 07:50:53 2013 +0000
Revision:
0:22a7683646d1
Child:
1:79ac4e293661
Created new project based on tellis2plus

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 0:22a7683646d1 13
Reiko 0:22a7683646d1 14 #define SERVER_PORT 8042
Reiko 0:22a7683646d1 15
Reiko 0:22a7683646d1 16 Ticker sensorUpdate;
Reiko 0:22a7683646d1 17
Reiko 0:22a7683646d1 18 DigitalOut led1(LED1);
Reiko 0:22a7683646d1 19
Reiko 0:22a7683646d1 20 Serial pc(USBTX, USBRX); // tx, rx
Reiko 0:22a7683646d1 21 //ADXL345_I2C accelerometer(p9, p10);
Reiko 0:22a7683646d1 22 //L3G4200D gyro(p9, p10);
Reiko 0:22a7683646d1 23 //HMC5883L compass(p9, p10);
Reiko 0:22a7683646d1 24
Reiko 0:22a7683646d1 25 PCA9555 ioExt(p9, p10, p8, 0x40);
Reiko 0:22a7683646d1 26
Reiko 0:22a7683646d1 27 //int readings[3] = {0, 0, 0};
Reiko 0:22a7683646d1 28 //int gyroData[3] = {0, 0, 0};
Reiko 0:22a7683646d1 29 //int16_t compassData[3] = {0, 0, 0};
Reiko 0:22a7683646d1 30
Reiko 0:22a7683646d1 31 //DigitalOut led3(LED3);
Reiko 0:22a7683646d1 32
Reiko 0:22a7683646d1 33 volatile int update = 0;
Reiko 0:22a7683646d1 34 volatile int extInChanged = 0;
Reiko 0:22a7683646d1 35
Reiko 0:22a7683646d1 36 void executeCommand(short *cmd);
Reiko 0:22a7683646d1 37 void executeCommandOld(char *buffer);
Reiko 0:22a7683646d1 38
Reiko 0:22a7683646d1 39 /*
Reiko 0:22a7683646d1 40 void updateSensors() {
Reiko 0:22a7683646d1 41 if (led1 == 1.0) {
Reiko 0:22a7683646d1 42 led1 = 0;
Reiko 0:22a7683646d1 43 } else {
Reiko 0:22a7683646d1 44 led1 = 1.0;
Reiko 0:22a7683646d1 45 }
Reiko 0:22a7683646d1 46 accelerometer.getOutput(readings);
Reiko 0:22a7683646d1 47 pc.printf("<acc:%i:%i:%i>\n", (int16_t)readings[0], (int16_t)readings[1], (int16_t)readings[2]);
Reiko 0:22a7683646d1 48
Reiko 0:22a7683646d1 49 gyro.read(gyroData);
Reiko 0:22a7683646d1 50 pc.printf("<gyro:%i:%i:%i>\n", (int16_t)gyroData[0], (int16_t)gyroData[1], (int16_t)gyroData[2]);
Reiko 0:22a7683646d1 51
Reiko 0:22a7683646d1 52 compass.getXYZ(compassData);
Reiko 0:22a7683646d1 53 pc.printf("<comps:%i:%i:%i>\n", compassData[0], compassData[1], compassData[2]);
Reiko 0:22a7683646d1 54 }*/
Reiko 0:22a7683646d1 55
Reiko 0:22a7683646d1 56 Motor motor1(p25, &ioExt, 0, 1, p5, p6);
Reiko 0:22a7683646d1 57 Motor motor2(p24, &ioExt, 2, 3, p7, p11);
Reiko 0:22a7683646d1 58 Motor motor3(p23, &ioExt, 4, 5, p12, p13);
Reiko 0:22a7683646d1 59 Motor motor4(p22, &ioExt, 6, 7, p14, p15);
Reiko 0:22a7683646d1 60 Motor motor5(p21, &ioExt, 9, 8, p16, p17);
Reiko 0:22a7683646d1 61
Reiko 0:22a7683646d1 62 LedOut redLed(&ioExt, 13);
Reiko 0:22a7683646d1 63 LedOut blueLed(&ioExt, 12);
Reiko 0:22a7683646d1 64 LedOut yellowLed(&ioExt, 11);
Reiko 0:22a7683646d1 65
Reiko 0:22a7683646d1 66 CoilGun coilgun(p19, p20, p18);
Reiko 0:22a7683646d1 67 //CoilGun coilgun(LED3, LED4, p18);
Reiko 0:22a7683646d1 68 //Timeout kickFinish;
Reiko 0:22a7683646d1 69
Reiko 0:22a7683646d1 70 void updateTick() {
Reiko 0:22a7683646d1 71 //led3 = 1;
Reiko 0:22a7683646d1 72 motor1.pid();
Reiko 0:22a7683646d1 73 motor2.pid();
Reiko 0:22a7683646d1 74 motor3.pid();
Reiko 0:22a7683646d1 75 motor4.pid();
Reiko 0:22a7683646d1 76 motor5.pid();
Reiko 0:22a7683646d1 77 //led3 = 0;
Reiko 0:22a7683646d1 78 update = 1;
Reiko 0:22a7683646d1 79 }
Reiko 0:22a7683646d1 80
Reiko 0:22a7683646d1 81 void extChangedCallback() {
Reiko 0:22a7683646d1 82 extInChanged = 1;
Reiko 0:22a7683646d1 83 }
Reiko 0:22a7683646d1 84
Reiko 0:22a7683646d1 85 BallSens ballSens(&ioExt, 10);
Reiko 0:22a7683646d1 86 ExternalIn button1(&ioExt, 14);
Reiko 0:22a7683646d1 87 ExternalIn button2(&ioExt, 15);
Reiko 0:22a7683646d1 88
Reiko 0:22a7683646d1 89 UDPSocket server;
Reiko 0:22a7683646d1 90 Endpoint client;
Reiko 0:22a7683646d1 91 char buffer[256];
Reiko 0:22a7683646d1 92 char sendBuffer[256];
Reiko 0:22a7683646d1 93 int charCounter = 0;
Reiko 0:22a7683646d1 94
Reiko 0:22a7683646d1 95 DigitalOut led2(LED2);
Reiko 0:22a7683646d1 96
Reiko 0:22a7683646d1 97 //void readSerial() {
Reiko 0:22a7683646d1 98
Reiko 0:22a7683646d1 99 //int n = pc.readable();
Reiko 0:22a7683646d1 100 //if (pc.readable()) {
Reiko 0:22a7683646d1 101
Reiko 0:22a7683646d1 102 //pc.printf("Received packet from: %s\n", client.get_address());
Reiko 0:22a7683646d1 103 //pc.printf("n: %d\n", n);
Reiko 0:22a7683646d1 104 //pc.scanf("%s", &buffer);
Reiko 0:22a7683646d1 105 //pc.printf("%c\n", pc.getc());
Reiko 0:22a7683646d1 106 //buffer[charCounter] = pc.getc();
Reiko 0:22a7683646d1 107 //led2 = !led2;
Reiko 0:22a7683646d1 108 /*printf("%c\n", buffer[charCounter]);
Reiko 0:22a7683646d1 109 //pc.printf("%s\n", buffer);
Reiko 0:22a7683646d1 110 if (buffer[charCounter] == '\n') {
Reiko 0:22a7683646d1 111 buffer[charCounter] = '\0';
Reiko 0:22a7683646d1 112 executeCommand(buffer);
Reiko 0:22a7683646d1 113 charCounter = 0;
Reiko 0:22a7683646d1 114 } else {
Reiko 0:22a7683646d1 115 charCounter++;
Reiko 0:22a7683646d1 116 }*/
Reiko 0:22a7683646d1 117 //}
Reiko 0:22a7683646d1 118
Reiko 0:22a7683646d1 119 //}
Reiko 0:22a7683646d1 120
Reiko 0:22a7683646d1 121 const char *byte_to_binary(int x) {
Reiko 0:22a7683646d1 122 static char b[9];
Reiko 0:22a7683646d1 123 b[0] = '\0';
Reiko 0:22a7683646d1 124
Reiko 0:22a7683646d1 125 int z;
Reiko 0:22a7683646d1 126 for (z = 32768; z > 0; z >>= 1) {
Reiko 0:22a7683646d1 127 strcat(b, ((x & z) == z) ? "1" : "0");
Reiko 0:22a7683646d1 128 }
Reiko 0:22a7683646d1 129
Reiko 0:22a7683646d1 130 return b;
Reiko 0:22a7683646d1 131 }
Reiko 0:22a7683646d1 132
Reiko 0:22a7683646d1 133 int main (void) {
Reiko 0:22a7683646d1 134 pc.baud(115200);
Reiko 0:22a7683646d1 135 EthernetInterface eth;
Reiko 0:22a7683646d1 136 //eth.init(); //Use DHCP
Reiko 0:22a7683646d1 137 eth.init("192.168.4.1", "255.255.255.0", "192.168.4.8");
Reiko 0:22a7683646d1 138 eth.connect();
Reiko 0:22a7683646d1 139 printf("IP Address is %s\n", eth.getIPAddress());
Reiko 0:22a7683646d1 140
Reiko 0:22a7683646d1 141 server.bind(SERVER_PORT);
Reiko 0:22a7683646d1 142
Reiko 0:22a7683646d1 143 //pc.printf("Starting ADXL345 test...\n");
Reiko 0:22a7683646d1 144 //wait(.001);
Reiko 0:22a7683646d1 145 //pc.printf("Device ID is: 0x%02x\n", accelerometer.getDeviceID());
Reiko 0:22a7683646d1 146 //wait(.001);
Reiko 0:22a7683646d1 147
Reiko 0:22a7683646d1 148 // These are here to test whether any of the initialization fails. It will print the failure
Reiko 0:22a7683646d1 149 /*if (accelerometer.setPowerControl(0x00)){
Reiko 0:22a7683646d1 150 pc.printf("didn't intitialize power control\n");
Reiko 0:22a7683646d1 151 return 0;
Reiko 0:22a7683646d1 152 }*/
Reiko 0:22a7683646d1 153 //Full resolution, +/-16g, 4mg/LSB.
Reiko 0:22a7683646d1 154 /*wait(.001);
Reiko 0:22a7683646d1 155
Reiko 0:22a7683646d1 156 if(accelerometer.setDataFormatControl(0x0B)){
Reiko 0:22a7683646d1 157 pc.printf("didn't set data format\n");
Reiko 0:22a7683646d1 158 return 0;
Reiko 0:22a7683646d1 159 }
Reiko 0:22a7683646d1 160
Reiko 0:22a7683646d1 161 wait(.001);*/
Reiko 0:22a7683646d1 162
Reiko 0:22a7683646d1 163 //3.2kHz data rate.
Reiko 0:22a7683646d1 164 /*if(accelerometer.setDataRate(ADXL345_3200HZ)){
Reiko 0:22a7683646d1 165 pc.printf("didn't set data rate\n");
Reiko 0:22a7683646d1 166 return 0;
Reiko 0:22a7683646d1 167 }
Reiko 0:22a7683646d1 168
Reiko 0:22a7683646d1 169 wait(.001);*/
Reiko 0:22a7683646d1 170
Reiko 0:22a7683646d1 171 //Measurement mode.
Reiko 0:22a7683646d1 172
Reiko 0:22a7683646d1 173 /*if (accelerometer.setPowerControl(MeasurementMode)) {
Reiko 0:22a7683646d1 174 pc.printf("didn't set the power control to measurement\n");
Reiko 0:22a7683646d1 175 return 0;
Reiko 0:22a7683646d1 176 }
Reiko 0:22a7683646d1 177 */
Reiko 0:22a7683646d1 178
Reiko 0:22a7683646d1 179 server.set_blocking(false, 1);
Reiko 0:22a7683646d1 180
Reiko 0:22a7683646d1 181 //int ioExtBefore = ioExt.read();
Reiko 0:22a7683646d1 182 //pc.printf("ioExt: %s\n", byte_to_binary(ioExt.read()));
Reiko 0:22a7683646d1 183 pc.printf("ioExt: %x\n", ioExt.read());
Reiko 0:22a7683646d1 184 ioExt.setDirection(0xC400);
Reiko 0:22a7683646d1 185 ioExt.write(0x0155);
Reiko 0:22a7683646d1 186 //int ioExtAfter = ioExt.read();
Reiko 0:22a7683646d1 187 pc.printf("ioExt: %x\n", ioExt.read());
Reiko 0:22a7683646d1 188 //pc.printf("ioExt: %s\n", byte_to_binary(ioExt.read()));
Reiko 0:22a7683646d1 189
Reiko 0:22a7683646d1 190 ballSens.change(&extChangedCallback);
Reiko 0:22a7683646d1 191 button1.change(&extChangedCallback);
Reiko 0:22a7683646d1 192 button2.change(&extChangedCallback);
Reiko 0:22a7683646d1 193
Reiko 0:22a7683646d1 194 //InterruptIn change(p8);
Reiko 0:22a7683646d1 195 //change.rise(&ballCallBack);
Reiko 0:22a7683646d1 196 //change.fall(&ballCallBack);
Reiko 0:22a7683646d1 197
Reiko 0:22a7683646d1 198
Reiko 0:22a7683646d1 199 redLed.set();
Reiko 0:22a7683646d1 200 //blueLed.set();
Reiko 0:22a7683646d1 201 //yellowLed.set();
Reiko 0:22a7683646d1 202
Reiko 0:22a7683646d1 203 //int charCount = sprintf(sendBuffer, "<ready>");
Reiko 0:22a7683646d1 204 //server.sendTo(client, sendBuffer, charCount);
Reiko 0:22a7683646d1 205
Reiko 0:22a7683646d1 206 sensorUpdate.attach(&updateTick, 1.0 / 60.0);
Reiko 0:22a7683646d1 207
Reiko 0:22a7683646d1 208 led1 = 1;
Reiko 0:22a7683646d1 209
Reiko 0:22a7683646d1 210 while (1) {
Reiko 0:22a7683646d1 211 //coilgun.kick(5000);
Reiko 0:22a7683646d1 212 //kickFinish.detach();
Reiko 0:22a7683646d1 213 //kickFinish.attach_us(&coilgun, &CoilGun::kickEnd, 1000);
Reiko 0:22a7683646d1 214
Reiko 0:22a7683646d1 215 if (update) {
Reiko 0:22a7683646d1 216 update = 0;
Reiko 0:22a7683646d1 217 ioExt.writePins();
Reiko 0:22a7683646d1 218 //coilgun.kick(100);
Reiko 0:22a7683646d1 219
Reiko 0:22a7683646d1 220 //led3 = 1;
Reiko 0:22a7683646d1 221 //updateSensors();
Reiko 0:22a7683646d1 222 //pc.printf("update");
Reiko 0:22a7683646d1 223 /*motor1.pid();
Reiko 0:22a7683646d1 224 motor2.pid();
Reiko 0:22a7683646d1 225 motor3.pid();
Reiko 0:22a7683646d1 226 motor4.pid();
Reiko 0:22a7683646d1 227 motor5.pid();*/
Reiko 0:22a7683646d1 228
Reiko 0:22a7683646d1 229 //led3 = 0;
Reiko 0:22a7683646d1 230 //int charCount = sprintf(sendBuffer, "<speeds:%d:%d:%d:%d:%d>", motor1.getSpeed(), motor2.getSpeed(), motor3.getSpeed(), motor4.getSpeed(), motor5.getSpeed());
Reiko 0:22a7683646d1 231 //server.sendTo(client, sendBuffer, charCount);
Reiko 0:22a7683646d1 232 //pc.printf("<speeds:%d:%d:%d:%d:%d>\n", motor1.getSpeed(), motor2.getSpeed(), motor3.getSpeed(), motor4.getSpeed(), motor5.getSpeed());
Reiko 0:22a7683646d1 233 //redLed.toggle();
Reiko 0:22a7683646d1 234 led1 = !led1;
Reiko 0:22a7683646d1 235 }
Reiko 0:22a7683646d1 236
Reiko 0:22a7683646d1 237 if (extInChanged) {
Reiko 0:22a7683646d1 238 extInChanged = 0;
Reiko 0:22a7683646d1 239 bool ballState = ballSens.read();
Reiko 0:22a7683646d1 240 bool button1State = button1.read();
Reiko 0:22a7683646d1 241 bool button2State = button2.read();
Reiko 0:22a7683646d1 242 if (button1State) {
Reiko 0:22a7683646d1 243 led1 = 1;
Reiko 0:22a7683646d1 244 } else {
Reiko 0:22a7683646d1 245 led1 = 0;
Reiko 0:22a7683646d1 246 }
Reiko 0:22a7683646d1 247 pc.printf("<ball:%d>\n", ballState ? 1 : 0);
Reiko 0:22a7683646d1 248 pc.printf("<btn1:%d>\n", button1State ? 1 : 0);
Reiko 0:22a7683646d1 249 pc.printf("<btn2:%d>\n", button2State ? 1 : 0);
Reiko 0:22a7683646d1 250 }
Reiko 0:22a7683646d1 251
Reiko 0:22a7683646d1 252 //printf("\nWait for packet...\n");
Reiko 0:22a7683646d1 253
Reiko 0:22a7683646d1 254 int n = server.receiveFrom(client, buffer, sizeof(buffer));
Reiko 0:22a7683646d1 255
Reiko 0:22a7683646d1 256 if (n > 0) {
Reiko 0:22a7683646d1 257 //pc.printf("Received packet from: %s\n", client.get_address());
Reiko 0:22a7683646d1 258 //pc.printf("n: %d\n", n);
Reiko 0:22a7683646d1 259 buffer[n] = '\0';
Reiko 0:22a7683646d1 260 //server.sendTo(client, buffer, n);
Reiko 0:22a7683646d1 261 executeCommandOld(buffer);
Reiko 0:22a7683646d1 262 //coilgun.kick(1000);
Reiko 0:22a7683646d1 263 //executeCommand((short*)buffer);
Reiko 0:22a7683646d1 264 }
Reiko 0:22a7683646d1 265
Reiko 0:22a7683646d1 266 //int n = pc.readable();
Reiko 0:22a7683646d1 267 /*if (pc.readable()) {
Reiko 0:22a7683646d1 268 //pc.printf("Received packet from: %s\n", client.get_address());
Reiko 0:22a7683646d1 269 //pc.printf("n: %d\n", n);
Reiko 0:22a7683646d1 270 //pc.scanf("%s", &buffer);
Reiko 0:22a7683646d1 271 buffer[charCounter] = pc.getc();
Reiko 0:22a7683646d1 272 //pc.printf("%c\n", buffer[charCounter]);
Reiko 0:22a7683646d1 273 //fflush(stdout);
Reiko 0:22a7683646d1 274 //fflush(stdin);
Reiko 0:22a7683646d1 275 //pc.printf("%s\n", buffer);
Reiko 0:22a7683646d1 276 if (buffer[charCounter] == '\n') {
Reiko 0:22a7683646d1 277 buffer[charCounter] = '\0';
Reiko 0:22a7683646d1 278 executeCommand(buffer);
Reiko 0:22a7683646d1 279 charCounter = 0;
Reiko 0:22a7683646d1 280 } else {
Reiko 0:22a7683646d1 281 charCounter++;
Reiko 0:22a7683646d1 282 }
Reiko 0:22a7683646d1 283
Reiko 0:22a7683646d1 284 //server.sendTo(client, buffer, n);
Reiko 0:22a7683646d1 285 }*/
Reiko 0:22a7683646d1 286 }
Reiko 0:22a7683646d1 287 }
Reiko 0:22a7683646d1 288
Reiko 0:22a7683646d1 289 void executeCommand(short *cmd) {
Reiko 0:22a7683646d1 290 if (cmd[0] == 1) {
Reiko 0:22a7683646d1 291 motor1.setSpeed(cmd[1]);
Reiko 0:22a7683646d1 292 motor2.setSpeed(cmd[2]);
Reiko 0:22a7683646d1 293 motor3.setSpeed(cmd[3]);
Reiko 0:22a7683646d1 294 motor4.setSpeed(cmd[4]);
Reiko 0:22a7683646d1 295 motor5.setSpeed(cmd[5]);
Reiko 0:22a7683646d1 296 } else if (cmd[0] == 2) {
Reiko 0:22a7683646d1 297 int charCount = sprintf(sendBuffer, "<speeds:%d:%d:%d:%d:%d>", motor1.getSpeed(), motor2.getSpeed(), motor3.getSpeed(), motor4.getSpeed(), motor5.getSpeed());
Reiko 0:22a7683646d1 298 //pc.printf("%d\n", sizeof(sendBuffer));
Reiko 0:22a7683646d1 299 //pc.printf("sendTo %s\n", client.get_address());
Reiko 0:22a7683646d1 300 server.sendTo(client, sendBuffer, charCount);
Reiko 0:22a7683646d1 301 led2 = !led2;
Reiko 0:22a7683646d1 302 } else if (cmd[0] == 3) {
Reiko 0:22a7683646d1 303 pc.printf("kick\n");
Reiko 0:22a7683646d1 304 coilgun.kick(cmd[1]);
Reiko 0:22a7683646d1 305 } else if (cmd[0] == 4) {
Reiko 0:22a7683646d1 306 pc.printf("charge\n");
Reiko 0:22a7683646d1 307 coilgun.setCharge(cmd[1]);
Reiko 0:22a7683646d1 308 }
Reiko 0:22a7683646d1 309 }
Reiko 0:22a7683646d1 310
Reiko 0:22a7683646d1 311 void executeCommandOld(char *buffer) {
Reiko 0:22a7683646d1 312 pc.printf("%s\n", buffer);
Reiko 0:22a7683646d1 313 char *cmd;
Reiko 0:22a7683646d1 314 cmd = strtok(buffer, " ");
Reiko 0:22a7683646d1 315
Reiko 0:22a7683646d1 316 //pc.printf("%s\n", cmd);
Reiko 0:22a7683646d1 317
Reiko 0:22a7683646d1 318 if (strncmp(cmd, "speeds", 6) == 0) {
Reiko 0:22a7683646d1 319 motor1.setSpeed(atoi(strtok(NULL, " ")));
Reiko 0:22a7683646d1 320 motor2.setSpeed(atoi(strtok(NULL, " ")));
Reiko 0:22a7683646d1 321 motor3.setSpeed(atoi(strtok(NULL, " ")));
Reiko 0:22a7683646d1 322 motor4.setSpeed(atoi(strtok(NULL, " ")));
Reiko 0:22a7683646d1 323 motor5.setSpeed(atoi(strtok(NULL, " ")));
Reiko 0:22a7683646d1 324 } else if (strncmp(cmd, "gs", 2) == 0) {
Reiko 0:22a7683646d1 325 int charCount = sprintf(sendBuffer, "<speeds:%d:%d:%d:%d:%d>", motor1.getSpeed(), motor2.getSpeed(), motor3.getSpeed(), motor4.getSpeed(), motor5.getSpeed());
Reiko 0:22a7683646d1 326 //pc.printf("%d\n", sizeof(sendBuffer));
Reiko 0:22a7683646d1 327 //pc.printf("sendTo %s\n", client.get_address());
Reiko 0:22a7683646d1 328 server.sendTo(client, sendBuffer, charCount);
Reiko 0:22a7683646d1 329 led2 = !led2;
Reiko 0:22a7683646d1 330 } else if (strncmp(cmd, "k", 1) == 0) {
Reiko 0:22a7683646d1 331 pc.printf("kick\n");
Reiko 0:22a7683646d1 332 int length = atoi(strtok(NULL, " "));
Reiko 0:22a7683646d1 333 coilgun.kick(length);
Reiko 0:22a7683646d1 334 //kickFinish.detach();
Reiko 0:22a7683646d1 335 //kickFinish.attach_us(&coilgun, &CoilGun::kickEnd, length);
Reiko 0:22a7683646d1 336 } else if (strncmp(cmd, "c", 1) == 0) {
Reiko 0:22a7683646d1 337 pc.printf("charge\n");
Reiko 0:22a7683646d1 338 coilgun.setCharge(atoi(strtok(NULL, " ")));
Reiko 0:22a7683646d1 339 } else if (strncmp(cmd, "d", 1) == 0) {
Reiko 0:22a7683646d1 340 pc.printf("discharge\n");
Reiko 0:22a7683646d1 341 coilgun.discharge();
Reiko 0:22a7683646d1 342 } /*else if (strncmp(cmd, "t", 1) == 0) {
Reiko 0:22a7683646d1 343 float newPWM = atof(strtok(NULL, " "));
Reiko 0:22a7683646d1 344 motor1.setPWM(newPWM);
Reiko 0:22a7683646d1 345 motor2.setPWM(newPWM);
Reiko 0:22a7683646d1 346 motor3.setPWM(newPWM);
Reiko 0:22a7683646d1 347 motor4.setPWM(newPWM);
Reiko 0:22a7683646d1 348 motor5.setPWM(newPWM);
Reiko 0:22a7683646d1 349 }*/
Reiko 0:22a7683646d1 350 }