MacroRat
Mouse code for the MacroRat
Diff: main.cpp
- Revision:
- 43:f22168a05c3e
- Parent:
- 42:75257e6c4c76
- Child:
- 44:85bf2c0cd518
--- 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