ECE 4180 Drawing Robot / Mbed 2 deprecated ECE_4180_Drawing_Robot

Basic motor code

Dependencies:   Motordriver mbed HALLFX_ENCODER Servo

main.cpp@8:e90d9ce42b33, 2018-05-01 (annotated)

Committer:
jsterling30
Date:
Tue May 01 15:07:14 2018 +0000
Revision:
8:e90d9ce42b33
Parent:
7:6ef9b0aab972
Child:
9:aa739588a86b
basic shapes

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 8:e90d9ce42b33 10 #define SPEED 0.32
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 3:c4b0460d8886 76 right.speed(SPEED);
jsterling30 3:c4b0460d8886 77 left.speed(-SPEED);
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 blue.printf("Left Turn\n Left Encoder: %d\n Right Encoder: %d\n\n", leftEnc.read(), rightEnc.read());
jsterling30 8:e90d9ce42b33 89 wait(0.5);
jsterling30 3:c4b0460d8886 90 }
jsterling30 3:c4b0460d8886 91
jsterling30 8:e90d9ce42b33 92 void turnRight(double degrees) {
amohile3 4:a21d15629407 93 leftEnc.reset();
amohile3 4:a21d15629407 94 rightEnc.reset();
amohile3 4:a21d15629407 95 right.speed(-SPEED);
amohile3 4:a21d15629407 96 left.speed(SPEED);
jsterling30 8:e90d9ce42b33 97 double numTicks = degrees * TicksPerDeg;
amohile3 4:a21d15629407 98 while (leftEnc.read() < numTicks && rightEnc.read() < numTicks) {}
amohile3 4:a21d15629407 99 stop();
jsterling30 8:e90d9ce42b33 100 blue.printf("Right Turn\n Left Encoder: %d\n Right Encoder: %d\n\n", leftEnc.read(), rightEnc.read());
jsterling30 8:e90d9ce42b33 101 wait(0.5);
jsterling30 8:e90d9ce42b33 102 }
jsterling30 8:e90d9ce42b33 103
jsterling30 8:e90d9ce42b33 104 void turn(double delta) {
jsterling30 8:e90d9ce42b33 105 penUp();
jsterling30 8:e90d9ce42b33 106 forward(1.75);
jsterling30 8:e90d9ce42b33 107 if (delta > 0) {
jsterling30 8:e90d9ce42b33 108 turnLeft(delta);
jsterling30 8:e90d9ce42b33 109 } else {
jsterling30 8:e90d9ce42b33 110 turnRight(-delta);
jsterling30 8:e90d9ce42b33 111 }
jsterling30 8:e90d9ce42b33 112 reverse(1.75);
jsterling30 8:e90d9ce42b33 113 penDown();
amohile3 4:a21d15629407 114 }
amohile3 4:a21d15629407 115
amohile3 4:a21d15629407 116 void makeCircle() {
amohile3 4:a21d15629407 117 leftEnc.reset();
amohile3 4:a21d15629407 118 rightEnc.reset();
amohile3 4:a21d15629407 119 right.speed(SPEED);
jsterling30 8:e90d9ce42b33 120 float numTicks = 2 * 360 * TicksPerDeg;
amohile3 5:7572f73a78f3 121 while (rightEnc.read() < numTicks) {}
amohile3 4:a21d15629407 122 stop();
amohile3 4:a21d15629407 123 wait(0.1);
amohile3 4:a21d15629407 124 }
amohile3 4:a21d15629407 125
jsterling30 3:c4b0460d8886 126 void manualCheck() {
jsterling30 3:c4b0460d8886 127 leftEnc.reset();
jsterling30 3:c4b0460d8886 128 rightEnc.reset();
jsterling30 3:c4b0460d8886 129 while (1) {
jsterling30 3:c4b0460d8886 130 printf("Left Ticks: %d\r\n", leftEnc.read());
jsterling30 3:c4b0460d8886 131 printf("Right Ticks: %d\r\n\r\n", rightEnc.read());
jsterling30 3:c4b0460d8886 132 wait(0.5);
jsterling30 3:c4b0460d8886 133 }
jsterling30 2:8c27831ce9a2 134 }
amohile3 0:191d0be60c80 135
jsterling30 7:6ef9b0aab972 136 // command character array for bluetooth parsing
jsterling30 7:6ef9b0aab972 137 char command[100];
jsterling30 7:6ef9b0aab972 138
jsterling30 7:6ef9b0aab972 139 void blue_interrupt() {
jsterling30 7:6ef9b0aab972 140 // bluetooth receiver interrupt
jsterling30 7:6ef9b0aab972 141 char letter = (char) blue.getc();
jsterling30 7:6ef9b0aab972 142 command[strlen(command)] = letter;
jsterling30 7:6ef9b0aab972 143 }
jsterling30 7:6ef9b0aab972 144
jsterling30 7:6ef9b0aab972 145 void get_command() {
jsterling30 7:6ef9b0aab972 146 memset(command, '\0', sizeof(command)); // resetting command buffer
jsterling30 7:6ef9b0aab972 147 while (!strlen(command)) { // waiting for full command to be read
jsterling30 7:6ef9b0aab972 148 led4 = !led4;
jsterling30 7:6ef9b0aab972 149 wait(1);
jsterling30 7:6ef9b0aab972 150 }
jsterling30 7:6ef9b0aab972 151 }
jsterling30 7:6ef9b0aab972 152
jsterling30 8:e90d9ce42b33 153 void drawLetter(Letter letter) {
jsterling30 8:e90d9ce42b33 154 double currAngle = 0;
jsterling30 8:e90d9ce42b33 155 for (int i = 1; i < sizeof(letter.points); i++) {
jsterling30 8:e90d9ce42b33 156 Transition prev = letter.points[i - 1];
jsterling30 8:e90d9ce42b33 157 Transition next = letter.points[i];
jsterling30 8:e90d9ce42b33 158 double desAngle = atan2(next.y - prev.y, next.x - prev.x) * 180 / PI;
jsterling30 8:e90d9ce42b33 159 double theta = desAngle - currAngle;
jsterling30 8:e90d9ce42b33 160 if (theta > 0) {
jsterling30 8:e90d9ce42b33 161 turnLeft(theta);
jsterling30 8:e90d9ce42b33 162 } else {
jsterling30 8:e90d9ce42b33 163 turnRight(-theta);
jsterling30 8:e90d9ce42b33 164 }
jsterling30 8:e90d9ce42b33 165 currAngle = desAngle;
jsterling30 8:e90d9ce42b33 166 double distance = sqrt(pow(next.y - prev.y, 2) + pow(next.x - prev.x, 2));
jsterling30 8:e90d9ce42b33 167 forward(distance);
jsterling30 8:e90d9ce42b33 168 }
jsterling30 8:e90d9ce42b33 169 }
jsterling30 8:e90d9ce42b33 170
amohile3 0:191d0be60c80 171 int main() {
jsterling30 7:6ef9b0aab972 172
jsterling30 8:e90d9ce42b33 173 penDown();
jsterling30 8:e90d9ce42b33 174 //makeCircle();
jsterling30 8:e90d9ce42b33 175
jsterling30 8:e90d9ce42b33 176 forward(5.0);
jsterling30 8:e90d9ce42b33 177 turn(120);
jsterling30 8:e90d9ce42b33 178 forward(5.0);
jsterling30 8:e90d9ce42b33 179 turn(120);
jsterling30 8:e90d9ce42b33 180 forward(5.0);
jsterling30 8:e90d9ce42b33 181 turn(120);
jsterling30 8:e90d9ce42b33 182 //forward(5.0);
jsterling30 8:e90d9ce42b33 183 //turn(90);
jsterling30 8:e90d9ce42b33 184
jsterling30 8:e90d9ce42b33 185 //drawLetter(let_a);
jsterling30 8:e90d9ce42b33 186 /*
jsterling30 7:6ef9b0aab972 187 blue.baud(9600);
jsterling30 7:6ef9b0aab972 188 blue.attach(&blue_interrupt);
jsterling30 7:6ef9b0aab972 189
jsterling30 7:6ef9b0aab972 190 while (1) {
jsterling30 7:6ef9b0aab972 191 get_command();
jsterling30 7:6ef9b0aab972 192 blue.printf("%s, len = %d\n", command, strlen(command));
jsterling30 7:6ef9b0aab972 193
jsterling30 7:6ef9b0aab972 194 const char *delimiter = " "; // space character to separate arguments
jsterling30 7:6ef9b0aab972 195 char *arg1;
jsterling30 7:6ef9b0aab972 196 char *arg2;
jsterling30 7:6ef9b0aab972 197
jsterling30 7:6ef9b0aab972 198 arg1 = strtok(command, delimiter);
jsterling30 7:6ef9b0aab972 199 arg2 = strtok(NULL, delimiter);
jsterling30 7:6ef9b0aab972 200
jsterling30 7:6ef9b0aab972 201 if (strcmp(arg1, "draw") == 0) {
jsterling30 7:6ef9b0aab972 202 // first argument is draw
jsterling30 7:6ef9b0aab972 203 blue.printf(" First argument is draw\n");
jsterling30 7:6ef9b0aab972 204
jsterling30 7:6ef9b0aab972 205 if (strcmp(arg2, "square") == 0) {
jsterling30 7:6ef9b0aab972 206 // second argument is square
jsterling30 7:6ef9b0aab972 207 blue.printf(" Second argument is square\n");
jsterling30 7:6ef9b0aab972 208 } else if (strcmp(arg2, "circle") == 0) {
jsterling30 7:6ef9b0aab972 209 // second argument is circle
jsterling30 7:6ef9b0aab972 210 blue.printf(" Second argument is circle\n");
jsterling30 7:6ef9b0aab972 211 } else {
jsterling30 7:6ef9b0aab972 212 // second argument is not recognized
jsterling30 7:6ef9b0aab972 213 blue.printf(" Second argument is not recognized (must be square or circle)\n");
jsterling30 7:6ef9b0aab972 214 }
jsterling30 7:6ef9b0aab972 215
jsterling30 7:6ef9b0aab972 216 } else if (strcmp(arg1, "write") == 0) {
jsterling30 7:6ef9b0aab972 217 // first argument is write
jsterling30 7:6ef9b0aab972 218 blue.printf(" First argument is write\n");
jsterling30 7:6ef9b0aab972 219 blue.printf(" Second argument is %s\n", arg2);
jsterling30 7:6ef9b0aab972 220 } else {
jsterling30 7:6ef9b0aab972 221 // first argument is not recognized
jsterling30 7:6ef9b0aab972 222 blue.printf(" First argument is not recognized (must be draw or write)\n");
jsterling30 7:6ef9b0aab972 223 }
jsterling30 7:6ef9b0aab972 224
jsterling30 7:6ef9b0aab972 225 blue.printf("\n");
jsterling30 7:6ef9b0aab972 226 }
jsterling30 8:e90d9ce42b33 227 */
jsterling30 7:6ef9b0aab972 228
jsterling30 7:6ef9b0aab972 229 /*
amohile3 5:7572f73a78f3 230 while(1) {
jsterling30 7:6ef9b0aab972 231 pen.calibrate(0.0005, 180.0);
jsterling30 7:6ef9b0aab972 232 for(float p=0; p<1.0; p += 0.1) {
jsterling30 7:6ef9b0aab972 233 pen.write(p);
jsterling30 7:6ef9b0aab972 234 wait(2);
jsterling30 7:6ef9b0aab972 235 }
amohile3 5:7572f73a78f3 236 }
jsterling30 7:6ef9b0aab972 237 */
jsterling30 7:6ef9b0aab972 238
jsterling30 7:6ef9b0aab972 239 //forward(5.0);
jsterling30 3:c4b0460d8886 240 //forward(5.0);
jsterling30 3:c4b0460d8886 241 //turnLeft(90);
amohile3 5:7572f73a78f3 242 //Should make a triangle
amohile3 5:7572f73a78f3 243 //for (int i = 0; i < 3; i++){
jsterling30 8:e90d9ce42b33 244 // forward(5.0);
jsterling30 8:e90d9ce42b33 245 // turnLeft(120);
jsterling30 8:e90d9ce42b33 246 //}
jsterling30 3:c4b0460d8886 247 //manualcheck();
jsterling30 3:c4b0460d8886 248 //forward(10.0);
amohile3 0:191d0be60c80 249 }