Mouse code for the MacroRat
Diff: main.h
- Revision:
- 36:9c4cc9944b69
- Parent:
- 35:a5bd9ef82210
- Child:
- 37:3dcc95e9321c
diff -r a5bd9ef82210 -r 9c4cc9944b69 main.h --- a/main.h Sat May 27 00:41:19 2017 +0000 +++ b/main.h Sat May 27 02:40:10 2017 +0000 @@ -10,6 +10,7 @@ #define PULSES 3520 #define SAMPLE_NUM 40 +#define WHEEL_SPEED 0.10 // Motors /* @@ -171,12 +172,12 @@ //#define II_CONSTANT 0.06 //#define ID_CONSTANT 7.55 -const int desiredCount180 = 3000; // change accordingly to the terrain -const int desiredCountR = 1700; -const int desiredCountL = 1700; +const int desiredCount180 = 3200; // change accordingly to the terrain +const int desiredCountR = 1600; +const int desiredCountL = 1590; const int oneCellCount = 5400; -const int oneCellCountMomentum = 4570;//4800; // one cell count is actually approximately 5400, but this value is considering momentum! +const int oneCellCountMomentum = 4800;//4570 (.15) speed;//4800; // one cell count is actually approximately 5400, but this value is considering momentum! double receiverOneReading = 0.0; double receiverTwoReading = 0.0; @@ -190,44 +191,41 @@ 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.12; // change back to 0.13 if turns stop working, testing something out! + double kp = 0.000085; + int counter = 0; int initial0 = encoder0.getPulses(); int initial1 = encoder1.getPulses(); - int desiredCount0 = initial0 - desiredCountL; - int desiredCount1 = initial1 + desiredCountL; + int desiredCount0 = initial0 - desiredCountL; // left wheel + int desiredCount1 = initial1 + desiredCountL; // right wheel int count0 = initial0; int count1 = initial1; - double error0 = count0 - desiredCount0; - double error1 = count1 - desiredCount1; + double error0 = desiredCount0 - count0; // is negative + double error1 = desiredCount1 - count1; // is positive while(1) { - if(!(abs(error0) < 1) && !(abs(error1) < 1)) { + if(!(abs(error0) < 3) && !(abs(error1) < 3)) { count0 = encoder0.getPulses(); count1 = encoder1.getPulses(); + + error0 = desiredCount0 - count0; // is negative + error1 = desiredCount1 - count1; // is positive - error0 = count0 - desiredCount0; - error1 = count1 - desiredCount1; + //serial.printf("%f, %f\n", error0, error1 ); - right_motor.move(speed0); - left_motor.move(speed1); + right_motor.move(error1*kp); + left_motor.move(error0*kp); counter = 0; } else { counter++; @@ -252,30 +250,34 @@ double speed0 = -0.11; double speed1 = 0.12; // change back to 0.13 if turns stop working, testing something out! + double kp = 0.00009; + int counter = 0; int initial0 = encoder0.getPulses(); int initial1 = encoder1.getPulses(); - int desiredCount0 = initial0 + desiredCountR; - int desiredCount1 = initial1 - desiredCountR; + int desiredCount0 = initial0 + desiredCountR; // left wheel + int desiredCount1 = initial1 - desiredCountR; // right wheel int count0 = initial0; int count1 = initial1; - double error0 = count0 - desiredCount0; - double error1 = count1 - desiredCount1; + double error0 = desiredCount0 - count0; // is positive + double error1 = desiredCount1 - count1; // is negative while(1) { - if(!(abs(error0) < 1) && !(abs(error1) < 1)) { + if(!(abs(error0) < 3) && !(abs(error1) < 3)) { count0 = encoder0.getPulses(); count1 = encoder1.getPulses(); - error0 = count0 - desiredCount0; - error1 = count1 - desiredCount1; + error0 = desiredCount0 - count0; // is positive + error1 = desiredCount1 - count1; // is negative - right_motor.move(speed0); - left_motor.move(speed1); + //serial.printf("%f, %f\n", error0, error1 ); + + right_motor.move(error1*kp); + left_motor.move(error0*kp); counter = 0; } else { counter++; @@ -294,7 +296,7 @@ currDir += 1; } -inline void turnRight180() +inline void turn180() { double speed0 = -0.15; double speed1 = 0.16;