Diff: main.cpp

Revision:

0:22a7683646d1

Child:

1:79ac4e293661

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Sep 18 07:50:53 2013 +0000
@@ -0,0 +1,350 @@
+#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);
+    }*/
+}
\ No newline at end of file