Diff: main.cpp

Revision:

1:79ac4e293661

Parent:

0:22a7683646d1

Child:

2:a3e6eceed838

--- a/main.cpp	Wed Sep 18 07:50:53 2013 +0000
+++ b/main.cpp	Thu Sep 19 13:18:02 2013 +0000
@@ -10,6 +10,7 @@
 #include "ledout.h"
 #include "externalin.h"
 #include "coilgun.h"
+#include "HumanInterface.h"
 
 #define SERVER_PORT   8042
 
@@ -32,6 +33,16 @@
 
 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;
+
+bool isCharged = false; //has charge been sent after discharge
 
 void executeCommand(short *cmd);
 void executeCommandOld(char *buffer);
@@ -53,20 +64,23 @@
     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);
+//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);
+//LedOut redLed(&ioExt, 13);
+//LedOut blueLed(&ioExt, 12);
+//LedOut yellowLed(&ioExt, 11);
 
-CoilGun coilgun(p19, p20, p18);
+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();
@@ -82,9 +96,25 @@
     extInChanged = 1;
 }
 
-BallSens ballSens(&ioExt, 10);
-ExternalIn button1(&ioExt, 14);
-ExternalIn button2(&ioExt, 15);
+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;
@@ -187,16 +217,26 @@
     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);
+    //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();
+    //redLed.set();
     //blueLed.set();
     //yellowLed.set();
     
@@ -217,6 +257,24 @@
             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();
+                    isCharged = false;
+                }  
+                if (!isCharged) {
+                    //int charCount = sprintf(sendBuffer, "<speeds:%d:%d:%d:%d:%d>");
+                    server.sendTo(client, "<discharged>", 12);
+                } 
+            }
+            
             //led3 = 1;
             //updateSensors();
             //pc.printf("update");                       
@@ -234,11 +292,23 @@
             led1 = !led1;
         }
         
-        if (extInChanged) {
-            extInChanged = 0;            
-            bool ballState = ballSens.read();
-            bool button1State = button1.read();
-            bool button2State = button2.read();
+        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 {
@@ -246,7 +316,23 @@
             }
             pc.printf("<ball:%d>\n", ballState ? 1 : 0);
             pc.printf("<btn1:%d>\n", button1State ? 1 : 0);
-            pc.printf("<btn2:%d>\n", button2State ? 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");
@@ -304,47 +390,73 @@
         coilgun.kick(cmd[1]);
     } else if (cmd[0] == 4) {
         pc.printf("charge\n");
-        coilgun.setCharge(cmd[1]);
+        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, " ");
+    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, " ")));
+        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());
-        //pc.printf("%d\n", sizeof(sendBuffer));
-        //pc.printf("sendTo %s\n", client.get_address());
+        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();
+        isCharged = true;
+    } else if (strncmp(cmd, "discharge", 9) == 0) {
+        pc.printf("discharge\n");
+        coilgun.discharge();
+        isCharged = false;
     } else if (strncmp(cmd, "k", 1) == 0) {
         pc.printf("kick\n");
-        int length = atoi(strtok(NULL, " "));
+        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, " ")));
+        if (atoi(strtok(NULL, ":")) == 1) {
+            coilgun.charge();
+            isCharged = true;
+        } else {
+            coilgun.chargeEnd();
+        }
     } 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);
-    }*/
+        isCharged = false;
+    } else if (strncmp(cmd, "fs", 2) == 0) {
+        failSafeEnabled = (bool)atoi(strtok(NULL, ":"));
+    } else if (strncmp(cmd, "target", 6) == 0) {
+        if (atoi(strtok(NULL, ":")) == 1) {
+            humanInterface.setGoal(HumanInterface::YELLOW);
+        } else {
+            humanInterface.setGoal(HumanInterface::BLUE);
+        }
+    } else if (strncmp(cmd, "error", 5) == 0) {
+        humanInterface.setError(atoi(strtok(NULL, ":")));
+    }
 }
\ No newline at end of file