Mouse code for the MacroRat
Diff: main.cpp
- Revision:
- 16:d9252437bd92
- Parent:
- 15:b80555a4a8b9
- Child:
- 17:f713758f6238
diff -r b80555a4a8b9 -r d9252437bd92 main.cpp --- a/main.cpp Sun May 14 04:45:21 2017 +0000 +++ b/main.cpp Sun May 14 20:58:55 2017 +0000 @@ -23,7 +23,10 @@ const int desiredCountR = 1400; const int desiredCountL = 1475; -const int oneCellCount = 5300; +const int oneCellCount = 5400; +const int oneCellCountMomentum = 4600; // one cell count is actually approximately 5400, but this value is considering momentum! + +void pidOnEncoders(); void turnLeft() { @@ -208,54 +211,20 @@ left_motor.brake(); } -void moveForwardOneCellEncoder(){ // 0 is left wheel! - double speed0 = 0.15; - double speed1 = 0.15; - - int counter = 0; - int initial0 = encoder0.getPulses(); // left - int initial1 = encoder1.getPulses(); // right - - double kpLeft = 0.0000005; - double kpRight = 0.0000005; - - int desiredCount0 = initial0 + oneCellCount; - int desiredCount1 = initial1 + oneCellCount; - - int count0 = initial0; - int count1 = initial1; - - int error = 0; - - while (1){ - if (count0 < desiredCount0 && count1 < desiredCount1){ - count0 = encoder0.getPulses(); - count1 = encoder1.getPulses(); +void moveForwardOneCellEncoder(){ + int desiredCount0 = encoder0.getPulses() + oneCellCountMomentum; + int desiredCount1 = encoder1.getPulses() + oneCellCountMomentum; - error = count0 - count1; // if error is positive, then left has moved more, so we want to decrease its speed and increase the right speed + left_motor.forward(0.12); + right_motor.forward(0.10); + wait_ms(2); + while (encoder0.getPulses() <= desiredCount0 && encoder1.getPulses() <= desiredCount1){ + // serial.printf("Encoder0 count is: %d, Encoder1 count is: %d, desiredEncoder0 = %d, desiredEncoder1 = %d\n", encoder0.getPulses(), encoder1.getPulses(), desiredCount0, desiredCount1); + pidOnEncoders(); + } - if (error > 0){ - speed0 = 0.15 - error*kpLeft; - speed1 = 0.15 + error*kpRight; - } - else if (error <= 0){ - speed0 = 0.15 + error*kpLeft; - speed1 = 0.15 - error*kpRight; - } - // serial.printf("The error is %d \n", error); - // serial.printf("The Left speed is: %f and the right speed is: %f \n", speed0, speed1); - right_motor.move(speed0); - left_motor.move(speed1); - counter = 0; - } - else { - break; - } - } - greenLed.write(1); - blueLed.write(0); + left_motor.brake(); right_motor.brake(); - left_motor.brake(); } void moveForwardUntilWallIr() @@ -373,8 +342,10 @@ count1 = encoder1.getPulses(); int diff = count0 - count1; double kp = 0.000075; - double kd = 0.000125; + double kd = 0.000135; int prev = 0; + + int counter = 0; while(1) { count0 = encoder0.getPulses(); @@ -396,12 +367,15 @@ left_motor.move( -d ); right_motor.move( d ); } - else - { - left_motor.brake(); - right_motor.brake(); - } + // else + // { + // left_motor.brake(); + // right_motor.brake(); + // } prev = x; + counter++; + if (counter == 4) + break; } } @@ -465,13 +439,12 @@ //left_motor.forward( 0.2 ); while (1) { - //moveForwardOneCellEncoder(); - pidOnEncoders(); + moveForwardOneCellEncoder(); + //pidOnEncoders(); //moveForwardUntilWallIr(); // wait(2); // turnRight(); // wait(1); - // moveForwardOneCellEncoder(); // moveForwardUntilWallIr(); break;