Basic motor code
Dependencies: Motordriver mbed HALLFX_ENCODER Servo
main.cpp
- Committer:
- jsterling30
- Date:
- 2018-04-21
- Revision:
- 6:235548599e79
- Parent:
- 5:7572f73a78f3
- Child:
- 7:6ef9b0aab972
File content as of revision 6:235548599e79:
#include "mbed.h" #include "motordriver.h" #include "HALLFX_ENCODER.h" #include <string> #define SPEED 0.2 #define TICKSPERREV 390 #define DISTPERREV 8.25 // 8.25 inches per revolution #define TicksPerDeg 2.756 using namespace std; Motor right(p23,p6,p5, 1); // pwm, fwd, rev Motor left(p21, p7, p8, 1); // pwm, fwd, rev HALLFX_ENCODER leftEnc(p15); HALLFX_ENCODER rightEnc(p16); RawSerial dev(p13,p14); RawSerial pc(USBTX, USBRX); DigitalOut led1(LED1); DigitalOut led4(LED4); int count = 0; void dev_recv() { count++; led1 = !led1; while(dev.readable()) { pc.putc(dev.getc()); } pc.printf("%d", count); } void pc_recv() { led4 = !led4; while(pc.readable()) { dev.putc(pc.getc()); } } string command; void blue_recv() { led1 = !led1; while(dev.readable()) { char letter = (char) dev.getc(); command += letter; //strcat(command, &letter); //pc.putc(dev.getc()); } pc.printf("%s", command); string circle = "circle"; if (circle.compare(0,6,command) == 0) { dev.puts("Recognized word circle"); } else { dev.puts("Did not recognize command"); } } void stop() { right.speed(0.0); left.speed(0.0); } void forward(float distance) { float numRevs = distance / DISTPERREV; float numTicks = numRevs * TICKSPERREV; leftEnc.reset(); rightEnc.reset(); printf("Left Ticks Start: %d\r\n", leftEnc.read()); printf("Right Ticks Start: %d\r\n", rightEnc.read()); right.speed(SPEED); left.speed(SPEED); while (leftEnc.read() < numTicks && rightEnc.read() < numTicks) {} stop(); wait(0.1); printf("Left Ticks End: %d\r\n", leftEnc.read()); printf("Right Ticks End: %d\r\n", rightEnc.read()); } void turnLeft(int degrees) { leftEnc.reset(); rightEnc.reset(); right.speed(SPEED); left.speed(-SPEED); float numTicks = degrees * TicksPerDeg; while (leftEnc.read() < numTicks && rightEnc.read() < numTicks) {} stop(); wait(0.1); } void turnRight(int degrees) { leftEnc.reset(); rightEnc.reset(); right.speed(-SPEED); left.speed(SPEED); float numTicks = degrees * TicksPerDeg; while (leftEnc.read() < numTicks && rightEnc.read() < numTicks) {} stop(); wait(0.1); } void makeCircle() { leftEnc.reset(); rightEnc.reset(); right.speed(SPEED); float numTicks = 360 * TicksPerDeg; while (rightEnc.read() < numTicks) {} stop(); wait(0.1); } void manualCheck() { leftEnc.reset(); rightEnc.reset(); while (1) { printf("Left Ticks: %d\r\n", leftEnc.read()); printf("Right Ticks: %d\r\n\r\n", rightEnc.read()); wait(0.5); } } int main() { pc.baud(9600); dev.baud(9600); pc.attach(&pc_recv, Serial::RxIrq); dev.attach(&blue_recv, Serial::RxIrq); while(1) { sleep(); } //forward(5.0); //turnLeft(90); //Should make a triangle //for (int i = 0; i < 3; i++){ // forward(5.0); // turnLeft(120); // } //manualcheck(); //forward(10.0); }