Mouse code for the MacroRat
Diff: main.cpp
- Revision:
- 29:ec2c5a69acd6
- Parent:
- 28:8126a4d620e8
- Child:
- 31:9b71b44e0867
--- a/main.cpp Sun May 21 13:04:21 2017 +0000 +++ b/main.cpp Wed May 24 01:57:01 2017 +0000 @@ -12,44 +12,7 @@ #include "stm32f4xx.h" #include "QEI.h" -/* Constants for when HIGH_PWM_VOLTAGE = 0.2 -#define IP_CONSTANT 6 -#define II_CONSTANT 0 -#define ID_CONSTANT 1 -*/ - -// Constants for when HIGH_PWM_VOLTAGE = 0.1 -// #define IP_CONSTANT 8.85 -// #define II_CONSTANT 0.005 -// #define ID_CONSTANT 3.15 -#define IP_CONSTANT 8.15 -#define II_CONSTANT 0.06 -#define ID_CONSTANT 7.5 - -const int desiredCount180 = 2700; -const int desiredCountR = 1575; -const int desiredCountL = 1650; - -const int oneCellCount = 5400; -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; -float receiverThreeReading = 0.0; -float receiverFourReading = 0.0; -float ir1base = 0.0; -float ir2base = 0.0; - -float ir3base = 0.0; - -float ir4base = 0.0; - -float initAverageL = 8.25; -float averageDivL = 27.8; //blue -float initAverageR = 8.75; //4.5 -float averageDivR = 28.5; //red -float averageDivUpper = 0.5; //IRSAvg= >: 0.143701, 0.128285 @@ -70,200 +33,9 @@ //0.003795, //0.005297, 0.007064 - -float noWallR = 0.007; -float noWallL = 0.007; - + void pidOnEncoders(); - - -void turnLeft() -{ - double speed0 = 0.11; - double speed1 = -0.13; - - int counter = 0; - int initial0 = encoder0.getPulses(); - int initial1 = encoder1.getPulses(); - - int desiredCount0 = initial0 - desiredCountL; - int desiredCount1 = initial1 + desiredCountL; - - int count0 = initial0; - int count1 = initial1; - - double error0 = count0 - desiredCount0; - double error1 = count1 - desiredCount1; - - while(1) { - - if(!(abs(error0) < 1) && !(abs(error1) < 1)) { - count0 = encoder0.getPulses(); - count1 = encoder1.getPulses(); - - error0 = count0 - desiredCount0; - error1 = count1 - desiredCount1; - - right_motor.move(speed0); - left_motor.move(speed1); - counter = 0; - } else { - counter++; - right_motor.brake(); - left_motor.brake(); - } - - if (counter > 60) { - break; - } - } - - right_motor.brake(); - left_motor.brake(); - turnFlag = 0; // zeroing out the flags! - currDir -= 1; -} - -void turnRight() -{ - double speed0 = -0.11; - double speed1 = 0.13; - - int counter = 0; - int initial0 = encoder0.getPulses(); - int initial1 = encoder1.getPulses(); - - int desiredCount0 = initial0 + desiredCountR; - int desiredCount1 = initial1 - desiredCountR; - - int count0 = initial0; - int count1 = initial1; - - double error0 = count0 - desiredCount0; - double error1 = count1 - desiredCount1; - - while(1) { - - if(!(abs(error0) < 1) && !(abs(error1) < 1)) { - count0 = encoder0.getPulses(); - count1 = encoder1.getPulses(); - - error0 = count0 - desiredCount0; - error1 = count1 - desiredCount1; - - right_motor.move(speed0); - left_motor.move(speed1); - counter = 0; - } else { - counter++; - right_motor.brake(); - left_motor.brake(); - } - - if (counter > 60) { - break; - } - } - - right_motor.brake(); - left_motor.brake(); - turnFlag = 0; - currDir += 1; -} - -void turnLeft180() -{ - double speed0 = 0.15; - double speed1 = -0.15; - - int counter = 0; - int initial0 = encoder0.getPulses(); - int initial1 = encoder1.getPulses(); - - int desiredCount0 = initial0 - desiredCountL*2; - int desiredCount1 = initial1 + desiredCountL*2; - - int count0 = initial0; - int count1 = initial1; - - double error0 = count0 - desiredCount0; - double error1 = count1 - desiredCount1; - - - while(1) { - - if(!(abs(error0) < 1) && !(abs(error1) < 1)) { - count0 = encoder0.getPulses(); - count1 = encoder1.getPulses(); - - error0 = count0 - desiredCount0; - error1 = count1 - desiredCount1; - - right_motor.move(speed0); - left_motor.move(speed1); - counter = 0; - } else { - counter++; - right_motor.brake(); - left_motor.brake(); - } - - if (counter > 60) { - break; - } - } - - right_motor.brake(); - left_motor.brake(); - currDir -= 2; -} - -void turnRight180() -{ - double speed0 = -0.16; - double speed1 = 0.16; - - int counter = 0; - int initial0 = encoder0.getPulses(); - int initial1 = encoder1.getPulses(); - - int desiredCount0 = initial0 + desiredCount180; - int desiredCount1 = initial1 - desiredCount180; - - int count0 = initial0; - int count1 = initial1; - - double error0 = count0 - desiredCount0; - double error1 = count1 - desiredCount1; - - - while(1) { - - if(!(abs(error0) < 1) && !(abs(error1) < 1)) { - count0 = encoder0.getPulses(); - count1 = encoder1.getPulses(); - - error0 = count0 - desiredCount0; - error1 = count1 - desiredCount1; - - right_motor.move(speed0); - left_motor.move(speed1); - counter = 0; - } else { - counter++; - right_motor.brake(); - left_motor.brake(); - } - - if (counter > 60) { - break; - } - } - right_motor.brake(); - left_motor.brake(); - currDir += 2; -} - + void moveForwardCellEncoder(double cellNum){ int desiredCount0 = encoder0.getPulses() + oneCellCountMomentum*cellNum; int desiredCount1 = encoder1.getPulses() + oneCellCountMomentum*cellNum; @@ -583,15 +355,17 @@ double derivError = 0; double sumError = 0; - double HIGH_PWM_VOLTAGE = 0.12; - double rightSpeed = 0.12; - double leftSpeed = 0.12; + double HIGH_PWM_VOLTAGE_R = 0.15; + double HIGH_PWM_VOLTAGE_L = 0.16; + + double rightSpeed = 0.15; + double leftSpeed = 0.16; int desiredCount0 = encoder0.getPulses() + oneCellCountMomentum*cellCount; int desiredCount1 = encoder1.getPulses() + oneCellCountMomentum*cellCount; - left_motor.forward(0.12); - right_motor.forward(0.10); + left_motor.forward(0.16); + right_motor.forward(0.15); float ir2 = IRP_2.getSamples( SAMPLE_NUM ); float ir3 = IRP_3.getSamples( SAMPLE_NUM ); @@ -602,10 +376,10 @@ int state = 0; while (encoder0.getPulses() <= desiredCount0 && encoder1.getPulses() <= desiredCount1){ - receiverTwoReading = IRP_2.getSamples(50); - receiverThreeReading = IRP_3.getSamples(50); - receiverOneReading = IRP_1.getSamples(50); - receiverFourReading = IRP_4.getSamples(50); + receiverTwoReading = IRP_2.getSamples(100); + receiverThreeReading = IRP_3.getSamples(100); + receiverOneReading = IRP_1.getSamples(100); + receiverFourReading = IRP_4.getSamples(100); if( receiverOneReading > IRP_1.sensorAvg*0.70 || receiverFourReading > IRP_4.sensorAvg*0.70 ){ if (currDir % 4 == 0){ @@ -715,11 +489,11 @@ derivError = currentError - previousError; double PIDSum = IP_CONSTANT*currentError + II_CONSTANT*sumError + ID_CONSTANT*derivError; if (PIDSum > 0) { // this means the leftWheel is faster than the right. So right speeds up, left slows down - rightSpeed = HIGH_PWM_VOLTAGE - abs(PIDSum*HIGH_PWM_VOLTAGE); - leftSpeed = HIGH_PWM_VOLTAGE + abs(PIDSum*HIGH_PWM_VOLTAGE); + rightSpeed = HIGH_PWM_VOLTAGE_R - abs(PIDSum*HIGH_PWM_VOLTAGE_R); + leftSpeed = HIGH_PWM_VOLTAGE_L + abs(PIDSum*HIGH_PWM_VOLTAGE_L); } else { // r is faster than L. speed up l, slow down r - rightSpeed = HIGH_PWM_VOLTAGE + abs(PIDSum*HIGH_PWM_VOLTAGE); - leftSpeed = HIGH_PWM_VOLTAGE - abs(PIDSum*HIGH_PWM_VOLTAGE); + rightSpeed = HIGH_PWM_VOLTAGE_R + abs(PIDSum*HIGH_PWM_VOLTAGE_R); + leftSpeed = HIGH_PWM_VOLTAGE_L - abs(PIDSum*HIGH_PWM_VOLTAGE_L); } if (leftSpeed > 0.30) leftSpeed = 0.30; if (leftSpeed < 0) leftSpeed = 0; @@ -746,6 +520,7 @@ mouseX -= 1; } } + left_motor.brake(); right_motor.brake(); @@ -987,55 +762,59 @@ void changeManhattanDistance(bool headCenter){ if (headCenter){ - for (int i = 0; i < MAZE_LEN / 2; i++) { - for (int j = 0; j < MAZE_LEN / 2; j++) { - manhattanDistances[MAZE_LEN - 1 - j][i] = abs(7 - j) + abs(7 - i); + for (int i = 0; i < MAZE_LEN; i++) { + for (int j = 0; j < MAZE_LEN; j++) { + distanceToCenter[i][j] = manhattanDistances[i][j]; } } - for (int i = MAZE_LEN / 2; i < MAZE_LEN; i++) { - for (int j = 0; j < MAZE_LEN / 2; j++) { - manhattanDistances[MAZE_LEN - 1 - j][i] = abs(7 - j) + abs(8 - i); - } - } - for (int i = 0; i < MAZE_LEN / 2; i++) { - for (int j = MAZE_LEN / 2; j < MAZE_LEN; j++) { - manhattanDistances[MAZE_LEN - 1 - j][i] = abs(8 - j) + abs(7 - i); - } - } - for (int i = MAZE_LEN / 2; i < MAZE_LEN; i++) { - for (int j = MAZE_LEN / 2; j < MAZE_LEN; j++) { - manhattanDistances[MAZE_LEN - 1 - j][i] = abs(8 - j) + abs(8 - i); + for (int i = 0; i < MAZE_LEN; i++) { + for (int j = 0; j < MAZE_LEN; j++) { + manhattanDistances[i][j] = distanceToStart[i][j]; } } } else { - for (int i = 0; i < MAZE_LEN / 2; i++) { + for (int i = 0; i < MAZE_LEN; i++) { + for (int j = 0; j < MAZE_LEN; j++) { + distanceToStart[i][j] = manhattanDistances[i][j]; + } + } + for (int i = 0; i < MAZE_LEN; i++) { + for (int j = 0; j < MAZE_LEN; j++) { + manhattanDistances[i][j] = distanceToCenter[i][j]; + } + } + } +} + +void initializeDistances(){ + for (int i = 0; i < MAZE_LEN / 2; i++) { for (int j = 0; j < MAZE_LEN / 2; j++) { - manhattanDistances[MAZE_LEN - 1 - j][i] = abs(0 - j) + abs(0 - i); + distanceToStart[MAZE_LEN - 1 - j][i] = abs(0 - j) + abs(0 - i); } } for (int i = MAZE_LEN / 2; i < MAZE_LEN; i++) { for (int j = 0; j < MAZE_LEN / 2; j++) { - manhattanDistances[MAZE_LEN - 1 - j][i] = abs(0 - j) + abs(0 - i); + distanceToStart[MAZE_LEN - 1 - j][i] = abs(0 - j) + abs(0 - i); } } for (int i = 0; i < MAZE_LEN / 2; i++) { for (int j = MAZE_LEN / 2; j < MAZE_LEN; j++) { - manhattanDistances[MAZE_LEN - 1 - j][i] = abs(0 - j) + abs(0 - i); + distanceToStart[MAZE_LEN - 1 - j][i] = abs(0 - j) + abs(0 - i); } } for (int i = MAZE_LEN / 2; i < MAZE_LEN; i++) { for (int j = MAZE_LEN / 2; j < MAZE_LEN; j++) { - manhattanDistances[MAZE_LEN - 1 - j][i] = abs(0 - j) + abs(0 - i); + distanceToStart[MAZE_LEN - 1 - j][i] = abs(0 - j) + abs(0 - i); } } - } } - + int main() { //Set highest bandwidth. //gyro.setLpBandwidth(LPFBW_42HZ); + initializeDistances(); serial.baud(9600); //serial.printf("The gyro's address is %s", gyro.getWhoAmI()); @@ -1119,49 +898,49 @@ //wait_ms(1500); 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; - turnRight180(); - } - } - 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(1000); // reduce this once we fine tune this! + // 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{ +// 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! //wait_ms(1500); //turnRight(); @@ -1263,10 +1042,9 @@ //break; //pidOnEncoders(); // moveForwardUntilWallIr(); - //serial.printf("%i, %i, %i\n", gyro.getGyroX(), gyro.getGyroY(), gyro.getGyroZ()); //serial.printf("Pulse Count=> e0:%d, e1:%d \r\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 ); + 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 ); //reading = Rec_4.read(); // serial.printf("reading: %f\n", reading);