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@11:92eecb155136, 2014-11-23 (annotated)

Committer:
Reiko
Date:
Sun Nov 23 02:29:08 2014 +0000
Revision:
11:92eecb155136
Parent:
9:2d2030d989d8 
Serial buffer changes

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Reiko 0:22a7683646d1 1 #include "mbed.h"
Reiko 9:2d2030d989d8 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 9:2d2030d989d8 9 //#include "ledout.h"
Reiko 9:2d2030d989d8 10 //#include "rgb-led-pca9555.h"
Reiko 9:2d2030d989d8 11 //#include "externalin.h"
Reiko 0:22a7683646d1 12 #include "coilgun.h"
Reiko 1:79ac4e293661 13 #include "HumanInterface.h"
Reiko 8:06124632c3e4 14 //#include "MPU6050.h"
Reiko 8:06124632c3e4 15 //#include "imu.h"
Reiko 9:2d2030d989d8 16 #include "MCP3021.h"
Reiko 9:2d2030d989d8 17 //#include "PinDetect.h"
Reiko 9:2d2030d989d8 18 //#include "uLCD_4DGL.h"
Reiko 9:2d2030d989d8 19 //#include "fsl_uart_hal.h"
Reiko 9:2d2030d989d8 20 #include "MODSERIAL.h"
Reiko 9:2d2030d989d8 21 #include "Watchdog.h"
Reiko 0:22a7683646d1 22
Reiko 0:22a7683646d1 23 #define SERVER_PORT 8042
Reiko 0:22a7683646d1 24
Reiko 11:92eecb155136 25 Watchdog watchdog;
Reiko 9:2d2030d989d8 26
Reiko 0:22a7683646d1 27 Ticker sensorUpdate;
Reiko 8:06124632c3e4 28 Ticker imuUpdate;
Reiko 8:06124632c3e4 29 Ticker testUpdate;
Reiko 0:22a7683646d1 30
Reiko 9:2d2030d989d8 31 DigitalOut led1(LED_RED);
Reiko 9:2d2030d989d8 32 DigitalOut led3(LED_GREEN);
Reiko 9:2d2030d989d8 33 DigitalOut led4(LED_BLUE);
Reiko 0:22a7683646d1 34
Reiko 9:2d2030d989d8 35 //Serial pc(USBTX, USBRX); // tx, rx
Reiko 11:92eecb155136 36 MODSERIAL pc(USBTX, USBRX, 512, 512);
Reiko 0:22a7683646d1 37 //ADXL345_I2C accelerometer(p9, p10);
Reiko 0:22a7683646d1 38 //L3G4200D gyro(p9, p10);
Reiko 0:22a7683646d1 39 //HMC5883L compass(p9, p10);
Reiko 0:22a7683646d1 40
Reiko 9:2d2030d989d8 41
Reiko 9:2d2030d989d8 42 MCP3021 coilADC(PTC11, PTC10, 5.0);
Reiko 9:2d2030d989d8 43 PCA9555 ioExt(PTC11, PTC10, PTB20, 0x40);
Reiko 0:22a7683646d1 44
Reiko 0:22a7683646d1 45 //int readings[3] = {0, 0, 0};
Reiko 0:22a7683646d1 46 //int gyroData[3] = {0, 0, 0};
Reiko 0:22a7683646d1 47 //int16_t compassData[3] = {0, 0, 0};
Reiko 0:22a7683646d1 48
Reiko 8:06124632c3e4 49 volatile int update = 0;
Reiko 8:06124632c3e4 50 volatile int updateIMU = 0;
Reiko 0:22a7683646d1 51
Reiko 8:06124632c3e4 52 volatile int updateTest = 0;
mlaane 6:5619aff36840 53
Reiko 1:79ac4e293661 54 volatile int stallChanged = 0;
Reiko 1:79ac4e293661 55
Reiko 9:2d2030d989d8 56 bool failSafeEnabled = true;
Reiko 9:2d2030d989d8 57 int failSafeCountMotors = 0;
Reiko 9:2d2030d989d8 58 int failSafeCountCoilgun = 0;
Reiko 9:2d2030d989d8 59 int failSafeLimitMotors = 60;
Reiko 9:2d2030d989d8 60 int failSafeLimitCoilgun = 300;
Reiko 1:79ac4e293661 61
Reiko 0:22a7683646d1 62
Reiko 5:b30229c6f32b 63 void executeCommand(char *buffer);
Reiko 0:22a7683646d1 64
Reiko 9:2d2030d989d8 65 /*static void enable_rx_fifo(int instance) {
Reiko 9:2d2030d989d8 66 uart_hal_disable_receiver(instance);
Reiko 9:2d2030d989d8 67 uart_hal_disable_transmitter(instance);
Reiko 9:2d2030d989d8 68 uart_hal_enable_rx_fifo(instance);
Reiko 9:2d2030d989d8 69 uart_hal_enable_receiver(instance);
Reiko 9:2d2030d989d8 70 uart_hal_enable_transmitter(instance);
Reiko 9:2d2030d989d8 71 uart_hal_flush_rx_fifo(instance);
Reiko 9:2d2030d989d8 72 uart_hal_flush_tx_fifo(instance);
Reiko 9:2d2030d989d8 73 }*/
Reiko 9:2d2030d989d8 74
Reiko 0:22a7683646d1 75 /*
Reiko 0:22a7683646d1 76 void updateSensors() {
Reiko 0:22a7683646d1 77 if (led1 == 1.0) {
Reiko 0:22a7683646d1 78 led1 = 0;
Reiko 0:22a7683646d1 79 } else {
Reiko 0:22a7683646d1 80 led1 = 1.0;
Reiko 0:22a7683646d1 81 }
Reiko 0:22a7683646d1 82 accelerometer.getOutput(readings);
Reiko 0:22a7683646d1 83 pc.printf("<acc:%i:%i:%i>\n", (int16_t)readings[0], (int16_t)readings[1], (int16_t)readings[2]);
Reiko 0:22a7683646d1 84
Reiko 0:22a7683646d1 85 gyro.read(gyroData);
Reiko 0:22a7683646d1 86 pc.printf("<gyro:%i:%i:%i>\n", (int16_t)gyroData[0], (int16_t)gyroData[1], (int16_t)gyroData[2]);
Reiko 0:22a7683646d1 87
Reiko 0:22a7683646d1 88 compass.getXYZ(compassData);
Reiko 0:22a7683646d1 89 pc.printf("<comps:%i:%i:%i>\n", compassData[0], compassData[1], compassData[2]);
Reiko 0:22a7683646d1 90 }*/
Reiko 0:22a7683646d1 91
Reiko 9:2d2030d989d8 92 //Dribbler motor without encoder
Reiko 9:2d2030d989d8 93 //PwmOut dribbler(PTC1);
Reiko 9:2d2030d989d8 94
Reiko 1:79ac4e293661 95 //motor order: FL, FR, RL, RR, DRIBBLER
Reiko 9:2d2030d989d8 96 //Motor(PinName PWMpin, PCA9555 *ioExt, unsigned int dirPin, PinName encA, PinName encB);
Reiko 9:2d2030d989d8 97 Motor motor3(PTA2, &ioExt, 2, PTC18, PTD6); //RL - M1
Reiko 9:2d2030d989d8 98 Motor motor1(PTA1, &ioExt, 1, PTD7, PTD5); //FL - M2
Reiko 9:2d2030d989d8 99 Motor motor5(PTC1, &ioExt, 0, PTD4, PTC12); //DRIBBLER - M3
Reiko 9:2d2030d989d8 100 Motor motor2(PTC4, &ioExt, 5, PTB18, PTB19); //FR - M4
Reiko 9:2d2030d989d8 101 Motor motor4(PTC3, &ioExt, 4, PTC8, PTC9); //RR - M5
Reiko 9:2d2030d989d8 102
Reiko 9:2d2030d989d8 103 Coilgun coilgun(PTB11, PTB10, PTB3, PTB2);
Reiko 9:2d2030d989d8 104 //Coilgun coilgun(PTB11, PTB10, PTB3);
Reiko 9:2d2030d989d8 105
Reiko 9:2d2030d989d8 106 //PinDetect buttonGoal(PTC15);
Reiko 9:2d2030d989d8 107 //PinDetect buttonStart(PTC14);
Reiko 9:2d2030d989d8 108 //PinDetect inputBall(PTB23);
Reiko 9:2d2030d989d8 109
Reiko 9:2d2030d989d8 110 //InterruptIn ball(PTB23);
Reiko 9:2d2030d989d8 111
Reiko 9:2d2030d989d8 112 bool goalButtonPressed = false;
Reiko 9:2d2030d989d8 113 bool startButtonReleased = false;
Reiko 9:2d2030d989d8 114 bool ballStateChanged = false;
Reiko 9:2d2030d989d8 115 int ballState = 0;
Reiko 9:2d2030d989d8 116
Reiko 9:2d2030d989d8 117 float coilCapVoltage = 0.0f;
Reiko 9:2d2030d989d8 118
Reiko 9:2d2030d989d8 119 /*void goalFall() {
Reiko 9:2d2030d989d8 120 goalButtonPressed = true;
Reiko 9:2d2030d989d8 121 }
Reiko 9:2d2030d989d8 122
Reiko 9:2d2030d989d8 123 void startRise() {
Reiko 9:2d2030d989d8 124 startButtonReleased = true;
Reiko 9:2d2030d989d8 125 }
Reiko 9:2d2030d989d8 126
Reiko 9:2d2030d989d8 127 void ballRise() {
Reiko 9:2d2030d989d8 128 ballState = 1;
Reiko 9:2d2030d989d8 129 ballStateChanged = true;
Reiko 9:2d2030d989d8 130 }
Reiko 9:2d2030d989d8 131
Reiko 9:2d2030d989d8 132 void ballFall() {
Reiko 9:2d2030d989d8 133 ballState = 0;
Reiko 9:2d2030d989d8 134 ballStateChanged = true;
Reiko 9:2d2030d989d8 135 }*/
Reiko 9:2d2030d989d8 136
Reiko 9:2d2030d989d8 137 //RgbLed rgbLed1(&ioExt, 14, 15, 13);
Reiko 9:2d2030d989d8 138 //RgbLed rgbLed2(&ioExt, 11, 12, 10);
Reiko 9:2d2030d989d8 139
Reiko 9:2d2030d989d8 140 HumanInterface humanInterface(&ioExt, 14, 15, 13, 11, 12, 10, PTC15, PTC14, PTB23);
Reiko 9:2d2030d989d8 141
Reiko 9:2d2030d989d8 142 //InterruptIn sw2(SW2);
Reiko 9:2d2030d989d8 143
Reiko 9:2d2030d989d8 144 PwmOut servo1(PTD1);
Reiko 9:2d2030d989d8 145 PwmOut servo2(PTD0);
Reiko 9:2d2030d989d8 146
Reiko 9:2d2030d989d8 147 //uLCD_4DGL uLCD(PTC17, PTC16, PTB9);
Reiko 9:2d2030d989d8 148
Reiko 9:2d2030d989d8 149 char buffer[256];
Reiko 9:2d2030d989d8 150 char sendBuffer[256];
Reiko 9:2d2030d989d8 151 int charCounter = 0;
Reiko 9:2d2030d989d8 152
Reiko 11:92eecb155136 153 char serialBuffer[512];
Reiko 0:22a7683646d1 154
Reiko 9:2d2030d989d8 155 // This function is called when a character goes from the TX buffer
Reiko 9:2d2030d989d8 156 // to the Uart THR FIFO register.
Reiko 9:2d2030d989d8 157 /*void txCallback(MODSERIAL_IRQ_INFO *q) {
Reiko 9:2d2030d989d8 158 led2 = !led2;
Reiko 9:2d2030d989d8 159 }*/
Reiko 9:2d2030d989d8 160
Reiko 9:2d2030d989d8 161 // This function is called when TX buffer goes empty
Reiko 9:2d2030d989d8 162 /*void txEmpty(MODSERIAL_IRQ_INFO *q) {
Reiko 9:2d2030d989d8 163 led2 = 0;
Reiko 9:2d2030d989d8 164 //pc.puts(" Done. ");
Reiko 9:2d2030d989d8 165 }*/
Reiko 9:2d2030d989d8 166
Reiko 9:2d2030d989d8 167 // This function is called when a character goes into the RX buffer.
Reiko 9:2d2030d989d8 168 /*void rxCallback(MODSERIAL_IRQ_INFO *q) {
Reiko 9:2d2030d989d8 169 //led3 = !led3;
Reiko 9:2d2030d989d8 170
Reiko 9:2d2030d989d8 171 while (pc.readable()) {
Reiko 9:2d2030d989d8 172 serialBuffer[charCounter] = pc.getc();
Reiko 9:2d2030d989d8 173 if (serialBuffer[charCounter] == '\n') {
Reiko 9:2d2030d989d8 174 serialBuffer[charCounter] = '\0';
Reiko 9:2d2030d989d8 175 //led2 = !led2;
Reiko 9:2d2030d989d8 176 executeCommand(serialBuffer);
Reiko 9:2d2030d989d8 177 charCounter = 0;
Reiko 9:2d2030d989d8 178 } else {
Reiko 9:2d2030d989d8 179 charCounter++;
Reiko 9:2d2030d989d8 180 }
Reiko 9:2d2030d989d8 181 }
Reiko 9:2d2030d989d8 182 }*/
Reiko 8:06124632c3e4 183
Reiko 9:2d2030d989d8 184 void PcRxIRQ() {
Reiko 9:2d2030d989d8 185 // Note: you need to actually read from the serial to clear the RX interrupt
Reiko 9:2d2030d989d8 186 printf("%c\n", pc.getc());
Reiko 9:2d2030d989d8 187
Reiko 9:2d2030d989d8 188 /*while (pc.readable()) {
Reiko 9:2d2030d989d8 189 serialBuffer[charCounter] = pc.getc();
Reiko 9:2d2030d989d8 190 if (serialBuffer[charCounter] == '\n') {
Reiko 9:2d2030d989d8 191 serialBuffer[charCounter] = '\0';
Reiko 9:2d2030d989d8 192 executeCommand(serialBuffer);
Reiko 9:2d2030d989d8 193 charCounter = 0;
Reiko 9:2d2030d989d8 194 } else {
Reiko 9:2d2030d989d8 195 charCounter++;
Reiko 9:2d2030d989d8 196 }
Reiko 9:2d2030d989d8 197 }*/
Reiko 9:2d2030d989d8 198 }
Reiko 0:22a7683646d1 199
Reiko 9:2d2030d989d8 200 void serialReceive() {
Reiko 9:2d2030d989d8 201 // Note: you need to actually read from the serial to clear the RX interrupt
Reiko 9:2d2030d989d8 202 printf("%c\n", pc.getc());
Reiko 9:2d2030d989d8 203 //led2 = !led2;
Reiko 9:2d2030d989d8 204 }
Reiko 9:2d2030d989d8 205
Reiko 9:2d2030d989d8 206 void sw2_release(void) {
Reiko 9:2d2030d989d8 207 led3 = !led3;
Reiko 9:2d2030d989d8 208 }
Reiko 9:2d2030d989d8 209
Reiko 9:2d2030d989d8 210 void sw2_press(void) {
Reiko 9:2d2030d989d8 211 led1 = !led1;
Reiko 9:2d2030d989d8 212 }
Reiko 9:2d2030d989d8 213
Reiko 9:2d2030d989d8 214 /*PinDetect buttonGoal(PTC14);
Reiko 9:2d2030d989d8 215 int goalStateChanged = 0;
Reiko 9:2d2030d989d8 216 int goalState = 0;
Reiko 9:2d2030d989d8 217
Reiko 9:2d2030d989d8 218 void goalFall() {
Reiko 9:2d2030d989d8 219 goalState ^= 1;
Reiko 9:2d2030d989d8 220 goalStateChanged = 1;
Reiko 9:2d2030d989d8 221 }*/
Reiko 8:06124632c3e4 222
Reiko 8:06124632c3e4 223 //IMU imu(0x69);
Reiko 1:79ac4e293661 224
Reiko 0:22a7683646d1 225 void updateTick() {
Reiko 0:22a7683646d1 226 //led3 = 1;
Reiko 0:22a7683646d1 227 motor1.pid();
Reiko 0:22a7683646d1 228 motor2.pid();
Reiko 0:22a7683646d1 229 motor3.pid();
Reiko 0:22a7683646d1 230 motor4.pid();
Reiko 0:22a7683646d1 231 motor5.pid();
Reiko 0:22a7683646d1 232 //led3 = 0;
Reiko 0:22a7683646d1 233 update = 1;
Reiko 0:22a7683646d1 234 }
Reiko 0:22a7683646d1 235
Reiko 8:06124632c3e4 236 void imuUpdateTick() {
Reiko 8:06124632c3e4 237 updateIMU = 1;
Reiko 8:06124632c3e4 238 }
Reiko 8:06124632c3e4 239
Reiko 8:06124632c3e4 240 void testUpdateTick() {
Reiko 8:06124632c3e4 241 updateTest = 1;
Reiko 8:06124632c3e4 242 }
Reiko 8:06124632c3e4 243
Reiko 1:79ac4e293661 244 void stallChangedCallback() {
Reiko 1:79ac4e293661 245 stallChanged = 1;
Reiko 1:79ac4e293661 246 }
Reiko 1:79ac4e293661 247
Reiko 9:2d2030d989d8 248 //UDPSocket server;
Reiko 9:2d2030d989d8 249 //Endpoint client;
Reiko 0:22a7683646d1 250
Reiko 0:22a7683646d1 251 //void readSerial() {
Reiko 0:22a7683646d1 252
Reiko 0:22a7683646d1 253 //int n = pc.readable();
Reiko 9:2d2030d989d8 254 /*if (pc.readable()) {
Reiko 0:22a7683646d1 255
Reiko 0:22a7683646d1 256 //pc.printf("Received packet from: %s\n", client.get_address());
Reiko 0:22a7683646d1 257 //pc.printf("n: %d\n", n);
Reiko 0:22a7683646d1 258 //pc.scanf("%s", &buffer);
Reiko 0:22a7683646d1 259 //pc.printf("%c\n", pc.getc());
Reiko 0:22a7683646d1 260 //buffer[charCounter] = pc.getc();
Reiko 9:2d2030d989d8 261 //printf("%c\n", buffer[charCounter]);
Reiko 0:22a7683646d1 262 //pc.printf("%s\n", buffer);
Reiko 9:2d2030d989d8 263 //if (buffer[charCounter] == '\n') {
Reiko 9:2d2030d989d8 264 // buffer[charCounter] = '\0';
Reiko 9:2d2030d989d8 265 // executeCommand(buffer);
Reiko 9:2d2030d989d8 266 // charCounter = 0;
Reiko 9:2d2030d989d8 267 //} else {
Reiko 9:2d2030d989d8 268 // charCounter++;
Reiko 0:22a7683646d1 269 }*/
Reiko 0:22a7683646d1 270 //}
Reiko 0:22a7683646d1 271
Reiko 0:22a7683646d1 272 //}
Reiko 0:22a7683646d1 273
Reiko 9:2d2030d989d8 274 Timer t;
Reiko 9:2d2030d989d8 275 Timer aliveTime;
Reiko 9:2d2030d989d8 276
Reiko 9:2d2030d989d8 277 float dtMax = 0.0f;
Reiko 0:22a7683646d1 278
Reiko 9:2d2030d989d8 279 unsigned int currentKickLength = 0;
Reiko 9:2d2030d989d8 280 unsigned int currentKickDelay = 0;
Reiko 9:2d2030d989d8 281 unsigned int currentChipLength = 0;
Reiko 9:2d2030d989d8 282 unsigned int currentChipDelay = 0;
Reiko 9:2d2030d989d8 283 bool kickWhenBall = false;
Reiko 9:2d2030d989d8 284 bool sendKicked = false;
Reiko 0:22a7683646d1 285
Reiko 9:2d2030d989d8 286 //EthernetInterface eth;
Reiko 8:06124632c3e4 287
Reiko 0:22a7683646d1 288 int main (void) {
Reiko 9:2d2030d989d8 289
Reiko 9:2d2030d989d8 290 //enable_rx_fifo(0);
Reiko 9:2d2030d989d8 291
Reiko 0:22a7683646d1 292 pc.baud(115200);
Reiko 9:2d2030d989d8 293
Reiko 9:2d2030d989d8 294 aliveTime.start();
Reiko 9:2d2030d989d8 295
Reiko 9:2d2030d989d8 296 //pc.attach(&txCallback, MODSERIAL::TxIrq);
Reiko 9:2d2030d989d8 297 //pc.attach(&rxCallback, MODSERIAL::RxIrq);
Reiko 9:2d2030d989d8 298 //pc.attach(&txEmpty, MODSERIAL::TxEmpty);
Reiko 9:2d2030d989d8 299
Reiko 9:2d2030d989d8 300 // enable the usb uart rx fifo
Reiko 9:2d2030d989d8 301 //UART_PFIFO_REG(UART0) |= 0x08;
Reiko 9:2d2030d989d8 302
Reiko 9:2d2030d989d8 303 // enable the UART Rx callback interrupt
Reiko 9:2d2030d989d8 304 //pc.attach(&PcRxIRQ, pc.RxIrq);
Reiko 9:2d2030d989d8 305
Reiko 9:2d2030d989d8 306 //pc.attach(&onSerialData);
Reiko 9:2d2030d989d8 307
Reiko 9:2d2030d989d8 308 //uLCD.baudrate(3000000);
Reiko 9:2d2030d989d8 309
Reiko 9:2d2030d989d8 310 //RED
Reiko 9:2d2030d989d8 311 led1 = 0;
Reiko 9:2d2030d989d8 312 led3 = 1;
Reiko 9:2d2030d989d8 313 led4 = 1;
Reiko 9:2d2030d989d8 314
Reiko 9:2d2030d989d8 315 //rgbLed1.setColor(RgbLed::WHITE);
Reiko 9:2d2030d989d8 316 //rgbLed2.setColor(RgbLed::YELLOW);
Reiko 9:2d2030d989d8 317
Reiko 9:2d2030d989d8 318 //chargeDone.rise(&sw2_release);
Reiko 9:2d2030d989d8 319
Reiko 9:2d2030d989d8 320 //GREEN
Reiko 9:2d2030d989d8 321 led1 = 1;
Reiko 9:2d2030d989d8 322 led3 = 0;
Reiko 9:2d2030d989d8 323 led4 = 1;
Reiko 9:2d2030d989d8 324
Reiko 9:2d2030d989d8 325
Reiko 9:2d2030d989d8 326 //YELLOW
Reiko 9:2d2030d989d8 327 led1 = 0;
Reiko 9:2d2030d989d8 328 led3 = 0;
Reiko 9:2d2030d989d8 329 led4 = 1;
Reiko 9:2d2030d989d8 330
Reiko 9:2d2030d989d8 331 //eth.init("192.168.4.1", "255.255.255.0", "192.168.4.8");
Reiko 9:2d2030d989d8 332 //eth.init("192.168.0.128", "255.255.255.0", "192.168.0.1");
Reiko 9:2d2030d989d8 333
Reiko 9:2d2030d989d8 334 //wait(3);
Reiko 9:2d2030d989d8 335
Reiko 8:06124632c3e4 336 //eth.init();
Reiko 9:2d2030d989d8 337
Reiko 9:2d2030d989d8 338 //MAGENTA
Reiko 9:2d2030d989d8 339 led1 = 0;
Reiko 9:2d2030d989d8 340 led3 = 1;
Reiko 9:2d2030d989d8 341 led4 = 0;
Reiko 9:2d2030d989d8 342
Reiko 9:2d2030d989d8 343 //eth.connect(10000);
Reiko 9:2d2030d989d8 344
Reiko 9:2d2030d989d8 345 //WHITE
Reiko 9:2d2030d989d8 346 /*led1 = 0;
Reiko 9:2d2030d989d8 347 led3 = 0;
Reiko 9:2d2030d989d8 348 led4 = 0;*/
Reiko 9:2d2030d989d8 349
Reiko 9:2d2030d989d8 350 //pc.printf("IP Address is %s\n", eth.getIPAddress());
Reiko 0:22a7683646d1 351
Reiko 9:2d2030d989d8 352 //server.bind(SERVER_PORT);
Reiko 9:2d2030d989d8 353
Reiko 9:2d2030d989d8 354 //CYAN
Reiko 9:2d2030d989d8 355 led1 = 1;
Reiko 9:2d2030d989d8 356 led3 = 0;
Reiko 9:2d2030d989d8 357 led4 = 0;
Reiko 9:2d2030d989d8 358
Reiko 9:2d2030d989d8 359 servo1.period_us(200000);
Reiko 9:2d2030d989d8 360
Reiko 9:2d2030d989d8 361 servo1.pulsewidth_us(1500);
Reiko 9:2d2030d989d8 362 servo2.pulsewidth_us(1500);
Reiko 0:22a7683646d1 363
Reiko 0:22a7683646d1 364 //pc.printf("Starting ADXL345 test...\n");
Reiko 0:22a7683646d1 365 //wait(.001);
Reiko 0:22a7683646d1 366 //pc.printf("Device ID is: 0x%02x\n", accelerometer.getDeviceID());
Reiko 0:22a7683646d1 367 //wait(.001);
Reiko 0:22a7683646d1 368
Reiko 0:22a7683646d1 369 // These are here to test whether any of the initialization fails. It will print the failure
Reiko 0:22a7683646d1 370 /*if (accelerometer.setPowerControl(0x00)){
Reiko 0:22a7683646d1 371 pc.printf("didn't intitialize power control\n");
Reiko 0:22a7683646d1 372 return 0;
Reiko 0:22a7683646d1 373 }*/
Reiko 0:22a7683646d1 374 //Full resolution, +/-16g, 4mg/LSB.
Reiko 0:22a7683646d1 375 /*wait(.001);
Reiko 0:22a7683646d1 376
Reiko 0:22a7683646d1 377 if(accelerometer.setDataFormatControl(0x0B)){
Reiko 0:22a7683646d1 378 pc.printf("didn't set data format\n");
Reiko 0:22a7683646d1 379 return 0;
Reiko 0:22a7683646d1 380 }
Reiko 0:22a7683646d1 381
Reiko 0:22a7683646d1 382 wait(.001);*/
Reiko 0:22a7683646d1 383
Reiko 0:22a7683646d1 384 //3.2kHz data rate.
Reiko 0:22a7683646d1 385 /*if(accelerometer.setDataRate(ADXL345_3200HZ)){
Reiko 0:22a7683646d1 386 pc.printf("didn't set data rate\n");
Reiko 0:22a7683646d1 387 return 0;
Reiko 0:22a7683646d1 388 }
Reiko 0:22a7683646d1 389
Reiko 0:22a7683646d1 390 wait(.001);*/
Reiko 0:22a7683646d1 391
Reiko 0:22a7683646d1 392 //Measurement mode.
Reiko 0:22a7683646d1 393
Reiko 0:22a7683646d1 394 /*if (accelerometer.setPowerControl(MeasurementMode)) {
Reiko 0:22a7683646d1 395 pc.printf("didn't set the power control to measurement\n");
Reiko 0:22a7683646d1 396 return 0;
Reiko 0:22a7683646d1 397 }
Reiko 0:22a7683646d1 398 */
Reiko 0:22a7683646d1 399
Reiko 9:2d2030d989d8 400
Reiko 9:2d2030d989d8 401
Reiko 9:2d2030d989d8 402 //server.set_blocking(false, 1);
Reiko 0:22a7683646d1 403
Reiko 0:22a7683646d1 404 //int ioExtBefore = ioExt.read();
Reiko 0:22a7683646d1 405 //pc.printf("ioExt: %s\n", byte_to_binary(ioExt.read()));
Reiko 5:b30229c6f32b 406 //pc.printf("ioExt: %x\n", ioExt.read());
Reiko 9:2d2030d989d8 407 ioExt.setDirection(0x0000);
Reiko 9:2d2030d989d8 408 ioExt.write(0xff00);
Reiko 9:2d2030d989d8 409 int ioExtAfter = ioExt.read();
Reiko 9:2d2030d989d8 410 pc.printf("ioExt: %x\n", ioExt.read());
Reiko 0:22a7683646d1 411 //pc.printf("ioExt: %s\n", byte_to_binary(ioExt.read()));
Reiko 0:22a7683646d1 412
Reiko 9:2d2030d989d8 413 //BLUE
Reiko 9:2d2030d989d8 414 led1 = 1;
Reiko 9:2d2030d989d8 415 led3 = 1;
Reiko 9:2d2030d989d8 416 led4 = 0;
Reiko 9:2d2030d989d8 417
Reiko 9:2d2030d989d8 418
Reiko 1:79ac4e293661 419 motor1.stallChange(&stallChangedCallback);
Reiko 1:79ac4e293661 420 motor2.stallChange(&stallChangedCallback);
Reiko 1:79ac4e293661 421 motor3.stallChange(&stallChangedCallback);
Reiko 1:79ac4e293661 422 motor4.stallChange(&stallChangedCallback);
Reiko 1:79ac4e293661 423 motor5.stallChange(&stallChangedCallback);
Reiko 0:22a7683646d1 424
Reiko 9:2d2030d989d8 425 //buttonGoal.mode(PullUp);
Reiko 9:2d2030d989d8 426 //buttonStart.mode(PullUp);
Reiko 0:22a7683646d1 427
Reiko 9:2d2030d989d8 428 //inputBall.mode(PullUp);
Reiko 9:2d2030d989d8 429 //ball.mode(PullUp);
Reiko 9:2d2030d989d8 430 //ball.fall(&ballFall);
Reiko 9:2d2030d989d8 431 //ball.rise(&ballRise);
Reiko 9:2d2030d989d8 432
Reiko 9:2d2030d989d8 433 //buttonGoal.attach_deasserted(&goalFall);
Reiko 9:2d2030d989d8 434 //buttonStart.attach_asserted(&startRise);
Reiko 9:2d2030d989d8 435 //inputBall.attach_asserted(&ballRise);
Reiko 9:2d2030d989d8 436 //inputBall.attach_deasserted(&ballFall);
Reiko 9:2d2030d989d8 437
Reiko 9:2d2030d989d8 438 //buttonGoal.setSamplesTillAssert(20);
Reiko 9:2d2030d989d8 439 //buttonStart.setSamplesTillAssert(20);
Reiko 9:2d2030d989d8 440 //inputBall.setSamplesTillAssert(2);
Reiko 9:2d2030d989d8 441
Reiko 9:2d2030d989d8 442 //buttonGoal.setSampleFrequency(1000);
Reiko 9:2d2030d989d8 443 //buttonStart.setSampleFrequency(1000);
Reiko 9:2d2030d989d8 444 //inputBall.setSampleFrequency(250);
Reiko 9:2d2030d989d8 445
Reiko 9:2d2030d989d8 446 /*int charCount = sprintf(sendBuffer, "<ready>");
Reiko 9:2d2030d989d8 447 server.sendTo(client, sendBuffer, charCount);*/
Reiko 9:2d2030d989d8 448
Reiko 8:06124632c3e4 449
Reiko 8:06124632c3e4 450 //MPU6050 mpu = imu.getMPU();
Reiko 8:06124632c3e4 451
Reiko 8:06124632c3e4 452 /*bool mpu6050TestResult = mpu.testConnection();
Reiko 8:06124632c3e4 453 if(mpu6050TestResult) {
Reiko 8:06124632c3e4 454 pc.printf("IMU test passed\n");
Reiko 8:06124632c3e4 455 } else {
Reiko 8:06124632c3e4 456 pc.printf("IMU test failed\n");
Reiko 8:06124632c3e4 457 }*/
Reiko 8:06124632c3e4 458
Reiko 8:06124632c3e4 459 //wait(2);
Reiko 8:06124632c3e4 460
Reiko 8:06124632c3e4 461 //pc.printf("IMU calibration\n");
Reiko 8:06124632c3e4 462
Reiko 8:06124632c3e4 463 //imu.zeroGyroZ();
Reiko 8:06124632c3e4 464 //imu.startPolling();
Reiko 0:22a7683646d1 465
Reiko 0:22a7683646d1 466 sensorUpdate.attach(&updateTick, 1.0 / 60.0);
Reiko 0:22a7683646d1 467
Reiko 8:06124632c3e4 468 //imuUpdate.attach(&imuUpdateTick, 1.0 / 200.0);
Reiko 8:06124632c3e4 469
Reiko 9:2d2030d989d8 470 testUpdate.attach(&testUpdateTick, 0.2);
Reiko 8:06124632c3e4 471
Reiko 9:2d2030d989d8 472 //pc.printf("IMU started\n");
Reiko 8:06124632c3e4 473
Reiko 8:06124632c3e4 474 //time_t seconds = time(NULL);
Reiko 8:06124632c3e4 475
Reiko 8:06124632c3e4 476 //printf("time = %d\n", time(NULL));
Reiko 8:06124632c3e4 477
Reiko 8:06124632c3e4 478 //set_time(1256729737);
Reiko 8:06124632c3e4 479
Reiko 8:06124632c3e4 480 /*int sampleCount = 0;
Reiko 8:06124632c3e4 481 int calibrationCount = 1000;
Reiko 8:06124632c3e4 482 bool calibrating = true;
Reiko 8:06124632c3e4 483
Reiko 8:06124632c3e4 484 pc.printf("Start IMU calibration\n");*/
Reiko 8:06124632c3e4 485
Reiko 8:06124632c3e4 486 //imu.startGzCalibration();
Reiko 9:2d2030d989d8 487
Reiko 9:2d2030d989d8 488 /*buttonGoal.mode(PullUp);
Reiko 9:2d2030d989d8 489 buttonGoal.attach_deasserted(&goalFall);
Reiko 9:2d2030d989d8 490 buttonGoal.setSamplesTillAssert(20);
Reiko 9:2d2030d989d8 491 buttonGoal.setSampleFrequency(1000);*/
Reiko 8:06124632c3e4 492
Reiko 9:2d2030d989d8 493 //sw2.rise(&sw2_release);
Reiko 9:2d2030d989d8 494 //sw2.fall(&sw2_press);
Reiko 9:2d2030d989d8 495
Reiko 9:2d2030d989d8 496 //OFF
Reiko 0:22a7683646d1 497 led1 = 1;
Reiko 9:2d2030d989d8 498 led3 = 1;
Reiko 9:2d2030d989d8 499 led4 = 1;
Reiko 9:2d2030d989d8 500
Reiko 9:2d2030d989d8 501 //uLCD.printf("Ready\n");
Reiko 9:2d2030d989d8 502
Reiko 9:2d2030d989d8 503 //humanInterface.setError(1);
Reiko 9:2d2030d989d8 504
Reiko 9:2d2030d989d8 505 bool isMainUpdate = false;
Reiko 9:2d2030d989d8 506
Reiko 9:2d2030d989d8 507 int serialReadUnblockCount = 0;
Reiko 9:2d2030d989d8 508 int serialReadUnblockLimit = 40;
Reiko 9:2d2030d989d8 509
Reiko 11:92eecb155136 510 watchdog.kick(3);
Reiko 0:22a7683646d1 511
Reiko 0:22a7683646d1 512 while (1) {
Reiko 8:06124632c3e4 513
Reiko 9:2d2030d989d8 514 serialReadUnblockCount = 0;
Reiko 9:2d2030d989d8 515
Reiko 9:2d2030d989d8 516 //int k = pc.readable();
Reiko 9:2d2030d989d8 517 while (pc.readable() && serialReadUnblockCount < serialReadUnblockLimit) {
Reiko 9:2d2030d989d8 518 serialReadUnblockCount++;
Reiko 9:2d2030d989d8 519 //pc.printf("Received packet from: %s\n", client.get_address());
Reiko 9:2d2030d989d8 520 //pc.printf("k: %d\n", k);
Reiko 9:2d2030d989d8 521 //pc.scanf("%s", &serialBuffer);
Reiko 9:2d2030d989d8 522 serialBuffer[charCounter] = pc.getc();
Reiko 9:2d2030d989d8 523 //pc.printf("%c\n", serialBuffer[charCounter]);
Reiko 9:2d2030d989d8 524 //fflush(stdout);
Reiko 9:2d2030d989d8 525 //fflush(stdin);
Reiko 9:2d2030d989d8 526 //pc.printf("%s\n", serialBuffer);
Reiko 9:2d2030d989d8 527 if (serialBuffer[charCounter] == '\n') {
Reiko 9:2d2030d989d8 528 serialBuffer[charCounter] = '\0';
Reiko 9:2d2030d989d8 529 executeCommand(serialBuffer);
Reiko 9:2d2030d989d8 530 led1 = !led1;
Reiko 9:2d2030d989d8 531 charCounter = 0;
Reiko 9:2d2030d989d8 532 break;
Reiko 9:2d2030d989d8 533 } else {
Reiko 9:2d2030d989d8 534 charCounter++;
Reiko 9:2d2030d989d8 535 }
Reiko 9:2d2030d989d8 536
Reiko 9:2d2030d989d8 537 //led3 = !led3;
Reiko 9:2d2030d989d8 538
Reiko 9:2d2030d989d8 539 //server.sendTo(client, serialBuffer, n);
Reiko 9:2d2030d989d8 540 }
Reiko 9:2d2030d989d8 541
Reiko 9:2d2030d989d8 542 //if (updateIMU) {
Reiko 9:2d2030d989d8 543 //updateIMU = 0;
Reiko 8:06124632c3e4 544 //t.stop();
Reiko 8:06124632c3e4 545 //float dt = t.read();
Reiko 8:06124632c3e4 546 //t.reset();
Reiko 8:06124632c3e4 547 //t.start();
Reiko 8:06124632c3e4 548 //printf("dt = %.6f\n", dt);
Reiko 8:06124632c3e4 549 //imu.update();
Reiko 8:06124632c3e4 550
Reiko 8:06124632c3e4 551 /*if (calibrating) {
Reiko 8:06124632c3e4 552 sampleCount++;
Reiko 8:06124632c3e4 553 if (sampleCount >= calibrationCount) {
Reiko 8:06124632c3e4 554 calibrating = false;
Reiko 8:06124632c3e4 555 imu.stopGzCalibration();
Reiko 8:06124632c3e4 556 pc.printf("Stop IMU calibration\n");
Reiko 8:06124632c3e4 557 pc.printf("GyroZ zero %.3f\n", imu.getGzZero());
Reiko 8:06124632c3e4 558 }
Reiko 8:06124632c3e4 559 }*/
Reiko 8:06124632c3e4 560
Reiko 9:2d2030d989d8 561 //led3 = !led3;
Reiko 9:2d2030d989d8 562 //}
Reiko 0:22a7683646d1 563
Reiko 9:2d2030d989d8 564 if (update) {
Reiko 0:22a7683646d1 565 update = 0;
Reiko 0:22a7683646d1 566 ioExt.writePins();
Reiko 8:06124632c3e4 567
Reiko 9:2d2030d989d8 568 failSafeCountMotors++;
Reiko 9:2d2030d989d8 569 failSafeCountCoilgun++;
Reiko 9:2d2030d989d8 570
Reiko 9:2d2030d989d8 571 if (failSafeCountMotors == failSafeLimitMotors) {
Reiko 9:2d2030d989d8 572 failSafeCountMotors = 0;
Reiko 1:79ac4e293661 573 if (failSafeEnabled) {
Reiko 9:2d2030d989d8 574
Reiko 1:79ac4e293661 575 motor1.setSpeed(0);
Reiko 1:79ac4e293661 576 motor2.setSpeed(0);
Reiko 1:79ac4e293661 577 motor3.setSpeed(0);
Reiko 1:79ac4e293661 578 motor4.setSpeed(0);
Reiko 1:79ac4e293661 579 motor5.setSpeed(0);
Reiko 9:2d2030d989d8 580 //dribbler.pulsewidth_us(0);
Reiko 9:2d2030d989d8 581
Reiko 11:92eecb155136 582 //pc.rxBufferFlush();
Reiko 11:92eecb155136 583 //pc.txBufferFlush();
Reiko 9:2d2030d989d8 584 }
Reiko 9:2d2030d989d8 585 }
Reiko 9:2d2030d989d8 586
Reiko 9:2d2030d989d8 587 if (failSafeCountCoilgun == failSafeLimitCoilgun) {
Reiko 9:2d2030d989d8 588 failSafeCountCoilgun = 0;
Reiko 9:2d2030d989d8 589 if (failSafeEnabled) {
Reiko 1:79ac4e293661 590 coilgun.discharge();
Reiko 9:2d2030d989d8 591
Reiko 9:2d2030d989d8 592 if (!coilgun.isCharged) {
Reiko 9:2d2030d989d8 593 //server.sendTo(client, "<discharged>", 12);
Reiko 9:2d2030d989d8 594 pc.printf("<discharged>\n");
Reiko 9:2d2030d989d8 595 }
Reiko 9:2d2030d989d8 596 }
Reiko 1:79ac4e293661 597 }
Reiko 1:79ac4e293661 598
Reiko 0:22a7683646d1 599 //led3 = 1;
Reiko 0:22a7683646d1 600 //updateSensors();
Reiko 0:22a7683646d1 601 //pc.printf("update");
Reiko 0:22a7683646d1 602 /*motor1.pid();
Reiko 0:22a7683646d1 603 motor2.pid();
Reiko 0:22a7683646d1 604 motor3.pid();
Reiko 0:22a7683646d1 605 motor4.pid();
Reiko 0:22a7683646d1 606 motor5.pid();*/
Reiko 0:22a7683646d1 607
Reiko 0:22a7683646d1 608 //led3 = 0;
Reiko 0:22a7683646d1 609 //int charCount = sprintf(sendBuffer, "<speeds:%d:%d:%d:%d:%d>", motor1.getSpeed(), motor2.getSpeed(), motor3.getSpeed(), motor4.getSpeed(), motor5.getSpeed());
Reiko 0:22a7683646d1 610 //server.sendTo(client, sendBuffer, charCount);
Reiko 0:22a7683646d1 611 //pc.printf("<speeds:%d:%d:%d:%d:%d>\n", motor1.getSpeed(), motor2.getSpeed(), motor3.getSpeed(), motor4.getSpeed(), motor5.getSpeed());
Reiko 0:22a7683646d1 612 //redLed.toggle();
Reiko 5:b30229c6f32b 613
Reiko 9:2d2030d989d8 614 //ioExt.setDirection(0x0400);
Reiko 9:2d2030d989d8 615
Reiko 9:2d2030d989d8 616 //led1 = !led1;
Reiko 9:2d2030d989d8 617
Reiko 11:92eecb155136 618 /*t.stop();
Reiko 9:2d2030d989d8 619 float dt = t.read();
Reiko 9:2d2030d989d8 620 t.reset();
Reiko 9:2d2030d989d8 621 t.start();
Reiko 9:2d2030d989d8 622
Reiko 9:2d2030d989d8 623 if (dt > dtMax) {
Reiko 9:2d2030d989d8 624 dtMax = dt;
Reiko 9:2d2030d989d8 625 }
Reiko 9:2d2030d989d8 626
Reiko 9:2d2030d989d8 627 if (dt >= 0.02f) {
Reiko 9:2d2030d989d8 628 printf("<dtmax=%.5f>\n", dt);
Reiko 9:2d2030d989d8 629 }
Reiko 9:2d2030d989d8 630
Reiko 9:2d2030d989d8 631 if (dtMax >= 0.02f) {
Reiko 9:2d2030d989d8 632 printf("<dtmax=%.5f>\n", dtMax);
Reiko 11:92eecb155136 633 }*/
Reiko 5:b30229c6f32b 634
Reiko 9:2d2030d989d8 635 isMainUpdate = true;
Reiko 0:22a7683646d1 636 }
Reiko 0:22a7683646d1 637
Reiko 9:2d2030d989d8 638 if (updateTest) {
Reiko 9:2d2030d989d8 639 updateTest = 0;
Reiko 9:2d2030d989d8 640
Reiko 9:2d2030d989d8 641 //watchdog.kick();
Reiko 9:2d2030d989d8 642
Reiko 9:2d2030d989d8 643 //if (!calibrating) {
Reiko 9:2d2030d989d8 644 // pc.printf("gz:%.3f\n", imu.getDegZ());
Reiko 9:2d2030d989d8 645 //}
Reiko 9:2d2030d989d8 646
Reiko 9:2d2030d989d8 647 //uLCD.printf("AyijklL1230\n");
Reiko 9:2d2030d989d8 648
Reiko 9:2d2030d989d8 649 //rgbLed1.toggle();
Reiko 9:2d2030d989d8 650 //rgbLed2.toggle();
Reiko 9:2d2030d989d8 651
Reiko 9:2d2030d989d8 652 led4 = !led4;
Reiko 9:2d2030d989d8 653
Reiko 9:2d2030d989d8 654 if (!isMainUpdate) {
Reiko 9:2d2030d989d8 655 coilCapVoltage = coilADC.read() * 80;
Reiko 9:2d2030d989d8 656 }
Reiko 9:2d2030d989d8 657 //coilCapVoltage = 300.0f;
Reiko 9:2d2030d989d8 658
Reiko 9:2d2030d989d8 659 //float dtSlow = aliveTime.read();
Reiko 9:2d2030d989d8 660 //printf("<up=%.1f>\n", dtSlow);
Reiko 9:2d2030d989d8 661 }
Reiko 9:2d2030d989d8 662
Reiko 9:2d2030d989d8 663 isMainUpdate = false;
Reiko 9:2d2030d989d8 664
Reiko 1:79ac4e293661 665 if (stallChanged) {
Reiko 1:79ac4e293661 666 stallChanged = 0;
Reiko 9:2d2030d989d8 667 /*int charCount = sprintf(sendBuffer, "<stall:%d:%d:%d:%d:%d>",
Reiko 1:79ac4e293661 668 motor1.getStallLevel(),
Reiko 1:79ac4e293661 669 motor2.getStallLevel(),
Reiko 1:79ac4e293661 670 motor3.getStallLevel(),
Reiko 1:79ac4e293661 671 motor4.getStallLevel(),
Reiko 1:79ac4e293661 672 motor5.getStallLevel());
Reiko 9:2d2030d989d8 673 server.sendTo(client, sendBuffer, charCount);*/
Reiko 9:2d2030d989d8 674 pc.printf(
Reiko 11:92eecb155136 675 "<stall:%d:%d:%d:%d:%d>\n",
Reiko 9:2d2030d989d8 676 motor1.getStallLevel(),
Reiko 9:2d2030d989d8 677 motor2.getStallLevel(),
Reiko 9:2d2030d989d8 678 motor3.getStallLevel(),
Reiko 9:2d2030d989d8 679 motor4.getStallLevel(),
Reiko 9:2d2030d989d8 680 motor5.getStallLevel()
Reiko 9:2d2030d989d8 681 );
Reiko 9:2d2030d989d8 682 }
Reiko 9:2d2030d989d8 683
Reiko 9:2d2030d989d8 684 /*if (ballStateChanged) {
Reiko 9:2d2030d989d8 685 ballStateChanged = false;
Reiko 9:2d2030d989d8 686 if (ballState) {
Reiko 9:2d2030d989d8 687 //server.sendTo(client, "<ball:1>", 8);
Reiko 9:2d2030d989d8 688 pc.printf("<ball:1>");
Reiko 9:2d2030d989d8 689 } else {
Reiko 9:2d2030d989d8 690 //server.sendTo(client, "<ball:0>", 8);
Reiko 9:2d2030d989d8 691 pc.printf("<ball:0>");
Reiko 9:2d2030d989d8 692 }
Reiko 9:2d2030d989d8 693 }*/
Reiko 1:79ac4e293661 694
Reiko 9:2d2030d989d8 695 /*if (goalButtonPressed) {
Reiko 9:2d2030d989d8 696 goalButtonPressed = false;
Reiko 9:2d2030d989d8 697 //server.sendTo(client, "<toggle-side>", 13);
Reiko 9:2d2030d989d8 698 }*/
Reiko 9:2d2030d989d8 699
Reiko 9:2d2030d989d8 700 /*if (startButtonReleased) {
Reiko 9:2d2030d989d8 701 startButtonReleased = false;
Reiko 9:2d2030d989d8 702 //server.sendTo(client, "<toggle-go>", 11);
Reiko 9:2d2030d989d8 703 }*/
Reiko 9:2d2030d989d8 704
Reiko 9:2d2030d989d8 705 int newBallState = humanInterface.getBallState();
Reiko 9:2d2030d989d8 706 if (ballState != newBallState) {
Reiko 9:2d2030d989d8 707 pc.printf("<ball:%d>\n", newBallState);
Reiko 9:2d2030d989d8 708 ballState = newBallState;
Reiko 9:2d2030d989d8 709
Reiko 9:2d2030d989d8 710 if (kickWhenBall && ballState) {
Reiko 9:2d2030d989d8 711 kickWhenBall = false;
Reiko 9:2d2030d989d8 712 coilgun.kick(currentKickLength, currentKickDelay, currentChipLength, currentChipDelay);
Reiko 9:2d2030d989d8 713 sendKicked = true;
Reiko 9:2d2030d989d8 714 }
Reiko 9:2d2030d989d8 715
Reiko 9:2d2030d989d8 716 if (!ballState && sendKicked) {
Reiko 9:2d2030d989d8 717 sendKicked = true;
Reiko 9:2d2030d989d8 718 pc.printf("<kicked>\n");
Reiko 9:2d2030d989d8 719 }
Reiko 9:2d2030d989d8 720 }
Reiko 1:79ac4e293661 721
Reiko 8:06124632c3e4 722 /*switch (humanInterface.getBallState()){
mlaane 6:5619aff36840 723 case -1:
mlaane 6:5619aff36840 724 //Ball lost
Reiko 9:2d2030d989d8 725 //server.sendTo(client, "<ball:0>", 8);
Reiko 9:2d2030d989d8 726 pc.printf("<ball:0>\n");
Reiko 9:2d2030d989d8 727 ballState = 0;
mlaane 6:5619aff36840 728 break;
mlaane 6:5619aff36840 729 case 0:
mlaane 6:5619aff36840 730 //Nothing has changed..
mlaane 6:5619aff36840 731 break;
mlaane 6:5619aff36840 732 case 1:
mlaane 6:5619aff36840 733 //Ball found
Reiko 9:2d2030d989d8 734 //server.sendTo(client, "<ball:1>", 8);
Reiko 9:2d2030d989d8 735 pc.printf("<ball:1>\n");
Reiko 9:2d2030d989d8 736 ballState = 1;
mlaane 6:5619aff36840 737 break;
mlaane 6:5619aff36840 738 default:
mlaane 6:5619aff36840 739 break;
Reiko 9:2d2030d989d8 740 }*/
Reiko 1:79ac4e293661 741
mlaane 6:5619aff36840 742 if (humanInterface.isGoalChange()) {
Reiko 9:2d2030d989d8 743 //server.sendTo(client, "<toggle-side>", 13);
Reiko 9:2d2030d989d8 744 pc.printf("<toggle-side>\n");
Reiko 1:79ac4e293661 745 }
Reiko 1:79ac4e293661 746
mlaane 6:5619aff36840 747 if (humanInterface.isStart()) {
Reiko 9:2d2030d989d8 748 //server.sendTo(client, "<toggle-go>", 11);
Reiko 9:2d2030d989d8 749 pc.printf("<toggle-go>\n");
Reiko 9:2d2030d989d8 750 }
Reiko 0:22a7683646d1 751
Reiko 0:22a7683646d1 752 //printf("\nWait for packet...\n");
Reiko 0:22a7683646d1 753
Reiko 9:2d2030d989d8 754 /*int n = server.receiveFrom(client, buffer, sizeof(buffer));
Reiko 0:22a7683646d1 755
Reiko 0:22a7683646d1 756 if (n > 0) {
Reiko 0:22a7683646d1 757 //pc.printf("Received packet from: %s\n", client.get_address());
Reiko 0:22a7683646d1 758 //pc.printf("n: %d\n", n);
Reiko 0:22a7683646d1 759 buffer[n] = '\0';
Reiko 0:22a7683646d1 760 //server.sendTo(client, buffer, n);
Reiko 5:b30229c6f32b 761 executeCommand(buffer);
Reiko 0:22a7683646d1 762 //coilgun.kick(1000);
Reiko 0:22a7683646d1 763 //executeCommand((short*)buffer);
Reiko 9:2d2030d989d8 764 }*/
Reiko 0:22a7683646d1 765
Reiko 0:22a7683646d1 766 /*if (pc.readable()) {
Reiko 9:2d2030d989d8 767 led1 = !led1;
Reiko 9:2d2030d989d8 768 pc.putc(pc.getc());
Reiko 0:22a7683646d1 769 }*/
Reiko 9:2d2030d989d8 770
Reiko 9:2d2030d989d8 771
Reiko 0:22a7683646d1 772 }
Reiko 0:22a7683646d1 773 }
Reiko 0:22a7683646d1 774
Reiko 5:b30229c6f32b 775 void executeCommand(char *buffer) {
Reiko 9:2d2030d989d8 776 failSafeCountMotors = 0;
Reiko 9:2d2030d989d8 777 failSafeCountCoilgun = 0;
Reiko 1:79ac4e293661 778
Reiko 9:2d2030d989d8 779 //pc.printf("cmd: %s\n", buffer);
Reiko 9:2d2030d989d8 780 //uLCD.printf("%s\n", buffer);
Reiko 9:2d2030d989d8 781
Reiko 0:22a7683646d1 782 char *cmd;
Reiko 1:79ac4e293661 783 cmd = strtok(buffer, ":");
Reiko 0:22a7683646d1 784
Reiko 0:22a7683646d1 785 //pc.printf("%s\n", cmd);
Reiko 0:22a7683646d1 786
Reiko 0:22a7683646d1 787 if (strncmp(cmd, "speeds", 6) == 0) {
Reiko 1:79ac4e293661 788 motor1.setSpeed(atoi(strtok(NULL, ":")));
Reiko 1:79ac4e293661 789 motor2.setSpeed(atoi(strtok(NULL, ":")));
Reiko 1:79ac4e293661 790 motor3.setSpeed(atoi(strtok(NULL, ":")));
Reiko 1:79ac4e293661 791 motor4.setSpeed(atoi(strtok(NULL, ":")));
Reiko 1:79ac4e293661 792 motor5.setSpeed(atoi(strtok(NULL, ":")));
Reiko 9:2d2030d989d8 793
Reiko 9:2d2030d989d8 794 /*int dribblerSpeed = atoi(strtok(NULL, ":")) * 10;
Reiko 9:2d2030d989d8 795
Reiko 9:2d2030d989d8 796 if (dribblerSpeed < 0) {
Reiko 9:2d2030d989d8 797 dribblerSpeed = -dribblerSpeed;
Reiko 9:2d2030d989d8 798 ioExt.setPin(0);
Reiko 9:2d2030d989d8 799 } else {
Reiko 9:2d2030d989d8 800 ioExt.clearPin(0);
Reiko 9:2d2030d989d8 801 }*/
Reiko 9:2d2030d989d8 802
Reiko 9:2d2030d989d8 803 //dribbler.pulsewidth_us(dribblerSpeed);
Reiko 9:2d2030d989d8 804
Reiko 9:2d2030d989d8 805 /*int charCount = sprintf(sendBuffer, "<speeds:%d:%d:%d:%d:%d>",
Reiko 1:79ac4e293661 806 motor1.getSpeed(), motor2.getSpeed(), motor3.getSpeed(), motor4.getSpeed(), motor5.getSpeed());
Reiko 9:2d2030d989d8 807 server.sendTo(client, sendBuffer, charCount);*/
Reiko 9:2d2030d989d8 808
Reiko 9:2d2030d989d8 809 pc.printf("<speeds:%d:%d:%d:%d:%d>\n", motor1.getSpeed(), motor2.getSpeed(), motor3.getSpeed(), motor4.getSpeed(), motor5.getSpeed());
Reiko 9:2d2030d989d8 810 } /*else if (strncmp(cmd, "ga", 2) == 0) {
Reiko 8:06124632c3e4 811 //int charCount = sprintf(sendBuffer, "<angle:%.2f>", imu.getDegZ());
Reiko 8:06124632c3e4 812 //server.sendTo(client, sendBuffer, charCount);
Reiko 9:2d2030d989d8 813 }*/ else if (strncmp(cmd, "kick", 4) == 0) {
Reiko 9:2d2030d989d8 814 unsigned int kickLength = atoi(strtok(NULL, ":"));
Reiko 9:2d2030d989d8 815 coilgun.kick(kickLength, 0, 0, 0);
Reiko 9:2d2030d989d8 816 pc.printf("<ball:%d>\n", ballState);
Reiko 9:2d2030d989d8 817 } else if (strncmp(cmd, "dkick", 5) == 0) {
Reiko 8:06124632c3e4 818 unsigned int kickLength = atoi(strtok(NULL, ":"));
Reiko 8:06124632c3e4 819 unsigned int kickDelay = atoi(strtok(NULL, ":"));
Reiko 8:06124632c3e4 820 unsigned int chipLength = atoi(strtok(NULL, ":"));
Reiko 8:06124632c3e4 821 unsigned int chipDelay = atoi(strtok(NULL, ":"));
Reiko 9:2d2030d989d8 822 //pc.printf("kick:%d:%d:%d:%d\n", kickLength, kickDelay, chipLength, chipDelay);
Reiko 8:06124632c3e4 823 coilgun.kick(kickLength, kickDelay, chipLength, chipDelay);
Reiko 9:2d2030d989d8 824 pc.printf("<ball:%d>\n", ballState);
Reiko 9:2d2030d989d8 825 } else if (strncmp(cmd, "bdkick", 6) == 0) {
Reiko 9:2d2030d989d8 826 currentKickLength = atoi(strtok(NULL, ":"));
Reiko 9:2d2030d989d8 827 currentKickDelay = atoi(strtok(NULL, ":"));
Reiko 9:2d2030d989d8 828 currentChipLength = atoi(strtok(NULL, ":"));
Reiko 9:2d2030d989d8 829 currentChipDelay = atoi(strtok(NULL, ":"));
Reiko 9:2d2030d989d8 830 //pc.printf("kick:%d:%d:%d:%d\n", kickLength, kickDelay, chipLength, chipDelay);
Reiko 9:2d2030d989d8 831 if (ballState) {
Reiko 9:2d2030d989d8 832 coilgun.kick(currentKickLength, currentKickDelay, currentChipLength, currentChipDelay);
Reiko 9:2d2030d989d8 833 kickWhenBall = false;
Reiko 9:2d2030d989d8 834 } else {
Reiko 9:2d2030d989d8 835 kickWhenBall = true;
Reiko 9:2d2030d989d8 836 }
Reiko 9:2d2030d989d8 837 pc.printf("<ball:%d>\n", ballState);
Reiko 9:2d2030d989d8 838 } else if (strncmp(cmd, "nokick", 6) == 0) {
Reiko 9:2d2030d989d8 839 kickWhenBall = false;
Reiko 1:79ac4e293661 840 } else if (strncmp(cmd, "charge", 6) == 0) {
Reiko 9:2d2030d989d8 841 //pc.printf("charge\n");
Reiko 1:79ac4e293661 842 coilgun.charge();
Reiko 1:79ac4e293661 843 } else if (strncmp(cmd, "discharge", 9) == 0) {
Reiko 9:2d2030d989d8 844 //pc.printf("discharge\n");
Reiko 1:79ac4e293661 845 coilgun.discharge();
Reiko 9:2d2030d989d8 846 } else if (strncmp(cmd, "servos", 6) == 0) {
Reiko 9:2d2030d989d8 847 int servo1Duty = atoi(strtok(NULL, ":"));
Reiko 9:2d2030d989d8 848 int servo2Duty = atoi(strtok(NULL, ":"));
Reiko 9:2d2030d989d8 849
Reiko 9:2d2030d989d8 850 if (servo1Duty + servo2Duty > 3340) {
Reiko 9:2d2030d989d8 851 pc.printf("<err:servo1Duty + servo2Duty must be smaller than 3340>\n");
Reiko 9:2d2030d989d8 852 } else {
Reiko 9:2d2030d989d8 853 if (servo1Duty < 600 || servo1Duty > 2500) {
Reiko 9:2d2030d989d8 854 pc.printf("<err:servo1Duty must be between 600 and 2500>\n");
Reiko 9:2d2030d989d8 855 } else {
Reiko 9:2d2030d989d8 856 servo1.pulsewidth_us(servo1Duty);
Reiko 9:2d2030d989d8 857 }
Reiko 9:2d2030d989d8 858
Reiko 9:2d2030d989d8 859 if (servo2Duty < 600 || servo2Duty > 2500) {
Reiko 9:2d2030d989d8 860 pc.printf("<err:servo2Duty must be between 600 and 2500>\n");
Reiko 9:2d2030d989d8 861 } else {
Reiko 9:2d2030d989d8 862 servo2.pulsewidth_us(servo2Duty);
Reiko 9:2d2030d989d8 863 }
Reiko 9:2d2030d989d8 864 }
Reiko 9:2d2030d989d8 865 } else if (strncmp(cmd, "gs", 2) == 0) {
Reiko 9:2d2030d989d8 866 /*int charCount = sprintf(sendBuffer, "<speeds:%d:%d:%d:%d:%d>",
Reiko 9:2d2030d989d8 867 motor1.getSpeed(), motor2.getSpeed(), motor3.getSpeed(), motor4.getSpeed(), motor5.getSpeed());
Reiko 9:2d2030d989d8 868 server.sendTo(client, sendBuffer, charCount);*/
Reiko 9:2d2030d989d8 869 pc.printf("<speeds:%d:%d:%d:%d:%d>\n", motor1.getSpeed(), motor2.getSpeed(), motor3.getSpeed(), motor4.getSpeed(), motor5.getSpeed());
Reiko 0:22a7683646d1 870 } else if (strncmp(cmd, "c", 1) == 0) {
Reiko 9:2d2030d989d8 871 //pc.printf("charge\n");
Reiko 1:79ac4e293661 872 if (atoi(strtok(NULL, ":")) == 1) {
Reiko 1:79ac4e293661 873 coilgun.charge();
Reiko 1:79ac4e293661 874 } else {
Reiko 1:79ac4e293661 875 coilgun.chargeEnd();
Reiko 1:79ac4e293661 876 }
Reiko 0:22a7683646d1 877 } else if (strncmp(cmd, "d", 1) == 0) {
Reiko 9:2d2030d989d8 878 //pc.printf("discharge\n");
Reiko 0:22a7683646d1 879 coilgun.discharge();
Reiko 4:c5cf0676baca 880 } else if (strncmp(cmd, "reset", 5) == 0) {
Reiko 4:c5cf0676baca 881 motor1.setSpeed(0);
Reiko 4:c5cf0676baca 882 motor2.setSpeed(0);
Reiko 4:c5cf0676baca 883 motor3.setSpeed(0);
Reiko 4:c5cf0676baca 884 motor4.setSpeed(0);
Reiko 4:c5cf0676baca 885 motor5.setSpeed(0);
Reiko 9:2d2030d989d8 886
Reiko 9:2d2030d989d8 887 //dribbler.pulsewidth_us(0);
Reiko 9:2d2030d989d8 888
Reiko 9:2d2030d989d8 889 humanInterface.setGoal(HumanInterface::UNSET);
Reiko 4:c5cf0676baca 890 humanInterface.setError(false);
Reiko 9:2d2030d989d8 891 humanInterface.setGo(false);
Reiko 1:79ac4e293661 892 } else if (strncmp(cmd, "fs", 2) == 0) {
Reiko 1:79ac4e293661 893 failSafeEnabled = (bool)atoi(strtok(NULL, ":"));
Reiko 1:79ac4e293661 894 } else if (strncmp(cmd, "target", 6) == 0) {
Reiko 3:4ec313b1b314 895 int target = atoi(strtok(NULL, ":"));
Reiko 8:06124632c3e4 896 /*if (target == 0) {
Reiko 3:4ec313b1b314 897 humanInterface.setGoal(HumanInterface::BLUE);
Reiko 3:4ec313b1b314 898 } else if (target == 1) {
Reiko 1:79ac4e293661 899 humanInterface.setGoal(HumanInterface::YELLOW);
Reiko 3:4ec313b1b314 900 } else if (target == 2) {
Reiko 3:4ec313b1b314 901 humanInterface.setGoal(HumanInterface::UNSET);
Reiko 8:06124632c3e4 902 }*/
Reiko 1:79ac4e293661 903 } else if (strncmp(cmd, "error", 5) == 0) {
Reiko 9:2d2030d989d8 904 humanInterface.setError(atoi(strtok(NULL, ":")));
Reiko 9:2d2030d989d8 905 } else if (strncmp(cmd, "go", 2) == 0) {
Reiko 9:2d2030d989d8 906 humanInterface.setGo(atoi(strtok(NULL, ":")));
Reiko 9:2d2030d989d8 907 } else if (strncmp(cmd, "adc", 5) == 0) {
Reiko 9:2d2030d989d8 908 //pc.printf("<adc:%.1f>\n", coilADC.read() * 80);
Reiko 9:2d2030d989d8 909 pc.printf("<adc:%.1f>\n", coilCapVoltage);
Reiko 9:2d2030d989d8 910 /*int charCount = sprintf(sendBuffer, "<adc:%.1f>\n", coilADC.read() * 80);
Reiko 9:2d2030d989d8 911 server.sendTo(client, sendBuffer, charCount);*/
Reiko 9:2d2030d989d8 912 } /*else if (strncmp(cmd, "red", 3) == 0) {
Reiko 9:2d2030d989d8 913 rgbLed1.setColor(RgbLed::RED);
Reiko 9:2d2030d989d8 914 } else if (strncmp(cmd, "green", 5) == 0) {
Reiko 9:2d2030d989d8 915 rgbLed1.setColor(RgbLed::GREEN);
Reiko 9:2d2030d989d8 916 } else if (strncmp(cmd, "blue", 4) == 0) {
Reiko 9:2d2030d989d8 917 rgbLed1.setColor(RgbLed::BLUE);
Reiko 9:2d2030d989d8 918 } else if (strncmp(cmd, "cyan", 4) == 0) {
Reiko 9:2d2030d989d8 919 rgbLed1.setColor(RgbLed::CYAN);
Reiko 9:2d2030d989d8 920 } else if (strncmp(cmd, "magenta", 7) == 0) {
Reiko 9:2d2030d989d8 921 rgbLed1.setColor(RgbLed::MAGENTA);
Reiko 9:2d2030d989d8 922 } else if (strncmp(cmd, "yellow", 6) == 0) {
Reiko 9:2d2030d989d8 923 rgbLed1.setColor(RgbLed::YELLOW);
Reiko 9:2d2030d989d8 924 } else if (strncmp(cmd, "white", 5) == 0) {
Reiko 9:2d2030d989d8 925 rgbLed1.setColor(RgbLed::WHITE);
Reiko 9:2d2030d989d8 926 } else if (strncmp(cmd, "off", 3) == 0) {
Reiko 9:2d2030d989d8 927 rgbLed1.setColor(RgbLed::OFF);
Reiko 9:2d2030d989d8 928 }*/
Reiko 0:22a7683646d1 929 }