main.cpp

00001 #include "mbed.h"
00002 #include "EthernetInterface.h"
00003 //#include "ADXL345_I2C.h"
00004 //#include "L3G4200D.h"
00005 //#include "HMC5883L.h"
00006 #include "PCA9555.h"
00007 #include "motor.h"
00008 #include "qed.h"
00009 #include "ledout.h"
00010 #include "externalin.h"
00011 #include "coilgun.h"
00012 #include "HumanInterface.h"
00013 
00014 #define SERVER_PORT   8042
00015 
00016 Ticker sensorUpdate;
00017 
00018 DigitalOut led1(LED1);
00019 DigitalOut led3(LED3);
00020 DigitalOut led4(LED4);
00021 
00022 Serial pc(USBTX, USBRX); // tx, rx
00023 //ADXL345_I2C accelerometer(p9, p10);
00024 //L3G4200D gyro(p9, p10);
00025 //HMC5883L compass(p9, p10);
00026 
00027 PCA9555 ioExt(p9, p10, p8, 0x40);
00028 
00029 //int readings[3] = {0, 0, 0};
00030 //int gyroData[3] = {0, 0, 0};
00031 //int16_t compassData[3] = {0, 0, 0};
00032 
00033 
00034 volatile int update = 0;
00035 
00036 volatile int stallChanged = 0;
00037 
00038 bool failSafeEnabled = true;
00039 int failSafeCount = 0;
00040 int failSafeLimit = 60;
00041 
00042 
00043 void executeCommand(char *buffer);
00044 
00045 /*
00046 void updateSensors() {
00047     if (led1 == 1.0) {
00048         led1 = 0;
00049     } else {
00050         led1 = 1.0;
00051     }
00052     accelerometer.getOutput(readings);
00053     pc.printf("<acc:%i:%i:%i>\n", (int16_t)readings[0], (int16_t)readings[1], (int16_t)readings[2]);
00054     
00055     gyro.read(gyroData);
00056     pc.printf("<gyro:%i:%i:%i>\n", (int16_t)gyroData[0], (int16_t)gyroData[1], (int16_t)gyroData[2]);
00057     
00058     compass.getXYZ(compassData);
00059     pc.printf("<comps:%i:%i:%i>\n", compassData[0], compassData[1], compassData[2]);
00060 }*/
00061 
00062 //motor order: FL, FR, RL, RR, DRIBBLER
00063 Motor motor4(p25, &ioExt, 0, 1, p5, p6); //RR - M1
00064 Motor motor2(p24, &ioExt, 2, 3, p7, p11); //FR - M2
00065 Motor motor5(p23, &ioExt, 4, 5, p12, p13); //DRIBBLER - M3
00066 Motor motor1(p22, &ioExt, 6, 7, p14, p15); //FL - M4
00067 Motor motor3(p21, &ioExt, 9, 8, p16, p17); //RL - M5
00068 
00069 Coilgun coilgun(p20, p19, p18);
00070 
00071 HumanInterface humanInterface(&ioExt, 15, 14, 13, 12, 11, p29, p30, p26); 
00072 
00073 void updateTick() { 
00074     //led3 = 1;   
00075     motor1.pid();
00076     motor2.pid();
00077     motor3.pid();
00078     motor4.pid();
00079     motor5.pid();
00080     //led3 = 0;
00081     update = 1;
00082 }
00083 
00084 void stallChangedCallback() {
00085     stallChanged = 1;
00086 }
00087 
00088 UDPSocket server;
00089 Endpoint client;
00090 char buffer[256];
00091 char sendBuffer[256];
00092 int charCounter = 0;
00093 
00094 //void readSerial() {
00095     
00096     //int n = pc.readable();
00097     //if (pc.readable()) {
00098         
00099         //pc.printf("Received packet from: %s\n", client.get_address());
00100         //pc.printf("n: %d\n", n);
00101         //pc.scanf("%s", &buffer);
00102         //pc.printf("%c\n", pc.getc());
00103         //buffer[charCounter] = pc.getc();
00104         /*printf("%c\n", buffer[charCounter]);
00105         //pc.printf("%s\n", buffer);
00106         if (buffer[charCounter] == '\n') {
00107             buffer[charCounter] = '\0';
00108             executeCommand(buffer);
00109             charCounter = 0;
00110         } else {
00111             charCounter++;
00112         }*/         
00113     //}
00114     
00115 //}
00116 
00117 const char *byte_to_binary(int x) {
00118     static char b[9];
00119     b[0] = '\0';
00120 
00121     int z;
00122     for (z = 32768; z > 0; z >>= 1) {
00123         strcat(b, ((x & z) == z) ? "1" : "0");
00124     }
00125 
00126     return b;
00127 }
00128 
00129 int main (void) {
00130     pc.baud(115200);
00131     EthernetInterface eth;
00132     eth.init("192.168.4.1", "255.255.255.0", "192.168.4.8");
00133     eth.connect();
00134     printf("IP Address is %s\n", eth.getIPAddress());
00135 
00136     server.bind(SERVER_PORT);
00137 
00138     //pc.printf("Starting ADXL345 test...\n");
00139     //wait(.001);
00140     //pc.printf("Device ID is: 0x%02x\n", accelerometer.getDeviceID());
00141     //wait(.001);
00142     
00143     // These are here to test whether any of the initialization fails. It will print the failure
00144     /*if (accelerometer.setPowerControl(0x00)){
00145         pc.printf("didn't intitialize power control\n"); 
00146         return 0;
00147     }*/
00148     //Full resolution, +/-16g, 4mg/LSB.
00149     /*wait(.001);
00150     
00151     if(accelerometer.setDataFormatControl(0x0B)){
00152         pc.printf("didn't set data format\n");
00153         return 0;
00154     }
00155      
00156     wait(.001);*/
00157      
00158     //3.2kHz data rate.
00159     /*if(accelerometer.setDataRate(ADXL345_3200HZ)){
00160         pc.printf("didn't set data rate\n");
00161         return 0;
00162     }
00163         
00164     wait(.001);*/
00165      
00166     //Measurement mode.
00167      
00168     /*if (accelerometer.setPowerControl(MeasurementMode)) {
00169         pc.printf("didn't set the power control to measurement\n"); 
00170         return 0;
00171     }
00172     */
00173     
00174     server.set_blocking(false, 1);
00175     
00176     //int ioExtBefore = ioExt.read();
00177     //pc.printf("ioExt: %s\n", byte_to_binary(ioExt.read()));
00178     //pc.printf("ioExt: %x\n", ioExt.read());
00179     ioExt.setDirection(0x0400);
00180     ioExt.write(0x0155);
00181     //int ioExtAfter = ioExt.read();
00182     //pc.printf("ioExt: %x\n", ioExt.read());
00183     //pc.printf("ioExt: %s\n", byte_to_binary(ioExt.read()));
00184     
00185     motor1.stallChange(&stallChangedCallback);
00186     motor2.stallChange(&stallChangedCallback);
00187     motor3.stallChange(&stallChangedCallback);
00188     motor4.stallChange(&stallChangedCallback);
00189     motor5.stallChange(&stallChangedCallback);
00190     
00191     
00192     //int charCount = sprintf(sendBuffer, "<ready>");
00193     //server.sendTo(client, sendBuffer, charCount);
00194     
00195     sensorUpdate.attach(&updateTick, 1.0 / 60.0);
00196     
00197     led1 = 1;
00198     
00199     while (1) {
00200     
00201         if (update) {
00202             update = 0;
00203             ioExt.writePins();
00204             
00205             failSafeCount++;
00206             if (failSafeCount == failSafeLimit) {
00207                 failSafeCount = 0;
00208                 if (failSafeEnabled) {
00209                     motor1.setSpeed(0);
00210                     motor2.setSpeed(0);
00211                     motor3.setSpeed(0);
00212                     motor4.setSpeed(0);
00213                     motor5.setSpeed(0);
00214                     coilgun.discharge();
00215                 }  
00216                 if (!coilgun.isCharged) {
00217                     server.sendTo(client, "<discharged>", 12);
00218                 } 
00219             }
00220             
00221             //led3 = 1;
00222             //updateSensors();
00223             //pc.printf("update");                       
00224             /*motor1.pid();
00225             motor2.pid();
00226             motor3.pid();
00227             motor4.pid();
00228             motor5.pid();*/
00229             
00230             //led3 = 0;
00231             //int charCount = sprintf(sendBuffer, "<speeds:%d:%d:%d:%d:%d>", motor1.getSpeed(), motor2.getSpeed(), motor3.getSpeed(), motor4.getSpeed(), motor5.getSpeed());
00232             //server.sendTo(client, sendBuffer, charCount); 
00233             //pc.printf("<speeds:%d:%d:%d:%d:%d>\n", motor1.getSpeed(), motor2.getSpeed(), motor3.getSpeed(), motor4.getSpeed(), motor5.getSpeed()); 
00234             //redLed.toggle();
00235             
00236             ioExt.setDirection(0x0400);
00237             
00238             led1 = !led1;
00239         }
00240         
00241         if (stallChanged) {
00242             stallChanged = 0;
00243             int charCount = sprintf(sendBuffer, "<stall:%d:%d:%d:%d:%d>",
00244                 motor1.getStallLevel(),
00245                 motor2.getStallLevel(),
00246                 motor3.getStallLevel(),
00247                 motor4.getStallLevel(),
00248                 motor5.getStallLevel());
00249             server.sendTo(client, sendBuffer, charCount);
00250         }     
00251         
00252         
00253         switch (humanInterface.getBallState()){
00254             case -1:
00255                 //Ball lost
00256                 server.sendTo(client, "<ball:0>", 8);
00257                 break;
00258             case 0:
00259                 //Nothing has changed..
00260                 break;
00261             case 1:
00262                 //Ball found
00263                 server.sendTo(client, "<ball:1>", 8);
00264                 break;
00265             default:
00266                 break;
00267         }
00268         
00269         if (humanInterface.isGoalChange()) {
00270             server.sendTo(client, "<toggle-side>", 13);
00271         }
00272         
00273         if (humanInterface.isStart()) {
00274             server.sendTo(client, "<toggle-go>", 11);
00275         }
00276     
00277         //printf("\nWait for packet...\n");
00278 
00279         int n = server.receiveFrom(client, buffer, sizeof(buffer));
00280 
00281         if (n > 0) {
00282             //pc.printf("Received packet from: %s\n", client.get_address());
00283             //pc.printf("n: %d\n", n);
00284             buffer[n] = '\0';
00285             //server.sendTo(client, buffer, n); 
00286             executeCommand(buffer);
00287             //coilgun.kick(1000);
00288             //executeCommand((short*)buffer);
00289         }
00290         
00291         //int n = pc.readable();
00292         /*if (pc.readable()) {
00293             //pc.printf("Received packet from: %s\n", client.get_address());
00294             //pc.printf("n: %d\n", n);
00295             //pc.scanf("%s", &buffer);
00296             buffer[charCounter] = pc.getc();
00297             //pc.printf("%c\n", buffer[charCounter]);
00298             //fflush(stdout);
00299             //fflush(stdin);
00300             //pc.printf("%s\n", buffer);
00301             if (buffer[charCounter] == '\n') {
00302                 buffer[charCounter] = '\0';
00303                 executeCommand(buffer);
00304                 charCounter = 0;
00305             } else {
00306                 charCounter++;
00307             }
00308             
00309             //server.sendTo(client, buffer, n);               
00310         }*/
00311     }
00312 }
00313 
00314 void executeCommand(char *buffer) {
00315     failSafeCount = 0;
00316 
00317     pc.printf("%s\n", buffer);    
00318     char *cmd;    
00319     cmd = strtok(buffer, ":");
00320     
00321     //pc.printf("%s\n", cmd);  
00322     
00323     if (strncmp(cmd, "speeds", 6) == 0) {
00324         motor1.setSpeed(atoi(strtok(NULL, ":")));
00325         motor2.setSpeed(atoi(strtok(NULL, ":")));
00326         motor3.setSpeed(atoi(strtok(NULL, ":")));
00327         motor4.setSpeed(atoi(strtok(NULL, ":")));
00328         motor5.setSpeed(atoi(strtok(NULL, ":")));
00329         int charCount = sprintf(sendBuffer, "<speeds:%d:%d:%d:%d:%d>",
00330             motor1.getSpeed(), motor2.getSpeed(), motor3.getSpeed(), motor4.getSpeed(), motor5.getSpeed());
00331         server.sendTo(client, sendBuffer, charCount);
00332     } else if (strncmp(cmd, "gs", 2) == 0) {
00333         int charCount = sprintf(sendBuffer, "<speeds:%d:%d:%d:%d:%d>",
00334             motor1.getSpeed(), motor2.getSpeed(), motor3.getSpeed(), motor4.getSpeed(), motor5.getSpeed());
00335         server.sendTo(client, sendBuffer, charCount);
00336     } else if (strncmp(cmd, "kick", 4) == 0) {
00337         pc.printf("kick\n");
00338         coilgun.kick(atoi(strtok(NULL, ":")));
00339     } else if (strncmp(cmd, "charge", 6) == 0) {
00340         pc.printf("charge\n");
00341         coilgun.charge();
00342     } else if (strncmp(cmd, "discharge", 9) == 0) {
00343         pc.printf("discharge\n");
00344         coilgun.discharge();
00345     } else if (strncmp(cmd, "k", 1) == 0) {
00346         pc.printf("kick\n");
00347         int length = atoi(strtok(NULL, ":"));
00348         coilgun.kick(length);
00349     } else if (strncmp(cmd, "c", 1) == 0) {
00350         pc.printf("charge\n");
00351         if (atoi(strtok(NULL, ":")) == 1) {
00352             coilgun.charge();
00353         } else {
00354             coilgun.chargeEnd();
00355         }
00356     } else if (strncmp(cmd, "d", 1) == 0) {
00357         pc.printf("discharge\n");
00358         coilgun.discharge();
00359     } else if (strncmp(cmd, "reset", 5) == 0) {
00360         motor1.setSpeed(0);
00361         motor2.setSpeed(0);
00362         motor3.setSpeed(0);
00363         motor4.setSpeed(0);
00364         motor5.setSpeed(0);
00365         humanInterface.setGoal(HumanInterface::UNSET);
00366         humanInterface.setError(false);
00367         humanInterface.setGo(false);
00368     } else if (strncmp(cmd, "fs", 2) == 0) {
00369         failSafeEnabled = (bool)atoi(strtok(NULL, ":"));
00370     } else if (strncmp(cmd, "target", 6) == 0) {
00371         int target = atoi(strtok(NULL, ":"));
00372         if (target == 0) {
00373             humanInterface.setGoal(HumanInterface::BLUE);
00374         } else if (target == 1) {
00375             humanInterface.setGoal(HumanInterface::YELLOW);
00376         } else if (target == 2) {
00377             humanInterface.setGoal(HumanInterface::UNSET);
00378         }
00379     } else if (strncmp(cmd, "error", 5) == 0) {
00380         humanInterface.setError(atoi(strtok(NULL, ":")));
00381     }  else if (strncmp(cmd, "go", 2) == 0) {
00382         humanInterface.setGo(atoi(strtok(NULL, ":")));
00383     }
00384 }