Diff: main.cpp

Revision:

9:2d2030d989d8

Parent:

8:06124632c3e4

Child:

11:92eecb155136

--- a/main.cpp	Tue Sep 02 15:40:53 2014 +0000
+++ b/main.cpp	Fri Nov 21 18:30:35 2014 +0000
@@ -1,35 +1,46 @@
 #include "mbed.h"
-#include "EthernetInterface.h"
+//#include "EthernetInterface.h"
 //#include "ADXL345_I2C.h"
 //#include "L3G4200D.h"
 //#include "HMC5883L.h"
 #include "PCA9555.h"
 #include "motor.h"
 #include "qed.h"
-#include "ledout.h"
-#include "externalin.h"
+//#include "ledout.h"
+//#include "rgb-led-pca9555.h"
+//#include "externalin.h"
 #include "coilgun.h"
 #include "HumanInterface.h"
 //#include "MPU6050.h"
 //#include "imu.h"
-//#include "MCP3021.h"
+#include "MCP3021.h"
+//#include "PinDetect.h"
+//#include "uLCD_4DGL.h"
+//#include "fsl_uart_hal.h"
+#include "MODSERIAL.h"
+#include "Watchdog.h" 
 
 #define SERVER_PORT   8042
 
+//Watchdog watchdog;
+
 Ticker sensorUpdate;
 Ticker imuUpdate;
 Ticker testUpdate;
 
-DigitalOut led1(LED1);
-DigitalOut led3(LED3);
-DigitalOut led4(LED4);
+DigitalOut led1(LED_RED);
+DigitalOut led3(LED_GREEN);
+DigitalOut led4(LED_BLUE);
 
-Serial pc(USBTX, USBRX); // tx, rx
+//Serial pc(USBTX, USBRX); // tx, rx
+MODSERIAL pc(USBTX, USBRX, 8192, 8192); 
 //ADXL345_I2C accelerometer(p9, p10);
 //L3G4200D gyro(p9, p10);
 //HMC5883L compass(p9, p10);
 
-PCA9555 ioExt(p9, p10, p8, 0x40);
+
+MCP3021 coilADC(PTC11, PTC10, 5.0);
+PCA9555 ioExt(PTC11, PTC10, PTB20, 0x40);
 
 //int readings[3] = {0, 0, 0};
 //int gyroData[3] = {0, 0, 0};
@@ -42,13 +53,25 @@
 
 volatile int stallChanged = 0;
 
-bool failSafeEnabled = false;
-int failSafeCount = 0;
-int failSafeLimit = 60;
+bool failSafeEnabled = true;
+int failSafeCountMotors = 0;
+int failSafeCountCoilgun = 0;
+int failSafeLimitMotors = 60;
+int failSafeLimitCoilgun = 300;
 
 
 void executeCommand(char *buffer);
 
+/*static void enable_rx_fifo(int instance) {
+    uart_hal_disable_receiver(instance);
+    uart_hal_disable_transmitter(instance);
+    uart_hal_enable_rx_fifo(instance);
+    uart_hal_enable_receiver(instance);
+    uart_hal_enable_transmitter(instance);
+    uart_hal_flush_rx_fifo(instance);
+    uart_hal_flush_tx_fifo(instance);
+}*/
+
 /*
 void updateSensors() {
     if (led1 == 1.0) {
@@ -66,18 +89,136 @@
     pc.printf("<comps:%i:%i:%i>\n", compassData[0], compassData[1], compassData[2]);
 }*/
 
+//Dribbler motor without encoder
+//PwmOut dribbler(PTC1);
+
 //motor order: FL, FR, RL, RR, DRIBBLER
-Motor motor4(p25, &ioExt, 0, 1, p5, p6); //RR - M1
-Motor motor2(p24, &ioExt, 2, 3, p7, p11); //FR - M2
-Motor motor5(p23, &ioExt, 4, 5, p12, p13); //DRIBBLER - M3
-Motor motor1(p22, &ioExt, 6, 7, p14, p15); //FL - M4
-Motor motor3(p21, &ioExt, 9, 8, p16, p17); //RL - M5
+//Motor(PinName PWMpin, PCA9555 *ioExt, unsigned int dirPin, PinName encA, PinName encB);
+Motor motor3(PTA2, &ioExt, 2, PTC18, PTD6); //RL - M1
+Motor motor1(PTA1, &ioExt, 1, PTD7, PTD5); //FL - M2
+Motor motor5(PTC1, &ioExt, 0, PTD4, PTC12); //DRIBBLER - M3
+Motor motor2(PTC4, &ioExt, 5, PTB18, PTB19); //FR - M4
+Motor motor4(PTC3, &ioExt, 4, PTC8, PTC9); //RR - M5
+
+Coilgun coilgun(PTB11, PTB10, PTB3, PTB2);
+//Coilgun coilgun(PTB11, PTB10, PTB3);
+
+//PinDetect buttonGoal(PTC15);
+//PinDetect buttonStart(PTC14);
+//PinDetect inputBall(PTB23);
+
+//InterruptIn ball(PTB23);
+
+bool goalButtonPressed = false;
+bool startButtonReleased = false;
+bool ballStateChanged = false;
+int ballState = 0;
+
+float coilCapVoltage = 0.0f;
+
+/*void goalFall() {
+    goalButtonPressed = true;
+}
+
+void startRise() {
+    startButtonReleased = true;
+}
+
+void ballRise() {    
+    ballState = 1;
+    ballStateChanged = true;
+}
+
+void ballFall() {
+    ballState = 0;
+    ballStateChanged = true;
+}*/
+
+//RgbLed rgbLed1(&ioExt, 14, 15, 13);
+//RgbLed rgbLed2(&ioExt, 11, 12, 10);
+
+HumanInterface humanInterface(&ioExt, 14, 15, 13, 11, 12, 10, PTC15, PTC14, PTB23);
+
+//InterruptIn sw2(SW2);
+
+PwmOut servo1(PTD1);
+PwmOut servo2(PTD0);
+
+//uLCD_4DGL uLCD(PTC17, PTC16, PTB9);
+
+char buffer[256];
+char sendBuffer[256];
+int charCounter = 0;
+
+char serialBuffer[8192];
 
-Coilgun coilgun(p20, p30, p19, p18);
+// This function is called when a character goes from the TX buffer
+// to the Uart THR FIFO register.
+/*void txCallback(MODSERIAL_IRQ_INFO *q) {
+    led2 = !led2;
+}*/
+ 
+// This function is called when TX buffer goes empty
+/*void txEmpty(MODSERIAL_IRQ_INFO *q) {
+    led2 = 0;
+    //pc.puts(" Done. ");
+}*/
+ 
+// This function is called when a character goes into the RX buffer.
+/*void rxCallback(MODSERIAL_IRQ_INFO *q) {
+    //led3 = !led3;
+    
+    while (pc.readable()) {
+        serialBuffer[charCounter] = pc.getc();
+        if (serialBuffer[charCounter] == '\n') {
+            serialBuffer[charCounter] = '\0';
+            //led2 = !led2;
+            executeCommand(serialBuffer);
+            charCounter = 0;
+        } else {
+            charCounter++;
+        }         
+    }
+}*/
 
-//HumanInterface humanInterface(&ioExt, 15, 14, 13, 12, 11, p29, p30, p26);
+void PcRxIRQ() {
+    // Note: you need to actually read from the serial to clear the RX interrupt
+    printf("%c\n", pc.getc());
+    
+    /*while (pc.readable()) {
+        serialBuffer[charCounter] = pc.getc();
+        if (serialBuffer[charCounter] == '\n') {
+            serialBuffer[charCounter] = '\0';
+            executeCommand(serialBuffer);
+            charCounter = 0;
+        } else {
+            charCounter++;
+        }         
+    }*/
+}
 
-//MCP3021 coilADC(p9, p10, 5.0);
+void serialReceive() {
+    // Note: you need to actually read from the serial to clear the RX interrupt
+    printf("%c\n", pc.getc());
+    //led2 = !led2;
+}
+
+void sw2_release(void) {
+    led3 = !led3;
+}
+
+void sw2_press(void) {
+    led1 = !led1;
+}
+
+/*PinDetect buttonGoal(PTC14);
+int goalStateChanged = 0;
+int goalState = 0;
+
+void goalFall() {
+    goalState ^= 1;
+    goalStateChanged = 1;
+}*/
 
 //IMU imu(0x69);
 
@@ -104,58 +245,121 @@
     stallChanged = 1;
 }
 
-UDPSocket server;
-Endpoint client;
-char buffer[256];
-char sendBuffer[256];
-int charCounter = 0;
+//UDPSocket server;
+//Endpoint client;
 
 //void readSerial() {
     
     //int n = pc.readable();
-    //if (pc.readable()) {
+    /*if (pc.readable()) {
         
         //pc.printf("Received packet from: %s\n", client.get_address());
         //pc.printf("n: %d\n", n);
         //pc.scanf("%s", &buffer);
         //pc.printf("%c\n", pc.getc());
         //buffer[charCounter] = pc.getc();
-        /*printf("%c\n", buffer[charCounter]);
+        //printf("%c\n", buffer[charCounter]);
         //pc.printf("%s\n", buffer);
-        if (buffer[charCounter] == '\n') {
-            buffer[charCounter] = '\0';
-            executeCommand(buffer);
-            charCounter = 0;
-        } else {
-            charCounter++;
+        //if (buffer[charCounter] == '\n') {
+        //    buffer[charCounter] = '\0';
+        //    executeCommand(buffer);
+        //    charCounter = 0;
+        //} else {
+        //    charCounter++;
         }*/         
     //}
     
 //}
 
-const char *byte_to_binary(int x) {
-    static char b[9];
-    b[0] = '\0';
+Timer t;
+Timer aliveTime;
+
+float dtMax = 0.0f;
 
-    int z;
-    for (z = 32768; z > 0; z >>= 1) {
-        strcat(b, ((x & z) == z) ? "1" : "0");
-    }
+unsigned int currentKickLength = 0;
+unsigned int currentKickDelay = 0;
+unsigned int currentChipLength = 0;
+unsigned int currentChipDelay = 0;
+bool kickWhenBall = false;
+bool sendKicked = false;
 
-    return b;
-}
-
-Timer t;
+//EthernetInterface eth;
 
 int main (void) {
+    
+    //enable_rx_fifo(0);
+    
     pc.baud(115200);
-    EthernetInterface eth;
-    eth.init("192.168.4.1", "255.255.255.0", "192.168.4.8");
+    
+    aliveTime.start();
+    
+    //pc.attach(&txCallback, MODSERIAL::TxIrq);
+    //pc.attach(&rxCallback, MODSERIAL::RxIrq);
+    //pc.attach(&txEmpty, MODSERIAL::TxEmpty);
+    
+    // enable the usb uart rx fifo
+    //UART_PFIFO_REG(UART0) |= 0x08;
+    
+    // enable the UART Rx callback interrupt
+    //pc.attach(&PcRxIRQ, pc.RxIrq);
+    
+    //pc.attach(&onSerialData);
+    
+    //uLCD.baudrate(3000000);
+    
+    //RED
+    led1 = 0;
+    led3 = 1;
+    led4 = 1;
+    
+    //rgbLed1.setColor(RgbLed::WHITE);
+    //rgbLed2.setColor(RgbLed::YELLOW);
+    
+    //chargeDone.rise(&sw2_release);
+    
+    //GREEN
+    led1 = 1;
+    led3 = 0;
+    led4 = 1;    
+    
+    
+    //YELLOW
+    led1 = 0;
+    led3 = 0;
+    led4 = 1;
+    
+    //eth.init("192.168.4.1", "255.255.255.0", "192.168.4.8");
+    //eth.init("192.168.0.128", "255.255.255.0", "192.168.0.1");
+    
+    //wait(3);
+    
     //eth.init();
-    eth.connect();
-    printf("IP Address is %s\n", eth.getIPAddress());
+    
+    //MAGENTA
+    led1 = 0;
+    led3 = 1;
+    led4 = 0;
+    
+    //eth.connect(10000);
+    
+    //WHITE
+    /*led1 = 0;
+    led3 = 0;
+    led4 = 0;*/
+    
+    //pc.printf("IP Address is %s\n", eth.getIPAddress());
 
-    server.bind(SERVER_PORT);
+    //server.bind(SERVER_PORT);
+    
+    //CYAN
+    led1 = 1;
+    led3 = 0;
+    led4 = 0;
+    
+    servo1.period_us(200000);
+    
+    servo1.pulsewidth_us(1500);
+    servo2.pulsewidth_us(1500);
 
     //pc.printf("Starting ADXL345 test...\n");
     //wait(.001);
@@ -193,26 +397,55 @@
     }
     */
     
-    server.set_blocking(false, 1);
+    
+    
+    //server.set_blocking(false, 1);
     
     //int ioExtBefore = ioExt.read();
     //pc.printf("ioExt: %s\n", byte_to_binary(ioExt.read()));
     //pc.printf("ioExt: %x\n", ioExt.read());
-    ioExt.setDirection(0x0400);
-    ioExt.write(0x0155);
-    //int ioExtAfter = ioExt.read();
-    //pc.printf("ioExt: %x\n", ioExt.read());
+    ioExt.setDirection(0x0000);
+    ioExt.write(0xff00);
+    int ioExtAfter = ioExt.read();
+    pc.printf("ioExt: %x\n", ioExt.read());
     //pc.printf("ioExt: %s\n", byte_to_binary(ioExt.read()));
     
+    //BLUE
+    led1 = 1;
+    led3 = 1;
+    led4 = 0;    
+    
+    
     motor1.stallChange(&stallChangedCallback);
     motor2.stallChange(&stallChangedCallback);
     motor3.stallChange(&stallChangedCallback);
     motor4.stallChange(&stallChangedCallback);
     motor5.stallChange(&stallChangedCallback);
     
+    //buttonGoal.mode(PullUp);
+    //buttonStart.mode(PullUp);
     
-    int charCount = sprintf(sendBuffer, "<ready>");
-    server.sendTo(client, sendBuffer, charCount);
+    //inputBall.mode(PullUp);
+    //ball.mode(PullUp);
+    //ball.fall(&ballFall);
+    //ball.rise(&ballRise);
+    
+    //buttonGoal.attach_deasserted(&goalFall);
+    //buttonStart.attach_asserted(&startRise);
+    //inputBall.attach_asserted(&ballRise);
+    //inputBall.attach_deasserted(&ballFall);
+    
+    //buttonGoal.setSamplesTillAssert(20);
+    //buttonStart.setSamplesTillAssert(20);
+    //inputBall.setSamplesTillAssert(2);
+    
+    //buttonGoal.setSampleFrequency(1000);
+    //buttonStart.setSampleFrequency(1000);
+    //inputBall.setSampleFrequency(250);
+    
+    /*int charCount = sprintf(sendBuffer, "<ready>");
+    server.sendTo(client, sendBuffer, charCount);*/
+    
        
     //MPU6050 mpu = imu.getMPU();
        
@@ -234,9 +467,9 @@
     
     //imuUpdate.attach(&imuUpdateTick, 1.0 / 200.0);
     
-    testUpdate.attach(&testUpdateTick, 1.0);
+    testUpdate.attach(&testUpdateTick, 0.2);
     
-    pc.printf("IMU started\n");
+    //pc.printf("IMU started\n");
     
     //time_t seconds = time(NULL);
         
@@ -251,13 +484,63 @@
     pc.printf("Start IMU calibration\n");*/
     
     //imu.startGzCalibration();
+        
+    /*buttonGoal.mode(PullUp);        
+    buttonGoal.attach_deasserted(&goalFall);        
+    buttonGoal.setSamplesTillAssert(20);        
+    buttonGoal.setSampleFrequency(1000);*/
     
+    //sw2.rise(&sw2_release);    
+    //sw2.fall(&sw2_press);
+    
+    //OFF
     led1 = 1;
+    led3 = 1;
+    led4 = 1;
+    
+    //uLCD.printf("Ready\n");
+    
+    //humanInterface.setError(1);
+    
+    bool isMainUpdate = false;
+    
+    int serialReadUnblockCount = 0;
+    int serialReadUnblockLimit = 40;
+    
+    //watchdog.kick(3);
     
     while (1) {
         
-        if (updateIMU) {
-            updateIMU = 0;
+        serialReadUnblockCount = 0;
+        
+        //int k = pc.readable();
+        while (pc.readable() && serialReadUnblockCount < serialReadUnblockLimit) {
+            serialReadUnblockCount++;
+            //pc.printf("Received packet from: %s\n", client.get_address());
+            //pc.printf("k: %d\n", k);
+            //pc.scanf("%s", &serialBuffer);
+            serialBuffer[charCounter] = pc.getc();
+            //pc.printf("%c\n", serialBuffer[charCounter]);
+            //fflush(stdout);
+            //fflush(stdin);
+            //pc.printf("%s\n", serialBuffer);
+            if (serialBuffer[charCounter] == '\n') {
+                serialBuffer[charCounter] = '\0';
+                executeCommand(serialBuffer);
+                led1 = !led1;
+                charCounter = 0;
+                break;
+            } else {
+                charCounter++;
+            }
+            
+            //led3 = !led3;
+            
+            //server.sendTo(client, serialBuffer, n);               
+        }
+        
+        //if (updateIMU) {
+            //updateIMU = 0;
             //t.stop();
             //float dt = t.read();
             //t.reset();
@@ -275,39 +558,42 @@
                }
             }*/
             
-            led3 = !led3;
-        }
-        
-        if (updateTest) {
-            updateTest = 0;
-            
-            /*if (!calibrating) {
-                pc.printf("gz:%.3f\n", imu.getDegZ());
-            }*/
-            
-            //pc.printf("adc: %.1f\n", coilADC.read() * 80);
-            
-            led4 = !led4;
-        }
+            //led3 = !led3;
+        //}
     
-        if (update) {
+        if (update) {            
             update = 0;
             ioExt.writePins();
 
-            failSafeCount++;
-            if (failSafeCount == failSafeLimit) {
-                failSafeCount = 0;
+            failSafeCountMotors++;
+            failSafeCountCoilgun++;
+            
+            if (failSafeCountMotors == failSafeLimitMotors) {
+                failSafeCountMotors = 0;
                 if (failSafeEnabled) {
+                    
                     motor1.setSpeed(0);
                     motor2.setSpeed(0);
                     motor3.setSpeed(0);
                     motor4.setSpeed(0);
                     motor5.setSpeed(0);
+                    //dribbler.pulsewidth_us(0);     
+                    
+                    pc.rxBufferFlush();
+                    pc.txBufferFlush();                
+                }                
+            }
+            
+            if (failSafeCountCoilgun == failSafeLimitCoilgun) {
+                failSafeCountCoilgun = 0;
+                if (failSafeEnabled) {
                     coilgun.discharge();
-                }  
-                if (!coilgun.isCharged) {
-                    server.sendTo(client, "<discharged>", 12);
-                } 
+                    
+                    if (!coilgun.isCharged) {
+                        //server.sendTo(client, "<discharged>", 12);
+                        pc.printf("<discharged>\n");
+                    }
+                }                
             }
             
             //led3 = 1;
@@ -325,50 +611,147 @@
             //pc.printf("<speeds:%d:%d:%d:%d:%d>\n", motor1.getSpeed(), motor2.getSpeed(), motor3.getSpeed(), motor4.getSpeed(), motor5.getSpeed()); 
             //redLed.toggle();
             
-            ioExt.setDirection(0x0400);
+            //ioExt.setDirection(0x0400);
+            
+            //led1 = !led1;
+            
+            t.stop();
+            float dt = t.read();
+            t.reset();
+            t.start();
+            
+            if (dt > dtMax) {
+                dtMax = dt;
+            }
+            
+            if (dt >= 0.02f) {
+                printf("<dtmax=%.5f>\n", dt);
+            } 
+            
+            if (dtMax >= 0.02f) {
+                printf("<dtmax=%.5f>\n", dtMax);
+            }            
             
-            led1 = !led1;
+            isMainUpdate = true;
         }
         
+        if (updateTest) {
+            updateTest = 0;
+            
+            //watchdog.kick();
+            
+            //if (!calibrating) {
+            //   pc.printf("gz:%.3f\n", imu.getDegZ());
+            //}
+            
+            //uLCD.printf("AyijklL1230\n");
+            
+            //rgbLed1.toggle();
+            //rgbLed2.toggle();
+            
+            led4 = !led4;
+            
+            if (!isMainUpdate) {
+                coilCapVoltage = coilADC.read() * 80;
+            }
+            //coilCapVoltage = 300.0f;
+            
+            //float dtSlow = aliveTime.read();
+            //printf("<up=%.1f>\n", dtSlow);
+        }
+        
+        isMainUpdate = false;
+        
         if (stallChanged) {
             stallChanged = 0;
-            int charCount = sprintf(sendBuffer, "<stall:%d:%d:%d:%d:%d>",
+            /*int charCount = sprintf(sendBuffer, "<stall:%d:%d:%d:%d:%d>",
                 motor1.getStallLevel(),
                 motor2.getStallLevel(),
                 motor3.getStallLevel(),
                 motor4.getStallLevel(),
                 motor5.getStallLevel());
-            server.sendTo(client, sendBuffer, charCount);
-        }     
+            server.sendTo(client, sendBuffer, charCount);*/
+            pc.printf(
+                "<stall:%d:%d:%d:%d:%d>", 
+                motor1.getStallLevel(),
+                motor2.getStallLevel(),
+                motor3.getStallLevel(),
+                motor4.getStallLevel(),
+                motor5.getStallLevel()
+            );
+        }
+        
+        /*if (ballStateChanged) {
+            ballStateChanged = false;
+            if (ballState) {
+                //server.sendTo(client, "<ball:1>", 8);
+                pc.printf("<ball:1>");
+            } else {
+                //server.sendTo(client, "<ball:0>", 8);
+                pc.printf("<ball:0>");
+            }
+        }*/
         
+        /*if (goalButtonPressed) {
+            goalButtonPressed = false;
+            //server.sendTo(client, "<toggle-side>", 13);
+        }*/
+        
+        /*if (startButtonReleased) {
+            startButtonReleased = false;
+            //server.sendTo(client, "<toggle-go>", 11);
+        }*/
+        
+        int newBallState = humanInterface.getBallState();
+        if (ballState != newBallState) {
+            pc.printf("<ball:%d>\n", newBallState);
+            ballState = newBallState;
+            
+            if (kickWhenBall && ballState) {
+                kickWhenBall = false;
+                coilgun.kick(currentKickLength, currentKickDelay, currentChipLength, currentChipDelay);
+                sendKicked = true;
+            }
+            
+            if (!ballState && sendKicked) {
+                sendKicked = true;
+                pc.printf("<kicked>\n");
+            }
+        }
         
         /*switch (humanInterface.getBallState()){
             case -1:
                 //Ball lost
-                server.sendTo(client, "<ball:0>", 8);
+                //server.sendTo(client, "<ball:0>", 8);
+                pc.printf("<ball:0>\n");
+                ballState = 0;
                 break;
             case 0:
                 //Nothing has changed..
                 break;
             case 1:
                 //Ball found
-                server.sendTo(client, "<ball:1>", 8);
+                //server.sendTo(client, "<ball:1>", 8);
+                pc.printf("<ball:1>\n");
+                ballState = 1;
                 break;
             default:
                 break;
-        }
+        }*/
         
         if (humanInterface.isGoalChange()) {
-            server.sendTo(client, "<toggle-side>", 13);
+            //server.sendTo(client, "<toggle-side>", 13);
+            pc.printf("<toggle-side>\n");
         }
         
         if (humanInterface.isStart()) {
-            server.sendTo(client, "<toggle-go>", 11);
-        }*/
+            //server.sendTo(client, "<toggle-go>", 11);
+            pc.printf("<toggle-go>\n");
+        }
     
         //printf("\nWait for packet...\n");
 
-        int n = server.receiveFrom(client, buffer, sizeof(buffer));
+        /*int n = server.receiveFrom(client, buffer, sizeof(buffer));
 
         if (n > 0) {
             //pc.printf("Received packet from: %s\n", client.get_address());
@@ -378,35 +761,24 @@
             executeCommand(buffer);
             //coilgun.kick(1000);
             //executeCommand((short*)buffer);
-        }
+        }*/
         
-        //int n = pc.readable();
         /*if (pc.readable()) {
-            //pc.printf("Received packet from: %s\n", client.get_address());
-            //pc.printf("n: %d\n", n);
-            //pc.scanf("%s", &buffer);
-            buffer[charCounter] = pc.getc();
-            //pc.printf("%c\n", buffer[charCounter]);
-            //fflush(stdout);
-            //fflush(stdin);
-            //pc.printf("%s\n", buffer);
-            if (buffer[charCounter] == '\n') {
-                buffer[charCounter] = '\0';
-                executeCommand(buffer);
-                charCounter = 0;
-            } else {
-                charCounter++;
-            }
-            
-            //server.sendTo(client, buffer, n);               
+            led1 = !led1;
+            pc.putc(pc.getc());
         }*/
+        
+        
     }
 }
 
 void executeCommand(char *buffer) {
-    failSafeCount = 0;
+    failSafeCountMotors = 0;
+    failSafeCountCoilgun = 0;
 
-    pc.printf("%s\n", buffer);    
+    //pc.printf("cmd: %s\n", buffer);
+    //uLCD.printf("%s\n", buffer);
+    
     char *cmd;    
     cmd = strtok(buffer, ":");
     
@@ -418,38 +790,92 @@
         motor3.setSpeed(atoi(strtok(NULL, ":")));
         motor4.setSpeed(atoi(strtok(NULL, ":")));
         motor5.setSpeed(atoi(strtok(NULL, ":")));
-        int charCount = sprintf(sendBuffer, "<speeds:%d:%d:%d:%d:%d>",
+        
+        /*int dribblerSpeed = atoi(strtok(NULL, ":")) * 10;
+        
+        if (dribblerSpeed < 0) {
+            dribblerSpeed = -dribblerSpeed;
+            ioExt.setPin(0);
+        } else {
+            ioExt.clearPin(0);
+        }*/
+        
+        //dribbler.pulsewidth_us(dribblerSpeed);
+        
+        /*int charCount = sprintf(sendBuffer, "<speeds:%d:%d:%d:%d:%d>",
             motor1.getSpeed(), motor2.getSpeed(), motor3.getSpeed(), motor4.getSpeed(), motor5.getSpeed());
-        server.sendTo(client, sendBuffer, charCount);
-    } else if (strncmp(cmd, "gs", 2) == 0) {
-        int charCount = sprintf(sendBuffer, "<speeds:%d:%d:%d:%d:%d>",
-            motor1.getSpeed(), motor2.getSpeed(), motor3.getSpeed(), motor4.getSpeed(), motor5.getSpeed());
-        server.sendTo(client, sendBuffer, charCount);
-    } else if (strncmp(cmd, "ga", 2) == 0) {
+        server.sendTo(client, sendBuffer, charCount);*/
+        
+        pc.printf("<speeds:%d:%d:%d:%d:%d>\n", motor1.getSpeed(), motor2.getSpeed(), motor3.getSpeed(), motor4.getSpeed(), motor5.getSpeed());
+    } /*else if (strncmp(cmd, "ga", 2) == 0) {
         //int charCount = sprintf(sendBuffer, "<angle:%.2f>", imu.getDegZ());
         //server.sendTo(client, sendBuffer, charCount);
-    } else if (strncmp(cmd, "kick", 4) == 0) {        
+    }*/ else if (strncmp(cmd, "kick", 4) == 0) {        
+        unsigned int kickLength = atoi(strtok(NULL, ":"));
+        coilgun.kick(kickLength, 0, 0, 0);
+        pc.printf("<ball:%d>\n", ballState);
+    } else if (strncmp(cmd, "dkick", 5) == 0) {        
         unsigned int kickLength = atoi(strtok(NULL, ":"));
         unsigned int kickDelay = atoi(strtok(NULL, ":"));
         unsigned int chipLength = atoi(strtok(NULL, ":"));
         unsigned int chipDelay = atoi(strtok(NULL, ":"));
-        pc.printf("kick:%d:%d:%d:%d\n", kickLength, kickDelay, chipLength, chipDelay);
+        //pc.printf("kick:%d:%d:%d:%d\n", kickLength, kickDelay, chipLength, chipDelay);
         coilgun.kick(kickLength, kickDelay, chipLength, chipDelay);
+        pc.printf("<ball:%d>\n", ballState);
+    } else if (strncmp(cmd, "bdkick", 6) == 0) {        
+        currentKickLength = atoi(strtok(NULL, ":"));
+        currentKickDelay = atoi(strtok(NULL, ":"));
+        currentChipLength = atoi(strtok(NULL, ":"));
+        currentChipDelay = atoi(strtok(NULL, ":"));
+        //pc.printf("kick:%d:%d:%d:%d\n", kickLength, kickDelay, chipLength, chipDelay);
+        if (ballState) {
+            coilgun.kick(currentKickLength, currentKickDelay, currentChipLength, currentChipDelay);
+            kickWhenBall = false;
+        } else {
+            kickWhenBall = true;
+        }
+        pc.printf("<ball:%d>\n", ballState);
+    } else if (strncmp(cmd, "nokick", 6) == 0) {
+        kickWhenBall = false;
     } else if (strncmp(cmd, "charge", 6) == 0) {
-        pc.printf("charge\n");
+        //pc.printf("charge\n");
         coilgun.charge();
     } else if (strncmp(cmd, "discharge", 9) == 0) {
-        pc.printf("discharge\n");
+        //pc.printf("discharge\n");
         coilgun.discharge();
+    } else if (strncmp(cmd, "servos", 6) == 0) {
+        int servo1Duty = atoi(strtok(NULL, ":"));
+        int servo2Duty = atoi(strtok(NULL, ":"));
+
+        if (servo1Duty + servo2Duty > 3340) {
+            pc.printf("<err:servo1Duty + servo2Duty must be smaller than 3340>\n");
+        } else {
+            if (servo1Duty < 600 || servo1Duty > 2500) {
+                pc.printf("<err:servo1Duty must be between 600 and 2500>\n");
+            } else {
+                servo1.pulsewidth_us(servo1Duty);
+            }
+            
+            if (servo2Duty < 600 || servo2Duty > 2500) {
+                pc.printf("<err:servo2Duty must be between 600 and 2500>\n");
+            } else {
+                servo2.pulsewidth_us(servo2Duty);
+            }
+        }
+    } else if (strncmp(cmd, "gs", 2) == 0) {
+        /*int charCount = sprintf(sendBuffer, "<speeds:%d:%d:%d:%d:%d>",
+            motor1.getSpeed(), motor2.getSpeed(), motor3.getSpeed(), motor4.getSpeed(), motor5.getSpeed());
+        server.sendTo(client, sendBuffer, charCount);*/
+        pc.printf("<speeds:%d:%d:%d:%d:%d>\n", motor1.getSpeed(), motor2.getSpeed(), motor3.getSpeed(), motor4.getSpeed(), motor5.getSpeed());
     } else if (strncmp(cmd, "c", 1) == 0) {
-        pc.printf("charge\n");
+        //pc.printf("charge\n");
         if (atoi(strtok(NULL, ":")) == 1) {
             coilgun.charge();
         } else {
             coilgun.chargeEnd();
         }
     } else if (strncmp(cmd, "d", 1) == 0) {
-        pc.printf("discharge\n");
+        //pc.printf("discharge\n");
         coilgun.discharge();
     } else if (strncmp(cmd, "reset", 5) == 0) {
         motor1.setSpeed(0);
@@ -457,9 +883,12 @@
         motor3.setSpeed(0);
         motor4.setSpeed(0);
         motor5.setSpeed(0);
-        /*humanInterface.setGoal(HumanInterface::UNSET);
+        
+        //dribbler.pulsewidth_us(0);        
+        
+        humanInterface.setGoal(HumanInterface::UNSET);
         humanInterface.setError(false);
-        humanInterface.setGo(false);*/
+        humanInterface.setGo(false);
     } else if (strncmp(cmd, "fs", 2) == 0) {
         failSafeEnabled = (bool)atoi(strtok(NULL, ":"));
     } else if (strncmp(cmd, "target", 6) == 0) {
@@ -472,8 +901,29 @@
             humanInterface.setGoal(HumanInterface::UNSET);
         }*/
     } else if (strncmp(cmd, "error", 5) == 0) {
-        //humanInterface.setError(atoi(strtok(NULL, ":")));
-    }  else if (strncmp(cmd, "go", 2) == 0) {
-        //humanInterface.setGo(atoi(strtok(NULL, ":")));
-    }
+        humanInterface.setError(atoi(strtok(NULL, ":")));
+    } else if (strncmp(cmd, "go", 2) == 0) {
+        humanInterface.setGo(atoi(strtok(NULL, ":")));
+    } else if (strncmp(cmd, "adc", 5) == 0) {
+        //pc.printf("<adc:%.1f>\n", coilADC.read() * 80);
+        pc.printf("<adc:%.1f>\n", coilCapVoltage);
+        /*int charCount = sprintf(sendBuffer, "<adc:%.1f>\n", coilADC.read() * 80);
+        server.sendTo(client, sendBuffer, charCount);*/
+    } /*else if (strncmp(cmd, "red", 3) == 0) {
+        rgbLed1.setColor(RgbLed::RED);
+    } else if (strncmp(cmd, "green", 5) == 0) {
+        rgbLed1.setColor(RgbLed::GREEN);
+    } else if (strncmp(cmd, "blue", 4) == 0) {
+        rgbLed1.setColor(RgbLed::BLUE);
+    } else if (strncmp(cmd, "cyan", 4) == 0) {
+        rgbLed1.setColor(RgbLed::CYAN);
+    } else if (strncmp(cmd, "magenta", 7) == 0) {
+        rgbLed1.setColor(RgbLed::MAGENTA);
+    } else if (strncmp(cmd, "yellow", 6) == 0) {
+        rgbLed1.setColor(RgbLed::YELLOW);
+    } else if (strncmp(cmd, "white", 5) == 0) {
+        rgbLed1.setColor(RgbLed::WHITE);
+    } else if (strncmp(cmd, "off", 3) == 0) {
+        rgbLed1.setColor(RgbLed::OFF);
+    }*/
 }
\ No newline at end of file