ECE 4180 Drawing Robot / Mbed 2 deprecated ECE_4180_Drawing_Robot

Basic motor code

Dependencies:   Motordriver mbed HALLFX_ENCODER Servo

main.cpp@5:7572f73a78f3, 2018-04-21 (annotated)

Committer:
amohile3
Date:
Sat Apr 21 20:09:35 2018 +0000
Revision:
5:7572f73a78f3
Parent:
4:a21d15629407
Child:
6:235548599e79
Added Bluetooth SerialPassThrough code

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 2:8c27831ce9a2 4
jsterling30 2:8c27831ce9a2 5 #define SPEED 0.2
jsterling30 3:c4b0460d8886 6 #define TICKSPERREV 390
jsterling30 2:8c27831ce9a2 7 #define DISTPERREV 8.25 // 8.25 inches per revolution
jsterling30 3:c4b0460d8886 8 #define TicksPerDeg 2.756
amohile3 0:191d0be60c80 9
jsterling30 2:8c27831ce9a2 10 Motor right(p23,p6,p5, 1); // pwm, fwd, rev
amohile3 0:191d0be60c80 11 Motor left(p21, p7, p8, 1); // pwm, fwd, rev
amohile3 0:191d0be60c80 12
jsterling30 2:8c27831ce9a2 13 HALLFX_ENCODER leftEnc(p15);
jsterling30 2:8c27831ce9a2 14 HALLFX_ENCODER rightEnc(p16);
jsterling30 2:8c27831ce9a2 15
amohile3 5:7572f73a78f3 16 RawSerial pc(USBTX, USBRX);
amohile3 5:7572f73a78f3 17 RawSerial dev(p28,p27);
amohile3 5:7572f73a78f3 18 DigitalOut led1(LED1);
amohile3 5:7572f73a78f3 19 DigitalOut led4(LED4);
amohile3 5:7572f73a78f3 20
amohile3 5:7572f73a78f3 21 void dev_recv()
amohile3 5:7572f73a78f3 22 {
amohile3 5:7572f73a78f3 23 led1 = !led1;
amohile3 5:7572f73a78f3 24 while(dev.readable()) {
amohile3 5:7572f73a78f3 25 pc.putc(dev.getc());
amohile3 5:7572f73a78f3 26 }
amohile3 5:7572f73a78f3 27 }
amohile3 5:7572f73a78f3 28
amohile3 5:7572f73a78f3 29 void pc_recv()
amohile3 5:7572f73a78f3 30 {
amohile3 5:7572f73a78f3 31 led4 = !led4;
amohile3 5:7572f73a78f3 32 while(pc.readable()) {
amohile3 5:7572f73a78f3 33 dev.putc(pc.getc());
amohile3 5:7572f73a78f3 34 }
amohile3 5:7572f73a78f3 35 }
amohile3 5:7572f73a78f3 36
jsterling30 2:8c27831ce9a2 37 void stop() {
jsterling30 2:8c27831ce9a2 38 right.speed(0.0);
jsterling30 2:8c27831ce9a2 39 left.speed(0.0);
jsterling30 2:8c27831ce9a2 40 }
jsterling30 2:8c27831ce9a2 41
jsterling30 2:8c27831ce9a2 42 void forward(float distance) {
jsterling30 2:8c27831ce9a2 43 float numRevs = distance / DISTPERREV;
jsterling30 2:8c27831ce9a2 44 float numTicks = numRevs * TICKSPERREV;
jsterling30 2:8c27831ce9a2 45 leftEnc.reset();
jsterling30 2:8c27831ce9a2 46 rightEnc.reset();
jsterling30 2:8c27831ce9a2 47 printf("Left Ticks Start: %d\r\n", leftEnc.read());
jsterling30 2:8c27831ce9a2 48 printf("Right Ticks Start: %d\r\n", rightEnc.read());
jsterling30 2:8c27831ce9a2 49 right.speed(SPEED);
jsterling30 2:8c27831ce9a2 50 left.speed(SPEED);
jsterling30 3:c4b0460d8886 51 while (leftEnc.read() < numTicks && rightEnc.read() < numTicks) {}
jsterling30 3:c4b0460d8886 52 stop();
jsterling30 3:c4b0460d8886 53 wait(0.1);
jsterling30 2:8c27831ce9a2 54 printf("Left Ticks End: %d\r\n", leftEnc.read());
jsterling30 3:c4b0460d8886 55 printf("Right Ticks End: %d\r\n", rightEnc.read());
jsterling30 3:c4b0460d8886 56 }
jsterling30 3:c4b0460d8886 57
jsterling30 3:c4b0460d8886 58 void turnLeft(int degrees) {
jsterling30 3:c4b0460d8886 59 leftEnc.reset();
jsterling30 3:c4b0460d8886 60 rightEnc.reset();
jsterling30 3:c4b0460d8886 61 right.speed(SPEED);
jsterling30 3:c4b0460d8886 62 left.speed(-SPEED);
jsterling30 3:c4b0460d8886 63 float numTicks = degrees * TicksPerDeg;
jsterling30 3:c4b0460d8886 64 while (leftEnc.read() < numTicks && rightEnc.read() < numTicks) {}
jsterling30 2:8c27831ce9a2 65 stop();
jsterling30 3:c4b0460d8886 66 wait(0.1);
jsterling30 3:c4b0460d8886 67 }
jsterling30 3:c4b0460d8886 68
amohile3 4:a21d15629407 69 void turnRight(int degrees) {
amohile3 4:a21d15629407 70 leftEnc.reset();
amohile3 4:a21d15629407 71 rightEnc.reset();
amohile3 4:a21d15629407 72 right.speed(-SPEED);
amohile3 4:a21d15629407 73 left.speed(SPEED);
amohile3 4:a21d15629407 74 float numTicks = degrees * TicksPerDeg;
amohile3 4:a21d15629407 75 while (leftEnc.read() < numTicks && rightEnc.read() < numTicks) {}
amohile3 4:a21d15629407 76 stop();
amohile3 4:a21d15629407 77 wait(0.1);
amohile3 4:a21d15629407 78 }
amohile3 4:a21d15629407 79
amohile3 4:a21d15629407 80 void makeCircle() {
amohile3 4:a21d15629407 81 leftEnc.reset();
amohile3 4:a21d15629407 82 rightEnc.reset();
amohile3 4:a21d15629407 83 right.speed(SPEED);
amohile3 4:a21d15629407 84 float numTicks = 360 * TicksPerDeg;
amohile3 5:7572f73a78f3 85 while (rightEnc.read() < numTicks) {}
amohile3 4:a21d15629407 86 stop();
amohile3 4:a21d15629407 87 wait(0.1);
amohile3 4:a21d15629407 88 }
amohile3 4:a21d15629407 89
jsterling30 3:c4b0460d8886 90 void manualCheck() {
jsterling30 3:c4b0460d8886 91 leftEnc.reset();
jsterling30 3:c4b0460d8886 92 rightEnc.reset();
jsterling30 3:c4b0460d8886 93 while (1) {
jsterling30 3:c4b0460d8886 94 printf("Left Ticks: %d\r\n", leftEnc.read());
jsterling30 3:c4b0460d8886 95 printf("Right Ticks: %d\r\n\r\n", rightEnc.read());
jsterling30 3:c4b0460d8886 96 wait(0.5);
jsterling30 3:c4b0460d8886 97 }
jsterling30 2:8c27831ce9a2 98 }
amohile3 0:191d0be60c80 99
amohile3 0:191d0be60c80 100 int main() {
amohile3 5:7572f73a78f3 101 pc.baud(9600);
amohile3 5:7572f73a78f3 102 dev.baud(9600);
amohile3 5:7572f73a78f3 103
amohile3 5:7572f73a78f3 104 pc.attach(&pc_recv, Serial::RxIrq);
amohile3 5:7572f73a78f3 105 dev.attach(&dev_recv, Serial::RxIrq);
amohile3 5:7572f73a78f3 106
amohile3 5:7572f73a78f3 107 while(1) {
amohile3 5:7572f73a78f3 108 sleep();
amohile3 5:7572f73a78f3 109 }
jsterling30 3:c4b0460d8886 110 //forward(5.0);
jsterling30 3:c4b0460d8886 111 //turnLeft(90);
amohile3 5:7572f73a78f3 112 //Should make a triangle
amohile3 5:7572f73a78f3 113 //for (int i = 0; i < 3; i++){
amohile3 5:7572f73a78f3 114 // forward(5.0);
amohile3 5:7572f73a78f3 115 // turnLeft(120);
amohile3 5:7572f73a78f3 116 // }
jsterling30 3:c4b0460d8886 117 //manualcheck();
jsterling30 3:c4b0460d8886 118 //forward(10.0);
amohile3 0:191d0be60c80 119 }