File content as of revision 8:06124632c3e4:

#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 "externalin.h"
#include "coilgun.h"
#include "HumanInterface.h"
//#include "MPU6050.h"
//#include "imu.h"
//#include "MCP3021.h"

#define SERVER_PORT   8042

Ticker sensorUpdate;
Ticker imuUpdate;
Ticker testUpdate;

DigitalOut led1(LED1);
DigitalOut led3(LED3);
DigitalOut led4(LED4);

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

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

volatile int updateTest = 0;

volatile int stallChanged = 0;

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


void executeCommand(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

Coilgun coilgun(p20, p30, p19, p18);

//HumanInterface humanInterface(&ioExt, 15, 14, 13, 12, 11, p29, p30, p26);

//MCP3021 coilADC(p9, p10, 5.0);

//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;
char buffer[256];
char sendBuffer[256];
int charCounter = 0;

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

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

Timer t;

int main (void) {
    pc.baud(115200);
    EthernetInterface eth;
    eth.init("192.168.4.1", "255.255.255.0", "192.168.4.8");
    //eth.init();
    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(0x0400);
    ioExt.write(0x0155);
    //int ioExtAfter = ioExt.read();
    //pc.printf("ioExt: %x\n", ioExt.read());
    //pc.printf("ioExt: %s\n", byte_to_binary(ioExt.read()));
    
    motor1.stallChange(&stallChangedCallback);
    motor2.stallChange(&stallChangedCallback);
    motor3.stallChange(&stallChangedCallback);
    motor4.stallChange(&stallChangedCallback);
    motor5.stallChange(&stallChangedCallback);
    
    
    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, 1.0);
    
    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();
    
    led1 = 1;
    
    while (1) {
        
        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 (updateTest) {
            updateTest = 0;
            
            /*if (!calibrating) {
                pc.printf("gz:%.3f\n", imu.getDegZ());
            }*/
            
            //pc.printf("adc: %.1f\n", coilADC.read() * 80);
            
            led4 = !led4;
        }
    
        if (update) {
            update = 0;
            ioExt.writePins();

            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) {
                    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();
            
            ioExt.setDirection(0x0400);
            
            led1 = !led1;
        }
        
        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);
        }     
        
        
        /*switch (humanInterface.getBallState()){
            case -1:
                //Ball lost
                server.sendTo(client, "<ball:0>", 8);
                break;
            case 0:
                //Nothing has changed..
                break;
            case 1:
                //Ball found
                server.sendTo(client, "<ball:1>", 8);
                break;
            default:
                break;
        }
        
        if (humanInterface.isGoalChange()) {
            server.sendTo(client, "<toggle-side>", 13);
        }
        
        if (humanInterface.isStart()) {
            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); 
            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);               
        }*/
    }
}

void executeCommand(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);
    } 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, ":"));
        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);
    } 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, "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);
        /*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, ":")));
    }
}