MacroRat
Mouse code for the MacroRat
Diff: main.h
- Revision:
- 29:ec2c5a69acd6
- Parent:
- 26:d20f1adac2d3
- Child:
- 30:11f4316a5ba7
diff -r 8126a4d620e8 -r ec2c5a69acd6 main.h
--- 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 receiverOneReading = 0.0;
+float receiverTwoReading = 0.0;
+float receiverThreeReading = 0.0;
+float receiverFourReading = 0.0;
+
+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