Bhaveet Shah / Mbed 2 deprecated m3pi_logo

Look for a LOGO (.LGO) file on the mbed and run the commands in it. Only supports a small subset of the LOGO commands.

Dependencies:   mbed

commands.cpp@0:864f6ee5169b, 2011-04-09 (annotated)

Committer:
nbbhav
Date:
Sat Apr 09 18:06:27 2011 +0000
Revision:
0:864f6ee5169b
Initial version supporting FORWARD, BACK, LEFT and RIGHT commands.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
nbbhav 0:864f6ee5169b 1 /*
nbbhav 0:864f6ee5169b 2 * LOGO command implementation
nbbhav 0:864f6ee5169b 3 */
nbbhav 0:864f6ee5169b 4
nbbhav 0:864f6ee5169b 5 #include "mbed.h"
nbbhav 0:864f6ee5169b 6 #include "m3pi.h"
nbbhav 0:864f6ee5169b 7
nbbhav 0:864f6ee5169b 8 #include "commands.h"
nbbhav 0:864f6ee5169b 9 #include "tokens.h"
nbbhav 0:864f6ee5169b 10
nbbhav 0:864f6ee5169b 11 extern m3pi m3pi;
nbbhav 0:864f6ee5169b 12
nbbhav 0:864f6ee5169b 13 // Speed at which the tutle moves...
nbbhav 0:864f6ee5169b 14 static const float SPEED = 0.25;
nbbhav 0:864f6ee5169b 15
nbbhav 0:864f6ee5169b 16 // Adjustment needed for rotation...
nbbhav 0:864f6ee5169b 17 static const float ROTATION_ADJUST = 1.1;
nbbhav 0:864f6ee5169b 18
nbbhav 0:864f6ee5169b 19 /* ======================================================================
nbbhav 0:864f6ee5169b 20 FORWARD
nbbhav 0:864f6ee5169b 21 ====================================================================== */
nbbhav 0:864f6ee5169b 22
nbbhav 0:864f6ee5169b 23 struct ForwardCommand : public Command
nbbhav 0:864f6ee5169b 24 {
nbbhav 0:864f6ee5169b 25 ForwardCommand(int steps) : steps_(steps) {}
nbbhav 0:864f6ee5169b 26 virtual ~ForwardCommand() {}
nbbhav 0:864f6ee5169b 27
nbbhav 0:864f6ee5169b 28 virtual void go() const {
nbbhav 0:864f6ee5169b 29 // Robot goes forward 470mm per second at half speed. We need to
nbbhav 0:864f6ee5169b 30 // convert this into steps.
nbbhav 0:864f6ee5169b 31 //
nbbhav 0:864f6ee5169b 32 // Full speed (1.0) is 940mm per second, a step of 47 gives us 20
nbbhav 0:864f6ee5169b 33 // steps per second at full speed.
nbbhav 0:864f6ee5169b 34 //
nbbhav 0:864f6ee5169b 35 static const float STEP_TIME = 0.05 / SPEED;
nbbhav 0:864f6ee5169b 36 m3pi.forward(SPEED);
nbbhav 0:864f6ee5169b 37 wait(STEP_TIME * steps_);
nbbhav 0:864f6ee5169b 38 m3pi.forward(0.0);
nbbhav 0:864f6ee5169b 39 }
nbbhav 0:864f6ee5169b 40
nbbhav 0:864f6ee5169b 41 virtual const char* string() const {
nbbhav 0:864f6ee5169b 42 static char buf[256];
nbbhav 0:864f6ee5169b 43 sprintf(buf, "FD %d", steps_);
nbbhav 0:864f6ee5169b 44 return buf;
nbbhav 0:864f6ee5169b 45 }
nbbhav 0:864f6ee5169b 46
nbbhav 0:864f6ee5169b 47 private:
nbbhav 0:864f6ee5169b 48 int steps_;
nbbhav 0:864f6ee5169b 49 };
nbbhav 0:864f6ee5169b 50
nbbhav 0:864f6ee5169b 51 static Command* build_forward_command(const char* str, int* pos) {
nbbhav 0:864f6ee5169b 52 Token token;
nbbhav 0:864f6ee5169b 53 get_token(&token, str, pos);
nbbhav 0:864f6ee5169b 54 if (token.isInteger()) {
nbbhav 0:864f6ee5169b 55 int steps = token.getInteger();
nbbhav 0:864f6ee5169b 56 return new ForwardCommand(steps);
nbbhav 0:864f6ee5169b 57 }
nbbhav 0:864f6ee5169b 58 return NULL;
nbbhav 0:864f6ee5169b 59 }
nbbhav 0:864f6ee5169b 60
nbbhav 0:864f6ee5169b 61 /* ======================================================================
nbbhav 0:864f6ee5169b 62 BACK
nbbhav 0:864f6ee5169b 63 ====================================================================== */
nbbhav 0:864f6ee5169b 64
nbbhav 0:864f6ee5169b 65 struct BackCommand : public Command
nbbhav 0:864f6ee5169b 66 {
nbbhav 0:864f6ee5169b 67 BackCommand(int steps) : steps_(steps) {}
nbbhav 0:864f6ee5169b 68 virtual ~BackCommand() {}
nbbhav 0:864f6ee5169b 69
nbbhav 0:864f6ee5169b 70 virtual void go() const {
nbbhav 0:864f6ee5169b 71 // Robot goes forward 470mm per second at half speed. We need to
nbbhav 0:864f6ee5169b 72 // convert this into steps.
nbbhav 0:864f6ee5169b 73 //
nbbhav 0:864f6ee5169b 74 // Full speed (1.0) is 940mm per second, a step of 47 gives us 20
nbbhav 0:864f6ee5169b 75 // steps per second at full speed.
nbbhav 0:864f6ee5169b 76 //
nbbhav 0:864f6ee5169b 77 static const float STEP_TIME = 0.05 / SPEED;
nbbhav 0:864f6ee5169b 78 m3pi.backward(SPEED);
nbbhav 0:864f6ee5169b 79 wait(STEP_TIME * steps_);
nbbhav 0:864f6ee5169b 80 m3pi.backward(0.0);
nbbhav 0:864f6ee5169b 81 }
nbbhav 0:864f6ee5169b 82
nbbhav 0:864f6ee5169b 83 virtual const char* string() const {
nbbhav 0:864f6ee5169b 84 static char buf[256];
nbbhav 0:864f6ee5169b 85 sprintf(buf, "BK %d", steps_);
nbbhav 0:864f6ee5169b 86 return buf;
nbbhav 0:864f6ee5169b 87 }
nbbhav 0:864f6ee5169b 88
nbbhav 0:864f6ee5169b 89 private:
nbbhav 0:864f6ee5169b 90 int steps_;
nbbhav 0:864f6ee5169b 91 };
nbbhav 0:864f6ee5169b 92
nbbhav 0:864f6ee5169b 93 static Command* build_back_command(const char* str, int* pos) {
nbbhav 0:864f6ee5169b 94 Token token;
nbbhav 0:864f6ee5169b 95 get_token(&token, str, pos);
nbbhav 0:864f6ee5169b 96 if (token.isInteger()) {
nbbhav 0:864f6ee5169b 97 int steps = token.getInteger();
nbbhav 0:864f6ee5169b 98 return new BackCommand(steps);
nbbhav 0:864f6ee5169b 99 }
nbbhav 0:864f6ee5169b 100 return NULL;
nbbhav 0:864f6ee5169b 101 }
nbbhav 0:864f6ee5169b 102
nbbhav 0:864f6ee5169b 103 /* ======================================================================
nbbhav 0:864f6ee5169b 104 LEFT
nbbhav 0:864f6ee5169b 105 ====================================================================== */
nbbhav 0:864f6ee5169b 106
nbbhav 0:864f6ee5169b 107 struct LeftCommand : public Command
nbbhav 0:864f6ee5169b 108 {
nbbhav 0:864f6ee5169b 109 LeftCommand(int angle) : angle_(angle) {}
nbbhav 0:864f6ee5169b 110 virtual ~LeftCommand() {}
nbbhav 0:864f6ee5169b 111
nbbhav 0:864f6ee5169b 112 virtual void go() const {
nbbhav 0:864f6ee5169b 113 // Robot rotates 720 degrees per second at half speed.
nbbhav 0:864f6ee5169b 114 //
nbbhav 0:864f6ee5169b 115 // So full speed (1.0) is 1440 degrees per second, and 1 degree is
nbbhav 0:864f6ee5169b 116 // 1.0 / 1440
nbbhav 0:864f6ee5169b 117 //
nbbhav 0:864f6ee5169b 118 static const float DEGREE = (1.0 / 1440) / SPEED;
nbbhav 0:864f6ee5169b 119 m3pi.left(SPEED);
nbbhav 0:864f6ee5169b 120 wait(DEGREE * angle_ * ROTATION_ADJUST);
nbbhav 0:864f6ee5169b 121 m3pi.left(0.0);
nbbhav 0:864f6ee5169b 122 }
nbbhav 0:864f6ee5169b 123
nbbhav 0:864f6ee5169b 124 virtual const char* string() const {
nbbhav 0:864f6ee5169b 125 static char buf[256];
nbbhav 0:864f6ee5169b 126 sprintf(buf, "LT %d", angle_);
nbbhav 0:864f6ee5169b 127 return buf;
nbbhav 0:864f6ee5169b 128 }
nbbhav 0:864f6ee5169b 129
nbbhav 0:864f6ee5169b 130 private:
nbbhav 0:864f6ee5169b 131 int angle_;
nbbhav 0:864f6ee5169b 132 };
nbbhav 0:864f6ee5169b 133
nbbhav 0:864f6ee5169b 134 static Command* build_left_command(const char* str, int* pos) {
nbbhav 0:864f6ee5169b 135 Token token;
nbbhav 0:864f6ee5169b 136 get_token(&token, str, pos);
nbbhav 0:864f6ee5169b 137 if (token.isInteger()) {
nbbhav 0:864f6ee5169b 138 int angle = token.getInteger();
nbbhav 0:864f6ee5169b 139 return new LeftCommand(angle);
nbbhav 0:864f6ee5169b 140 }
nbbhav 0:864f6ee5169b 141 return NULL;
nbbhav 0:864f6ee5169b 142 }
nbbhav 0:864f6ee5169b 143
nbbhav 0:864f6ee5169b 144 /* ======================================================================
nbbhav 0:864f6ee5169b 145 RIGHT
nbbhav 0:864f6ee5169b 146 ====================================================================== */
nbbhav 0:864f6ee5169b 147
nbbhav 0:864f6ee5169b 148 struct RightCommand : public Command
nbbhav 0:864f6ee5169b 149 {
nbbhav 0:864f6ee5169b 150 RightCommand(int angle) : angle_(angle) {}
nbbhav 0:864f6ee5169b 151 virtual ~RightCommand() {}
nbbhav 0:864f6ee5169b 152
nbbhav 0:864f6ee5169b 153 virtual void go() const {
nbbhav 0:864f6ee5169b 154 // Robot rotates 720 degrees per second at half speed.
nbbhav 0:864f6ee5169b 155 //
nbbhav 0:864f6ee5169b 156 // So full speed (1.0) is 1440 degrees per second, and 1 degree is
nbbhav 0:864f6ee5169b 157 // 1.0 / 1440
nbbhav 0:864f6ee5169b 158 //
nbbhav 0:864f6ee5169b 159 static const float DEGREE = (1.0 / 1440) / SPEED;
nbbhav 0:864f6ee5169b 160 m3pi.right(SPEED);
nbbhav 0:864f6ee5169b 161 wait(DEGREE * angle_ * ROTATION_ADJUST);
nbbhav 0:864f6ee5169b 162 m3pi.right(0.0);
nbbhav 0:864f6ee5169b 163 }
nbbhav 0:864f6ee5169b 164
nbbhav 0:864f6ee5169b 165 virtual const char* string() const {
nbbhav 0:864f6ee5169b 166 static char buf[256];
nbbhav 0:864f6ee5169b 167 sprintf(buf, "RT %d", angle_);
nbbhav 0:864f6ee5169b 168 return buf;
nbbhav 0:864f6ee5169b 169 }
nbbhav 0:864f6ee5169b 170
nbbhav 0:864f6ee5169b 171 private:
nbbhav 0:864f6ee5169b 172 int angle_;
nbbhav 0:864f6ee5169b 173 };
nbbhav 0:864f6ee5169b 174
nbbhav 0:864f6ee5169b 175 static Command* build_right_command(const char* str, int* pos) {
nbbhav 0:864f6ee5169b 176 Token token;
nbbhav 0:864f6ee5169b 177 get_token(&token, str, pos);
nbbhav 0:864f6ee5169b 178 if (token.isInteger()) {
nbbhav 0:864f6ee5169b 179 int angle = token.getInteger();
nbbhav 0:864f6ee5169b 180 return new RightCommand(angle);
nbbhav 0:864f6ee5169b 181 }
nbbhav 0:864f6ee5169b 182 return NULL;
nbbhav 0:864f6ee5169b 183 }
nbbhav 0:864f6ee5169b 184
nbbhav 0:864f6ee5169b 185 /* ======================================================================
nbbhav 0:864f6ee5169b 186 PENDOWN
nbbhav 0:864f6ee5169b 187 ====================================================================== */
nbbhav 0:864f6ee5169b 188
nbbhav 0:864f6ee5169b 189 struct PenDownCommand : public Command
nbbhav 0:864f6ee5169b 190 {
nbbhav 0:864f6ee5169b 191 PenDownCommand() {}
nbbhav 0:864f6ee5169b 192 virtual ~PenDownCommand() {}
nbbhav 0:864f6ee5169b 193
nbbhav 0:864f6ee5169b 194 virtual void go() const {
nbbhav 0:864f6ee5169b 195 }
nbbhav 0:864f6ee5169b 196
nbbhav 0:864f6ee5169b 197 virtual const char* string() const {
nbbhav 0:864f6ee5169b 198 return "PD";
nbbhav 0:864f6ee5169b 199 }
nbbhav 0:864f6ee5169b 200 };
nbbhav 0:864f6ee5169b 201
nbbhav 0:864f6ee5169b 202 static Command* build_pendown_command(const char* str, int* pos) {
nbbhav 0:864f6ee5169b 203 return new PenDownCommand();
nbbhav 0:864f6ee5169b 204 }
nbbhav 0:864f6ee5169b 205
nbbhav 0:864f6ee5169b 206 /* ======================================================================
nbbhav 0:864f6ee5169b 207 PENUP
nbbhav 0:864f6ee5169b 208 ====================================================================== */
nbbhav 0:864f6ee5169b 209
nbbhav 0:864f6ee5169b 210 struct PenUpCommand : public Command
nbbhav 0:864f6ee5169b 211 {
nbbhav 0:864f6ee5169b 212 PenUpCommand() {}
nbbhav 0:864f6ee5169b 213 virtual ~PenUpCommand() {}
nbbhav 0:864f6ee5169b 214
nbbhav 0:864f6ee5169b 215 virtual void go() const {
nbbhav 0:864f6ee5169b 216 }
nbbhav 0:864f6ee5169b 217
nbbhav 0:864f6ee5169b 218 virtual const char* string() const {
nbbhav 0:864f6ee5169b 219 return "PU";
nbbhav 0:864f6ee5169b 220 }
nbbhav 0:864f6ee5169b 221 };
nbbhav 0:864f6ee5169b 222
nbbhav 0:864f6ee5169b 223 static Command* build_penup_command(const char* str, int* pos) {
nbbhav 0:864f6ee5169b 224 return new PenUpCommand();
nbbhav 0:864f6ee5169b 225 }
nbbhav 0:864f6ee5169b 226
nbbhav 0:864f6ee5169b 227 /* ======================================================================
nbbhav 0:864f6ee5169b 228 Command Builder
nbbhav 0:864f6ee5169b 229 ====================================================================== */
nbbhav 0:864f6ee5169b 230
nbbhav 0:864f6ee5169b 231 typedef Command* build_command_fn(const char* str, int* pos);
nbbhav 0:864f6ee5169b 232
nbbhav 0:864f6ee5169b 233 static const struct {
nbbhav 0:864f6ee5169b 234 const char* name;
nbbhav 0:864f6ee5169b 235 build_command_fn* fn;
nbbhav 0:864f6ee5169b 236 } command_builders[] = {
nbbhav 0:864f6ee5169b 237 { "FD", build_forward_command },
nbbhav 0:864f6ee5169b 238 { "FORWARD", build_forward_command },
nbbhav 0:864f6ee5169b 239 { "BK", build_back_command },
nbbhav 0:864f6ee5169b 240 { "BACK", build_back_command },
nbbhav 0:864f6ee5169b 241 { "LT", build_left_command },
nbbhav 0:864f6ee5169b 242 { "LEFT", build_left_command },
nbbhav 0:864f6ee5169b 243 { "RT", build_right_command },
nbbhav 0:864f6ee5169b 244 { "RIGHT", build_right_command },
nbbhav 0:864f6ee5169b 245 { "PD", build_pendown_command },
nbbhav 0:864f6ee5169b 246 { "PENDOWN", build_pendown_command },
nbbhav 0:864f6ee5169b 247 { "PU", build_penup_command },
nbbhav 0:864f6ee5169b 248 { "PENUP", build_penup_command },
nbbhav 0:864f6ee5169b 249 { NULL, NULL }
nbbhav 0:864f6ee5169b 250 };
nbbhav 0:864f6ee5169b 251
nbbhav 0:864f6ee5169b 252 Command* build_command(int line, const char* str, int* pos) {
nbbhav 0:864f6ee5169b 253 Token token;
nbbhav 0:864f6ee5169b 254 get_token(&token, str, pos);
nbbhav 0:864f6ee5169b 255 if (token.isValid()) {
nbbhav 0:864f6ee5169b 256 if (token.isWord()) {
nbbhav 0:864f6ee5169b 257 for (int i = 0; command_builders[i].name != NULL; ++i) {
nbbhav 0:864f6ee5169b 258 if (strcmp(token.getWord(), command_builders[i].name) == 0) {
nbbhav 0:864f6ee5169b 259 return command_builders[i].fn(str, pos);
nbbhav 0:864f6ee5169b 260 }
nbbhav 0:864f6ee5169b 261 }
nbbhav 0:864f6ee5169b 262 m3pi.cls();
nbbhav 0:864f6ee5169b 263 m3pi.printf("BadCmd");
nbbhav 0:864f6ee5169b 264 m3pi.locate(0,1);
nbbhav 0:864f6ee5169b 265 m3pi.printf("L%d,C%d", line, *pos);
nbbhav 0:864f6ee5169b 266 } else if (!token.isEOL()) {
nbbhav 0:864f6ee5169b 267 m3pi.cls();
nbbhav 0:864f6ee5169b 268 m3pi.printf("BadToken");
nbbhav 0:864f6ee5169b 269 m3pi.locate(0,1);
nbbhav 0:864f6ee5169b 270 m3pi.printf("L%d,C%d", line, *pos);
nbbhav 0:864f6ee5169b 271 }
nbbhav 0:864f6ee5169b 272 } else {
nbbhav 0:864f6ee5169b 273 m3pi.cls();
nbbhav 0:864f6ee5169b 274 m3pi.printf("UnkToken");
nbbhav 0:864f6ee5169b 275 m3pi.locate(0,1);
nbbhav 0:864f6ee5169b 276 m3pi.printf("L%d,C%d", line, *pos);
nbbhav 0:864f6ee5169b 277 }
nbbhav 0:864f6ee5169b 278 return NULL;
nbbhav 0:864f6ee5169b 279 }
nbbhav 0:864f6ee5169b 280
nbbhav 0:864f6ee5169b 281 void destroy_command(Command* cmd) {
nbbhav 0:864f6ee5169b 282 delete cmd;
nbbhav 0:864f6ee5169b 283 }
nbbhav 0:864f6ee5169b 284