Diff: driver.cpp

Revision:

0:9fd64882c5aa

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/driver.cpp	Tue Dec 10 02:17:48 2013 +0000
@@ -0,0 +1,129 @@
+#include "driver.h"
+#include "wifiunit.h"
+
+void processCommands(vector<robotCommand> &commands) {
+    printLCD("Processing", NULL);
+    forward();
+    stop();
+    for(int i=0; i<commands.size(); i++) {
+        double mag = commands[i].magnitude;
+        int angle;
+        switch(commands[i].commandType) {
+            case(Forward):
+                printLCD("Forward", NULL);
+                forward();
+                wait(mag*4);
+                break;
+            case(Reverse):
+                printLCD("Reverse", NULL);
+                reverse();
+                printf("%f\n", mag*4);
+                wait(mag*4);
+                break;
+            case(Left):
+                printLCD("Left", NULL);
+                angle = (int) (mag*90);
+                //create script
+                device.printf("%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c", NewScript, 13, Drive, char((speed>>8)&0xFF),  char(speed&0xFF), char(((1)>>8)&0xFF),  char((1)&0xFF), WaitAngle, char((angle>>8)&0xFF), char(angle&0xFF), Drive, 0, 0, 0, 0);
+                device.printf("%c", DoScript);
+                break;
+            case(Right):
+                printLCD("Right", NULL);
+                angle = (int) (mag*90);
+                //create script
+                device.printf("%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c", NewScript, 13, Drive, char((speed>>8)&0xFF),  char(speed&0xFF), char(((-1)>>8)&0xFF),  char((-1)&0xFF), WaitAngle, char(((-angle)>>8)&0xFF), char((-angle)&0xFF), Drive, 0, 0, 0, 0);
+                device.printf("%c", DoScript);
+                break;
+            case(PlaySound):
+                printLCD("Sound", NULL);
+                playsong((int) mag);
+                break;
+        }
+        stop();
+    }
+}
+
+void initialize() {
+    initializeRobot();
+    forward();
+    wait(.5);
+    stop();
+    reverse();
+    wait(.5);
+    stop();
+    setupSDCard();
+    setupWiFi();
+    down.mode(PullUp);
+    up.mode(PullUp);
+    back.mode(PullUp);
+    select.mode(PullUp);
+}
+
+void cancel() {
+    printLCD("Cancelled", NULL);
+    wait(1.5);
+}
+
+// Demo to move around using basic commands
+int main() {
+    initialize();
+    const char* mainMenu[4] = {"Read From RFID", "Read From SD", "Reprogram", "WiFi Mode"};
+    const char* reprogramType[2] = {"Command", "Magnitude"};
+    const char* allCommands[5] = {"Forward", "Reverse", "Left", "Right", "Play Sound"};
+    const char* allMagnitudes[6] = {".5", "1", "1.5", "2", "2.5", "3"};
+    vector<int> tagValues;
+    vector<robotCommand>* commands;
+    
+    while(1) {
+        wait(.2);
+        int option = displayMenu("Main Menu", mainMenu, 4);
+        wait(.2);
+        switch(option) {
+            case(0):  { //read from RFID
+                tagValues.clear();
+                int rt = readTagSequence(tagValues);
+                if(!rt) {
+                    cancel();
+                    break;
+                }
+                commands = translateTags(tagValues);
+                processCommands(*commands);
+                delete commands;
+            }
+            break;
+            case(1): //read from SD
+                commands = getProgrammedPath(1);
+                processCommands(*commands);
+                delete commands;
+                break;
+            case(2): //reprogram
+                int type = displayMenu("Reprogram Type", reprogramType, 2);
+                if(type == 0) { //reprogram command
+                    CommandType repCommand = (CommandType) displayMenu("Command Type", allCommands, 5);
+                    int tagID = readTag();
+                    if(!tagID) {
+                        cancel();
+                        break;
+                    }
+                    writeTagCommand(tagID, repCommand);
+                }
+                else { //reprogram magnitude
+                    double repMag = displayMenu("Mag Type", allMagnitudes, 6)*.5+.5;
+                    int tagID = readTag();
+                    if(!tagID) {
+                        cancel();
+                        break;
+                    }
+                    writeTagCommand(tagID, repMag);
+                }
+                break;
+                
+           case(3):
+                int ret = readTag();
+                commands = getTagCommands(ret%1000);
+                processCommands(*commands);
+                delete commands;
+                break;
+        }
+    }
+}
\ No newline at end of file