Mouse code for the MacroRat
Diff: main.cpp
- Revision:
- 43:f22168a05c3e
- Parent:
- 42:75257e6c4c76
- Child:
- 44:85bf2c0cd518
diff -r 75257e6c4c76 -r f22168a05c3e main.cpp --- a/main.cpp Sun May 28 06:50:22 2017 +0000 +++ b/main.cpp Sun May 28 10:09:56 2017 +0000 @@ -57,50 +57,66 @@ double kd = 0.00020; int prev = 0; - double speed0 = WHEEL_SPEED+0.015; + double speed0 = WHEEL_SPEED; double speed1 = WHEEL_SPEED; receiverOneReading = IRP_1.getSamples(100); receiverFourReading = IRP_4.getSamples(100); right_motor.move(speed0); left_motor.move(speed1); + wait_us(60000); - double distance_to_go = (oneCellCountMomentum-270)*num; + double distance_to_go = (oneCellCount)*num; while( ((encoder0.getPulses() - initial0) <= distance_to_go && (encoder1.getPulses() - initial1) <= distance_to_go) && receiverOneReading < IRP_1.sensorAvg*frontStop && receiverFourReading < IRP_4.sensorAvg*frontStop) { //while( (IRP_1.getSamples(50) + IRP_4.getSamples(50))/2 < ((IRP_1.sensorAvg+IRP_2.sensorAvg)/2)*0.4 ){ //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; + // count0 = encoder0.getPulses() - initial0; // left + // count1 = encoder1.getPulses() - initial1; // right + // int x = count0 - count1; // negative if right spins faster, positive if left spins faster + // //double d = kp * x + kd * ( x - prev ); + // double kppart = kp * x; + // double kdpart = kd * (x-prev); + // double d = kppart + kdpart; - double leftspeed = speed1; - double rightspeed = speed0; + // double leftspeed = speed0; + // double rightspeed = speed1; - if( ( distance_to_go - (encoder0.getPulses() - initial0) ) <= 1000 || (distance_to_go - (encoder1.getPulses() - initial1)) <= 1000 ) + + if( ( distance_to_go - count0 ) <= 900 || (distance_to_go - count1) <= 900 ) { - leftspeed *= .1/1000; - rightspeed *= .1/1000; + //leftspeed = speed0/900; + //rightspeed = speed1/900; + left_motor.move( speed0/900 ); + right_motor.move( speed1/900 ); + } + else + { + left_motor.move(speed0); + right_motor.move(speed1); + wait_us(16000); + pidOnEncoders(); } - + // else + // { + // if( x < diff + 3 ) { // count1 is bigger, right wheel pushed forward + // leftspeed -= d/4; + // rightspeed += d; + // } else if( x > diff + 3 ) { + // leftspeed -= d; + // rightspeed += d; + // } + // } + // //serial.printf("%d, %f, %f\n", x, leftspeed, rightspeed ); + // left_motor.move( leftspeed ); + // right_motor.move( rightspeed ); //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( leftspeed-d ); - right_motor.move( rightspeed+d ); - } else if( x > diff + 40 ) { - left_motor.move( leftspeed-d ); - right_motor.move( rightspeed+d ); - } - prev = x; + + // prev = x; receiverOneReading = IRP_1.getSamples(100); receiverFourReading = IRP_4.getSamples(100); } - //pidOnEncoders(); //pidBrake(); right_motor.brake(); @@ -116,56 +132,77 @@ int initial1 = count1; int initial0 = count0; int diff = count0 - count1; - double kp = 0.00022; - double kd = 0.00020; + double kp = 0.00008; + double kd = 0.0000; int prev = 0; - double speed0 = WHEEL_SPEED+0.015; + double speed0 = WHEEL_SPEED; // left wheel double speed1 = WHEEL_SPEED; receiverOneReading = IRP_1.getSamples(100); receiverFourReading = IRP_4.getSamples(100); right_motor.move(speed0); left_motor.move(speed1); + wait_us(60000); - double distance_to_go = (oneCellCountMomentum-270)*num; + double distance_to_go = (oneCellCount)*num; while( ((encoder0.getPulses() - initial0) <= distance_to_go && (encoder1.getPulses() - initial1) <= distance_to_go) && receiverOneReading < IRP_1.sensorAvg*frontStop && receiverFourReading < IRP_4.sensorAvg*frontStop) { //while( (IRP_1.getSamples(50) + IRP_4.getSamples(50))/2 < ((IRP_1.sensorAvg+IRP_2.sensorAvg)/2)*0.4 ){ //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; + // count0 = encoder0.getPulses() - initial0; // left + // count1 = encoder1.getPulses() - initial1; // right + // int x = count0 - count1; // negative if right spins faster, positive if left spins faster + // //double d = kp * x + kd * ( x - prev ); + // double kppart = kp * x; + // double kdpart = kd * (x-prev); + // double d = kppart + kdpart; - double leftspeed = speed1; - double rightspeed = speed0; + // double leftspeed = speed0; + // double rightspeed = speed1; - if( ( distance_to_go - (encoder0.getPulses() - initial0) ) <= 1000 || (distance_to_go - (encoder1.getPulses() - initial1)) <= 1000 ) + + if( ( distance_to_go - count0 ) <= 900 || (distance_to_go - count1) <= 900 ) { - leftspeed *= .1/1000; - rightspeed *= .1/1000; + //leftspeed = speed0/900; + //rightspeed = speed1/900; + left_motor.move( speed0/900 ); + right_motor.move( speed1/900 ); + } + else + { + left_motor.move(speed0); + right_motor.move(speed1); + wait_us(16000); + pidOnEncoders(); } - + // else + // { + // if( x < diff + 3 ) { // count1 is bigger, right wheel pushed forward + // leftspeed -= d/4; + // rightspeed += d; + // } else if( x > diff + 3 ) { + // leftspeed -= d; + // rightspeed += d; + // } + // } + // //serial.printf("%d, %f, %f\n", x, leftspeed, rightspeed ); + // left_motor.move( leftspeed ); + // right_motor.move( rightspeed ); //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( leftspeed-d ); - right_motor.move( rightspeed+d ); - } else if( x > diff + 40 ) { - left_motor.move( leftspeed-d ); - right_motor.move( rightspeed+d ); - } - prev = x; + + // prev = x; receiverOneReading = IRP_1.getSamples(100); receiverFourReading = IRP_4.getSamples(100); } right_motor.brake(); left_motor.brake(); + + double curr0 = encoder0.getPulses(); + double curr1 = encoder1.getPulses(); + if ((curr0 - initial0) >= distance_to_go*0.6 && (curr1 - initial1) >= distance_to_go*0.6){ if (currDir % 4 == 0) { mouseY += 1; } else if (currDir % 4 == 1) { @@ -182,11 +219,11 @@ receiverThreeReading = IRP_3.getSamples(100); receiverFourReading = IRP_4.getSamples(100); - serial.printf("R1: %f \t R4: %f \t R1Avg: %f \t R4Avg: %f\n", receiverOneReading, receiverFourReading, IRP_1.sensorAvg, IRP_4.sensorAvg); - serial.printf("R2: %f \t R3: %f \t R2Avg: %f \t R3Avg: %f\n", receiverTwoReading, receiverThreeReading, IRP_2.sensorAvg, IRP_3.sensorAvg); + // serial.printf("R1: %f \t R4: %f \t R1Avg: %f \t R4Avg: %f\n", receiverOneReading, receiverFourReading, IRP_1.sensorAvg, IRP_4.sensorAvg); + // serial.printf("R2: %f \t R3: %f \t R2Avg: %f \t R3Avg: %f\n", receiverTwoReading, receiverThreeReading, IRP_2.sensorAvg, IRP_3.sensorAvg); if (receiverOneReading >= IRP_1.sensorAvg * 2.5 || receiverFourReading >= IRP_4.sensorAvg * 2.5) { - serial.printf("Front wall is there\n"); + // serial.printf("Front wall is there\n"); if (currDir % 4 == 0) { wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= F_WALL; } else if (currDir % 4 == 1) { @@ -200,7 +237,7 @@ if((receiverThreeReading < IRP_3.sensorAvg/5) && (receiverTwoReading < IRP_2.sensorAvg/5)) { // do nothing, the walls are not there } else if (receiverThreeReading < IRP_3.sensorAvg/LRAvg) { // right wall gone, left wall is there RED - serial.printf("Left wall, mouseY: %d, mouseX: %d\n", mouseY, mouseX); + // serial.printf("Left wall, mouseY: %d, mouseX: %d\n", mouseY, mouseX); if (currDir % 4 == 0) { wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= L_WALL; } else if (currDir % 4 == 1) { @@ -211,7 +248,7 @@ wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= B_WALL; } } else if (receiverTwoReading < IRP_2.sensorAvg/LRAvg) { // left wall gone, right wall is there - serial.printf("Right wall, mouseY: %d, mouseX: %d\n", mouseY, mouseX); + // serial.printf("Right wall, mouseY: %d, mouseX: %d\n", mouseY, mouseX); if (currDir % 4 == 0) { wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= R_WALL; } else if (currDir % 4 == 1) { @@ -222,7 +259,7 @@ wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= F_WALL; } } else if ((receiverTwoReading > ((IRP_2.sensorAvg)*averageDivUpper)) && (receiverThreeReading > ((IRP_3.sensorAvg)*averageDivUpper))) { - serial.printf("Both walls \n"); + // serial.printf("Both walls \n"); if (currDir %4 == 0){ wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= R_WALL; wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= L_WALL; @@ -237,6 +274,7 @@ wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= B_WALL; } } + } } void printDipFlag() @@ -934,14 +972,15 @@ // prevEncoder0Count = encoder0.getPulses(); // prevEncoder1Count = encoder1.getPulses(); + if (!overrideFloodFill){ nextMovement = chooseNextMovement(); // serial.printf("MouseX: %d, MouseY: %d\n", mouseX, mouseY); if (nextMovement == 0){ // serial.printf("Just Turned, want to go forward now\n"); - redLed.write(1); - greenLed.write(0); - blueLed.write(1); + //redLed.write(1); + //greenLed.write(0); + //blueLed.write(1); encoderAfterTurn(1); // nCellEncoderAndIR(1); } @@ -955,14 +994,16 @@ } else if (nextMovement == 3){ nCellEncoderAndIR(1); + //encoderAfterTurn(1); } else if (nextMovement == 4){ justTurned = true; turn180(); } } - wait_ms(500); // reduce this once we fine tune this! + wait_ms(700); // reduce this once we fine tune this! + //////////////////////// TESTING CODE GOES BELOW /////////////////////////// @@ -971,5 +1012,7 @@ // serial.printf("Encoder 0 is : %d \t Encoder 1 is %d\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 ); + //encoderAfterTurn(1); + //waitButton4(); } } \ No newline at end of file