main.cpp

00001 #include "mbed.h"
00002 //#include "EthernetInterface.h"
00003 //#include "ADXL345_I2C.h"
00004 //#include "L3G4200D.h"
00005 //#include "HMC5883L.h"
00006 #include "PCA9555.h"
00007 #include "motor.h"
00008 #include "qed.h"
00009 //#include "ledout.h"
00010 //#include "rgb-led-pca9555.h"
00011 //#include "externalin.h"
00012 #include "coilgun.h"
00013 #include "HumanInterface.h"
00014 //#include "MPU6050.h"
00015 //#include "imu.h"
00016 #include "MCP3021.h"
00017 //#include "PinDetect.h"
00018 //#include "uLCD_4DGL.h"
00019 //#include "fsl_uart_hal.h"
00020 #include "MODSERIAL.h"
00021 #include "Watchdog.h" 
00022 
00023 #define SERVER_PORT   8042
00024 
00025 Watchdog watchdog;
00026 
00027 Ticker sensorUpdate;
00028 Ticker imuUpdate;
00029 Ticker testUpdate;
00030 
00031 DigitalOut led1(LED_RED);
00032 DigitalOut led3(LED_GREEN);
00033 DigitalOut led4(LED_BLUE);
00034 
00035 //Serial pc(USBTX, USBRX); // tx, rx
00036 MODSERIAL pc(USBTX, USBRX, 512, 512); 
00037 //ADXL345_I2C accelerometer(p9, p10);
00038 //L3G4200D gyro(p9, p10);
00039 //HMC5883L compass(p9, p10);
00040 
00041 
00042 MCP3021 coilADC(PTC11, PTC10, 5.0);
00043 PCA9555 ioExt(PTC11, PTC10, PTB20, 0x40);
00044 
00045 //int readings[3] = {0, 0, 0};
00046 //int gyroData[3] = {0, 0, 0};
00047 //int16_t compassData[3] = {0, 0, 0};
00048 
00049 volatile int update = 0;
00050 volatile int updateIMU = 0;
00051 
00052 volatile int updateTest = 0;
00053 
00054 volatile int stallChanged = 0;
00055 
00056 bool failSafeEnabled = true;
00057 int failSafeCountMotors = 0;
00058 int failSafeCountCoilgun = 0;
00059 int failSafeLimitMotors = 60;
00060 int failSafeLimitCoilgun = 300;
00061 
00062 
00063 void executeCommand(char *buffer);
00064 
00065 /*static void enable_rx_fifo(int instance) {
00066     uart_hal_disable_receiver(instance);
00067     uart_hal_disable_transmitter(instance);
00068     uart_hal_enable_rx_fifo(instance);
00069     uart_hal_enable_receiver(instance);
00070     uart_hal_enable_transmitter(instance);
00071     uart_hal_flush_rx_fifo(instance);
00072     uart_hal_flush_tx_fifo(instance);
00073 }*/
00074 
00075 /*
00076 void updateSensors() {
00077     if (led1 == 1.0) {
00078         led1 = 0;
00079     } else {
00080         led1 = 1.0;
00081     }
00082     accelerometer.getOutput(readings);
00083     pc.printf("<acc:%i:%i:%i>\n", (int16_t)readings[0], (int16_t)readings[1], (int16_t)readings[2]);
00084     
00085     gyro.read(gyroData);
00086     pc.printf("<gyro:%i:%i:%i>\n", (int16_t)gyroData[0], (int16_t)gyroData[1], (int16_t)gyroData[2]);
00087     
00088     compass.getXYZ(compassData);
00089     pc.printf("<comps:%i:%i:%i>\n", compassData[0], compassData[1], compassData[2]);
00090 }*/
00091 
00092 //Dribbler motor without encoder
00093 //PwmOut dribbler(PTC1);
00094 
00095 //motor order: FL, FR, RL, RR, DRIBBLER
00096 //Motor(PinName PWMpin, PCA9555 *ioExt, unsigned int dirPin, PinName encA, PinName encB);
00097 Motor motor3(PTA2, &ioExt, 2, PTC18, PTD6); //RL - M1
00098 Motor motor1(PTA1, &ioExt, 1, PTD7, PTD5); //FL - M2
00099 Motor motor5(PTC1, &ioExt, 0, PTD4, PTC12); //DRIBBLER - M3
00100 Motor motor2(PTC4, &ioExt, 5, PTB18, PTB19); //FR - M4
00101 Motor motor4(PTC3, &ioExt, 4, PTC8, PTC9); //RR - M5
00102 
00103 Coilgun coilgun(PTB11, PTB10, PTB3, PTB2);
00104 //Coilgun coilgun(PTB11, PTB10, PTB3);
00105 
00106 //PinDetect buttonGoal(PTC15);
00107 //PinDetect buttonStart(PTC14);
00108 //PinDetect inputBall(PTB23);
00109 
00110 //InterruptIn ball(PTB23);
00111 
00112 bool goalButtonPressed = false;
00113 bool startButtonReleased = false;
00114 bool ballStateChanged = false;
00115 int ballState = 0;
00116 
00117 float coilCapVoltage = 0.0f;
00118 
00119 /*void goalFall() {
00120     goalButtonPressed = true;
00121 }
00122 
00123 void startRise() {
00124     startButtonReleased = true;
00125 }
00126 
00127 void ballRise() {    
00128     ballState = 1;
00129     ballStateChanged = true;
00130 }
00131 
00132 void ballFall() {
00133     ballState = 0;
00134     ballStateChanged = true;
00135 }*/
00136 
00137 //RgbLed rgbLed1(&ioExt, 14, 15, 13);
00138 //RgbLed rgbLed2(&ioExt, 11, 12, 10);
00139 
00140 HumanInterface humanInterface(&ioExt, 14, 15, 13, 11, 12, 10, PTC15, PTC14, PTB23);
00141 
00142 //InterruptIn sw2(SW2);
00143 
00144 PwmOut servo1(PTD1);
00145 PwmOut servo2(PTD0);
00146 
00147 //uLCD_4DGL uLCD(PTC17, PTC16, PTB9);
00148 
00149 char buffer[256];
00150 char sendBuffer[256];
00151 int charCounter = 0;
00152 
00153 char serialBuffer[512];
00154 
00155 // This function is called when a character goes from the TX buffer
00156 // to the Uart THR FIFO register.
00157 /*void txCallback(MODSERIAL_IRQ_INFO *q) {
00158     led2 = !led2;
00159 }*/
00160  
00161 // This function is called when TX buffer goes empty
00162 /*void txEmpty(MODSERIAL_IRQ_INFO *q) {
00163     led2 = 0;
00164     //pc.puts(" Done. ");
00165 }*/
00166  
00167 // This function is called when a character goes into the RX buffer.
00168 /*void rxCallback(MODSERIAL_IRQ_INFO *q) {
00169     //led3 = !led3;
00170     
00171     while (pc.readable()) {
00172         serialBuffer[charCounter] = pc.getc();
00173         if (serialBuffer[charCounter] == '\n') {
00174             serialBuffer[charCounter] = '\0';
00175             //led2 = !led2;
00176             executeCommand(serialBuffer);
00177             charCounter = 0;
00178         } else {
00179             charCounter++;
00180         }         
00181     }
00182 }*/
00183 
00184 void PcRxIRQ() {
00185     // Note: you need to actually read from the serial to clear the RX interrupt
00186     printf("%c\n", pc.getc());
00187     
00188     /*while (pc.readable()) {
00189         serialBuffer[charCounter] = pc.getc();
00190         if (serialBuffer[charCounter] == '\n') {
00191             serialBuffer[charCounter] = '\0';
00192             executeCommand(serialBuffer);
00193             charCounter = 0;
00194         } else {
00195             charCounter++;
00196         }         
00197     }*/
00198 }
00199 
00200 void serialReceive() {
00201     // Note: you need to actually read from the serial to clear the RX interrupt
00202     printf("%c\n", pc.getc());
00203     //led2 = !led2;
00204 }
00205 
00206 void sw2_release(void) {
00207     led3 = !led3;
00208 }
00209 
00210 void sw2_press(void) {
00211     led1 = !led1;
00212 }
00213 
00214 /*PinDetect buttonGoal(PTC14);
00215 int goalStateChanged = 0;
00216 int goalState = 0;
00217 
00218 void goalFall() {
00219     goalState ^= 1;
00220     goalStateChanged = 1;
00221 }*/
00222 
00223 //IMU imu(0x69);
00224 
00225 void updateTick() { 
00226     //led3 = 1;   
00227     motor1.pid();
00228     motor2.pid();
00229     motor3.pid();
00230     motor4.pid();
00231     motor5.pid();
00232     //led3 = 0;
00233     update = 1;
00234 }
00235 
00236 void imuUpdateTick() {
00237     updateIMU = 1;
00238 }
00239 
00240 void testUpdateTick() {
00241     updateTest = 1;
00242 }
00243 
00244 void stallChangedCallback() {
00245     stallChanged = 1;
00246 }
00247 
00248 //UDPSocket server;
00249 //Endpoint client;
00250 
00251 //void readSerial() {
00252     
00253     //int n = pc.readable();
00254     /*if (pc.readable()) {
00255         
00256         //pc.printf("Received packet from: %s\n", client.get_address());
00257         //pc.printf("n: %d\n", n);
00258         //pc.scanf("%s", &buffer);
00259         //pc.printf("%c\n", pc.getc());
00260         //buffer[charCounter] = pc.getc();
00261         //printf("%c\n", buffer[charCounter]);
00262         //pc.printf("%s\n", buffer);
00263         //if (buffer[charCounter] == '\n') {
00264         //    buffer[charCounter] = '\0';
00265         //    executeCommand(buffer);
00266         //    charCounter = 0;
00267         //} else {
00268         //    charCounter++;
00269         }*/         
00270     //}
00271     
00272 //}
00273 
00274 Timer t;
00275 Timer aliveTime;
00276 
00277 float dtMax = 0.0f;
00278 
00279 unsigned int currentKickLength = 0;
00280 unsigned int currentKickDelay = 0;
00281 unsigned int currentChipLength = 0;
00282 unsigned int currentChipDelay = 0;
00283 bool kickWhenBall = false;
00284 bool sendKicked = false;
00285 
00286 //EthernetInterface eth;
00287 
00288 int main (void) {
00289     
00290     //enable_rx_fifo(0);
00291     
00292     pc.baud(115200);
00293     
00294     aliveTime.start();
00295     
00296     //pc.attach(&txCallback, MODSERIAL::TxIrq);
00297     //pc.attach(&rxCallback, MODSERIAL::RxIrq);
00298     //pc.attach(&txEmpty, MODSERIAL::TxEmpty);
00299     
00300     // enable the usb uart rx fifo
00301     //UART_PFIFO_REG(UART0) |= 0x08;
00302     
00303     // enable the UART Rx callback interrupt
00304     //pc.attach(&PcRxIRQ, pc.RxIrq);
00305     
00306     //pc.attach(&onSerialData);
00307     
00308     //uLCD.baudrate(3000000);
00309     
00310     //RED
00311     led1 = 0;
00312     led3 = 1;
00313     led4 = 1;
00314     
00315     //rgbLed1.setColor(RgbLed::WHITE);
00316     //rgbLed2.setColor(RgbLed::YELLOW);
00317     
00318     //chargeDone.rise(&sw2_release);
00319     
00320     //GREEN
00321     led1 = 1;
00322     led3 = 0;
00323     led4 = 1;    
00324     
00325     
00326     //YELLOW
00327     led1 = 0;
00328     led3 = 0;
00329     led4 = 1;
00330     
00331     //eth.init("192.168.4.1", "255.255.255.0", "192.168.4.8");
00332     //eth.init("192.168.0.128", "255.255.255.0", "192.168.0.1");
00333     
00334     //wait(3);
00335     
00336     //eth.init();
00337     
00338     //MAGENTA
00339     led1 = 0;
00340     led3 = 1;
00341     led4 = 0;
00342     
00343     //eth.connect(10000);
00344     
00345     //WHITE
00346     /*led1 = 0;
00347     led3 = 0;
00348     led4 = 0;*/
00349     
00350     //pc.printf("IP Address is %s\n", eth.getIPAddress());
00351 
00352     //server.bind(SERVER_PORT);
00353     
00354     //CYAN
00355     led1 = 1;
00356     led3 = 0;
00357     led4 = 0;
00358     
00359     servo1.period_us(200000);
00360     
00361     servo1.pulsewidth_us(1500);
00362     servo2.pulsewidth_us(1500);
00363 
00364     //pc.printf("Starting ADXL345 test...\n");
00365     //wait(.001);
00366     //pc.printf("Device ID is: 0x%02x\n", accelerometer.getDeviceID());
00367     //wait(.001);
00368     
00369     // These are here to test whether any of the initialization fails. It will print the failure
00370     /*if (accelerometer.setPowerControl(0x00)){
00371         pc.printf("didn't intitialize power control\n"); 
00372         return 0;
00373     }*/
00374     //Full resolution, +/-16g, 4mg/LSB.
00375     /*wait(.001);
00376     
00377     if(accelerometer.setDataFormatControl(0x0B)){
00378         pc.printf("didn't set data format\n");
00379         return 0;
00380     }
00381      
00382     wait(.001);*/
00383      
00384     //3.2kHz data rate.
00385     /*if(accelerometer.setDataRate(ADXL345_3200HZ)){
00386         pc.printf("didn't set data rate\n");
00387         return 0;
00388     }
00389         
00390     wait(.001);*/
00391      
00392     //Measurement mode.
00393      
00394     /*if (accelerometer.setPowerControl(MeasurementMode)) {
00395         pc.printf("didn't set the power control to measurement\n"); 
00396         return 0;
00397     }
00398     */
00399     
00400     
00401     
00402     //server.set_blocking(false, 1);
00403     
00404     //int ioExtBefore = ioExt.read();
00405     //pc.printf("ioExt: %s\n", byte_to_binary(ioExt.read()));
00406     //pc.printf("ioExt: %x\n", ioExt.read());
00407     ioExt.setDirection(0x0000);
00408     ioExt.write(0xff00);
00409     int ioExtAfter = ioExt.read();
00410     pc.printf("ioExt: %x\n", ioExt.read());
00411     //pc.printf("ioExt: %s\n", byte_to_binary(ioExt.read()));
00412     
00413     //BLUE
00414     led1 = 1;
00415     led3 = 1;
00416     led4 = 0;    
00417     
00418     
00419     motor1.stallChange(&stallChangedCallback);
00420     motor2.stallChange(&stallChangedCallback);
00421     motor3.stallChange(&stallChangedCallback);
00422     motor4.stallChange(&stallChangedCallback);
00423     motor5.stallChange(&stallChangedCallback);
00424     
00425     //buttonGoal.mode(PullUp);
00426     //buttonStart.mode(PullUp);
00427     
00428     //inputBall.mode(PullUp);
00429     //ball.mode(PullUp);
00430     //ball.fall(&ballFall);
00431     //ball.rise(&ballRise);
00432     
00433     //buttonGoal.attach_deasserted(&goalFall);
00434     //buttonStart.attach_asserted(&startRise);
00435     //inputBall.attach_asserted(&ballRise);
00436     //inputBall.attach_deasserted(&ballFall);
00437     
00438     //buttonGoal.setSamplesTillAssert(20);
00439     //buttonStart.setSamplesTillAssert(20);
00440     //inputBall.setSamplesTillAssert(2);
00441     
00442     //buttonGoal.setSampleFrequency(1000);
00443     //buttonStart.setSampleFrequency(1000);
00444     //inputBall.setSampleFrequency(250);
00445     
00446     /*int charCount = sprintf(sendBuffer, "<ready>");
00447     server.sendTo(client, sendBuffer, charCount);*/
00448     
00449        
00450     //MPU6050 mpu = imu.getMPU();
00451        
00452     /*bool mpu6050TestResult = mpu.testConnection();
00453     if(mpu6050TestResult) {
00454         pc.printf("IMU test passed\n");
00455     } else {
00456         pc.printf("IMU test failed\n");
00457     }*/
00458     
00459     //wait(2);
00460     
00461     //pc.printf("IMU calibration\n");
00462     
00463     //imu.zeroGyroZ();
00464     //imu.startPolling();
00465     
00466     sensorUpdate.attach(&updateTick, 1.0 / 60.0);
00467     
00468     //imuUpdate.attach(&imuUpdateTick, 1.0 / 200.0);
00469     
00470     testUpdate.attach(&testUpdateTick, 0.2);
00471     
00472     //pc.printf("IMU started\n");
00473     
00474     //time_t seconds = time(NULL);
00475         
00476     //printf("time = %d\n", time(NULL));
00477     
00478     //set_time(1256729737);
00479     
00480     /*int sampleCount = 0;
00481     int calibrationCount = 1000;
00482     bool calibrating = true;    
00483     
00484     pc.printf("Start IMU calibration\n");*/
00485     
00486     //imu.startGzCalibration();
00487         
00488     /*buttonGoal.mode(PullUp);        
00489     buttonGoal.attach_deasserted(&goalFall);        
00490     buttonGoal.setSamplesTillAssert(20);        
00491     buttonGoal.setSampleFrequency(1000);*/
00492     
00493     //sw2.rise(&sw2_release);    
00494     //sw2.fall(&sw2_press);
00495     
00496     //OFF
00497     led1 = 1;
00498     led3 = 1;
00499     led4 = 1;
00500     
00501     //uLCD.printf("Ready\n");
00502     
00503     //humanInterface.setError(1);
00504     
00505     bool isMainUpdate = false;
00506     
00507     int serialReadUnblockCount = 0;
00508     int serialReadUnblockLimit = 40;
00509     
00510     watchdog.kick(3);
00511     
00512     while (1) {
00513         
00514         serialReadUnblockCount = 0;
00515         
00516         //int k = pc.readable();
00517         while (pc.readable() && serialReadUnblockCount < serialReadUnblockLimit) {
00518             serialReadUnblockCount++;
00519             //pc.printf("Received packet from: %s\n", client.get_address());
00520             //pc.printf("k: %d\n", k);
00521             //pc.scanf("%s", &serialBuffer);
00522             serialBuffer[charCounter] = pc.getc();
00523             //pc.printf("%c\n", serialBuffer[charCounter]);
00524             //fflush(stdout);
00525             //fflush(stdin);
00526             //pc.printf("%s\n", serialBuffer);
00527             if (serialBuffer[charCounter] == '\n') {
00528                 serialBuffer[charCounter] = '\0';
00529                 executeCommand(serialBuffer);
00530                 led1 = !led1;
00531                 charCounter = 0;
00532                 break;
00533             } else {
00534                 charCounter++;
00535             }
00536             
00537             //led3 = !led3;
00538             
00539             //server.sendTo(client, serialBuffer, n);               
00540         }
00541         
00542         //if (updateIMU) {
00543             //updateIMU = 0;
00544             //t.stop();
00545             //float dt = t.read();
00546             //t.reset();
00547             //t.start();
00548             //printf("dt = %.6f\n", dt);
00549             //imu.update();
00550             
00551             /*if (calibrating) {
00552                sampleCount++;
00553                if (sampleCount >= calibrationCount) {
00554                    calibrating = false;
00555                    imu.stopGzCalibration();
00556                    pc.printf("Stop IMU calibration\n");
00557                    pc.printf("GyroZ zero %.3f\n", imu.getGzZero());
00558                }
00559             }*/
00560             
00561             //led3 = !led3;
00562         //}
00563     
00564         if (update) {            
00565             update = 0;
00566             ioExt.writePins();
00567 
00568             failSafeCountMotors++;
00569             failSafeCountCoilgun++;
00570             
00571             if (failSafeCountMotors == failSafeLimitMotors) {
00572                 failSafeCountMotors = 0;
00573                 if (failSafeEnabled) {
00574                     
00575                     motor1.setSpeed(0);
00576                     motor2.setSpeed(0);
00577                     motor3.setSpeed(0);
00578                     motor4.setSpeed(0);
00579                     motor5.setSpeed(0);
00580                     //dribbler.pulsewidth_us(0);     
00581                     
00582                     //pc.rxBufferFlush();
00583                     //pc.txBufferFlush();                
00584                 }                
00585             }
00586             
00587             if (failSafeCountCoilgun == failSafeLimitCoilgun) {
00588                 failSafeCountCoilgun = 0;
00589                 if (failSafeEnabled) {
00590                     coilgun.discharge();
00591                     
00592                     if (!coilgun.isCharged) {
00593                         //server.sendTo(client, "<discharged>", 12);
00594                         pc.printf("<discharged>\n");
00595                     }
00596                 }                
00597             }
00598             
00599             //led3 = 1;
00600             //updateSensors();
00601             //pc.printf("update");                       
00602             /*motor1.pid();
00603             motor2.pid();
00604             motor3.pid();
00605             motor4.pid();
00606             motor5.pid();*/
00607             
00608             //led3 = 0;
00609             //int charCount = sprintf(sendBuffer, "<speeds:%d:%d:%d:%d:%d>", motor1.getSpeed(), motor2.getSpeed(), motor3.getSpeed(), motor4.getSpeed(), motor5.getSpeed());
00610             //server.sendTo(client, sendBuffer, charCount); 
00611             //pc.printf("<speeds:%d:%d:%d:%d:%d>\n", motor1.getSpeed(), motor2.getSpeed(), motor3.getSpeed(), motor4.getSpeed(), motor5.getSpeed()); 
00612             //redLed.toggle();
00613             
00614             //ioExt.setDirection(0x0400);
00615             
00616             //led1 = !led1;
00617             
00618             /*t.stop();
00619             float dt = t.read();
00620             t.reset();
00621             t.start();
00622             
00623             if (dt > dtMax) {
00624                 dtMax = dt;
00625             }
00626             
00627             if (dt >= 0.02f) {
00628                 printf("<dtmax=%.5f>\n", dt);
00629             } 
00630             
00631             if (dtMax >= 0.02f) {
00632                 printf("<dtmax=%.5f>\n", dtMax);
00633             }*/
00634             
00635             isMainUpdate = true;
00636         }
00637         
00638         if (updateTest) {
00639             updateTest = 0;
00640             
00641             //watchdog.kick();
00642             
00643             //if (!calibrating) {
00644             //   pc.printf("gz:%.3f\n", imu.getDegZ());
00645             //}
00646             
00647             //uLCD.printf("AyijklL1230\n");
00648             
00649             //rgbLed1.toggle();
00650             //rgbLed2.toggle();
00651             
00652             led4 = !led4;
00653             
00654             if (!isMainUpdate) {
00655                 coilCapVoltage = coilADC.read() * 80;
00656             }
00657             //coilCapVoltage = 300.0f;
00658             
00659             //float dtSlow = aliveTime.read();
00660             //printf("<up=%.1f>\n", dtSlow);
00661         }
00662         
00663         isMainUpdate = false;
00664         
00665         if (stallChanged) {
00666             stallChanged = 0;
00667             /*int charCount = sprintf(sendBuffer, "<stall:%d:%d:%d:%d:%d>",
00668                 motor1.getStallLevel(),
00669                 motor2.getStallLevel(),
00670                 motor3.getStallLevel(),
00671                 motor4.getStallLevel(),
00672                 motor5.getStallLevel());
00673             server.sendTo(client, sendBuffer, charCount);*/
00674             pc.printf(
00675                 "<stall:%d:%d:%d:%d:%d>\n", 
00676                 motor1.getStallLevel(),
00677                 motor2.getStallLevel(),
00678                 motor3.getStallLevel(),
00679                 motor4.getStallLevel(),
00680                 motor5.getStallLevel()
00681             );
00682         }
00683         
00684         /*if (ballStateChanged) {
00685             ballStateChanged = false;
00686             if (ballState) {
00687                 //server.sendTo(client, "<ball:1>", 8);
00688                 pc.printf("<ball:1>");
00689             } else {
00690                 //server.sendTo(client, "<ball:0>", 8);
00691                 pc.printf("<ball:0>");
00692             }
00693         }*/
00694         
00695         /*if (goalButtonPressed) {
00696             goalButtonPressed = false;
00697             //server.sendTo(client, "<toggle-side>", 13);
00698         }*/
00699         
00700         /*if (startButtonReleased) {
00701             startButtonReleased = false;
00702             //server.sendTo(client, "<toggle-go>", 11);
00703         }*/
00704         
00705         int newBallState = humanInterface.getBallState();
00706         if (ballState != newBallState) {
00707             pc.printf("<ball:%d>\n", newBallState);
00708             ballState = newBallState;
00709             
00710             if (kickWhenBall && ballState) {
00711                 kickWhenBall = false;
00712                 coilgun.kick(currentKickLength, currentKickDelay, currentChipLength, currentChipDelay);
00713                 sendKicked = true;
00714             }
00715             
00716             if (!ballState && sendKicked) {
00717                 sendKicked = true;
00718                 pc.printf("<kicked>\n");
00719             }
00720         }
00721         
00722         /*switch (humanInterface.getBallState()){
00723             case -1:
00724                 //Ball lost
00725                 //server.sendTo(client, "<ball:0>", 8);
00726                 pc.printf("<ball:0>\n");
00727                 ballState = 0;
00728                 break;
00729             case 0:
00730                 //Nothing has changed..
00731                 break;
00732             case 1:
00733                 //Ball found
00734                 //server.sendTo(client, "<ball:1>", 8);
00735                 pc.printf("<ball:1>\n");
00736                 ballState = 1;
00737                 break;
00738             default:
00739                 break;
00740         }*/
00741         
00742         if (humanInterface.isGoalChange()) {
00743             //server.sendTo(client, "<toggle-side>", 13);
00744             pc.printf("<toggle-side>\n");
00745         }
00746         
00747         if (humanInterface.isStart()) {
00748             //server.sendTo(client, "<toggle-go>", 11);
00749             pc.printf("<toggle-go>\n");
00750         }
00751     
00752         //printf("\nWait for packet...\n");
00753 
00754         /*int n = server.receiveFrom(client, buffer, sizeof(buffer));
00755 
00756         if (n > 0) {
00757             //pc.printf("Received packet from: %s\n", client.get_address());
00758             //pc.printf("n: %d\n", n);
00759             buffer[n] = '\0';
00760             //server.sendTo(client, buffer, n); 
00761             executeCommand(buffer);
00762             //coilgun.kick(1000);
00763             //executeCommand((short*)buffer);
00764         }*/
00765         
00766         /*if (pc.readable()) {
00767             led1 = !led1;
00768             pc.putc(pc.getc());
00769         }*/
00770         
00771         
00772     }
00773 }
00774 
00775 void executeCommand(char *buffer) {
00776     failSafeCountMotors = 0;
00777     failSafeCountCoilgun = 0;
00778 
00779     //pc.printf("cmd: %s\n", buffer);
00780     //uLCD.printf("%s\n", buffer);
00781     
00782     char *cmd;    
00783     cmd = strtok(buffer, ":");
00784     
00785     //pc.printf("%s\n", cmd);  
00786     
00787     if (strncmp(cmd, "speeds", 6) == 0) {
00788         motor1.setSpeed(atoi(strtok(NULL, ":")));
00789         motor2.setSpeed(atoi(strtok(NULL, ":")));
00790         motor3.setSpeed(atoi(strtok(NULL, ":")));
00791         motor4.setSpeed(atoi(strtok(NULL, ":")));
00792         motor5.setSpeed(atoi(strtok(NULL, ":")));
00793         
00794         /*int dribblerSpeed = atoi(strtok(NULL, ":")) * 10;
00795         
00796         if (dribblerSpeed < 0) {
00797             dribblerSpeed = -dribblerSpeed;
00798             ioExt.setPin(0);
00799         } else {
00800             ioExt.clearPin(0);
00801         }*/
00802         
00803         //dribbler.pulsewidth_us(dribblerSpeed);
00804         
00805         /*int charCount = sprintf(sendBuffer, "<speeds:%d:%d:%d:%d:%d>",
00806             motor1.getSpeed(), motor2.getSpeed(), motor3.getSpeed(), motor4.getSpeed(), motor5.getSpeed());
00807         server.sendTo(client, sendBuffer, charCount);*/
00808         
00809         pc.printf("<speeds:%d:%d:%d:%d:%d>\n", motor1.getSpeed(), motor2.getSpeed(), motor3.getSpeed(), motor4.getSpeed(), motor5.getSpeed());
00810     } /*else if (strncmp(cmd, "ga", 2) == 0) {
00811         //int charCount = sprintf(sendBuffer, "<angle:%.2f>", imu.getDegZ());
00812         //server.sendTo(client, sendBuffer, charCount);
00813     }*/ else if (strncmp(cmd, "kick", 4) == 0) {        
00814         unsigned int kickLength = atoi(strtok(NULL, ":"));
00815         coilgun.kick(kickLength, 0, 0, 0);
00816         pc.printf("<ball:%d>\n", ballState);
00817     } else if (strncmp(cmd, "dkick", 5) == 0) {        
00818         unsigned int kickLength = atoi(strtok(NULL, ":"));
00819         unsigned int kickDelay = atoi(strtok(NULL, ":"));
00820         unsigned int chipLength = atoi(strtok(NULL, ":"));
00821         unsigned int chipDelay = atoi(strtok(NULL, ":"));
00822         //pc.printf("kick:%d:%d:%d:%d\n", kickLength, kickDelay, chipLength, chipDelay);
00823         coilgun.kick(kickLength, kickDelay, chipLength, chipDelay);
00824         pc.printf("<ball:%d>\n", ballState);
00825     } else if (strncmp(cmd, "bdkick", 6) == 0) {        
00826         currentKickLength = atoi(strtok(NULL, ":"));
00827         currentKickDelay = atoi(strtok(NULL, ":"));
00828         currentChipLength = atoi(strtok(NULL, ":"));
00829         currentChipDelay = atoi(strtok(NULL, ":"));
00830         //pc.printf("kick:%d:%d:%d:%d\n", kickLength, kickDelay, chipLength, chipDelay);
00831         if (ballState) {
00832             coilgun.kick(currentKickLength, currentKickDelay, currentChipLength, currentChipDelay);
00833             kickWhenBall = false;
00834         } else {
00835             kickWhenBall = true;
00836         }
00837         pc.printf("<ball:%d>\n", ballState);
00838     } else if (strncmp(cmd, "nokick", 6) == 0) {
00839         kickWhenBall = false;
00840     } else if (strncmp(cmd, "charge", 6) == 0) {
00841         //pc.printf("charge\n");
00842         coilgun.charge();
00843     } else if (strncmp(cmd, "discharge", 9) == 0) {
00844         //pc.printf("discharge\n");
00845         coilgun.discharge();
00846     } else if (strncmp(cmd, "servos", 6) == 0) {
00847         int servo1Duty = atoi(strtok(NULL, ":"));
00848         int servo2Duty = atoi(strtok(NULL, ":"));
00849 
00850         if (servo1Duty + servo2Duty > 3340) {
00851             pc.printf("<err:servo1Duty + servo2Duty must be smaller than 3340>\n");
00852         } else {
00853             if (servo1Duty < 600 || servo1Duty > 2500) {
00854                 pc.printf("<err:servo1Duty must be between 600 and 2500>\n");
00855             } else {
00856                 servo1.pulsewidth_us(servo1Duty);
00857             }
00858             
00859             if (servo2Duty < 600 || servo2Duty > 2500) {
00860                 pc.printf("<err:servo2Duty must be between 600 and 2500>\n");
00861             } else {
00862                 servo2.pulsewidth_us(servo2Duty);
00863             }
00864         }
00865     } else if (strncmp(cmd, "gs", 2) == 0) {
00866         /*int charCount = sprintf(sendBuffer, "<speeds:%d:%d:%d:%d:%d>",
00867             motor1.getSpeed(), motor2.getSpeed(), motor3.getSpeed(), motor4.getSpeed(), motor5.getSpeed());
00868         server.sendTo(client, sendBuffer, charCount);*/
00869         pc.printf("<speeds:%d:%d:%d:%d:%d>\n", motor1.getSpeed(), motor2.getSpeed(), motor3.getSpeed(), motor4.getSpeed(), motor5.getSpeed());
00870     } else if (strncmp(cmd, "c", 1) == 0) {
00871         //pc.printf("charge\n");
00872         if (atoi(strtok(NULL, ":")) == 1) {
00873             coilgun.charge();
00874         } else {
00875             coilgun.chargeEnd();
00876         }
00877     } else if (strncmp(cmd, "d", 1) == 0) {
00878         //pc.printf("discharge\n");
00879         coilgun.discharge();
00880     } else if (strncmp(cmd, "reset", 5) == 0) {
00881         motor1.setSpeed(0);
00882         motor2.setSpeed(0);
00883         motor3.setSpeed(0);
00884         motor4.setSpeed(0);
00885         motor5.setSpeed(0);
00886         
00887         //dribbler.pulsewidth_us(0);        
00888         
00889         humanInterface.setGoal(HumanInterface::UNSET);
00890         humanInterface.setError(false);
00891         humanInterface.setGo(false);
00892     } else if (strncmp(cmd, "fs", 2) == 0) {
00893         failSafeEnabled = (bool)atoi(strtok(NULL, ":"));
00894     } else if (strncmp(cmd, "target", 6) == 0) {
00895         int target = atoi(strtok(NULL, ":"));
00896         /*if (target == 0) {
00897             humanInterface.setGoal(HumanInterface::BLUE);
00898         } else if (target == 1) {
00899             humanInterface.setGoal(HumanInterface::YELLOW);
00900         } else if (target == 2) {
00901             humanInterface.setGoal(HumanInterface::UNSET);
00902         }*/
00903     } else if (strncmp(cmd, "error", 5) == 0) {
00904         humanInterface.setError(atoi(strtok(NULL, ":")));
00905     } else if (strncmp(cmd, "go", 2) == 0) {
00906         humanInterface.setGo(atoi(strtok(NULL, ":")));
00907     } else if (strncmp(cmd, "adc", 5) == 0) {
00908         //pc.printf("<adc:%.1f>\n", coilADC.read() * 80);
00909         pc.printf("<adc:%.1f>\n", coilCapVoltage);
00910         /*int charCount = sprintf(sendBuffer, "<adc:%.1f>\n", coilADC.read() * 80);
00911         server.sendTo(client, sendBuffer, charCount);*/
00912     } /*else if (strncmp(cmd, "red", 3) == 0) {
00913         rgbLed1.setColor(RgbLed::RED);
00914     } else if (strncmp(cmd, "green", 5) == 0) {
00915         rgbLed1.setColor(RgbLed::GREEN);
00916     } else if (strncmp(cmd, "blue", 4) == 0) {
00917         rgbLed1.setColor(RgbLed::BLUE);
00918     } else if (strncmp(cmd, "cyan", 4) == 0) {
00919         rgbLed1.setColor(RgbLed::CYAN);
00920     } else if (strncmp(cmd, "magenta", 7) == 0) {
00921         rgbLed1.setColor(RgbLed::MAGENTA);
00922     } else if (strncmp(cmd, "yellow", 6) == 0) {
00923         rgbLed1.setColor(RgbLed::YELLOW);
00924     } else if (strncmp(cmd, "white", 5) == 0) {
00925         rgbLed1.setColor(RgbLed::WHITE);
00926     } else if (strncmp(cmd, "off", 3) == 0) {
00927         rgbLed1.setColor(RgbLed::OFF);
00928     }*/
00929 }