Diff: main.cpp

Revision:

2:8c27831ce9a2

Parent:

1:560118dc2f99

Child:

3:c4b0460d8886

--- a/main.cpp	Wed Apr 18 17:15:57 2018 +0000
+++ b/main.cpp	Fri Apr 20 16:53:24 2018 +0000
@@ -1,24 +1,44 @@
 #include "mbed.h"
 #include "motordriver.h"
+#include "HALLFX_ENCODER.h"
+
+#define SPEED 0.2
+#define TICKSPERREV 384
+#define DISTPERREV 8.25     // 8.25 inches per revolution
  
-Motor right(p23,p5,p6, 1); // pwm, fwd, rev
+Motor right(p23,p6,p5, 1); // pwm, fwd, rev
 Motor left(p21, p7, p8, 1); // pwm, fwd, rev
 
-//void forward(int distance) {
-//
-//}
-//
-//void reverse(int distance) {
-//
-//}
-//void turnLeft() {
-//
-//}
+HALLFX_ENCODER leftEnc(p15);
+HALLFX_ENCODER rightEnc(p16);
+
+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) {}
+    printf("Left Ticks End: %d\r\n", leftEnc.read());
+    printf("Right Ticks End: %d\r\n", rightEnc.read()); 
+    stop();
+}
 
 int main() {
-    right.speed(0.15);
-    left.speed(0.15);
-    wait(5);
-    right.speed(0.0);
-    left.speed(0.0);
+    //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);
+    //}
+    forward(10.0);
 }
\ No newline at end of file