ECE 4180 Drawing Robot / Mbed 2 deprecated ECE_4180_Drawing_Robot

Basic motor code

Dependencies:   Motordriver mbed HALLFX_ENCODER Servo

main.cpp@12:b107ba24faa4, 2018-05-01 (annotated)

Committer:
amohile3
Date:
Tue May 01 16:17:24 2018 +0000
Revision:
12:b107ba24faa4
Parent:
10:9066603296bd
Child:
13:294c06406d06
Added makePattern()

Who changed what in which revision?

UserRevisionLine numberNew contents of line
amohile3 0:191d0be60c80 1 #include "mbed.h"
amohile3 0:191d0be60c80 2 #include "motordriver.h"
jsterling30 2:8c27831ce9a2 3 #include "HALLFX_ENCODER.h"
jsterling30 7:6ef9b0aab972 4 #include "Servo.h"
jsterling30 8:e90d9ce42b33 5 #include "letters.h"
jsterling30 2:8c27831ce9a2 6
jsterling30 8:e90d9ce42b33 7 #include <math.h>
jsterling30 7:6ef9b0aab972 8 #include <string.h>
jsterling30 6:235548599e79 9
jsterling30 10:9066603296bd 10 #define SPEED 0.33
jsterling30 3:c4b0460d8886 11 #define TICKSPERREV 390
jsterling30 7:6ef9b0aab972 12 #define DISTPERREV 8.25 // 8.25 inches per revolution
jsterling30 8:e90d9ce42b33 13 #define TicksPerDeg 2.73
jsterling30 8:e90d9ce42b33 14
jsterling30 8:e90d9ce42b33 15 #define PI 3.14159265
jsterling30 8:e90d9ce42b33 16 #define abs(X) ((X < 0) ? -1 * X : X)
amohile3 0:191d0be60c80 17
jsterling30 7:6ef9b0aab972 18 Motor right(p23, p6, p5, 1); // pwm, fwd, rev
jsterling30 7:6ef9b0aab972 19 Motor left(p21, p7, p8, 1); // pwm, fwd, rev
amohile3 0:191d0be60c80 20
jsterling30 2:8c27831ce9a2 21 HALLFX_ENCODER leftEnc(p15);
jsterling30 2:8c27831ce9a2 22 HALLFX_ENCODER rightEnc(p16);
jsterling30 2:8c27831ce9a2 23
jsterling30 7:6ef9b0aab972 24 Serial blue(p13,p14);
jsterling30 7:6ef9b0aab972 25 Serial pc(USBTX, USBRX);
amohile3 5:7572f73a78f3 26 DigitalOut led1(LED1);
amohile3 5:7572f73a78f3 27 DigitalOut led4(LED4);
amohile3 5:7572f73a78f3 28
jsterling30 7:6ef9b0aab972 29 Servo pen(p24);
jsterling30 6:235548599e79 30
jsterling30 8:e90d9ce42b33 31 double penUpValue = 0.8;
jsterling30 8:e90d9ce42b33 32 double penDownValue = 1.0;
jsterling30 8:e90d9ce42b33 33
jsterling30 8:e90d9ce42b33 34 void penUp() {
jsterling30 8:e90d9ce42b33 35 pen = penUpValue;
jsterling30 8:e90d9ce42b33 36 wait(0.5);
jsterling30 8:e90d9ce42b33 37 }
jsterling30 8:e90d9ce42b33 38
jsterling30 8:e90d9ce42b33 39 void penDown() {
jsterling30 8:e90d9ce42b33 40 pen = penDownValue;
jsterling30 8:e90d9ce42b33 41 wait(0.5);
jsterling30 8:e90d9ce42b33 42 }
jsterling30 8:e90d9ce42b33 43
jsterling30 2:8c27831ce9a2 44 void stop() {
jsterling30 2:8c27831ce9a2 45 right.speed(0.0);
jsterling30 2:8c27831ce9a2 46 left.speed(0.0);
jsterling30 2:8c27831ce9a2 47 }
jsterling30 2:8c27831ce9a2 48
jsterling30 8:e90d9ce42b33 49 void forward(double distance) {
jsterling30 8:e90d9ce42b33 50 double numRevs = distance / DISTPERREV;
jsterling30 8:e90d9ce42b33 51 double numTicks = numRevs * TICKSPERREV;
jsterling30 2:8c27831ce9a2 52 leftEnc.reset();
jsterling30 2:8c27831ce9a2 53 rightEnc.reset();
jsterling30 2:8c27831ce9a2 54 right.speed(SPEED);
jsterling30 2:8c27831ce9a2 55 left.speed(SPEED);
jsterling30 3:c4b0460d8886 56 while (leftEnc.read() < numTicks && rightEnc.read() < numTicks) {}
jsterling30 3:c4b0460d8886 57 stop();
jsterling30 8:e90d9ce42b33 58 wait(0.5);
jsterling30 3:c4b0460d8886 59 }
jsterling30 3:c4b0460d8886 60
jsterling30 8:e90d9ce42b33 61 void reverse(double distance) {
jsterling30 8:e90d9ce42b33 62 double numRevs = distance / DISTPERREV;
jsterling30 8:e90d9ce42b33 63 double numTicks = numRevs * TICKSPERREV;
jsterling30 8:e90d9ce42b33 64 leftEnc.reset();
jsterling30 8:e90d9ce42b33 65 rightEnc.reset();
jsterling30 8:e90d9ce42b33 66 right.speed(-SPEED);
jsterling30 8:e90d9ce42b33 67 left.speed(-SPEED);
jsterling30 8:e90d9ce42b33 68 while (leftEnc.read() < numTicks && rightEnc.read() < numTicks) {}
jsterling30 8:e90d9ce42b33 69 stop();
jsterling30 8:e90d9ce42b33 70 wait(0.5);
jsterling30 8:e90d9ce42b33 71 }
jsterling30 8:e90d9ce42b33 72
jsterling30 8:e90d9ce42b33 73 void turnLeft(double degrees) {
jsterling30 3:c4b0460d8886 74 leftEnc.reset();
jsterling30 3:c4b0460d8886 75 rightEnc.reset();
jsterling30 10:9066603296bd 76 right.speed(SPEED + 0.05);
jsterling30 10:9066603296bd 77 left.speed(-SPEED - 0.05);
jsterling30 8:e90d9ce42b33 78 double numTicks = degrees * TicksPerDeg;
jsterling30 8:e90d9ce42b33 79 while (leftEnc.read() < numTicks || rightEnc.read() < numTicks) {
jsterling30 8:e90d9ce42b33 80 if (leftEnc.read() >= numTicks) {
jsterling30 8:e90d9ce42b33 81 left.speed(0.0);
jsterling30 8:e90d9ce42b33 82 }
jsterling30 8:e90d9ce42b33 83 if (rightEnc.read() >= numTicks) {
jsterling30 8:e90d9ce42b33 84 right.speed(0.0);
jsterling30 8:e90d9ce42b33 85 }
jsterling30 8:e90d9ce42b33 86 }
jsterling30 2:8c27831ce9a2 87 stop();
jsterling30 8:e90d9ce42b33 88 wait(0.5);
jsterling30 3:c4b0460d8886 89 }
jsterling30 3:c4b0460d8886 90
jsterling30 8:e90d9ce42b33 91 void turnRight(double degrees) {
amohile3 4:a21d15629407 92 leftEnc.reset();
amohile3 4:a21d15629407 93 rightEnc.reset();
jsterling30 10:9066603296bd 94 right.speed(-SPEED - 0.05);
jsterling30 10:9066603296bd 95 left.speed(SPEED + 0.05);
jsterling30 8:e90d9ce42b33 96 double numTicks = degrees * TicksPerDeg;
amohile3 4:a21d15629407 97 while (leftEnc.read() < numTicks && rightEnc.read() < numTicks) {}
amohile3 4:a21d15629407 98 stop();
jsterling30 8:e90d9ce42b33 99 wait(0.5);
jsterling30 8:e90d9ce42b33 100 }
jsterling30 8:e90d9ce42b33 101
jsterling30 8:e90d9ce42b33 102 void turn(double delta) {
jsterling30 8:e90d9ce42b33 103 penUp();
jsterling30 8:e90d9ce42b33 104 forward(1.75);
jsterling30 8:e90d9ce42b33 105 if (delta > 0) {
jsterling30 8:e90d9ce42b33 106 turnLeft(delta);
jsterling30 8:e90d9ce42b33 107 } else {
jsterling30 8:e90d9ce42b33 108 turnRight(-delta);
jsterling30 8:e90d9ce42b33 109 }
jsterling30 8:e90d9ce42b33 110 reverse(1.75);
jsterling30 8:e90d9ce42b33 111 penDown();
amohile3 4:a21d15629407 112 }
amohile3 4:a21d15629407 113
amohile3 4:a21d15629407 114 void makeCircle() {
jsterling30 10:9066603296bd 115 penDown();
amohile3 4:a21d15629407 116 leftEnc.reset();
amohile3 4:a21d15629407 117 rightEnc.reset();
jsterling30 10:9066603296bd 118 right.speed(0.4);
jsterling30 8:e90d9ce42b33 119 float numTicks = 2 * 360 * TicksPerDeg;
amohile3 5:7572f73a78f3 120 while (rightEnc.read() < numTicks) {}
amohile3 4:a21d15629407 121 stop();
amohile3 4:a21d15629407 122 wait(0.1);
amohile3 4:a21d15629407 123 }
amohile3 4:a21d15629407 124
amohile3 9:aa739588a86b 125 void makeSquare(int sideLength) {
jsterling30 10:9066603296bd 126 penDown();
amohile3 9:aa739588a86b 127 forward(sideLength);
amohile3 9:aa739588a86b 128 turn(90);
amohile3 9:aa739588a86b 129 forward(sideLength);
amohile3 9:aa739588a86b 130 turn(90);
amohile3 9:aa739588a86b 131 forward(sideLength);
amohile3 9:aa739588a86b 132 turn(90);
amohile3 9:aa739588a86b 133 forward(sideLength);
amohile3 9:aa739588a86b 134 turn(90);
amohile3 9:aa739588a86b 135 }
amohile3 9:aa739588a86b 136
amohile3 9:aa739588a86b 137 void makeTriangle(int sideLength) {
jsterling30 10:9066603296bd 138 penDown();
amohile3 9:aa739588a86b 139 forward(sideLength);
amohile3 9:aa739588a86b 140 turn(120);
amohile3 9:aa739588a86b 141 forward(sideLength);
amohile3 9:aa739588a86b 142 turn(120);
amohile3 9:aa739588a86b 143 forward(sideLength);
amohile3 9:aa739588a86b 144 turn(120);
amohile3 9:aa739588a86b 145 }
amohile3 9:aa739588a86b 146
amohile3 9:aa739588a86b 147 void makeHexagon(int sideLength) {
jsterling30 10:9066603296bd 148 penDown();
amohile3 9:aa739588a86b 149 forward(sideLength);
amohile3 9:aa739588a86b 150 turn(60);
amohile3 9:aa739588a86b 151 forward(sideLength);
amohile3 9:aa739588a86b 152 turn(60);
amohile3 9:aa739588a86b 153 forward(sideLength);
amohile3 9:aa739588a86b 154 turn(60);
amohile3 9:aa739588a86b 155 forward(sideLength);
amohile3 9:aa739588a86b 156 turn(60);
amohile3 9:aa739588a86b 157 forward(sideLength);
amohile3 9:aa739588a86b 158 turn(60);
amohile3 9:aa739588a86b 159 forward(sideLength);
amohile3 9:aa739588a86b 160 turn(60);
amohile3 9:aa739588a86b 161 }
amohile3 9:aa739588a86b 162
amohile3 12:b107ba24faa4 163 void makePattern(string shape, int sideLength){
amohile3 12:b107ba24faa4 164 if (shape.compare("triangle") == 0) {
amohile3 12:b107ba24faa4 165 makeTriangle(sideLength);
amohile3 12:b107ba24faa4 166 turn(120);
amohile3 12:b107ba24faa4 167 makeTriangle(sideLength);
amohile3 12:b107ba24faa4 168 turn(120);
amohile3 12:b107ba24faa4 169 makeTriangle(sideLength);
amohile3 12:b107ba24faa4 170 turn(120);
amohile3 12:b107ba24faa4 171 } else if (shape.compare("circle") == 0) {
amohile3 12:b107ba24faa4 172 makeCircle(sideLength);
amohile3 12:b107ba24faa4 173 turn(120);
amohile3 12:b107ba24faa4 174 makeCircle(sideLength);
amohile3 12:b107ba24faa4 175 turn(120);
amohile3 12:b107ba24faa4 176 makeCircle(sideLength);
amohile3 12:b107ba24faa4 177 turn(120);
amohile3 12:b107ba24faa4 178 } else if (shape.compare("hexagon") == 0) {
amohile3 12:b107ba24faa4 179 makeHexagon(sideLength);
amohile3 12:b107ba24faa4 180 turn(120);
amohile3 12:b107ba24faa4 181 makeHexagon(sideLength);
amohile3 12:b107ba24faa4 182 turn(120);
amohile3 12:b107ba24faa4 183 makeHexagon(sideLength);
amohile3 12:b107ba24faa4 184 turn(120);
amohile3 12:b107ba24faa4 185 } else if (shape.compare("square") == 0) {
amohile3 12:b107ba24faa4 186 makeSquare(sideLength);
amohile3 12:b107ba24faa4 187 turn(120);
amohile3 12:b107ba24faa4 188 makeSquare(sideLength);
amohile3 12:b107ba24faa4 189 turn(120);
amohile3 12:b107ba24faa4 190 makeSquare(sideLength);
amohile3 12:b107ba24faa4 191 turn(120);
amohile3 12:b107ba24faa4 192 }
amohile3 12:b107ba24faa4 193 }
amohile3 12:b107ba24faa4 194
jsterling30 3:c4b0460d8886 195 void manualCheck() {
jsterling30 3:c4b0460d8886 196 leftEnc.reset();
jsterling30 3:c4b0460d8886 197 rightEnc.reset();
jsterling30 3:c4b0460d8886 198 while (1) {
jsterling30 3:c4b0460d8886 199 printf("Left Ticks: %d\r\n", leftEnc.read());
jsterling30 3:c4b0460d8886 200 printf("Right Ticks: %d\r\n\r\n", rightEnc.read());
jsterling30 3:c4b0460d8886 201 wait(0.5);
jsterling30 3:c4b0460d8886 202 }
jsterling30 2:8c27831ce9a2 203 }
amohile3 0:191d0be60c80 204
jsterling30 7:6ef9b0aab972 205 // command character array for bluetooth parsing
jsterling30 7:6ef9b0aab972 206 char command[100];
jsterling30 7:6ef9b0aab972 207
jsterling30 7:6ef9b0aab972 208 void blue_interrupt() {
jsterling30 7:6ef9b0aab972 209 // bluetooth receiver interrupt
jsterling30 7:6ef9b0aab972 210 char letter = (char) blue.getc();
jsterling30 7:6ef9b0aab972 211 command[strlen(command)] = letter;
jsterling30 7:6ef9b0aab972 212 }
jsterling30 7:6ef9b0aab972 213
jsterling30 7:6ef9b0aab972 214 void get_command() {
jsterling30 7:6ef9b0aab972 215 memset(command, '\0', sizeof(command)); // resetting command buffer
jsterling30 7:6ef9b0aab972 216 while (!strlen(command)) { // waiting for full command to be read
jsterling30 7:6ef9b0aab972 217 led4 = !led4;
jsterling30 7:6ef9b0aab972 218 wait(1);
jsterling30 7:6ef9b0aab972 219 }
jsterling30 7:6ef9b0aab972 220 }
jsterling30 7:6ef9b0aab972 221
jsterling30 8:e90d9ce42b33 222 void drawLetter(Letter letter) {
jsterling30 8:e90d9ce42b33 223 double currAngle = 0;
jsterling30 8:e90d9ce42b33 224 for (int i = 1; i < sizeof(letter.points); i++) {
jsterling30 8:e90d9ce42b33 225 Transition prev = letter.points[i - 1];
jsterling30 8:e90d9ce42b33 226 Transition next = letter.points[i];
jsterling30 8:e90d9ce42b33 227 double desAngle = atan2(next.y - prev.y, next.x - prev.x) * 180 / PI;
jsterling30 8:e90d9ce42b33 228 double theta = desAngle - currAngle;
jsterling30 8:e90d9ce42b33 229 if (theta > 0) {
jsterling30 8:e90d9ce42b33 230 turnLeft(theta);
jsterling30 8:e90d9ce42b33 231 } else {
jsterling30 8:e90d9ce42b33 232 turnRight(-theta);
jsterling30 8:e90d9ce42b33 233 }
jsterling30 8:e90d9ce42b33 234 currAngle = desAngle;
jsterling30 8:e90d9ce42b33 235 double distance = sqrt(pow(next.y - prev.y, 2) + pow(next.x - prev.x, 2));
jsterling30 8:e90d9ce42b33 236 forward(distance);
jsterling30 8:e90d9ce42b33 237 }
jsterling30 8:e90d9ce42b33 238 }
jsterling30 8:e90d9ce42b33 239
amohile3 0:191d0be60c80 240 int main() {
jsterling30 7:6ef9b0aab972 241
jsterling30 8:e90d9ce42b33 242 penDown();
jsterling30 8:e90d9ce42b33 243
jsterling30 7:6ef9b0aab972 244 blue.baud(9600);
jsterling30 7:6ef9b0aab972 245 blue.attach(&blue_interrupt);
jsterling30 7:6ef9b0aab972 246
jsterling30 7:6ef9b0aab972 247 while (1) {
jsterling30 7:6ef9b0aab972 248 get_command();
jsterling30 7:6ef9b0aab972 249
jsterling30 7:6ef9b0aab972 250 const char *delimiter = " "; // space character to separate arguments
jsterling30 7:6ef9b0aab972 251
jsterling30 10:9066603296bd 252 char *arg1 = strtok(command, delimiter);
jsterling30 10:9066603296bd 253 char *arg2 = strtok(NULL, delimiter);
jsterling30 10:9066603296bd 254 char *arg3 = strtok(NULL, delimiter);
jsterling30 7:6ef9b0aab972 255
jsterling30 7:6ef9b0aab972 256 if (strcmp(arg1, "draw") == 0) {
jsterling30 7:6ef9b0aab972 257 // first argument is draw
jsterling30 7:6ef9b0aab972 258
jsterling30 7:6ef9b0aab972 259 if (strcmp(arg2, "square") == 0) {
jsterling30 7:6ef9b0aab972 260 // second argument is square
jsterling30 10:9066603296bd 261 int sideDistance = 3;
jsterling30 10:9066603296bd 262 if (arg3 != NULL) {
jsterling30 10:9066603296bd 263 sideDistance = arg3[0] - '0';
jsterling30 10:9066603296bd 264 }
jsterling30 10:9066603296bd 265 blue.printf("Drawing Square with side length = %d\n", sideDistance);
jsterling30 10:9066603296bd 266 makeSquare(sideDistance);
jsterling30 7:6ef9b0aab972 267 } else if (strcmp(arg2, "circle") == 0) {
jsterling30 7:6ef9b0aab972 268 // second argument is circle
jsterling30 10:9066603296bd 269 blue.printf("Drawing Circle\n");
jsterling30 10:9066603296bd 270 makeCircle();
jsterling30 10:9066603296bd 271 } else if (strcmp(arg2, "triangle") == 0) {
jsterling30 10:9066603296bd 272 // second argument is triangle
jsterling30 10:9066603296bd 273 int sideDistance = 3;
jsterling30 10:9066603296bd 274 if (arg3 != NULL) {
jsterling30 10:9066603296bd 275 sideDistance = arg3[0] - '0';
jsterling30 10:9066603296bd 276 }
jsterling30 10:9066603296bd 277 blue.printf("Drawing Triangle with side length = %d\n", sideDistance);
jsterling30 10:9066603296bd 278 makeTriangle(sideDistance);
jsterling30 10:9066603296bd 279 } else if (strcmp(arg2, "hexagon") == 0) {
jsterling30 10:9066603296bd 280 // second argument is hexagon
jsterling30 10:9066603296bd 281 int sideDistance = 3;
jsterling30 10:9066603296bd 282 if (arg3 != NULL) {
jsterling30 10:9066603296bd 283 sideDistance = arg3[0] - '0';
jsterling30 10:9066603296bd 284 }
jsterling30 10:9066603296bd 285 blue.printf("Drawing Hexagon with side length = %d\n", sideDistance);
jsterling30 10:9066603296bd 286 makeHexagon(sideDistance);
jsterling30 7:6ef9b0aab972 287 } else {
jsterling30 7:6ef9b0aab972 288 // second argument is not recognized
jsterling30 10:9066603296bd 289 blue.printf("Second argument is not recognized (must be square, circle, triangle, hexagon)\n");
jsterling30 7:6ef9b0aab972 290 }
jsterling30 7:6ef9b0aab972 291
jsterling30 7:6ef9b0aab972 292 } else if (strcmp(arg1, "write") == 0) {
jsterling30 7:6ef9b0aab972 293 // first argument is write
jsterling30 10:9066603296bd 294 blue.printf("First argument is write\n");
jsterling30 10:9066603296bd 295 blue.printf("Second argument is %s\n", arg2);
jsterling30 7:6ef9b0aab972 296 } else {
jsterling30 7:6ef9b0aab972 297 // first argument is not recognized
jsterling30 10:9066603296bd 298 blue.printf("ERROR: First argument is not recognized (must be draw or write)\n");
jsterling30 7:6ef9b0aab972 299 }
jsterling30 7:6ef9b0aab972 300
jsterling30 7:6ef9b0aab972 301 blue.printf("\n");
jsterling30 7:6ef9b0aab972 302 }
amohile3 0:191d0be60c80 303 }