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