MacroRat
/
MouseCode
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Revision 28:8126a4d620e8, committed 2017-05-21
- Comitter:
- sahilmgandhi
- Date:
- Sun May 21 13:04:21 2017 +0000
- Parent:
- 27:02ce1040f331
- Child:
- 29:ec2c5a69acd6
- Commit message:
- Final commit before CAMM ... it is not looking so good with floodfill and detecting empty spots in the maze
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- 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);