File content as of revision 11:92eecb155136:

#include "mbed.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 "rgb-led-pca9555.h"
//#include "externalin.h"
#include "coilgun.h"
#include "HumanInterface.h"
//#include "MPU6050.h"
//#include "imu.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(LED_RED);
DigitalOut led3(LED_GREEN);
DigitalOut led4(LED_BLUE);

//Serial pc(USBTX, USBRX); // tx, rx
MODSERIAL pc(USBTX, USBRX, 512, 512); 
//ADXL345_I2C accelerometer(p9, p10);
//L3G4200D gyro(p9, p10);
//HMC5883L compass(p9, p10);


MCP3021 coilADC(PTC11, PTC10, 5.0);
PCA9555 ioExt(PTC11, PTC10, PTB20, 0x40);

//int readings[3] = {0, 0, 0};
//int gyroData[3] = {0, 0, 0};
//int16_t compassData[3] = {0, 0, 0};

volatile int update = 0;
volatile int updateIMU = 0;

volatile int updateTest = 0;

volatile int stallChanged = 0;

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) {
        led1 = 0;
    } else {
        led1 = 1.0;
    }
    accelerometer.getOutput(readings);
    pc.printf("<acc:%i:%i:%i>\n", (int16_t)readings[0], (int16_t)readings[1], (int16_t)readings[2]);
    
    gyro.read(gyroData);
    pc.printf("<gyro:%i:%i:%i>\n", (int16_t)gyroData[0], (int16_t)gyroData[1], (int16_t)gyroData[2]);
    
    compass.getXYZ(compassData);
    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(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[512];

// 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++;
        }         
    }
}*/

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++;
        }         
    }*/
}

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);

void updateTick() { 
    //led3 = 1;   
    motor1.pid();
    motor2.pid();
    motor3.pid();
    motor4.pid();
    motor5.pid();
    //led3 = 0;
    update = 1;
}

void imuUpdateTick() {
    updateIMU = 1;
}

void testUpdateTick() {
    updateTest = 1;
}

void stallChangedCallback() {
    stallChanged = 1;
}

//UDPSocket server;
//Endpoint client;

//void readSerial() {
    
    //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);
        //pc.printf("%c\n", pc.getc());
        //buffer[charCounter] = pc.getc();
        //printf("%c\n", buffer[charCounter]);
        //pc.printf("%s\n", buffer);
        //if (buffer[charCounter] == '\n') {
        //    buffer[charCounter] = '\0';
        //    executeCommand(buffer);
        //    charCounter = 0;
        //} else {
        //    charCounter++;
        }*/         
    //}
    
//}

Timer t;
Timer aliveTime;

float dtMax = 0.0f;

unsigned int currentKickLength = 0;
unsigned int currentKickDelay = 0;
unsigned int currentChipLength = 0;
unsigned int currentChipDelay = 0;
bool kickWhenBall = false;
bool sendKicked = false;

//EthernetInterface eth;

int main (void) {
    
    //enable_rx_fifo(0);
    
    pc.baud(115200);
    
    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();
    
    //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);
    
    //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);
    //pc.printf("Device ID is: 0x%02x\n", accelerometer.getDeviceID());
    //wait(.001);
    
    // These are here to test whether any of the initialization fails. It will print the failure
    /*if (accelerometer.setPowerControl(0x00)){
        pc.printf("didn't intitialize power control\n"); 
        return 0;
    }*/
    //Full resolution, +/-16g, 4mg/LSB.
    /*wait(.001);
    
    if(accelerometer.setDataFormatControl(0x0B)){
        pc.printf("didn't set data format\n");
        return 0;
    }
     
    wait(.001);*/
     
    //3.2kHz data rate.
    /*if(accelerometer.setDataRate(ADXL345_3200HZ)){
        pc.printf("didn't set data rate\n");
        return 0;
    }
        
    wait(.001);*/
     
    //Measurement mode.
     
    /*if (accelerometer.setPowerControl(MeasurementMode)) {
        pc.printf("didn't set the power control to measurement\n"); 
        return 0;
    }
    */
    
    
    
    //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(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);
    
    //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();
       
    /*bool mpu6050TestResult = mpu.testConnection();
    if(mpu6050TestResult) {
        pc.printf("IMU test passed\n");
    } else {
        pc.printf("IMU test failed\n");
    }*/
    
    //wait(2);
    
    //pc.printf("IMU calibration\n");
    
    //imu.zeroGyroZ();
    //imu.startPolling();
    
    sensorUpdate.attach(&updateTick, 1.0 / 60.0);
    
    //imuUpdate.attach(&imuUpdateTick, 1.0 / 200.0);
    
    testUpdate.attach(&testUpdateTick, 0.2);
    
    //pc.printf("IMU started\n");
    
    //time_t seconds = time(NULL);
        
    //printf("time = %d\n", time(NULL));
    
    //set_time(1256729737);
    
    /*int sampleCount = 0;
    int calibrationCount = 1000;
    bool calibrating = true;    
    
    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) {
        
        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();
            //t.start();
            //printf("dt = %.6f\n", dt);
            //imu.update();
            
            /*if (calibrating) {
               sampleCount++;
               if (sampleCount >= calibrationCount) {
                   calibrating = false;
                   imu.stopGzCalibration();
                   pc.printf("Stop IMU calibration\n");
                   pc.printf("GyroZ zero %.3f\n", imu.getGzZero());
               }
            }*/
            
            //led3 = !led3;
        //}
    
        if (update) {            
            update = 0;
            ioExt.writePins();

            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);
                        pc.printf("<discharged>\n");
                    }
                }                
            }
            
            //led3 = 1;
            //updateSensors();
            //pc.printf("update");                       
            /*motor1.pid();
            motor2.pid();
            motor3.pid();
            motor4.pid();
            motor5.pid();*/
            
            //led3 = 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()); 
            //redLed.toggle();
            
            //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);
            }*/
            
            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>",
                motor1.getStallLevel(),
                motor2.getStallLevel(),
                motor3.getStallLevel(),
                motor4.getStallLevel(),
                motor5.getStallLevel());
            server.sendTo(client, sendBuffer, charCount);*/
            pc.printf(
                "<stall:%d:%d:%d:%d:%d>\n", 
                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);
                pc.printf("<ball:0>\n");
                ballState = 0;
                break;
            case 0:
                //Nothing has changed..
                break;
            case 1:
                //Ball found
                //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);
            pc.printf("<toggle-side>\n");
        }
        
        if (humanInterface.isStart()) {
            //server.sendTo(client, "<toggle-go>", 11);
            pc.printf("<toggle-go>\n");
        }
    
        //printf("\nWait for packet...\n");

        /*int n = server.receiveFrom(client, buffer, sizeof(buffer));

        if (n > 0) {
            //pc.printf("Received packet from: %s\n", client.get_address());
            //pc.printf("n: %d\n", n);
            buffer[n] = '\0';
            //server.sendTo(client, buffer, n); 
            executeCommand(buffer);
            //coilgun.kick(1000);
            //executeCommand((short*)buffer);
        }*/
        
        /*if (pc.readable()) {
            led1 = !led1;
            pc.putc(pc.getc());
        }*/
        
        
    }
}

void executeCommand(char *buffer) {
    failSafeCountMotors = 0;
    failSafeCountCoilgun = 0;

    //pc.printf("cmd: %s\n", buffer);
    //uLCD.printf("%s\n", buffer);
    
    char *cmd;    
    cmd = strtok(buffer, ":");
    
    //pc.printf("%s\n", cmd);  
    
    if (strncmp(cmd, "speeds", 6) == 0) {
        motor1.setSpeed(atoi(strtok(NULL, ":")));
        motor2.setSpeed(atoi(strtok(NULL, ":")));
        motor3.setSpeed(atoi(strtok(NULL, ":")));
        motor4.setSpeed(atoi(strtok(NULL, ":")));
        motor5.setSpeed(atoi(strtok(NULL, ":")));
        
        /*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);*/
        
        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) {        
        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);
        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");
        coilgun.charge();
    } else if (strncmp(cmd, "discharge", 9) == 0) {
        //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");
        if (atoi(strtok(NULL, ":")) == 1) {
            coilgun.charge();
        } else {
            coilgun.chargeEnd();
        }
    } else if (strncmp(cmd, "d", 1) == 0) {
        //pc.printf("discharge\n");
        coilgun.discharge();
    } else if (strncmp(cmd, "reset", 5) == 0) {
        motor1.setSpeed(0);
        motor2.setSpeed(0);
        motor3.setSpeed(0);
        motor4.setSpeed(0);
        motor5.setSpeed(0);
        
        //dribbler.pulsewidth_us(0);        
        
        humanInterface.setGoal(HumanInterface::UNSET);
        humanInterface.setError(false);
        humanInterface.setGo(false);
    } else if (strncmp(cmd, "fs", 2) == 0) {
        failSafeEnabled = (bool)atoi(strtok(NULL, ":"));
    } else if (strncmp(cmd, "target", 6) == 0) {
        int target = atoi(strtok(NULL, ":"));
        /*if (target == 0) {
            humanInterface.setGoal(HumanInterface::BLUE);
        } else if (target == 1) {
            humanInterface.setGoal(HumanInterface::YELLOW);
        } else if (target == 2) {
            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, ":")));
    } 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);
    }*/
}