Mouse code for the MacroRat
Revision 45:8b0bee6baf38, committed 2017-06-02
- Comitter:
- sahilmgandhi
- Date:
- Fri Jun 02 19:35:26 2017 +0000
- Parent:
- 44:85bf2c0cd518
- Child:
- 46:b156ef445742
- Commit message:
- Final CAMM AAMC Working Code
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Fri Jun 02 18:50:51 2017 +0000 +++ b/main.cpp Fri Jun 02 19:35:26 2017 +0000 @@ -59,7 +59,7 @@ double speed0 = WHEEL_SPEED; double speed1 = WHEEL_SPEED; - + receiverOneReading = IRP_1.getSamples(100); receiverFourReading = IRP_4.getSamples(100); right_motor.move(speed0); @@ -69,62 +69,28 @@ 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; // 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 = speed0; - // double rightspeed = speed1; - - - if( ( distance_to_go - count0 ) <= 900 || (distance_to_go - count1) <= 900 ) - { - //leftspeed = speed0/900; - //rightspeed = speed1/900; + if( ( distance_to_go - count0 ) <= 900 || (distance_to_go - count1) <= 900 ) { left_motor.move( speed0/900 ); right_motor.move( speed1/900 ); - } - else - { + } 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 ); - - // prev = x; + receiverOneReading = IRP_1.getSamples(100); receiverFourReading = IRP_4.getSamples(100); } - //pidOnEncoders(); - //pidBrake(); + right_motor.brake(); left_motor.brake(); return; } -void encoderAfterTurn(double num){ +void encoderAfterTurn(double num) +{ int count0; int count1; count0 = encoder0.getPulses(); @@ -148,51 +114,16 @@ 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; // 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 = speed0; - // double rightspeed = speed1; - - - if( ( distance_to_go - count0 ) <= 900 || (distance_to_go - count1) <= 900 ) - { - //leftspeed = speed0/900; - //rightspeed = speed1/900; + if( ( distance_to_go - count0 ) <= 900 || (distance_to_go - count1) <= 900 ) { left_motor.move( speed0/900 ); right_motor.move( speed1/900 ); - } - else - { + } 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 ); - - // prev = x; receiverOneReading = IRP_1.getSamples(100); receiverFourReading = IRP_4.getSamples(100); } @@ -202,7 +133,7 @@ double curr0 = encoder0.getPulses(); double curr1 = encoder1.getPulses(); - if ((curr0 - initial0) >= distance_to_go*0.6 && (curr1 - initial1) >= distance_to_go*0.6){ + 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) { @@ -260,16 +191,16 @@ } } else if ((receiverTwoReading > ((IRP_2.sensorAvg)*averageDivUpper)) && (receiverThreeReading > ((IRP_3.sensorAvg)*averageDivUpper))) { // serial.printf("Both walls \n"); - if (currDir %4 == 0){ + if (currDir %4 == 0) { wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= R_WALL; wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= L_WALL; - } else if (currDir %4 == 1){ + } else if (currDir %4 == 1) { wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= F_WALL; wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= B_WALL; - } else if (currDir % 4 == 2){ + } else if (currDir % 4 == 2) { wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= R_WALL; wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= L_WALL; - } else if (currDir %4 == 3){ + } else if (currDir %4 == 3) { wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= F_WALL; wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= B_WALL; } @@ -279,7 +210,7 @@ void printDipFlag() { - if (DEBUGGING) + if (DEBUGGING) serial.printf("Flag value is %d", dipFlags); } @@ -506,13 +437,13 @@ previousError = currentError; } - + // GO BACK A BIT BEFORE BRAKING?? left_motor.backward(0.01); right_motor.backward(0.01); wait_us(150); // DELETE THIS IF IT DOES NOT WORK!! - + left_motor.brake(); right_motor.brake(); if (encoder0.getPulses() >= 0.4*desiredCount0 && encoder1.getPulses() >= 0.4*desiredCount1) { @@ -573,16 +504,16 @@ } } else if ((receiverTwoReading > ((IRP_2.sensorAvg)*averageDivUpper)) && (receiverThreeReading > ((IRP_3.sensorAvg)*averageDivUpper))) { // serial.printf("Both walls \n"); - if (currDir %4 == 0){ + if (currDir %4 == 0) { wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= R_WALL; wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= L_WALL; - } else if (currDir %4 == 1){ + } else if (currDir %4 == 1) { wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= F_WALL; wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= B_WALL; - } else if (currDir % 4 == 2){ + } else if (currDir % 4 == 2) { wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= R_WALL; wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= L_WALL; - } else if (currDir %4 == 3){ + } else if (currDir %4 == 3) { wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= F_WALL; wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= B_WALL; } @@ -884,8 +815,7 @@ { //serial.printf("%d, %d, %d\n", dipFlags, BUTTON1_FLAG, dipFlags & BUTTON1_FLAG); int init_val = dipFlags & BUTTON4_FLAG; - while( (dipFlags & BUTTON4_FLAG) == init_val ) - { + while( (dipFlags & BUTTON4_FLAG) == init_val ) { // do nothing until ready } //serial.printf("%d, %d, %d\n", dipFlags, BUTTON1_FLAG, dipFlags & BUTTON1_FLAG); @@ -964,102 +894,80 @@ //turn180(); //wait_ms(1500); int nextMovement = 0; - + // moveForwardEncoder(1); - + while (1) { - // break; - // prevEncoder0Count = encoder0.getPulses(); - // prevEncoder1Count = encoder1.getPulses(); - // turnRight(); - // wait_ms(2000); - // turnLeft(); - // wait_ms(2000); - // turnRight(); - // wait_ms(2000); - // turnRight(); - // wait_ms(2000); - - 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); - // encoderAfterTurn(1); - moveForwardEncoder(0.4); - nCellEncoderAndIR(0.6); - } - else if (nextMovement == 1){ - justTurned = true; - turnRight(); - } - else if (nextMovement == 2){ - justTurned = true; - turnLeft(); - } - else if (nextMovement == 3){ - nCellEncoderAndIR(1); + if (!overrideFloodFill) { + nextMovement = chooseNextMovement(); + if (nextMovement == 0) { + moveForwardEncoder(0.4); + nCellEncoderAndIR(0.6); + } else if (nextMovement == 1) { + justTurned = true; + turnRight(); + } else if (nextMovement == 2) { + justTurned = true; + turnLeft(); + } else if (nextMovement == 3) { + nCellEncoderAndIR(1); //encoderAfterTurn(1); - } - else if (nextMovement == 4){ - justTurned = true; - turn180(); - } - }else{ - receiverOneReading = IRP_1.getSamples(100); - receiverTwoReading = IRP_2.getSamples(100); - receiverThreeReading = IRP_3.getSamples(100); - receiverFourReading = IRP_4.getSamples(100); - - if(receiverOneReading > IRP_1.sensorAvg * frontStop || receiverFourReading > IRP_4.sensorAvg * frontStop ){ - - int randomNum = rand() %2; - if(randomNum == 0){ - turnRight(); - }else{ - turnLeft(); + } else if (nextMovement == 4) { + justTurned = true; + turn180(); } - }else if ((receiverTwoReading > ((IRP_2.sensorAvg)*averageDivUpper)) && (receiverThreeReading > ((IRP_3.sensorAvg)*averageDivUpper))) { - // both walls there - nCellEncoderAndIR(1); - redLed.write(1); - greenLed.write(0); - blueLed.write(1); - }else if((receiverThreeReading < IRP_3.sensorAvg/5) && (receiverTwoReading < IRP_2.sensorAvg/5)) { - // both sides gone + } else { + receiverOneReading = IRP_1.getSamples(100); + receiverTwoReading = IRP_2.getSamples(100); + receiverThreeReading = IRP_3.getSamples(100); + receiverFourReading = IRP_4.getSamples(100); + + if(receiverOneReading > IRP_1.sensorAvg * frontStop || receiverFourReading > IRP_4.sensorAvg * frontStop ) { + + int randomNum = rand() %2; + if(randomNum == 0) { + turnRight(); + } else { + turnLeft(); + } + } else if ((receiverTwoReading > ((IRP_2.sensorAvg)*averageDivUpper)) && (receiverThreeReading > ((IRP_3.sensorAvg)*averageDivUpper))) { + // both walls there + nCellEncoderAndIR(1); + redLed.write(1); + greenLed.write(0); + blueLed.write(1); + } else if((receiverThreeReading < IRP_3.sensorAvg/5) && (receiverTwoReading < IRP_2.sensorAvg/5)) { + // both sides gone - redLed.write(1); - greenLed.write(0); - blueLed.write(0); - // serial.printf("Going to go over to move forward with encoder, and passing %f\n", valToPass); - moveForwardEncoder(1); - continue; - } else if (receiverThreeReading < IRP_3.sensorAvg/LRAvg) { // right wall gone RED - turnRight(); - redLed.write(0); - greenLed.write(1); - blueLed.write(1); - // moveForwardEncoder(valToPass); - // break; - } else if (receiverTwoReading < IRP_2.sensorAvg/LRAvg) { // left wall gone - turnLeft(); - redLed.write(1); - greenLed.write(1); - blueLed.write(0); - // moveForwardEncoder(valToPass); - // break; - } - } + redLed.write(1); + greenLed.write(0); + blueLed.write(0); + // serial.printf("Going to go over to move forward with encoder, and passing %f\n", valToPass); + moveForwardEncoder(1); + continue; + } else if (receiverThreeReading < IRP_3.sensorAvg/LRAvg) { // right wall gone RED + turnRight(); + redLed.write(0); + greenLed.write(1); + blueLed.write(1); + // moveForwardEncoder(valToPass); + // break; + } else if (receiverTwoReading < IRP_2.sensorAvg/LRAvg) { // left wall gone + turnLeft(); + redLed.write(1); + greenLed.write(1); + blueLed.write(0); + // moveForwardEncoder(valToPass); + // break; + } + } - wait_ms(500); // reduce this once we fine tune this! + wait_ms(500); // reduce this once we fine tune this! //////////////////////// TESTING CODE GOES BELOW /////////////////////////// - + // encoderAfterTurn(1); // waitButton4(); // serial.printf("Encoder 0 is : %d \t Encoder 1 is %d\n",encoder0.getPulses(), encoder1.getPulses() );