Mouse code for the MacroRat

Dependencies:   ITG3200 QEI

Revision:
29:ec2c5a69acd6
Parent:
Child:
30:11f4316a5ba7
```--- a/main.h	Sun May 21 13:04:21 2017 +0000
+++ b/main.h	Wed May 24 01:57:01 2017 +0000
@@ -134,4 +134,257 @@
{14, 13, 12, 11, 10, 9, 8, 7, 7, 8, 9, 10, 11, 12, 13, 14},
};

+int distanceToCenter[16][16] = {
+                                  {14, 13, 12, 11, 10, 9, 8, 7, 7, 8, 9, 10, 11, 12, 13, 14},
+                                  {13, 12, 11, 10, 9, 8, 7, 6, 6, 7, 8, 9, 10, 11, 12, 13},
+                                  {12, 11, 10, 9, 8, 7, 6, 5, 5, 6, 7, 8, 9, 10, 11, 12},
+                                  {11, 10, 9, 8, 7, 6, 5, 4, 4, 5, 6, 7, 8, 9, 10, 11},
+                                  {10, 9, 8, 7, 6, 5, 4, 3, 3, 4, 5, 6, 7, 8, 9, 10},
+                                  {9, 8, 7, 6, 5, 4, 3, 2, 2, 3, 4, 5, 6, 7, 8, 9},
+                                  {8, 7, 6, 5, 4, 3, 2, 1, 1, 2, 3, 4, 5, 6, 7, 8},
+                                  {7, 6, 5, 4, 3, 2, 1, 0, 0, 1, 2, 3, 4, 5, 6, 7},
+                                  {7, 6, 5, 4, 3, 2, 1, 0, 0, 1, 2, 3, 4, 5, 6, 7},
+                                  {8, 7, 6, 5, 4, 3, 2, 1, 1, 2, 3, 4, 5, 6, 7, 8},
+                                  {9, 8, 7, 6, 5, 4, 3, 2, 2, 3, 4, 5, 6, 7, 8, 9},
+                                  {10, 9, 8, 7, 6, 5, 4, 3, 3, 4, 5, 6, 7, 8, 9, 10},
+                                  {11, 10, 9, 8, 7, 6, 5, 4, 4, 5, 6, 7, 8, 9, 10, 11},
+                                  {12, 11, 10, 9, 8, 7, 6, 5, 5, 6, 7, 8, 9, 10, 11, 12},
+                                  {13, 12, 11, 10, 9, 8, 7, 6, 6, 7, 8, 9, 10, 11, 12, 13},
+                                  {14, 13, 12, 11, 10, 9, 8, 7, 7, 8, 9, 10, 11, 12, 13, 14},
+                          };
+
+int distanceToStart[16][16] = {0};
+
+
+
+/* Constants for when HIGH_PWM_VOLTAGE = 0.2
+#define IP_CONSTANT 6
+#define II_CONSTANT 0
+#define ID_CONSTANT 1
+*/
+
+// Constants for when HIGH_PWM_VOLTAGE = 0.1
+// #define IP_CONSTANT 8.85
+// #define II_CONSTANT 0.005
+// #define ID_CONSTANT 3.15
+#define IP_CONSTANT 8.2
+#define II_CONSTANT 0.06
+#define ID_CONSTANT 7.55
+
+const int desiredCount180 = 2870;
+const int desiredCountR = 1700;
+const int desiredCountL = 1700;
+
+const int oneCellCount = 5400;
+const int oneCellCountMomentum = 4550;//4800;      // one cell count is actually approximately 5400, but this value is considering momentum!
+
+
+float ir1base = 0.0;
+float ir2base = 0.0;
+
+float ir3base = 0.0;
+
+float ir4base = 0.0;
+
+float initAverageL = 8.25;
+float averageDivL = 27.8; //blue
+float initAverageR = 8.75; //4.5
+float averageDivR = 28.5; //red
+float averageDivUpper = 0.5;
+
+float noWallR = 0.007;
+float noWallL = 0.007;
+
+inline void turnLeft()
+{
+    double speed0 = 0.11;
+    double speed1 = -0.13;
+
+    int counter = 0;
+    int initial0 = encoder0.getPulses();
+    int initial1 = encoder1.getPulses();
+
+    int desiredCount0 = initial0 - desiredCountL;
+    int desiredCount1 = initial1 + desiredCountL;
+
+    int count0 = initial0;
+    int count1 = initial1;
+
+    double error0 = count0 - desiredCount0;
+    double error1 = count1 - desiredCount1;
+
+    while(1) {
+
+        if(!(abs(error0) < 1) && !(abs(error1) < 1)) {
+            count0 = encoder0.getPulses();
+            count1 = encoder1.getPulses();
+
+            error0 = count0 - desiredCount0;
+            error1 = count1 - desiredCount1;
+
+            right_motor.move(speed0);
+            left_motor.move(speed1);
+            counter = 0;
+        } else {
+            counter++;
+            right_motor.brake();
+            left_motor.brake();
+        }
+
+        if (counter > 60) {
+            break;
+        }
+    }
+
+    right_motor.brake();
+    left_motor.brake();
+    turnFlag = 0;           // zeroing out the flags!
+    currDir -= 1;
+}
+
+
+inline void turnRight()
+{
+    double speed0 = -0.11;
+    double speed1 = 0.13;
+
+    int counter = 0;
+    int initial0 = encoder0.getPulses();
+    int initial1 = encoder1.getPulses();
+
+    int desiredCount0 = initial0 + desiredCountR;
+    int desiredCount1 = initial1 - desiredCountR;
+
+    int count0 = initial0;
+    int count1 = initial1;
+
+    double error0 = count0 - desiredCount0;
+    double error1 = count1 - desiredCount1;
+
+    while(1) {
+
+        if(!(abs(error0) < 1) && !(abs(error1) < 1)) {
+            count0 = encoder0.getPulses();
+            count1 = encoder1.getPulses();
+
+            error0 = count0 - desiredCount0;
+            error1 = count1 - desiredCount1;
+
+            right_motor.move(speed0);
+            left_motor.move(speed1);
+            counter = 0;
+        } else {
+            counter++;
+            right_motor.brake();
+            left_motor.brake();
+        }
+
+        if (counter > 60) {
+            break;
+        }
+    }
+
+    right_motor.brake();
+    left_motor.brake();
+    turnFlag = 0;
+    currDir += 1;
+}
+
+inline void turnLeft180()
+{
+    double speed0 = 0.15;
+    double speed1 = -0.15;
+
+    int counter = 0;
+    int initial0 = encoder0.getPulses();
+    int initial1 = encoder1.getPulses();
+
+    int desiredCount0 = initial0 - desiredCountL*2;
+    int desiredCount1 = initial1 + desiredCountL*2;
+
+    int count0 = initial0;
+    int count1 = initial1;
+
+    double error0 = count0 - desiredCount0;
+    double error1 = count1 - desiredCount1;
+
+
+    while(1) {
+
+        if(!(abs(error0) < 1) && !(abs(error1) < 1)) {
+            count0 = encoder0.getPulses();
+            count1 = encoder1.getPulses();
+
+            error0 = count0 - desiredCount0;
+            error1 = count1 - desiredCount1;
+
+            right_motor.move(speed0);
+            left_motor.move(speed1);
+            counter = 0;
+        } else {
+            counter++;
+            right_motor.brake();
+            left_motor.brake();
+        }
+
+        if (counter > 60) {
+            break;
+        }
+    }
+
+    right_motor.brake();
+    left_motor.brake();
+    currDir -= 2;
+}
+
+inline void turnRight180()
+{
+    double speed0 = -0.16;
+    double speed1 = 0.16;
+
+    int counter = 0;
+    int initial0 = encoder0.getPulses();
+    int initial1 = encoder1.getPulses();
+
+    int desiredCount0 = initial0 + desiredCount180;
+    int desiredCount1 = initial1 - desiredCount180;
+
+    int count0 = initial0;
+    int count1 = initial1;
+
+    double error0 = count0 - desiredCount0;
+    double error1 = count1 - desiredCount1;
+
+
+    while(1) {
+
+        if(!(abs(error0) < 1) && !(abs(error1) < 1)) {
+            count0 = encoder0.getPulses();
+            count1 = encoder1.getPulses();
+
+            error0 = count0 - desiredCount0;
+            error1 = count1 - desiredCount1;
+
+            right_motor.move(speed0);
+            left_motor.move(speed1);
+            counter = 0;
+        } else {
+            counter++;
+            right_motor.brake();
+            left_motor.brake();
+        }
+
+        if (counter > 60) {
+            break;
+        }
+    }
+    right_motor.brake();
+    left_motor.brake();
+    currDir += 2;
+}
+
#endif
\ No newline at end of file
```