ECE 4180 Drawing Robot / Mbed 2 deprecated ECE_4180_Drawing_Robot

Basic motor code

Dependencies:   Motordriver mbed HALLFX_ENCODER Servo

main.cpp@7:6ef9b0aab972, 2018-04-30 (annotated)

Committer:
jsterling30
Date:
Mon Apr 30 20:01:22 2018 +0000
Revision:
7:6ef9b0aab972
Parent:
6:235548599e79
Child:
8:e90d9ce42b33
bluetooth instructions shell

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 2:8c27831ce9a2 5
jsterling30 7:6ef9b0aab972 6 #include <string.h>
jsterling30 6:235548599e79 7
jsterling30 2:8c27831ce9a2 8 #define SPEED 0.2
jsterling30 3:c4b0460d8886 9 #define TICKSPERREV 390
jsterling30 7:6ef9b0aab972 10 #define DISTPERREV 8.25 // 8.25 inches per revolution
jsterling30 3:c4b0460d8886 11 #define TicksPerDeg 2.756
amohile3 0:191d0be60c80 12
jsterling30 7:6ef9b0aab972 13 Motor right(p23, p6, p5, 1); // pwm, fwd, rev
jsterling30 7:6ef9b0aab972 14 Motor left(p21, p7, p8, 1); // pwm, fwd, rev
amohile3 0:191d0be60c80 15
jsterling30 2:8c27831ce9a2 16 HALLFX_ENCODER leftEnc(p15);
jsterling30 2:8c27831ce9a2 17 HALLFX_ENCODER rightEnc(p16);
jsterling30 2:8c27831ce9a2 18
jsterling30 7:6ef9b0aab972 19 Serial blue(p13,p14);
jsterling30 7:6ef9b0aab972 20 Serial pc(USBTX, USBRX);
amohile3 5:7572f73a78f3 21 DigitalOut led1(LED1);
amohile3 5:7572f73a78f3 22 DigitalOut led4(LED4);
amohile3 5:7572f73a78f3 23
jsterling30 7:6ef9b0aab972 24 Servo pen(p24);
jsterling30 6:235548599e79 25
jsterling30 2:8c27831ce9a2 26 void stop() {
jsterling30 2:8c27831ce9a2 27 right.speed(0.0);
jsterling30 2:8c27831ce9a2 28 left.speed(0.0);
jsterling30 2:8c27831ce9a2 29 }
jsterling30 2:8c27831ce9a2 30
jsterling30 2:8c27831ce9a2 31 void forward(float distance) {
jsterling30 2:8c27831ce9a2 32 float numRevs = distance / DISTPERREV;
jsterling30 2:8c27831ce9a2 33 float numTicks = numRevs * TICKSPERREV;
jsterling30 2:8c27831ce9a2 34 leftEnc.reset();
jsterling30 2:8c27831ce9a2 35 rightEnc.reset();
jsterling30 2:8c27831ce9a2 36 printf("Left Ticks Start: %d\r\n", leftEnc.read());
jsterling30 2:8c27831ce9a2 37 printf("Right Ticks Start: %d\r\n", rightEnc.read());
jsterling30 2:8c27831ce9a2 38 right.speed(SPEED);
jsterling30 2:8c27831ce9a2 39 left.speed(SPEED);
jsterling30 3:c4b0460d8886 40 while (leftEnc.read() < numTicks && rightEnc.read() < numTicks) {}
jsterling30 3:c4b0460d8886 41 stop();
jsterling30 3:c4b0460d8886 42 wait(0.1);
jsterling30 2:8c27831ce9a2 43 printf("Left Ticks End: %d\r\n", leftEnc.read());
jsterling30 3:c4b0460d8886 44 printf("Right Ticks End: %d\r\n", rightEnc.read());
jsterling30 3:c4b0460d8886 45 }
jsterling30 3:c4b0460d8886 46
jsterling30 3:c4b0460d8886 47 void turnLeft(int degrees) {
jsterling30 3:c4b0460d8886 48 leftEnc.reset();
jsterling30 3:c4b0460d8886 49 rightEnc.reset();
jsterling30 3:c4b0460d8886 50 right.speed(SPEED);
jsterling30 3:c4b0460d8886 51 left.speed(-SPEED);
jsterling30 3:c4b0460d8886 52 float numTicks = degrees * TicksPerDeg;
jsterling30 3:c4b0460d8886 53 while (leftEnc.read() < numTicks && rightEnc.read() < numTicks) {}
jsterling30 2:8c27831ce9a2 54 stop();
jsterling30 3:c4b0460d8886 55 wait(0.1);
jsterling30 3:c4b0460d8886 56 }
jsterling30 3:c4b0460d8886 57
amohile3 4:a21d15629407 58 void turnRight(int degrees) {
amohile3 4:a21d15629407 59 leftEnc.reset();
amohile3 4:a21d15629407 60 rightEnc.reset();
amohile3 4:a21d15629407 61 right.speed(-SPEED);
amohile3 4:a21d15629407 62 left.speed(SPEED);
amohile3 4:a21d15629407 63 float numTicks = degrees * TicksPerDeg;
amohile3 4:a21d15629407 64 while (leftEnc.read() < numTicks && rightEnc.read() < numTicks) {}
amohile3 4:a21d15629407 65 stop();
amohile3 4:a21d15629407 66 wait(0.1);
amohile3 4:a21d15629407 67 }
amohile3 4:a21d15629407 68
amohile3 4:a21d15629407 69 void makeCircle() {
amohile3 4:a21d15629407 70 leftEnc.reset();
amohile3 4:a21d15629407 71 rightEnc.reset();
amohile3 4:a21d15629407 72 right.speed(SPEED);
amohile3 4:a21d15629407 73 float numTicks = 360 * TicksPerDeg;
amohile3 5:7572f73a78f3 74 while (rightEnc.read() < numTicks) {}
amohile3 4:a21d15629407 75 stop();
amohile3 4:a21d15629407 76 wait(0.1);
amohile3 4:a21d15629407 77 }
amohile3 4:a21d15629407 78
jsterling30 3:c4b0460d8886 79 void manualCheck() {
jsterling30 3:c4b0460d8886 80 leftEnc.reset();
jsterling30 3:c4b0460d8886 81 rightEnc.reset();
jsterling30 3:c4b0460d8886 82 while (1) {
jsterling30 3:c4b0460d8886 83 printf("Left Ticks: %d\r\n", leftEnc.read());
jsterling30 3:c4b0460d8886 84 printf("Right Ticks: %d\r\n\r\n", rightEnc.read());
jsterling30 3:c4b0460d8886 85 wait(0.5);
jsterling30 3:c4b0460d8886 86 }
jsterling30 2:8c27831ce9a2 87 }
amohile3 0:191d0be60c80 88
jsterling30 7:6ef9b0aab972 89 // command character array for bluetooth parsing
jsterling30 7:6ef9b0aab972 90 char command[100];
jsterling30 7:6ef9b0aab972 91
jsterling30 7:6ef9b0aab972 92 void blue_interrupt() {
jsterling30 7:6ef9b0aab972 93 // bluetooth receiver interrupt
jsterling30 7:6ef9b0aab972 94 char letter = (char) blue.getc();
jsterling30 7:6ef9b0aab972 95 command[strlen(command)] = letter;
jsterling30 7:6ef9b0aab972 96 }
jsterling30 7:6ef9b0aab972 97
jsterling30 7:6ef9b0aab972 98 void get_command() {
jsterling30 7:6ef9b0aab972 99 memset(command, '\0', sizeof(command)); // resetting command buffer
jsterling30 7:6ef9b0aab972 100 while (!strlen(command)) { // waiting for full command to be read
jsterling30 7:6ef9b0aab972 101 led4 = !led4;
jsterling30 7:6ef9b0aab972 102 wait(1);
jsterling30 7:6ef9b0aab972 103 }
jsterling30 7:6ef9b0aab972 104 }
jsterling30 7:6ef9b0aab972 105
amohile3 0:191d0be60c80 106 int main() {
jsterling30 7:6ef9b0aab972 107
jsterling30 7:6ef9b0aab972 108 blue.baud(9600);
jsterling30 7:6ef9b0aab972 109 blue.attach(&blue_interrupt);
jsterling30 7:6ef9b0aab972 110
jsterling30 7:6ef9b0aab972 111 while (1) {
jsterling30 7:6ef9b0aab972 112 get_command();
jsterling30 7:6ef9b0aab972 113 blue.printf("%s, len = %d\n", command, strlen(command));
jsterling30 7:6ef9b0aab972 114
jsterling30 7:6ef9b0aab972 115 const char *delimiter = " "; // space character to separate arguments
jsterling30 7:6ef9b0aab972 116 char *arg1;
jsterling30 7:6ef9b0aab972 117 char *arg2;
jsterling30 7:6ef9b0aab972 118
jsterling30 7:6ef9b0aab972 119 arg1 = strtok(command, delimiter);
jsterling30 7:6ef9b0aab972 120 arg2 = strtok(NULL, delimiter);
jsterling30 7:6ef9b0aab972 121
jsterling30 7:6ef9b0aab972 122 if (strcmp(arg1, "draw") == 0) {
jsterling30 7:6ef9b0aab972 123 // first argument is draw
jsterling30 7:6ef9b0aab972 124 blue.printf(" First argument is draw\n");
jsterling30 7:6ef9b0aab972 125
jsterling30 7:6ef9b0aab972 126 if (strcmp(arg2, "square") == 0) {
jsterling30 7:6ef9b0aab972 127 // second argument is square
jsterling30 7:6ef9b0aab972 128 blue.printf(" Second argument is square\n");
jsterling30 7:6ef9b0aab972 129 } else if (strcmp(arg2, "circle") == 0) {
jsterling30 7:6ef9b0aab972 130 // second argument is circle
jsterling30 7:6ef9b0aab972 131 blue.printf(" Second argument is circle\n");
jsterling30 7:6ef9b0aab972 132 } else {
jsterling30 7:6ef9b0aab972 133 // second argument is not recognized
jsterling30 7:6ef9b0aab972 134 blue.printf(" Second argument is not recognized (must be square or circle)\n");
jsterling30 7:6ef9b0aab972 135 }
jsterling30 7:6ef9b0aab972 136
jsterling30 7:6ef9b0aab972 137 } else if (strcmp(arg1, "write") == 0) {
jsterling30 7:6ef9b0aab972 138 // first argument is write
jsterling30 7:6ef9b0aab972 139 blue.printf(" First argument is write\n");
jsterling30 7:6ef9b0aab972 140 blue.printf(" Second argument is %s\n", arg2);
jsterling30 7:6ef9b0aab972 141 } else {
jsterling30 7:6ef9b0aab972 142 // first argument is not recognized
jsterling30 7:6ef9b0aab972 143 blue.printf(" First argument is not recognized (must be draw or write)\n");
jsterling30 7:6ef9b0aab972 144 }
jsterling30 7:6ef9b0aab972 145
jsterling30 7:6ef9b0aab972 146 blue.printf("\n");
jsterling30 7:6ef9b0aab972 147 }
jsterling30 7:6ef9b0aab972 148
jsterling30 7:6ef9b0aab972 149 /*
amohile3 5:7572f73a78f3 150 while(1) {
jsterling30 7:6ef9b0aab972 151 pen.calibrate(0.0005, 180.0);
jsterling30 7:6ef9b0aab972 152 for(float p=0; p<1.0; p += 0.1) {
jsterling30 7:6ef9b0aab972 153 pen.write(p);
jsterling30 7:6ef9b0aab972 154 wait(2);
jsterling30 7:6ef9b0aab972 155 }
amohile3 5:7572f73a78f3 156 }
jsterling30 7:6ef9b0aab972 157 */
jsterling30 7:6ef9b0aab972 158
jsterling30 7:6ef9b0aab972 159 //forward(5.0);
jsterling30 3:c4b0460d8886 160 //forward(5.0);
jsterling30 3:c4b0460d8886 161 //turnLeft(90);
amohile3 5:7572f73a78f3 162 //Should make a triangle
amohile3 5:7572f73a78f3 163 //for (int i = 0; i < 3; i++){
amohile3 5:7572f73a78f3 164 // forward(5.0);
amohile3 5:7572f73a78f3 165 // turnLeft(120);
amohile3 5:7572f73a78f3 166 // }
jsterling30 3:c4b0460d8886 167 //manualcheck();
jsterling30 3:c4b0460d8886 168 //forward(10.0);
amohile3 0:191d0be60c80 169 }