Revision 0:864f6ee5169b, committed 2011-04-09

Comitter:

nbbhav

Date:

Sat Apr 09 18:06:27 2011 +0000

Commit message:

Initial version supporting FORWARD, BACK, LEFT and RIGHT commands.

Changed in this revision

diff -r 000000000000 -r 864f6ee5169b commands.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/commands.cpp	Sat Apr 09 18:06:27 2011 +0000
@@ -0,0 +1,284 @@
+/*
+ * LOGO command implementation
+ */
+ 
+#include "mbed.h"
+#include "m3pi.h"
+
+#include "commands.h"
+#include "tokens.h"
+
+extern m3pi m3pi;
+
+// Speed at which the tutle moves...
+static const float SPEED = 0.25;
+
+// Adjustment needed for rotation...
+static const float ROTATION_ADJUST = 1.1;
+
+/*  ======================================================================
+    FORWARD
+    ====================================================================== */
+
+struct ForwardCommand : public Command
+{
+    ForwardCommand(int steps) : steps_(steps) {}
+    virtual ~ForwardCommand() {}
+    
+    virtual void go() const {
+        // Robot goes forward 470mm per second at half speed. We need to 
+        // convert this into steps.
+        //
+        // Full speed (1.0) is 940mm per second, a step of 47 gives us 20
+        // steps per second at full speed.
+        //
+        static const float STEP_TIME = 0.05 / SPEED;
+        m3pi.forward(SPEED);
+        wait(STEP_TIME * steps_);
+        m3pi.forward(0.0);
+    }
+    
+    virtual const char* string() const {
+        static char buf[256];
+        sprintf(buf, "FD %d", steps_);
+        return buf;
+    }
+    
+private:
+    int steps_;
+};
+
+static Command* build_forward_command(const char* str, int* pos) {
+    Token token;
+    get_token(&token, str, pos);
+    if (token.isInteger()) {
+        int steps = token.getInteger();
+        return new ForwardCommand(steps);
+    }
+    return NULL;
+}
+
+/*  ======================================================================
+    BACK
+    ====================================================================== */
+
+struct BackCommand : public Command
+{
+    BackCommand(int steps) : steps_(steps) {}
+    virtual ~BackCommand() {}
+    
+    virtual void go() const {
+        // Robot goes forward 470mm per second at half speed. We need to 
+        // convert this into steps.
+        //
+        // Full speed (1.0) is 940mm per second, a step of 47 gives us 20
+        // steps per second at full speed.
+        //
+        static const float STEP_TIME = 0.05 / SPEED;
+        m3pi.backward(SPEED);
+        wait(STEP_TIME * steps_);
+        m3pi.backward(0.0);
+    }
+    
+    virtual const char* string() const {
+        static char buf[256];
+        sprintf(buf, "BK %d", steps_);
+        return buf;
+    }
+    
+private:
+    int steps_;
+};
+
+static Command* build_back_command(const char* str, int* pos) {
+    Token token;
+    get_token(&token, str, pos);
+    if (token.isInteger()) {
+        int steps = token.getInteger();
+        return new BackCommand(steps);
+    }
+    return NULL;
+}
+
+/*  ======================================================================
+    LEFT
+    ====================================================================== */
+
+struct LeftCommand : public Command
+{
+    LeftCommand(int angle) : angle_(angle) {}
+    virtual ~LeftCommand() {}
+    
+    virtual void go() const {
+        // Robot rotates 720 degrees per second at half speed.
+        //
+        // So full speed (1.0) is 1440 degrees per second, and 1 degree is
+        // 1.0 / 1440
+        //
+        static const float DEGREE = (1.0 / 1440) / SPEED;
+        m3pi.left(SPEED);
+        wait(DEGREE * angle_ * ROTATION_ADJUST);
+        m3pi.left(0.0);
+    }
+    
+    virtual const char* string() const {
+        static char buf[256];
+        sprintf(buf, "LT %d", angle_);
+        return buf;
+    }
+    
+private:
+    int angle_;
+};
+
+static Command* build_left_command(const char* str, int* pos) {
+    Token token;
+    get_token(&token, str, pos);
+    if (token.isInteger()) {
+        int angle = token.getInteger();
+        return new LeftCommand(angle);
+    }
+    return NULL;
+}
+
+/*  ======================================================================
+    RIGHT
+    ====================================================================== */
+
+struct RightCommand : public Command
+{
+    RightCommand(int angle) : angle_(angle) {}
+    virtual ~RightCommand() {}
+    
+    virtual void go() const {
+        // Robot rotates 720 degrees per second at half speed.
+        //
+        // So full speed (1.0) is 1440 degrees per second, and 1 degree is
+        // 1.0 / 1440
+        //
+        static const float DEGREE = (1.0 / 1440) / SPEED;
+        m3pi.right(SPEED);
+        wait(DEGREE * angle_ * ROTATION_ADJUST);
+        m3pi.right(0.0);
+    }
+    
+    virtual const char* string() const {
+        static char buf[256];
+        sprintf(buf, "RT %d", angle_);
+        return buf;
+    }
+    
+private:
+    int angle_;
+};
+
+static Command* build_right_command(const char* str, int* pos) {
+    Token token;
+    get_token(&token, str, pos);
+    if (token.isInteger()) {
+        int angle = token.getInteger();
+        return new RightCommand(angle);
+    }
+    return NULL;
+}
+
+/*  ======================================================================
+    PENDOWN
+    ====================================================================== */
+
+struct PenDownCommand : public Command
+{
+    PenDownCommand() {}
+    virtual ~PenDownCommand() {}
+    
+    virtual void go() const {
+    }
+    
+    virtual const char* string() const {
+        return "PD";
+    }
+};
+
+static Command* build_pendown_command(const char* str, int* pos) {
+    return new PenDownCommand();
+}
+
+/*  ======================================================================
+    PENUP
+    ====================================================================== */
+
+struct PenUpCommand : public Command
+{
+    PenUpCommand() {}
+    virtual ~PenUpCommand() {}
+    
+    virtual void go() const {
+    }
+    
+    virtual const char* string() const {
+        return "PU";
+    }
+};
+
+static Command* build_penup_command(const char* str, int* pos) {
+    return new PenUpCommand();
+}
+
+/*  ======================================================================
+    Command Builder
+    ====================================================================== */
+
+typedef Command* build_command_fn(const char* str, int* pos);
+
+static const struct {
+    const char* name;
+    build_command_fn* fn;
+} command_builders[] = {
+    { "FD", build_forward_command },
+    { "FORWARD", build_forward_command },
+    { "BK", build_back_command },
+    { "BACK", build_back_command },
+    { "LT", build_left_command },
+    { "LEFT", build_left_command },
+    { "RT", build_right_command },
+    { "RIGHT", build_right_command },
+    { "PD", build_pendown_command },
+    { "PENDOWN", build_pendown_command },
+    { "PU", build_penup_command },
+    { "PENUP", build_penup_command },
+    { NULL, NULL }
+};
+
+Command* build_command(int line, const char* str, int* pos) {
+    Token token;
+    get_token(&token, str, pos);
+    if (token.isValid()) {
+        if (token.isWord()) {
+            for (int i = 0; command_builders[i].name != NULL; ++i) {
+                if (strcmp(token.getWord(), command_builders[i].name) == 0) {
+                    return command_builders[i].fn(str, pos);
+                }
+            }
+            m3pi.cls();
+            m3pi.printf("BadCmd");
+            m3pi.locate(0,1);
+            m3pi.printf("L%d,C%d", line, *pos);
+        } else if (!token.isEOL()) {
+            m3pi.cls();
+            m3pi.printf("BadToken");
+            m3pi.locate(0,1);
+            m3pi.printf("L%d,C%d", line, *pos);
+        }
+    } else {
+        m3pi.cls();
+        m3pi.printf("UnkToken");
+        m3pi.locate(0,1);
+        m3pi.printf("L%d,C%d", line, *pos);
+    }
+    return NULL;
+}
+
+void destroy_command(Command* cmd) {
+    delete cmd;
+}
+

diff -r 000000000000 -r 864f6ee5169b commands.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/commands.h	Sat Apr 09 18:06:27 2011 +0000
@@ -0,0 +1,13 @@
+/*
+ * LOGO command implementation
+ */
+ 
+struct Command
+{
+    virtual void go() const = 0;
+    virtual const char* string() const = 0;
+};
+
+Command* build_command(int line, const char* str, int* pos);
+
+void destroy_command(Command* cmd);

diff -r 000000000000 -r 864f6ee5169b m3pi.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/m3pi.lib	Sat Apr 09 18:06:27 2011 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/chris/code/m3pi/#9b128cebb3c2

diff -r 000000000000 -r 864f6ee5169b main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sat Apr 09 18:06:27 2011 +0000
@@ -0,0 +1,79 @@
+/*
+ * Main program, looks for LOGO file on robot and executes the commands
+ * within it.
+ */
+ 
+#include "mbed.h"
+#include "m3pi.h"
+
+#include "commands.h"
+
+m3pi m3pi(p23,p9,p10);
+LocalFileSystem local("local");
+
+FILE* open_last_lgo_file() {
+    char* fn = NULL;
+    
+    DIR *d = opendir("/local");
+    for (struct dirent *p = readdir(d); p != NULL; p = readdir(d)) {
+        const char* dot = strchr(p->d_name, '.');
+        if (dot != NULL) {
+            const char* ext = dot+1; // skip '.'
+            if (strcmp(ext, "LGO") == 0) {
+                m3pi.cls();
+                m3pi.printf("Found");
+                m3pi.locate(0,1);
+                m3pi.printf("%.*s", 8, p->d_name);
+                
+                // Save reference to the file. Treat the last file as the 
+                // most recent. This may or may not be correct!
+                size_t fn_len = strlen("/local/") + strlen(p->d_name) + 0;
+                fn = (char*)((fn != NULL) ? realloc(fn, fn_len) : malloc(fn_len));
+                sprintf(fn, "/local/%s", p->d_name);
+            }
+        }
+    }
+    closedir(d);
+    
+    FILE* f = NULL;
+    if (fn != NULL) {
+        f = fopen(fn, "r");
+        free(fn);
+        fn = NULL;
+        wait(2.0);
+    }
+    return f;
+}
+
+int main() {
+    FILE* f = open_last_lgo_file();
+    if (f != NULL) {
+        int line = 1;
+        char buf[256];
+        while (!feof(f)) {
+            const char* str = fgets(buf, sizeof(buf), f);
+            if (str == NULL) {
+                break;
+            }
+            int pos = 0;
+            while (true) {
+                Command* cmd =  build_command(line, str, &pos);
+                if (cmd != NULL) {
+                    m3pi.cls();
+                    m3pi.printf("%s", cmd->string());
+                    wait(2.0);
+                    cmd->go();
+                    destroy_command(cmd);
+                } else
+                    break;
+            }
+            ++line;
+        }
+        fclose(f);
+        m3pi.cls();
+        m3pi.printf("Success");
+    } else {
+        m3pi.cls();
+        m3pi.printf("NotFound");
+    }
+}

diff -r 000000000000 -r 864f6ee5169b mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Sat Apr 09 18:06:27 2011 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/63bcd7ba4912

diff -r 000000000000 -r 864f6ee5169b tokens.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/tokens.cpp	Sat Apr 09 18:06:27 2011 +0000
@@ -0,0 +1,67 @@
+/*
+ * Source file tokenization
+ */
+ 
+#include <ctype.h>
+#include <stdlib.h>
+
+#include "tokens.h"
+
+Token::Token() : type_(UNKNOWN) {
+}
+
+Token::~Token() {
+    if (isWord()) {
+        free(u.word);
+    }
+}
+
+const char* Token::getWord() const {
+    return isWord() ? u.word : NULL;
+}
+
+int Token::getInteger() const {
+    return isInteger() ? u.value : 0;
+}
+
+void Token::get_token(Token* token, const char* str, int* pos) {
+    // Reset state of the token...
+    if (token->isWord()) {
+        free(token->u.word);
+        token->u.word = NULL;
+    }
+    token->type_ = UNKNOWN;
+
+    // Skip any spaces in the token stream...
+    int i;
+    for (i = *pos; isspace(str[i]); ++i) {
+    }
+    
+    // Now find and return the token...
+    int start = i;
+    if (isalpha(str[i])) {
+        for ( ; isalnum(str[i]); ++i) {
+        }
+        char* word = (char*)malloc(i - start + 1);
+        int k = 0;
+        for (int j = start; j < i; ++j, ++k) {
+            word[k] = toupper(str[j]);
+        }
+        word[k] = 0;
+        
+        token->type_ = WORD;
+        token->u.word = word;
+    } else if (isdigit(str[i])) {
+        int val = str[i] - '0';
+        for (++i; isdigit(str[i]); ++i) {
+            val *= 10;
+            val += str[i] - '0';
+        }
+        
+        token->type_ = INTEGER;
+        token->u.value = val;
+    } else if (str[i] == 0) {
+        token->type_ = EOL;
+    }
+    *pos = i;
+}
\ No newline at end of file

diff -r 000000000000 -r 864f6ee5169b tokens.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/tokens.h	Sat Apr 09 18:06:27 2011 +0000
@@ -0,0 +1,39 @@
+/*
+ * Source file tokenization
+ */
+ 
+enum TokenType {
+    UNKNOWN,
+    WORD,
+    INTEGER,
+    EOL
+};
+
+struct Token
+{
+    Token();
+    ~Token();
+
+    TokenType getType() const { return type_; }
+    bool isValid() const { return type_ != UNKNOWN; }
+    
+    bool isWord() const { return type_ == WORD; }
+    bool isInteger() const { return type_ == INTEGER; }
+    bool isEOL() const { return type_ == EOL; }
+    
+    const char* getWord() const;
+    int getInteger() const;
+    
+    static void get_token(Token* token, const char* str, int* pos);
+    
+private:
+    TokenType type_;
+    union {
+        char* word;
+        int value;
+    } u;
+};
+
+inline void get_token(Token* token, const char* str, int* pos) {
+    Token::get_token(token, str, pos);
+}
\ No newline at end of file