MacroRat
Mouse code for the MacroRat
Diff: main.cpp
- Revision:
- 28:8126a4d620e8
- Parent:
- 27:02ce1040f331
- Child:
- 29:ec2c5a69acd6
--- a/main.cpp Sun May 21 10:30:19 2017 +0000
+++ b/main.cpp Sun May 21 13:04:21 2017 +0000
@@ -22,16 +22,16 @@
// #define IP_CONSTANT 8.85
// #define II_CONSTANT 0.005
// #define ID_CONSTANT 3.15
-#define IP_CONSTANT 8.5
-#define II_CONSTANT 0.095
-#define ID_CONSTANT 6.85
+#define IP_CONSTANT 8.15
+#define II_CONSTANT 0.06
+#define ID_CONSTANT 7.5
-const int desiredCount180 = 2800;
+const int desiredCount180 = 2700;
const int desiredCountR = 1575;
const int desiredCountL = 1650;
const int oneCellCount = 5400;
-const int oneCellCountMomentum = 4700;//4800; // one cell count is actually approximately 5400, but this value is considering momentum!
+const int oneCellCountMomentum = 4450;//4800; // one cell count is actually approximately 5400, but this value is considering momentum!
float receiverOneReading = 0.0;
float receiverTwoReading = 0.0;
@@ -45,11 +45,11 @@
float ir4base = 0.0;
-float initAverageL = 8.28;
-float averageDivL = 29.5; //blue
+float initAverageL = 8.25;
+float averageDivL = 27.8; //blue
float initAverageR = 8.75; //4.5
-float averageDivR = 29.5; //red
-float averageDivUpper = 0.9;
+float averageDivR = 28.5; //red
+float averageDivUpper = 0.5;
//IRSAvg= >: 0.143701, 0.128285
@@ -539,8 +539,8 @@
count0 = encoder0.getPulses();
count1 = encoder1.getPulses();
int diff = count0 - count1;
- double kp = 0.00011;
- double kd = 0.00014;
+ double kp = 0.00013;
+ double kd = 0.00016;
int prev = 0;
int counter = 0;
@@ -555,12 +555,12 @@
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
+ if( x < diff - 60 ) // count1 is bigger, right wheel pushed forward
{
left_motor.move( -d );
right_motor.move( d );
}
- else if( x > diff + 40 )
+ else if( x > diff + 60 )
{
left_motor.move( -d );
right_motor.move( d );
@@ -572,7 +572,7 @@
// }
prev = x;
counter++;
- if (counter == 5)
+ if (counter == 10)
break;
}
}
@@ -583,18 +583,15 @@
double derivError = 0;
double sumError = 0;
- double HIGH_PWM_VOLTAGE = 0.1;
- double rightSpeed = 0.10;
- double leftSpeed = 0.10;
+ double HIGH_PWM_VOLTAGE = 0.12;
+ double rightSpeed = 0.12;
+ double leftSpeed = 0.12;
int desiredCount0 = encoder0.getPulses() + oneCellCountMomentum*cellCount;
int desiredCount1 = encoder1.getPulses() + oneCellCountMomentum*cellCount;
- left_motor.forward(0.28);
- right_motor.forward(0.25);
-
- float receiverTwoReading = 0.0;
- float receiverThreeReading = 0.0;
+ left_motor.forward(0.12);
+ right_motor.forward(0.10);
float ir2 = IRP_2.getSamples( SAMPLE_NUM );
float ir3 = IRP_3.getSamples( SAMPLE_NUM );
@@ -605,10 +602,10 @@
int state = 0;
while (encoder0.getPulses() <= desiredCount0 && encoder1.getPulses() <= desiredCount1){
- receiverTwoReading = IRP_2.getSamples(60);
- receiverThreeReading = IRP_3.getSamples(60);
- receiverOneReading = IRP_1.getSamples(60);
- receiverFourReading = IRP_4.getSamples(60);
+ receiverTwoReading = IRP_2.getSamples(50);
+ receiverThreeReading = IRP_3.getSamples(50);
+ receiverOneReading = IRP_1.getSamples(50);
+ receiverFourReading = IRP_4.getSamples(50);
if( receiverOneReading > IRP_1.sensorAvg*0.70 || receiverFourReading > IRP_4.sensorAvg*0.70 ){
if (currDir % 4 == 0){
@@ -626,28 +623,8 @@
break;
}
- if((receiverThreeReading < 1.3*IRP_3.sensorAvg/(averageDivR)) && (receiverTwoReading < 1.3*IRP_2.sensorAvg/(averageDivL))){
+ if((receiverThreeReading < 3*IRP_3.sensorAvg/(averageDivR)) && (receiverTwoReading < 3*IRP_2.sensorAvg/(averageDivL))){
// both sides gone
- redLed.write(1);
- greenLed.write(1);
- blueLed.write(1);
- wait_ms(100);
- redLed.write(1);
- greenLed.write(1);
- blueLed.write(0);
- wait_ms(200);
- redLed.write(1);
- greenLed.write(1);
- blueLed.write(0);
- wait_ms(200);
- redLed.write(1);
- greenLed.write(1);
- blueLed.write(0);
- wait_ms(200);
- redLed.write(1);
- greenLed.write(1);
- blueLed.write(0);
-
int prev0 = encoder0.getPulses();
int prev1 = encoder1.getPulses();
int diff0 = desiredCount0 - prev0;
@@ -655,7 +632,7 @@
int valToPass = ((diff0 + diff1)/2)/(oneCellCountMomentum);
moveForwardEncoder(valToPass);
}
- else if (receiverThreeReading < IRP_3.sensorAvg/averageDivR){// right wall gone
+ else if (receiverThreeReading < 3*IRP_3.sensorAvg/averageDivR){// right wall gone
// if (currDir % 4 == 0){
// wallArray[MAZE_LEN - 1 - (mouseY + 1)][mouseX] |= L_WALL;
// }
@@ -673,7 +650,7 @@
redLed.write(0);
greenLed.write(1);
blueLed.write(1);
- }else if (receiverTwoReading < IRP_2.sensorAvg/averageDivL){// left wall gone
+ }else if (receiverTwoReading < 3*IRP_2.sensorAvg/averageDivL){// left wall gone
// if (currDir % 4 == 0){
// wallArray[MAZE_LEN - 1 - (mouseY + 1)][mouseX] |= R_WALL;
// }
@@ -692,22 +669,22 @@
greenLed.write(1);
blueLed.write(0);
}else if ((receiverTwoReading > ((IRP_2.sensorAvg/initAverageL)*averageDivUpper)) && (receiverThreeReading > ((IRP_3.sensorAvg/initAverageR)*averageDivUpper))){
- if (currDir % 4 == 0){
- wallArray[MAZE_LEN - 1 - (mouseY + 1)][mouseX] |= R_WALL;
- wallArray[MAZE_LEN - 1 - (mouseY + 1)][mouseX] |= L_WALL;
- }
- else if (currDir % 4 == 1){
- wallArray[MAZE_LEN - 1 - (mouseY)][mouseX+1] |= F_WALL;
- wallArray[MAZE_LEN - 1 - (mouseY)][mouseX+1] |= B_WALL;
- }
- else if (currDir % 4 == 2){
- wallArray[MAZE_LEN - 1 - (mouseY - 1)][mouseX] |= R_WALL;
- wallArray[MAZE_LEN - 1 - (mouseY - 1)][mouseX] |= L_WALL;
- }
- else if (currDir % 4 == 3){
- wallArray[MAZE_LEN - 1 - (mouseY)][mouseX-1] |= F_WALL;
- wallArray[MAZE_LEN - 1 - (mouseY)][mouseX-1] |= B_WALL;
- }
+ // if (currDir % 4 == 0){
+ // wallArray[MAZE_LEN - 1 - (mouseY + 1)][mouseX] |= R_WALL;
+ // wallArray[MAZE_LEN - 1 - (mouseY + 1)][mouseX] |= L_WALL;
+ // }
+ // else if (currDir % 4 == 1){
+ // wallArray[MAZE_LEN - 1 - (mouseY)][mouseX+1] |= F_WALL;
+ // wallArray[MAZE_LEN - 1 - (mouseY)][mouseX+1] |= B_WALL;
+ // }
+ // else if (currDir % 4 == 2){
+ // wallArray[MAZE_LEN - 1 - (mouseY - 1)][mouseX] |= R_WALL;
+ // wallArray[MAZE_LEN - 1 - (mouseY - 1)][mouseX] |= L_WALL;
+ // }
+ // else if (currDir % 4 == 3){
+ // wallArray[MAZE_LEN - 1 - (mouseY)][mouseX-1] |= F_WALL;
+ // wallArray[MAZE_LEN - 1 - (mouseY)][mouseX-1] |= B_WALL;
+ // }
// both walls there
state = 0;
redLed.write(1);
@@ -730,7 +707,6 @@
}
default:{
currentError = ( receiverTwoReading - IRP_2.sensorAvg/initAverageL) - ( receiverThreeReading - IRP_3.sensorAvg/initAverageR);
- //currentError = ( receiverTwoReading - IRP_2.sensorAvg/initAverageL) - ( receiverThreeReading - IRP_3.sensorAvg/initAverageR);
break;
}
}
@@ -755,25 +731,24 @@
pidOnEncoders();
previousError = currentError;
- ir2 = IRP_2.getSamples( SAMPLE_NUM );
- ir3 = IRP_3.getSamples( SAMPLE_NUM );
}
- if (currDir % 4 == 0){
+ if (encoder0.getPulses() >= 0.6*desiredCount0 && encoder1.getPulses() >= 0.6*desiredCount1){
+ if (currDir % 4 == 0){
mouseY += 1;
- }
- else if (currDir % 4 == 1){
- mouseX + 1;
- }
- else if (currDir % 4 == 2){
- mouseY -= 1;
- }
- else if (currDir % 4 == 3){
- mouseX -= 1;
+ }
+ else if (currDir % 4 == 1){
+ mouseX + 1;
+ }
+ else if (currDir % 4 == 2){
+ mouseY -= 1;
+ }
+ else if (currDir % 4 == 3){
+ mouseX -= 1;
+ }
}
left_motor.brake();
right_motor.brake();
- return;
}
bool isWallInFront(int x, int y){
@@ -1132,31 +1107,60 @@
wallArray[MAZE_LEN - 1 - mouseY][mouseX] = 0xE;
visitedCells[MAZE_LEN - 1 - mouseY][mouseX] = 1;
+ int prevEncoder0Count = 0;
+ int prevEncoder1Count = 0;
+ int currEncoder0Count = 0;
+ int currEncoder1Count = 0;
+
+ bool overrideFloodFill = false;
//right_motor.forward( 0.2 );
//left_motor.forward( 0.2 );
//turnRight180();
//wait_ms(1500);
int nextMovement = 0;
while (1) {
- nextMovement = chooseNextMovement();
- if (nextMovement == 0){
- nCellEncoderAndIR(1);
- }
- else if (nextMovement == 1){
- justTurned = true;
- turnRight();
+ prevEncoder0Count = encoder0.getPulses();
+ prevEncoder1Count = encoder1.getPulses();
+
+ if (!overrideFloodFill){
+ nextMovement = chooseNextMovement();
+ if (nextMovement == 0){
+ nCellEncoderAndIR(1);
+ }
+ else if (nextMovement == 1){
+ justTurned = true;
+ turnRight();
+ }
+ else if (nextMovement == 2){
+ justTurned = true;
+ turnLeft();
+ }
+ else if (nextMovement == 3){
+ nCellEncoderAndIR(1);
+ }
+ else if (nextMovement == 4){
+ justTurned = true;
+ turnRight180();
+ }
}
- else if (nextMovement == 2){
- justTurned = true;
- turnLeft();
+ else{
+ overrideFloodFill = false;
+ if ((rand()%2 + 1) == 1){
+ justTurned = true;
+ turnLeft();
+ }
+ else{
+ justTurned = true;
+ turnRight();
+ }
}
- else if (nextMovement == 3){
- nCellEncoderAndIR(1);
+ currEncoder0Count = encoder0.getPulses();
+ currEncoder1Count = encoder1.getPulses();
+
+ if (!justTurned && (currEncoder0Count <= prevEncoder0Count + 100) && (currEncoder1Count <= prevEncoder1Count + 100) && !overrideFloodFill){
+ overrideFloodFill = true;
}
- else if (nextMovement == 4){
- justTurned = true;
- turnRight180();
- }
+
wait_ms(1000); // reduce this once we fine tune this!
//wait_ms(1500);