File content as of revision 3:4ec313b1b314:

#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"
#include "HumanInterface.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;
volatile int ballChanged = 0;
volatile int goalButtonReleased = 0;
volatile int startButtonReleased = 0;
volatile int stallChanged = 0;

bool failSafeEnabled = true;
int failSafeCount = 0;
int failSafeLimit = 60;


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

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

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

HumanInterface humanInterface(&pc, &ioExt, 11, 12, 13, 14, 15, 10); 

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

void extChangedCallback() {
    extInChanged = 1;
}

void ballChangedCallback() {
    ballChanged = 1;
}

void goalRiseCallback() {
    goalButtonReleased = 1;
}

void startRiseCallback() {
    startButtonReleased = 1;
}

void stallChangedCallback() {
    stallChanged = 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);
    //humanInterface.change(&extChangedCallback);
    humanInterface.ballChange(&ballChangedCallback);
    humanInterface.goalRise(&goalRiseCallback);
    humanInterface.startRise(&startRiseCallback);
    
    motor1.stallChange(&stallChangedCallback);
    motor2.stallChange(&stallChangedCallback);
    motor3.stallChange(&stallChangedCallback);
    motor4.stallChange(&stallChangedCallback);
    motor5.stallChange(&stallChangedCallback);
    
    //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);
            
            failSafeCount++;
            if (failSafeCount == failSafeLimit) {
                failSafeCount = 0;
                if (failSafeEnabled) {
                    motor1.setSpeed(0);
                    motor2.setSpeed(0);
                    motor3.setSpeed(0);
                    motor4.setSpeed(0);
                    motor5.setSpeed(0);
                    coilgun.discharge();
                }  
                if (!coilgun.isCharged) {
                    //int charCount = sprintf(sendBuffer, "<speeds:%d:%d:%d:%d:%d>");
                    server.sendTo(client, "<discharged>", 12);
                } 
            }
            
            //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 (stallChanged) {
            stallChanged = 0;
            int charCount = sprintf(sendBuffer, "<stall:%d:%d:%d:%d:%d>\n",
                motor1.getStallLevel(),
                motor2.getStallLevel(),
                motor3.getStallLevel(),
                motor4.getStallLevel(),
                motor5.getStallLevel());
            server.sendTo(client, sendBuffer, charCount);
        }
        
        /*if (extInChanged) {
            extInChanged = 0;     
            pc.printf("extChanged\n");   
            //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);          
        }*/
        
        if (ballChanged) {
            ballChanged = 0;
            int charCount = sprintf(sendBuffer, "<ball:%d>", humanInterface.getBallState());
            server.sendTo(client, sendBuffer, charCount); 
        }
        
        if (goalButtonReleased) {
            goalButtonReleased = 0;
            server.sendTo(client, "<toggle-side>", 13);
        }
        
        if (startButtonReleased) {
            startButtonReleased = 0;
            server.sendTo(client, "<toggle-go>", 11);
        }
    
        //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");
        if (cmd[1] == 1) {
            coilgun.charge();
        } else {
            coilgun.chargeEnd();
        }
    }
}

void executeCommandOld(char *buffer) {
    failSafeCount = 0;

    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, ":")));
        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);
        led2 = !led2;
    } else if (strncmp(cmd, "kick", 4) == 0) {
        pc.printf("kick\n");
        coilgun.kick(atoi(strtok(NULL, ":")));
    } 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, "k", 1) == 0) {
        pc.printf("kick\n");
        int length = atoi(strtok(NULL, ":"));
        coilgun.kick(length);
    } 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, "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, ":")));
    }
}