Diff: commands.cpp

Revision:

0:864f6ee5169b

--- /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;
+}
+