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@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?
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 | 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 | } |