Mouse code for the MacroRat
Diff: main.cpp
- Revision:
- 15:b80555a4a8b9
- Parent:
- 14:9e7bb03ddccb
- Child:
- 16:d9252437bd92
diff -r 9e7bb03ddccb -r b80555a4a8b9 main.cpp --- a/main.cpp Sat May 13 23:49:02 2017 +0000 +++ b/main.cpp Sun May 14 04:45:21 2017 +0000 @@ -16,9 +16,9 @@ */ // Constants for when HIGH_PWM_VOLTAGE = 0.1 -#define IP_CONSTANT 12 -#define II_CONSTANT 0 -#define ID_CONSTANT 2 +#define IP_CONSTANT 9 +#define II_CONSTANT 1 +#define ID_CONSTANT 3 const int desiredCountR = 1400; const int desiredCountL = 1475; @@ -137,7 +137,7 @@ while(1) { - + if(!(abs(error0) < 1) && !(abs(error1) < 1)) { count0 = encoder0.getPulses(); count1 = encoder1.getPulses(); @@ -213,11 +213,11 @@ double speed1 = 0.15; int counter = 0; - int initial0 = encoder0.getPulses(); - int initial1 = encoder1.getPulses(); + int initial0 = encoder0.getPulses(); // left + int initial1 = encoder1.getPulses(); // right - double kpLeft = 0.000005; - double kpRight = 0.000005; + double kpLeft = 0.0000005; + double kpRight = 0.0000005; int desiredCount0 = initial0 + oneCellCount; int desiredCount1 = initial1 + oneCellCount; @@ -365,9 +365,48 @@ printDipFlag(); } +void pidOnEncoders() +{ + int count0; + int count1; + count0 = encoder0.getPulses(); + count1 = encoder1.getPulses(); + int diff = count0 - count1; + double kp = 0.000075; + double kd = 0.000125; + int prev = 0; + while(1) + { + count0 = encoder0.getPulses(); + count1 = encoder1.getPulses(); + int x = count0 - count1; + //double d = kp * x + kd * ( x - prev ); + double kppart = kp * x; + double kdpart = kd * (x-prev); + double d = kppart + kdpart; + + //serial.printf( "x: %d,\t prev: %d,\t d: %f,\t kppart: %f,\t kdpart: %f\n", x, prev, d, kppart, kdpart ); + if( x < diff - 50 ) // count1 is bigger, right wheel pushed forward + { + left_motor.move( -d ); + right_motor.move( d ); + } + else if( x > diff + 50 ) + { + left_motor.move( -d ); + right_motor.move( d ); + } + else + { + left_motor.brake(); + right_motor.brake(); + } + prev = x; + } +} + int main() { - //enableMotors(); //Set highest bandwidth. gyro.setLpBandwidth(LPFBW_42HZ); serial.baud(9600); @@ -375,10 +414,6 @@ wait (0.1); -// IR_1.write(1); -// IR_2.write(1); -// IR_3.write(1); -// IR_4.write(1); redLed.write(1); greenLed.write(0); @@ -426,10 +461,13 @@ dipButton3.fall(&disableButton3); dipButton4.fall(&disableButton4); + //right_motor.forward( 0.2 ); + //left_motor.forward( 0.2 ); + while (1) { - moveForwardOneCellEncoder(); - - // moveForwardUntilWallIr(); + //moveForwardOneCellEncoder(); + pidOnEncoders(); + //moveForwardUntilWallIr(); // wait(2); // turnRight(); // wait(1); @@ -438,7 +476,7 @@ break; //serial.printf("%i, %i, %i\n", gyro.getGyroX(), gyro.getGyroY(), gyro.getGyroZ()); -// serial.printf("Pulse Count=> e0:%d, e1:%d \r\n", encoder0.getPulses(),encoder1.getPulses()); + //serial.printf("Pulse Count=> e0:%d, e1:%d \r\n", encoder0.getPulses(),encoder1.getPulses()); //double currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - IRP_2.sensorAvg) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg) ) ; //serial.printf("IRS= >: %f, %f, %f, %f, %f \r\n",