File content as of revision 0:9fd64882c5aa:

#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;
        }
    }
}