Mouse code for the MacroRat
Diff: main.cpp
- Revision:
- 21:9a6cb07bdcb6
- Parent:
- 20:82836745332e
- Child:
- 22:681190ff98f0
diff -r 82836745332e -r 9a6cb07bdcb6 main.cpp --- a/main.cpp Wed May 17 02:01:34 2017 +0000 +++ b/main.cpp Thu May 18 02:52:22 2017 +0000 @@ -16,9 +16,12 @@ */ // 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.85 +// #define II_CONSTANT 0.005 +// #define ID_CONSTANT 3.15 +#define IP_CONSTANT 7.85 +#define II_CONSTANT 0.095 +#define ID_CONSTANT 20.85 const int desiredCountR = 1400; const int desiredCountL = 1475; @@ -33,6 +36,7 @@ void pidOnEncoders(); + void turnLeft() { double speed0 = 0.15; @@ -265,6 +269,112 @@ } } +void pidBrake(){ + + int count0; + int count1; + count0 = encoder0.getPulses(); + count1 = encoder1.getPulses(); + int initial0 = count0; + int initial1 = count1; + double kp = 0.00011; + + + + int error = count0 - count1; + + int counter = 0; + right_motor.move(0.7); + left_motor.move(0.7); + + double speed0 = 0.7; + double speed1 = 0.7; + + while(1) + { + if(abs(error) < 3){ + right_motor.brake(); + left_motor.brake(); + counter++; + }else{ + count0 = encoder0.getPulses() - initial0; + count1 = encoder1.getPulses() - initial1; + error = count0 - count1; + speed0 = -error*kp + speed0; + speed1 = error*kp + speed1; + + right_motor.move(speed0); + left_motor.move(speed1); + + counter = 0; + } + if (counter > 10){ + break; + } + + } + return; +} + +void moveForwardEncoder(){ + + int count0; + int count1; + count0 = encoder0.getPulses(); + count1 = encoder1.getPulses(); + int initial1 = count1; + int initial0 = count0; + int diff = count0 - count1; + double kp = 0.00015; + double kd = 0.00019; + int prev = 0; + + + + double speed0 = 0.1; + double speed1 = 0.13; + right_motor.move(speed0); + left_motor.move(speed1); + + + while((IRP_2.getSamples(SAMPLE_NUM) < 0.005 || IRP_3.getSamples(SAMPLE_NUM) < 0.005) && ((IRP_1.getSamples( SAMPLE_NUM ) + IRP_4.getSamples( SAMPLE_NUM ) )/2 < 0.25) ) { + + //serial.printf("IRS= >: %f, %f \r\n", IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 )); + + count0 = encoder0.getPulses() - initial0; + count1 = encoder1.getPulses() - initial1; + 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 - 40 ) // count1 is bigger, right wheel pushed forward + { + left_motor.move( speed1-0.8*d ); + right_motor.move( speed0+d ); + } + else if( x > diff + 40 ) + { + left_motor.move( speed1-0.8*d ); + right_motor.move( speed0+d ); + } + // else + // { + // left_motor.brake(); + // right_motor.brake(); + // } + prev = x; + } + + //pidOnEncoders(); + pidBrake(); + //right_motor.brake(); + //left_motor.brake(); + return; +} + void moveForwardUntilWallIr() { double currentError = 0; @@ -274,53 +384,62 @@ double HIGH_PWM_VOLTAGE = 0.1; - double rightSpeed = 0.1; - double leftSpeed = 0.12; + double rightSpeed = 0.14; + double leftSpeed = 0.17; float ir2 = IRP_2.getSamples( SAMPLE_NUM ); float ir3 = IRP_3.getSamples( SAMPLE_NUM ); int count = encoder0.getPulses(); - while ((IRP_1.getSamples( SAMPLE_NUM ) + IRP_4.getSamples( SAMPLE_NUM ) )/2 < 0.05f) { - int pulseCount = (encoder0.getPulses()-count) % 5600; - if (pulseCount > 5400 && pulseCount < 5800) { - blueLed.write(0); - } else { - blueLed.write(1); - } - sumError += currentError; - currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - IRP_2.sensorAvg) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg) ) ; - derivError = currentError - previousError; - double PIDSum = IP_CONSTANT*currentError + II_CONSTANT*sumError + ID_CONSTANT*derivError; - if (PIDSum > 0) { // this means the leftWheel is faster than the right. So right speeds up, left slows down - rightSpeed = HIGH_PWM_VOLTAGE - abs(PIDSum*HIGH_PWM_VOLTAGE); - leftSpeed = HIGH_PWM_VOLTAGE + abs(PIDSum*HIGH_PWM_VOLTAGE); - } else { // r is faster than L. speed up l, slow down r - rightSpeed = HIGH_PWM_VOLTAGE + abs(PIDSum*HIGH_PWM_VOLTAGE); - leftSpeed = HIGH_PWM_VOLTAGE - abs(PIDSum*HIGH_PWM_VOLTAGE); + while ((IRP_1.getSamples( SAMPLE_NUM ) + IRP_4.getSamples( SAMPLE_NUM ) )/2 < 0.05f) { // while the front facing IR's arent covered + + if((IRP_2.getSamples(SAMPLE_NUM) < 0.005 || IRP_3.getSamples(SAMPLE_NUM) < 0.005)) { + serial.printf("asdfasdf \n"); + moveForwardEncoder(); + }else{ + // will move forward using encoders only + // if cell ahead doesn't have a wall on either one side or both sides + + int pulseCount = (encoder0.getPulses()-count) % 5600; + if (pulseCount > 5400 && pulseCount < 5800) { + blueLed.write(0); + } else { + blueLed.write(1); + } + sumError += currentError; + currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - IRP_2.sensorAvg) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg) ) ; + derivError = currentError - previousError; + double PIDSum = IP_CONSTANT*currentError + II_CONSTANT*sumError + ID_CONSTANT*derivError; + if (PIDSum > 0) { // this means the leftWheel is faster than the right. So right speeds up, left slows down + rightSpeed = HIGH_PWM_VOLTAGE - abs(PIDSum*HIGH_PWM_VOLTAGE); + leftSpeed = HIGH_PWM_VOLTAGE + abs(PIDSum*HIGH_PWM_VOLTAGE); + } else { // r is faster than L. speed up l, slow down r + rightSpeed = HIGH_PWM_VOLTAGE + abs(PIDSum*HIGH_PWM_VOLTAGE); + leftSpeed = HIGH_PWM_VOLTAGE - abs(PIDSum*HIGH_PWM_VOLTAGE); + } + + if (leftSpeed > 0.30) leftSpeed = 0.30; + if (leftSpeed < 0) leftSpeed = 0; + if (rightSpeed > 0.30) rightSpeed = 0.30; + if (rightSpeed < 0) rightSpeed = 0; + + right_motor.forward(rightSpeed); + left_motor.forward(leftSpeed); + + previousError = currentError; + + ir2 = IRP_2.getSamples( SAMPLE_NUM ); + ir3 = IRP_3.getSamples( SAMPLE_NUM ); + } - if (leftSpeed > 0.30) leftSpeed = 0.30; - if (leftSpeed < 0) leftSpeed = 0; - if (rightSpeed > 0.30) rightSpeed = 0.30; - if (rightSpeed < 0) rightSpeed = 0; - - right_motor.forward(rightSpeed); - left_motor.forward(leftSpeed); - - previousError = currentError; + //backward(); + wait_ms(40); + //brake(); - ir2 = IRP_2.getSamples( SAMPLE_NUM ); - ir3 = IRP_3.getSamples( SAMPLE_NUM ); - + left_motor.brake(); + right_motor.brake(); } - - //backward(); - wait_ms(40); - //brake(); - - left_motor.brake(); - right_motor.brake(); } void printDipFlag() @@ -472,6 +591,8 @@ ir3 = IRP_3.getSamples( SAMPLE_NUM ); } + + left_motor.brake(); right_motor.brake(); } @@ -536,23 +657,34 @@ //left_motor.forward( 0.2 ); while (1) { + + //serial.printf("IRS= >: %f, %f \r\n", IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 )); + // oneCellEncoderAndIR(5); - // break; - moveForwardCellEncoder(1); - wait(0.5); - handleTurns(); - wait(0.5); - moveForwardCellEncoder(1); - wait(0.5); - handleTurns(); - break; + moveForwardUntilWallIr(); + //moveForwardEncoder(); + //serial.printf("ded \n"); + //break; + + + //serial.printf("IRS= >: %f, %f \r\n", IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 )); + + + //break; + // moveForwardCellEncoder(1); + // wait(0.5); + // handleTurns(); + // wait(0.5); + // moveForwardCellEncoder(1); + // wait(0.5); + // handleTurns(); + //break; //pidOnEncoders(); // moveForwardUntilWallIr(); //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()); // 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", - // IRP_1.getSamples( 100 ), IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 ), IRP_4.getSamples(100), currentError ); + //serial.printf("IRS= >: %f, %f, %f, %f, %f \r\n", IRP_1.getSamples( 100 ), IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 ), IRP_4.getSamples(100), currentError ); //reading = Rec_4.read(); // serial.printf("reading: %f\n", reading);