Diff: main.cpp

Revision:

8:06124632c3e4

Parent:

7:a9e98f642a89

Child:

9:2d2030d989d8

--- a/main.cpp	Mon Nov 04 23:57:09 2013 +0000
+++ b/main.cpp	Tue Sep 02 15:40:53 2014 +0000
@@ -10,10 +10,15 @@
 #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);
@@ -30,12 +35,14 @@
 //int gyroData[3] = {0, 0, 0};
 //int16_t compassData[3] = {0, 0, 0};
 
+volatile int update = 0;
+volatile int updateIMU = 0;
 
-volatile int update = 0;
+volatile int updateTest = 0;
 
 volatile int stallChanged = 0;
 
-bool failSafeEnabled = true;
+bool failSafeEnabled = false;
 int failSafeCount = 0;
 int failSafeLimit = 60;
 
@@ -66,9 +73,13 @@
 Motor motor1(p22, &ioExt, 6, 7, p14, p15); //FL - M4
 Motor motor3(p21, &ioExt, 9, 8, p16, p17); //RL - M5
 
-Coilgun coilgun(p20, p19, p18);
+Coilgun coilgun(p20, p30, p19, p18);
+
+//HumanInterface humanInterface(&ioExt, 15, 14, 13, 12, 11, p29, p30, p26);
 
-HumanInterface humanInterface(&ioExt, 15, 14, 13, 12, 11, p29, p30, p26); 
+//MCP3021 coilADC(p9, p10, 5.0);
+
+//IMU imu(0x69);
 
 void updateTick() { 
     //led3 = 1;   
@@ -81,6 +92,14 @@
     update = 1;
 }
 
+void imuUpdateTick() {
+    updateIMU = 1;
+}
+
+void testUpdateTick() {
+    updateTest = 1;
+}
+
 void stallChangedCallback() {
     stallChanged = 1;
 }
@@ -126,10 +145,13 @@
     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());
 
@@ -189,19 +211,89 @@
     motor5.stallChange(&stallChangedCallback);
     
     
-    //int charCount = sprintf(sendBuffer, "<ready>");
-    //server.sendTo(client, sendBuffer, charCount);
+    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;
@@ -250,7 +342,7 @@
         }     
         
         
-        switch (humanInterface.getBallState()){
+        /*switch (humanInterface.getBallState()){
             case -1:
                 //Ball lost
                 server.sendTo(client, "<ball:0>", 8);
@@ -272,7 +364,7 @@
         
         if (humanInterface.isStart()) {
             server.sendTo(client, "<toggle-go>", 11);
-        }
+        }*/
     
         //printf("\nWait for packet...\n");
 
@@ -333,19 +425,22 @@
         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, "kick", 4) == 0) {
-        pc.printf("kick\n");
-        coilgun.kick(atoi(strtok(NULL, ":")));
+    } 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, "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) {
@@ -362,23 +457,23 @@
         motor3.setSpeed(0);
         motor4.setSpeed(0);
         motor5.setSpeed(0);
-        humanInterface.setGoal(HumanInterface::UNSET);
+        /*humanInterface.setGoal(HumanInterface::UNSET);
         humanInterface.setError(false);
-        humanInterface.setGo(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) {
+        /*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, ":")));
+        //humanInterface.setError(atoi(strtok(NULL, ":")));
     }  else if (strncmp(cmd, "go", 2) == 0) {
-        humanInterface.setGo(atoi(strtok(NULL, ":")));
+        //humanInterface.setGo(atoi(strtok(NULL, ":")));
     }
 }
\ No newline at end of file