Mouse code for the MacroRat
Diff: main.h
- Revision:
- 39:058fb32c24e0
- Parent:
- 38:fe05f93009a2
- Child:
- 40:465d2b565977
--- a/main.h Sat May 27 21:29:55 2017 +0000 +++ b/main.h Sun May 28 03:42:59 2017 +0000 @@ -172,67 +172,76 @@ //#define II_CONSTANT 0.06 //#define ID_CONSTANT 7.55 -const int desiredCount180 = 3380; // change accordingly to the terrain -const int desiredCountR = 1600; -const int desiredCountL = 1600; - +const int desiredCount180 = 2900; // change accordingly to the terrain +const int desiredCountR = 1490; +const int desiredCountL = 1500; + const int oneCellCount = 5400; -const int oneCellCountMomentum = 5070;//4570 (.15) speed;//4800; // one cell count is actually approximately 5400, but this value is considering momentum! - +const int oneCellCountMomentum = 4630;//4570 (.15) speed;//4800; // one cell count is actually approximately 5400, but this value is considering momentum! +// const int oneCellCountMomentum = 4400; double receiverOneReading = 0.0; double receiverTwoReading = 0.0; double receiverThreeReading = 0.0; double receiverFourReading = 0.0; +const double frontStop = 7.2; +const double LRAvg = 3.5; + float ir1base = 0.0; float ir2base = 0.0; float ir3base = 0.0; float ir4base = 0.0; float averageDivUpper = 0.5; - + inline void turnLeft() { double speed0 = 0.11; double speed1 = -0.12; // change back to 0.13 if turns stop working, testing something out! - - double kp = 0.000082; - + + //double kp = 0.000082; + double kp = 0.00010; + int counter = 0; int initial0 = encoder0.getPulses(); int initial1 = encoder1.getPulses(); - + int desiredCount0 = initial0 - desiredCountL; // left wheel int desiredCount1 = initial1 + desiredCountL; // right wheel - + int count0 = initial0; int count1 = initial1; - + double error0 = desiredCount0 - count0; // is negative double error1 = desiredCount1 - count1; // is positive - + while(1) { - if(!(abs(error0) < 3) && !(abs(error1) < 3)) { + if(!(abs(error0) < 4) && !(abs(error1) < 4)) { count0 = encoder0.getPulses(); count1 = encoder1.getPulses(); - + error0 = desiredCount0 - count0; // is negative error1 = desiredCount1 - count1; // is positive - + right_motor.move(error1*kp); left_motor.move(error0*kp); counter = 0; } else { counter++; + count0 = encoder0.getPulses(); + count1 = encoder1.getPulses(); + + error0 = desiredCount0 - count0; // is negative + error1 = desiredCount1 - count1; // is positive right_motor.brake(); left_motor.brake(); } - if (counter > 60) { + if (counter > 100) { break; } } - + right_motor.brake(); left_motor.brake(); turnFlag = 0; // zeroing out the flags! @@ -245,7 +254,8 @@ double speed0 = -0.11; double speed1 = 0.12; // change back to 0.13 if turns stop working, testing something out! - double kp = 0.00009; +// double kp = 0.00009; + double kp = 0.0002; int counter = 0; int initial0 = encoder0.getPulses(); @@ -262,7 +272,7 @@ while(1) { - if(!(abs(error0) < 3) && !(abs(error1) < 3)) { + if(!(abs(error0) < 2) && !(abs(error1) < 2)) { count0 = encoder0.getPulses(); count1 = encoder1.getPulses(); @@ -274,10 +284,15 @@ counter = 0; } else { counter++; + count0 = encoder0.getPulses(); + count1 = encoder1.getPulses(); + + error0 = desiredCount0 - count0; // is positive + error1 = desiredCount1 - count1; // is negative right_motor.brake(); left_motor.brake(); } - if (counter > 60) { + if (counter > 100) { break; } } @@ -293,7 +308,8 @@ double speed0 = -0.10; double speed1 = 0.11; - double kp = 0.000055; +// double kp = 0.000055; + double kp = 0.00006; int counter = 0; int initial0 = encoder0.getPulses(); @@ -322,10 +338,17 @@ counter = 0; } else { counter++; + + count0 = encoder0.getPulses(); + count1 = encoder1.getPulses(); + + error0 = desiredCount0 - count0; + error1 = desiredCount1 - count1; + right_motor.brake(); left_motor.brake(); } - if (counter > 60) { + if (counter > 150) { break; } }