MacroRat
Mouse code for the MacroRat
Diff: main.cpp
- Revision:
- 45:8b0bee6baf38
- Parent:
- 44:85bf2c0cd518
- Child:
- 46:b156ef445742
--- 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() );