MacroRat
Mouse code for the MacroRat
Diff: main.cpp
- Revision:
- 39:058fb32c24e0
- Parent:
- 38:fe05f93009a2
- Child:
- 40:465d2b565977
--- a/main.cpp Sat May 27 21:29:55 2017 +0000
+++ b/main.cpp Sun May 28 03:42:59 2017 +0000
@@ -40,7 +40,7 @@
#define IP_CONSTANT 2.00
#define II_CONSTANT 0.00001
-#define ID_CONSTANT 0.190
+#define ID_CONSTANT 0.2
void pidOnEncoders();
@@ -93,7 +93,7 @@
receiverOneReading = IRP_1.getSamples(100);
receiverTwoReading = IRP_4.getSamples(100);
- while( ((encoder0.getPulses() - initial0) <= (oneCellCountMomentum-200)*num && (encoder1.getPulses() - initial1) <= (oneCellCountMomentum-200)*num) && receiverOneReading < IRP_1.sensorAvg*4 && receiverFourReading < IRP_4.sensorAvg*4) {
+ while( ((encoder0.getPulses() - initial0) <= (oneCellCountMomentum-200)*num && (encoder1.getPulses() - initial1) <= (oneCellCountMomentum-200)*num) && 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 ));
@@ -289,7 +289,6 @@
int desiredCount0 = encoder0.getPulses() + oneCellCountMomentum*cellCount;
int desiredCount1 = encoder1.getPulses() + oneCellCountMomentum*cellCount;
- serial.printf("%d, %d\n", desiredCount0, desiredCount1 );
left_motor.forward(WHEEL_SPEED);
right_motor.forward(WHEEL_SPEED);
@@ -306,7 +305,7 @@
receiverOneReading = IRP_1.getSamples( SAMPLE_NUM );
receiverFourReading = IRP_4.getSamples( SAMPLE_NUM );
// serial.printf("IR2 = %f, IR2AVE = %f, IR3 = %f, IR3_AVE = %f\n", receiverTwoReading, IRP_2.sensorAvg, receiverThreeReading, IRP_3.sensorAvg);
- if( receiverOneReading > IRP_1.sensorAvg * 5.5 || receiverFourReading > IRP_4.sensorAvg * 5.5) {
+ if( receiverOneReading > IRP_1.sensorAvg * frontStop || receiverFourReading > IRP_4.sensorAvg * frontStop ) {
break;
}
@@ -317,15 +316,18 @@
double diff0 = desiredCount0 - prev0;
double diff1 = desiredCount1 - prev1;
double valToPass = ((diff0 + diff1)/2)/(oneCellCountMomentum);
+ 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(valToPass);
continue;
- } else if (receiverThreeReading < IRP_3.sensorAvg/3.5) { // right wall gone RED
+ } else if (receiverThreeReading < IRP_3.sensorAvg/LRAvg) { // right wall gone RED
state = 1;
redLed.write(0);
greenLed.write(1);
blueLed.write(1);
- } else if (receiverTwoReading < IRP_2.sensorAvg/3.5) { // left wall gone
+ } else if (receiverTwoReading < IRP_2.sensorAvg/LRAvg) { // left wall gone
// BLUE BLUE BLUE BLUE
state = 2;
redLed.write(1);
@@ -363,14 +365,30 @@
sumError += currentError;
derivError = currentError - previousError;
double PIDSum = IP_CONSTANT*currentError + II_CONSTANT*sumError + ID_CONSTANT*derivError;
+
+ double prev0 = encoder0.getPulses();
+ double prev1 = encoder1.getPulses();
+ double diff0 = desiredCount0 - prev0;
+ double diff1 = desiredCount1 - prev1;
+ double kp = .1/1000;
+
+
+ rightSpeed = HIGH_PWM_VOLTAGE_R;
+ leftSpeed = HIGH_PWM_VOLTAGE_L;
+ if (diff1 < 1000 || diff0 < 1000) {
+ rightSpeed = kp * HIGH_PWM_VOLTAGE_R;
+ leftSpeed = kp * HIGH_PWM_VOLTAGE_L;
+ }
+
if (PIDSum > 0) { // this means the leftWheel is faster than the right. So right speeds up, left slows down
- rightSpeed = HIGH_PWM_VOLTAGE_R - abs(PIDSum*HIGH_PWM_VOLTAGE_R);
- leftSpeed = HIGH_PWM_VOLTAGE_L + abs(PIDSum*HIGH_PWM_VOLTAGE_L);
+ rightSpeed = rightSpeed - abs(PIDSum*HIGH_PWM_VOLTAGE_R);
+ leftSpeed = leftSpeed + abs(PIDSum*HIGH_PWM_VOLTAGE_L);
} else { // r is faster than L. speed up l, slow down r
- rightSpeed = HIGH_PWM_VOLTAGE_R + abs(PIDSum*HIGH_PWM_VOLTAGE_R);
- leftSpeed = HIGH_PWM_VOLTAGE_L - abs(PIDSum*HIGH_PWM_VOLTAGE_L);
+ rightSpeed = rightSpeed + abs(PIDSum*HIGH_PWM_VOLTAGE_R);
+ leftSpeed = leftSpeed - abs(PIDSum*HIGH_PWM_VOLTAGE_L);
}
+
//serial.printf("%f, %f\n", leftSpeed, rightSpeed);
if (leftSpeed > 0.30) leftSpeed = 0.30;
if (leftSpeed < 0) leftSpeed = 0;
@@ -409,37 +427,59 @@
receiverThreeReading = IRP_3.getSamples(75);
receiverFourReading = IRP_4.getSamples(75);
- if (receiverOneReading >= 0.3f && receiverFourReading >= 0.3f) {
+ // 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");
if (currDir % 4 == 0) {
wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= F_WALL;
} else if (currDir % 4 == 1) {
wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= R_WALL;
} else if (currDir % 4 == 2) {
+ wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= B_WALL;
+ } else if (currDir % 4 == 3) {
wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= L_WALL;
+ }
+ }
+ 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\n");
+ if (currDir % 4 == 0) {
+ wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= L_WALL;
+ } else if (currDir % 4 == 1) {
+ wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= F_WALL;
+ } else if (currDir % 4 == 2) {
+ wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= R_WALL;
} else if (currDir % 4 == 3) {
wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= B_WALL;
}
- }
- if (receiverThreeReading >= IRP_3.sensorAvg*0.6) {
+ } else if (receiverTwoReading < IRP_2.sensorAvg/LRAvg) { // left wall gone, right wall is there
+ // serial.printf("Right wall\n");
if (currDir % 4 == 0) {
- wallArray[MAZE_LEN - 1 - (mouseY + 1)][mouseX] |= R_WALL;
+ wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= R_WALL;
} else if (currDir % 4 == 1) {
- wallArray[MAZE_LEN - 1 - (mouseY)][mouseX+1] |= B_WALL;
+ wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= B_WALL;
} else if (currDir % 4 == 2) {
- wallArray[MAZE_LEN - 1 - (mouseY - 1)][mouseX] |= L_WALL;
+ wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= L_WALL;
} else if (currDir % 4 == 3) {
- wallArray[MAZE_LEN - 1 - (mouseY)][mouseX-1] |= F_WALL;
+ wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= F_WALL;
}
- }
- if (receiverTwoReading >= IRP_2.sensorAvg*0.6) {
- if (currDir % 4 == 0) {
- wallArray[MAZE_LEN - 1 - (mouseY + 1)][mouseX] |= L_WALL;
- } else if (currDir % 4 == 1) {
- wallArray[MAZE_LEN - 1 - (mouseY)][mouseX+1] |= F_WALL;
- } else if (currDir % 4 == 2) {
- wallArray[MAZE_LEN - 1 - (mouseY - 1)][mouseX] |= R_WALL;
- } else if (currDir % 4 == 3) {
- wallArray[MAZE_LEN - 1 - (mouseY)][mouseX-1] |= B_WALL;
+ } else if ((receiverTwoReading > ((IRP_2.sensorAvg)*averageDivUpper)) && (receiverThreeReading > ((IRP_3.sensorAvg)*averageDivUpper))) {
+ // 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;
+ } 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){
+ wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= R_WALL;
+ wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= L_WALL;
+ } else if (currDir %4 == 3){
+ wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= F_WALL;
+ wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= B_WALL;
}
}
}
@@ -743,7 +783,7 @@
// do nothing until ready
}
//serial.printf("%d, %d, %d\n", dipFlags, BUTTON1_FLAG, dipFlags & BUTTON1_FLAG);
- wait( 2 );
+ wait( 3 );
}
int main()
@@ -807,89 +847,63 @@
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;
+ int prevEncoder0Count = 0;
+ int prevEncoder1Count = 0;
+ int currEncoder0Count = 0;
+ int currEncoder1Count = 0;
- //bool overrideFloodFill = false;
+ bool overrideFloodFill = false;
right_motor.forward( WHEEL_SPEED );
left_motor.forward( WHEEL_SPEED );
//turn180();
//wait_ms(1500);
- //int nextMovement = 0;
+ int nextMovement = 0;
+
+
while (1) {
- // 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;
-// turn180();
-// }
-// }
-// else{
-// overrideFloodFill = false;
-// if ((rand()%2 + 1) == 1){
-// justTurned = true;
-// turnLeft();
-// }
-// else{
-// justTurned = true;
-// turnRight();
-// }
-// }
-// currEncoder0Count = encoder0.getPulses();
-// currEncoder1Count = encoder1.getPulses();
-//
-// if (!justTurned && (currEncoder0Count <= prevEncoder0Count + 100) && (currEncoder1Count <= prevEncoder1Count + 100) && !overrideFloodFill){
-// overrideFloodFill = true;
-// }
-//
-// wait_ms(300); // reduce this once we fine tune this!
+ prevEncoder0Count = encoder0.getPulses();
+ prevEncoder1Count = encoder1.getPulses();
+
+ if (!overrideFloodFill){
+ nextMovement = chooseNextMovement();
+ serial.printf("MouseX: %d, MouseY: %d\n", mouseX, mouseY);
+ 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;
+ turn180();
+ }
+ }
+ else{
+ overrideFloodFill = false;
+ }
+ currEncoder0Count = encoder0.getPulses();
+ currEncoder1Count = encoder1.getPulses();
+
+ if (!justTurned && (currEncoder0Count <= prevEncoder0Count + 100) && (currEncoder1Count <= prevEncoder1Count + 100) && !overrideFloodFill){
+ overrideFloodFill = true;
+ waitButton4();
+ }
+
+ wait_ms(500); // reduce this once we fine tune this!
//////////////////////// TESTING CODE GOES BELOW ///////////////////////////
- nCellEncoderAndIR(4);
- wait_ms(300);
- turnRight();
- nCellEncoderAndIR(1);
- wait_ms(300);
- turnRight();
- nCellEncoderAndIR(4);
- wait_ms(300);
- turnLeft();
- wait_ms(300);
- nCellEncoderAndIR(1);
- wait_ms(300);
- turnLeft();
- wait_ms(300);
- nCellEncoderAndIR(3);
- wait_ms(300);
- turnRight();
- wait_ms(300);
- nCellEncoderAndIR(3);
- wait_ms(300);
- turnRight();
- wait_ms(300);
- nCellEncoderAndIR(3);
- waitButton4();
+ // nCellEncoderAndIR(2);
+ // waitButton4();
// 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 );