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
main.cpp@4:c5cf0676baca, 2013-09-23 (annotated)
- Committer:
- Reiko
- Date:
- Mon Sep 23 14:05:38 2013 +0000
- Revision:
- 4:c5cf0676baca
- Parent:
- 3:4ec313b1b314
- Child:
- 5:b30229c6f32b
Added reset command
Who changed what in which revision?
User | Revision | Line number | New 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 | 4:c5cf0676baca | 445 | } else if (strncmp(cmd, "reset", 5) == 0) { |
Reiko | 4:c5cf0676baca | 446 | motor1.setSpeed(0); |
Reiko | 4:c5cf0676baca | 447 | motor2.setSpeed(0); |
Reiko | 4:c5cf0676baca | 448 | motor3.setSpeed(0); |
Reiko | 4:c5cf0676baca | 449 | motor4.setSpeed(0); |
Reiko | 4:c5cf0676baca | 450 | motor5.setSpeed(0); |
Reiko | 4:c5cf0676baca | 451 | humanInterface.setGoal(HumanInterface::UNSET); |
Reiko | 4:c5cf0676baca | 452 | humanInterface.setError(false); |
Reiko | 1:79ac4e293661 | 453 | } else if (strncmp(cmd, "fs", 2) == 0) { |
Reiko | 1:79ac4e293661 | 454 | failSafeEnabled = (bool)atoi(strtok(NULL, ":")); |
Reiko | 1:79ac4e293661 | 455 | } else if (strncmp(cmd, "target", 6) == 0) { |
Reiko | 3:4ec313b1b314 | 456 | int target = atoi(strtok(NULL, ":")); |
Reiko | 3:4ec313b1b314 | 457 | if (target == 0) { |
Reiko | 3:4ec313b1b314 | 458 | humanInterface.setGoal(HumanInterface::BLUE); |
Reiko | 3:4ec313b1b314 | 459 | } else if (target == 1) { |
Reiko | 1:79ac4e293661 | 460 | humanInterface.setGoal(HumanInterface::YELLOW); |
Reiko | 3:4ec313b1b314 | 461 | } else if (target == 2) { |
Reiko | 3:4ec313b1b314 | 462 | humanInterface.setGoal(HumanInterface::UNSET); |
Reiko | 1:79ac4e293661 | 463 | } |
Reiko | 1:79ac4e293661 | 464 | } else if (strncmp(cmd, "error", 5) == 0) { |
Reiko | 1:79ac4e293661 | 465 | humanInterface.setError(atoi(strtok(NULL, ":"))); |
Reiko | 1:79ac4e293661 | 466 | } |
Reiko | 0:22a7683646d1 | 467 | } |