File content as of revision 0:864f6ee5169b:

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