File content as of revision 0:22a7683646d1:

#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 "ballsens.h"
#include "ledout.h"
#include "externalin.h"
#include "coilgun.h"

#define SERVER_PORT   8042

Ticker sensorUpdate;

DigitalOut led1(LED1);

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

PCA9555 ioExt(p9, p10, p8, 0x40);

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

//DigitalOut led3(LED3);

volatile int update = 0;
volatile int extInChanged = 0;

void executeCommand(short *cmd);
void executeCommandOld(char *buffer);

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

Motor motor1(p25, &ioExt, 0, 1, p5, p6);
Motor motor2(p24, &ioExt, 2, 3, p7, p11);
Motor motor3(p23, &ioExt, 4, 5, p12, p13);
Motor motor4(p22, &ioExt, 6, 7, p14, p15);
Motor motor5(p21, &ioExt, 9, 8, p16, p17);

LedOut redLed(&ioExt, 13);
LedOut blueLed(&ioExt, 12);
LedOut yellowLed(&ioExt, 11);

CoilGun coilgun(p19, p20, p18);
//CoilGun coilgun(LED3, LED4, p18);
//Timeout kickFinish;

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

void extChangedCallback() {
    extInChanged = 1;
}

BallSens ballSens(&ioExt, 10);
ExternalIn button1(&ioExt, 14);
ExternalIn button2(&ioExt, 15);

UDPSocket server;
Endpoint client;
char buffer[256];
char sendBuffer[256];
int charCounter = 0;

DigitalOut led2(LED2);

//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();
        //led2 = !led2;
        /*printf("%c\n", buffer[charCounter]);
        //pc.printf("%s\n", buffer);
        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';

    int z;
    for (z = 32768; z > 0; z >>= 1) {
        strcat(b, ((x & z) == z) ? "1" : "0");
    }

    return b;
}

int main (void) {
    pc.baud(115200);
    EthernetInterface eth;
    //eth.init(); //Use DHCP
    eth.init("192.168.4.1", "255.255.255.0", "192.168.4.8");
    eth.connect();
    printf("IP Address is %s\n", eth.getIPAddress());

    server.bind(SERVER_PORT);

    //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(0xC400);
    ioExt.write(0x0155);
    //int ioExtAfter = ioExt.read();
    pc.printf("ioExt: %x\n", ioExt.read());
    //pc.printf("ioExt: %s\n", byte_to_binary(ioExt.read()));
    
    ballSens.change(&extChangedCallback);
    button1.change(&extChangedCallback);
    button2.change(&extChangedCallback);
    
    //InterruptIn change(p8);
    //change.rise(&ballCallBack);
    //change.fall(&ballCallBack);
    
    
    redLed.set();
    //blueLed.set();
    //yellowLed.set();
    
    //int charCount = sprintf(sendBuffer, "<ready>");
    //server.sendTo(client, sendBuffer, charCount);
    
    sensorUpdate.attach(&updateTick, 1.0 / 60.0);
    
    led1 = 1;
    
    while (1) {
        //coilgun.kick(5000);
        //kickFinish.detach();
        //kickFinish.attach_us(&coilgun, &CoilGun::kickEnd, 1000);
    
        if (update) {
            update = 0;
            ioExt.writePins();
            //coilgun.kick(100);
            
            //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();
            led1 = !led1;
        }
        
        if (extInChanged) {
            extInChanged = 0;            
            bool ballState = ballSens.read();
            bool button1State = button1.read();
            bool button2State = button2.read();
            if (button1State) {
                led1 = 1;
            } else {
                led1 = 0;
            }
            pc.printf("<ball:%d>\n", ballState ? 1 : 0);
            pc.printf("<btn1:%d>\n", button1State ? 1 : 0);
            pc.printf("<btn2:%d>\n", button2State ? 1 : 0);            
        }
    
        //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); 
            executeCommandOld(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);               
        }*/
    }
}

void executeCommand(short *cmd) {   
    if (cmd[0] == 1) {
        motor1.setSpeed(cmd[1]);
        motor2.setSpeed(cmd[2]);
        motor3.setSpeed(cmd[3]);
        motor4.setSpeed(cmd[4]);
        motor5.setSpeed(cmd[5]);
    } else if (cmd[0] == 2) {
        int charCount = sprintf(sendBuffer, "<speeds:%d:%d:%d:%d:%d>", motor1.getSpeed(), motor2.getSpeed(), motor3.getSpeed(), motor4.getSpeed(), motor5.getSpeed());
        //pc.printf("%d\n", sizeof(sendBuffer));
        //pc.printf("sendTo %s\n", client.get_address());
        server.sendTo(client, sendBuffer, charCount);
        led2 = !led2;
    } else if (cmd[0] == 3) {
        pc.printf("kick\n");
        coilgun.kick(cmd[1]);
    } else if (cmd[0] == 4) {
        pc.printf("charge\n");
        coilgun.setCharge(cmd[1]);
    }
}

void executeCommandOld(char *buffer) {
    pc.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, " ")));
    } 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());
        //pc.printf("%d\n", sizeof(sendBuffer));
        //pc.printf("sendTo %s\n", client.get_address());
        server.sendTo(client, sendBuffer, charCount);
        led2 = !led2;
    } else if (strncmp(cmd, "k", 1) == 0) {
        pc.printf("kick\n");
        int length = atoi(strtok(NULL, " "));
        coilgun.kick(length);
        //kickFinish.detach();
        //kickFinish.attach_us(&coilgun, &CoilGun::kickEnd, length);
    } else if (strncmp(cmd, "c", 1) == 0) {
        pc.printf("charge\n");
        coilgun.setCharge(atoi(strtok(NULL, " ")));
    } else if (strncmp(cmd, "d", 1) == 0) {
        pc.printf("discharge\n");
        coilgun.discharge();
    } /*else if (strncmp(cmd, "t", 1) == 0) {
        float newPWM = atof(strtok(NULL, " "));
        motor1.setPWM(newPWM);
        motor2.setPWM(newPWM);
        motor3.setPWM(newPWM);
        motor4.setPWM(newPWM);
        motor5.setPWM(newPWM);
    }*/
}