Diff: main.cpp

Revision:

3:c4b0460d8886

Parent:

2:8c27831ce9a2

Child:

4:a21d15629407

--- a/main.cpp	Fri Apr 20 16:53:24 2018 +0000
+++ b/main.cpp	Fri Apr 20 17:54:08 2018 +0000
@@ -3,8 +3,9 @@
 #include "HALLFX_ENCODER.h"
 
 #define SPEED 0.2
-#define TICKSPERREV 384
+#define TICKSPERREV 390
 #define DISTPERREV 8.25     // 8.25 inches per revolution
+#define TicksPerDeg 2.756
  
 Motor right(p23,p6,p5, 1); // pwm, fwd, rev
 Motor left(p21, p7, p8, 1); // pwm, fwd, rev
@@ -26,19 +27,40 @@
     printf("Right Ticks Start: %d\r\n", rightEnc.read());
     right.speed(SPEED);
     left.speed(SPEED);
-    while (leftEnc.read() < numTicks || rightEnc.read() < numTicks) {}
+    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()); 
+    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 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() {
-    //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);
+    //forward(5.0);
+    //turnLeft(90);
+    forward(5.0);
+    turnLeft(45);
+    forward(5.0);
+    //manualcheck();
+    //forward(10.0);
 }
\ No newline at end of file