Mouse code for the MacroRat
Diff: main.cpp
- Revision:
- 25:f827a8b7880e
- Parent:
- 24:e7063765d6f0
- Child:
- 26:d20f1adac2d3
diff -r e7063765d6f0 -r f827a8b7880e main.cpp --- a/main.cpp Sun May 21 01:04:26 2017 +0000 +++ b/main.cpp Sun May 21 08:52:43 2017 +0000 @@ -1,109 +1,125 @@ #include "mbed.h" - + #include "irpair.h" #include "main.h" #include "motor.h" - + #include <stdlib.h> #include "ITG3200.h" #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 13.5 +#define IP_CONSTANT 8.5 #define II_CONSTANT 0.095 -#define ID_CONSTANT 8.85 - -const int desiredCount180 = 2900; -const int desiredCountR = 1500; -const int desiredCountL = 1575; - +#define ID_CONSTANT 6.85 + +const int desiredCount180 = 2850; +const int desiredCountR = 1575; +const int desiredCountL = 1650; + const int oneCellCount = 5400; -const int oneCellCountMomentum = 4400;//4800; // one cell count is actually approximately 5400, but this value is considering momentum! - +const int oneCellCountMomentum = 4700; // 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; - -//IRSAvg= >: 0.143701, 0.128285 - - - - - - - - //facing wall ir2 and ir3 - //0.144562, 0.113971 in maze - - // normal hall ir2 and ir3 - //0.013665, 0.010889 in maze - - //0.014462, 0.009138 - // 0.013888, 0.010530 - - - //no walls ir2 and ir3 - //0.003265, 0.002904 in maze9 - - //0.003162, 0.003123 - //0.003795, - -//0.005297, 0.007064 - - - float noWallR = 0.007; - float noWallL = 0.007; - +float initAverageL = 8.28; +float averageDivL = 29.5; //blue +float initAverageR = 8.75; //4.5 +float averageDivR = 29.5; //red +float averageDivUpper = 0.9; + 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; @@ -112,45 +128,46 @@ right_motor.brake(); left_motor.brake(); } - + if (counter > 60) { break; } } - + right_motor.brake(); left_motor.brake(); - turnFlag = 0; // zeroing out the flags! - currDir -= 1; + turnFlag = 0; + currDir += 1; } - -void turnRight() + +void turnLeft180() { - double speed0 = -0.11; - double speed1 = 0.13; - + double speed0 = 0.15; + double speed1 = -0.15; + int counter = 0; int initial0 = encoder0.getPulses(); int initial1 = encoder1.getPulses(); - - int desiredCount0 = initial0 + desiredCountR; - int desiredCount1 = initial1 - desiredCountR; - + + 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; @@ -159,46 +176,45 @@ right_motor.brake(); left_motor.brake(); } - + if (counter > 60) { break; } } - + right_motor.brake(); left_motor.brake(); - turnFlag = 0; - currDir += 1; + currDir -= 2; } - -void turnLeft180() + +void turnRight180() { - double speed0 = 0.15; - double speed1 = -0.15; - + double speed0 = -0.16; + double speed1 = 0.16; + int counter = 0; int initial0 = encoder0.getPulses(); int initial1 = encoder1.getPulses(); - - int desiredCount0 = initial0 - desiredCountL*2; - int desiredCount1 = initial1 + desiredCountL*2; - + + 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; @@ -207,54 +223,7 @@ 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; } @@ -263,11 +232,11 @@ left_motor.brake(); currDir += 2; } - + void moveForwardCellEncoder(double cellNum){ int desiredCount0 = encoder0.getPulses() + oneCellCountMomentum*cellNum; int desiredCount1 = encoder1.getPulses() + oneCellCountMomentum*cellNum; - + left_motor.forward(0.125); right_motor.forward(0.095); wait_ms(1); @@ -275,23 +244,23 @@ receiverTwoReading = IRP_2.getSamples(100); receiverThreeReading = IRP_3.getSamples(100); // serial.printf("Average 2: %f Average 3: %f Sensor 2: %f Sensor 3: %f\n", IRP_2.sensorAvg, IRP_3.sensorAvg, receiverTwoReading, receiverThreeReading); - if (receiverThreeReading < ir3base){ + if (receiverThreeReading < IRP_3.sensorAvg/averageDivR){ // redLed.write(1); // blueLed.write(0); turnFlag |= RIGHT_FLAG; } - else if (receiverTwoReading < ir2base){ + else if (receiverTwoReading < IRP_2.sensorAvg/averageDivL){ // redLed.write(0); // blueLed.write(1); turnFlag |= LEFT_FLAG; } pidOnEncoders(); } - + left_motor.brake(); right_motor.brake(); } - + void handleTurns(){ if (turnFlag == 0x1){ // moveForwardCellEncoder(0.3); @@ -307,9 +276,9 @@ turnRight(); } } - + void pidBrake(){ - + int count0; int count1; count0 = encoder0.getPulses(); @@ -317,18 +286,18 @@ int initial0 = count0; int initial1 = count1; double kp = 0.00011; - - - + + + int error = count0 - count1; - + int counter = 0; right_motor.move(0.7); left_motor.move(0.7); - + double speed0 = 0.7; double speed1 = 0.7; - + while(1) { if(abs(error) < 3){ @@ -341,22 +310,22 @@ error = count0 - count1; speed0 = -error*kp + speed0; speed1 = error*kp + speed1; - + right_motor.move(speed0); left_motor.move(speed1); - + counter = 0; } if (counter > 10){ break; } - + } return; } - + void moveForwardEncoder(){ - + int count0; int count1; count0 = encoder0.getPulses(); @@ -367,16 +336,16 @@ double kp = 0.00015; double kd = 0.00019; int prev = 0; - - - + + + double speed0 = 0.10; double speed1 = 0.12; right_motor.move(speed0); left_motor.move(speed1); - - - while( ((encoder0.getPulses() - initial0) <= (oneCellCountMomentum-200) && (encoder1.getPulses() - initial1) <= (oneCellCountMomentum-200)) || IRP_1.getSamples(50) > IRP_1.sensorAvg*0.8 || IRP_4.getSamples(50) > IRP_4.sensorAvg*0.8){ + + + while((encoder0.getPulses() - initial0) <= (oneCellCountMomentum-200) && (encoder1.getPulses() - initial1) <= (oneCellCountMomentum-200)) { //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 )); @@ -406,17 +375,17 @@ // } prev = x; } - + //pidOnEncoders(); //pidBrake(); right_motor.brake(); left_motor.brake(); return; } - - + + void moveForwardWallEncoder(){ - + int count0; int count1; count0 = encoder0.getPulses(); @@ -427,21 +396,24 @@ double kp = 0.00015; double kd = 0.00019; int prev = 0; - - - - double speed0 = 0.07; - double speed1 = 0.07; + + + + double speed0 = 0.11; + double speed1 = 0.13; right_motor.move(speed0); left_motor.move(speed1); - if( IRP_1.getSamples(50) > IRP_1.sensorAvg*0.8 || IRP_4.getSamples(50) > IRP_4.sensorAvg*0.8){ + float ir1 = IRP_1.getSamples(50); + float ir4 = IRP_4.getSamples(50); + + if((ir1 + ir4)/2 > ((IRP_1.sensorAvg+IRP_4.sensorAvg)/2)*0.4){ return; } - - + //while((encoder0.getPulses() - initial0) <= (oneCellCountMomentum-200) && (encoder1.getPulses() - initial1) <= (oneCellCountMomentum-200)) { - while( (IRP_1.getSamples(50) + IRP_4.getSamples(50))/2 < ((IRP_1.sensorAvg+IRP_2.sensorAvg)/2)*0.4 ){ + //while( (ir1 + ir4)/2 < ((IRP_1.sensorAvg+IRP_4.sensorAvg)/2)*0.4 ){ + while( ir1 < IRP_1.sensorAvg*0.7 || ir4 < IRP_4.sensorAvg*0.7 ){ //serial.printf("IRS= >: %f, %f \r\n", IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 )); count0 = encoder0.getPulses() - initial0; @@ -469,30 +441,32 @@ // right_motor.brake(); // } prev = x; + ir1 = IRP_1.getSamples(50); + ir4 = IRP_4.getSamples(50); } - + //pidOnEncoders(); //pidBrake(); right_motor.brake(); left_motor.brake(); return; } - + void moveForwardUntilWallIr() { double currentError = 0; double previousError = 0; double derivError = 0; double sumError = 0; - + double HIGH_PWM_VOLTAGE = 0.1; - - double rightSpeed = 0.14; - double leftSpeed = 0.17; - + + double rightSpeed = 0.25; + double leftSpeed = 0.23; + float ir2 = IRP_2.getSamples( SAMPLE_NUM ); float ir3 = IRP_3.getSamples( SAMPLE_NUM ); - + int count = encoder0.getPulses(); while ((IRP_1.getSamples( SAMPLE_NUM ) + IRP_4.getSamples( SAMPLE_NUM ) )/2 < 0.05f) { // while the front facing IR's arent covered @@ -505,7 +479,7 @@ }else{ // will move forward using encoders only // if cell ahead doesn't have a wall on either one side or both sides - + int pulseCount = (encoder0.getPulses()-count) % 5600; if (pulseCount > 5400 && pulseCount < 5800) { blueLed.write(0); @@ -513,7 +487,7 @@ blueLed.write(1); } sumError += currentError; - currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - ir2base) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - ir3base) ) ; + currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - IRP_2.sensorAvg/initAverageL) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg/initAverageR) ) ; 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 @@ -523,36 +497,36 @@ rightSpeed = HIGH_PWM_VOLTAGE + abs(PIDSum*HIGH_PWM_VOLTAGE); leftSpeed = HIGH_PWM_VOLTAGE - abs(PIDSum*HIGH_PWM_VOLTAGE); } - + if (leftSpeed > 0.30) leftSpeed = 0.30; if (leftSpeed < 0) leftSpeed = 0; if (rightSpeed > 0.30) rightSpeed = 0.30; if (rightSpeed < 0) rightSpeed = 0; - + right_motor.forward(rightSpeed); left_motor.forward(leftSpeed); - + previousError = currentError; - + ir2 = IRP_2.getSamples( SAMPLE_NUM ); ir3 = IRP_3.getSamples( SAMPLE_NUM ); - + } - + //backward(); //wait_ms(40); //brake(); - + left_motor.brake(); right_motor.brake(); } } - + void printDipFlag() { if (DEBUGGING) serial.printf("Flag value is %d", dipFlags); } - + void enableButton1() { dipFlags |= BUTTON1_FLAG; @@ -593,7 +567,7 @@ dipFlags &= ~BUTTON4_FLAG; printDipFlag(); } - + void pidOnEncoders() { int count0; @@ -604,7 +578,7 @@ double kp = 0.00011; double kd = 0.00014; int prev = 0; - + int counter = 0; while(1) { @@ -638,158 +612,211 @@ break; } } - + void nCellEncoderAndIR(double cellCount){ double currentError = 0; double previousError = 0; double derivError = 0; double sumError = 0; - - double HIGH_PWM_VOLTAGE = 0.15; - double rightSpeed = 0.095; - double leftSpeed = 0.115; - - - int initial0 = encoder0.getPulses(); - int initial1 = encoder1.getPulses(); - + + double HIGH_PWM_VOLTAGE = 0.1; + double rightSpeed = 0.10; + double leftSpeed = 0.10; + + 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; + float ir2 = IRP_2.getSamples( SAMPLE_NUM ); float ir3 = IRP_3.getSamples( SAMPLE_NUM ); - float ir1 = IRP_1.getSamples(50); - float ir4 = IRP_4.getSamples(50); - + + // float previr2 = ir2; + // float previr3 = ir3; + int state = 0; - if(IRP_3.sensorAvg > noWallR){ - ir3base = IRP_3.sensorAvg; - } - if(IRP_2.sensorAvg > noWallL){ - ir2base = IRP_2.sensorAvg; + + while (encoder0.getPulses() <= desiredCount0 && encoder1.getPulses() <= desiredCount1 && receiverOneReading < IRP_1.sensorAvg*0.8 && receiverFourReading < IRP_4.sensorAvg*0.8){ + receiverTwoReading = IRP_2.getSamples(100); + receiverThreeReading = IRP_3.getSamples(100); + // previr2 = receiverTwoReading; + // previr3 = receiverThreeReading; + receiverOneReading = IRP_1.getSamples(100); + receiverFourReading = IRP_4.getSamples(100); + + //if ((receiverOneReading+receiverFourReading)/2 > ((IRP_1.sensorAvg+IRP_4.sensorAvg)/2)*0.15 ){ + if( receiverOneReading > IRP_1.sensorAvg*0.7 || receiverFourReading > IRP_4.sensorAvg*0.7 ){ + // almost to the end + right_motor.move(-0.15); + left_motor.move(-0.15); + + wait_ms(150); + right_motor.brake(); + left_motor.brake(); + + + redLed.write(1); + greenLed.write(1); + blueLed.write(1); + wait_ms(200); + redLed.write(1); + greenLed.write(1); + blueLed.write(0); + wait_ms(200); + + + + redLed.write(1); + greenLed.write(0); + blueLed.write(1); + wait_ms(200); + redLed.write(0); + greenLed.write(1); + blueLed.write(1); + + + + + //moveForwardWallEncoder(); + + + return; + + }else if((receiverThreeReading < 1.3*IRP_3.sensorAvg/(averageDivR)) && (receiverTwoReading < 1.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); + moveForwardEncoder(); + }else if (receiverThreeReading < IRP_3.sensorAvg/averageDivR){// right wall gone + // RED RED RED RED RED + state = 1; + redLed.write(0); + greenLed.write(1); + blueLed.write(1); + }else if (receiverTwoReading < IRP_2.sensorAvg/averageDivL){// left wall gone + // BLUE BLUE BLUE BLUE + state = 2; + redLed.write(1); + greenLed.write(1); + blueLed.write(0); + }else if ((receiverTwoReading > ((IRP_2.sensorAvg/initAverageL)*averageDivUpper)) && (receiverThreeReading > ((IRP_3.sensorAvg/initAverageR)*averageDivUpper))){ + // both walls there + state = 0; + redLed.write(1); + greenLed.write(0); + blueLed.write(1); + } + + switch(state){ + case(0):{ // both walls there + currentError = ( receiverTwoReading - IRP_2.sensorAvg/initAverageL) - ( receiverThreeReading - IRP_3.sensorAvg/initAverageR); + break; + } + case(1):{// RED RED RED RED RED + currentError = (receiverTwoReading - IRP_2.sensorAvg/initAverageL) - (IRP_2.sensorAvg/initAverageL); + break; + } + case(2):{// blue + currentError = (IRP_3.sensorAvg/initAverageR) - (receiverThreeReading - IRP_3.sensorAvg/initAverageR); + break; + } + default:{ + currentError = ( receiverTwoReading - IRP_2.sensorAvg/initAverageL) - ( receiverThreeReading - IRP_3.sensorAvg/initAverageR); + //currentError = ( receiverTwoReading - IRP_2.sensorAvg/initAverageL) - ( receiverThreeReading - IRP_3.sensorAvg/initAverageR); + break; + } + } + //currentError = ( receiverTwoReading - IRP_2.sensorAvg/initAverageL) - ( receiverThreeReading - IRP_3.sensorAvg/initAverageR); + + + + + + sumError += currentError; + 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); + } 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); + } + if (leftSpeed > 0.30) leftSpeed = 0.30; + if (leftSpeed < 0) leftSpeed = 0; + if (rightSpeed > 0.30) rightSpeed = 0.30; + if (rightSpeed < 0) rightSpeed = 0; + + right_motor.forward(rightSpeed); + left_motor.forward(leftSpeed); + pidOnEncoders(); + + previousError = currentError; + ir2 = IRP_2.getSamples( SAMPLE_NUM ); + ir3 = IRP_3.getSamples( SAMPLE_NUM ); + } - for (int i = 0; i < cellCount; i++){ - while ( ((encoder0.getPulses()-initial0) <= oneCellCountMomentum && (encoder1.getPulses()-initial1) <= oneCellCountMomentum) && ir1 < IRP_1.sensorAvg*0.7 && ir4 < IRP_4.sensorAvg*0.7 ){ - ir2 = IRP_2.getSamples(50); - ir3 = IRP_3.getSamples(50); - ir1 = IRP_1.getSamples(50); - ir4 = IRP_4.getSamples(50); - if((ir3 < ir3base/4) && (ir2 < ir2base/4)){ - // 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); - moveForwardEncoder(); - }else if (ir3 < ir3base/4){// right wall gone - // RED RED RED RED RED - currentError = (ir3 - ir2base); - redLed.write(0); - greenLed.write(1); - blueLed.write(1); - }else if (ir2 < ir2base){// left wall gone - // BLUE BLUE BLUE BLUE - currentError = (ir3base - ir2); - redLed.write(1); - greenLed.write(1); - blueLed.write(0); - }else{ - // both walls there - currentError = (ir2 - ir2base) - (ir3 - ir3base); - redLed.write(1); - greenLed.write(0); - blueLed.write(1); - } - - sumError += currentError; - 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); - } 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); - } - if (leftSpeed > 0.30) leftSpeed = 0.30; - if (leftSpeed < 0) leftSpeed = 0; - if (rightSpeed > 0.30) rightSpeed = 0.30; - if (rightSpeed < 0) rightSpeed = 0; - - right_motor.forward(rightSpeed); - left_motor.forward(leftSpeed); - pidOnEncoders(); - - previousError = currentError; - ir2 = IRP_2.getSamples( SAMPLE_NUM ); - ir3 = IRP_3.getSamples( SAMPLE_NUM ); - - if(IRP_3.sensorAvg > noWallR){ - continue; - }else if(ir3 > noWallR){ - ir3base = ir3; - } - - if(IRP_2.sensorAvg > noWallL){ - continue; - }else if(ir2 > noWallL){ - ir2base = ir2; - } - } - } - left_motor.brake(); right_motor.brake(); return; } - + int main() { //Set highest bandwidth. //gyro.setLpBandwidth(LPFBW_42HZ); serial.baud(9600); //serial.printf("The gyro's address is %s", gyro.getWhoAmI()); - + wait (0.1); - - + + redLed.write(1); greenLed.write(0); blueLed.write(1); - + //left_motor.forward(0.1); //right_motor.forward(0.1); - + // PA_1 is A of right // PA_0 is B of right // PA_5 is A of left // PB_3 is B of left //QEI encoder0( PA_5, PB_3, NC, PULSES, QEI::X4_ENCODING ); // QEI encoder1( PA_1, PA_0, NC, PULSES, QEI::X4_ENCODING ); - + // TODO: Setting all the registers and what not for Quadrature Encoders /* RCC->APB1ENR |= 0x1001; // Enable clock for Tim2 (Bit 0) and Tim5 (Bit 3) RCC->AHB1ENR |= 0x11; // Enable GPIO port clock enables for Tim2(A) and Tim5(B) GPIOA->AFR[0] |= 0x10; // Set GPIO alternate function modes for Tim2 GPIOB->AFR[0] |= 0x100; // Set GPIO alternate function modes for Tim5 */ - + // set GPIO pins to alternate for the pins corresponding to A/B for eacah encoder, and 2 alternate function registers need to be selected for each type // of alternate function specified // 4 modes sets AHB1ENR @@ -803,44 +830,100 @@ // SMCR - encoder mode // CR1 reenabales // then read CNT reg of timer - - + + dipButton1.rise(&enableButton1); dipButton2.rise(&enableButton2); dipButton3.rise(&enableButton3); dipButton4.rise(&enableButton4); - + dipButton1.fall(&disableButton1); dipButton2.fall(&disableButton2); dipButton3.fall(&disableButton3); dipButton4.fall(&disableButton4); - if(dipFlags == 0x1){ - turnRight180(); - wait_ms(1000); - }else{ - turnRight(); - IRP_1.calibrateSensor(); - IRP_4.calibrateSensor(); - wait_ms(300); - turnLeft(); - wait_ms(300); - } + //right_motor.forward( 0.2 ); + //left_motor.forward( 0.2 ); + turnRight180(); + wait_ms(1500); - - - //right_motor.forward( 0.2 ); - //left_motor.forward( 0.2 ); - //turnRight180(); - //wait_ms(1500); while (1) { //wait_ms(1500); //turnRight(); //wait_ms(1500); //turnLeft(); + + + // float ir2 = IRP_2.getSamples(100); + // float ir3 = IRP_3.getSamples(100); + // float ir1 = IRP_1.getSamples(100); + // float ir4 = IRP_4.getSamples(100); + + + // if( ir1 > IRP_1.sensorAvg*0.3 || ir4 > IRP_4.sensorAvg*0.3 ){ + // // almost to the end + // redLed.write(1); + // greenLed.write(1); + // blueLed.write(1); + // wait_ms(200); + // redLed.write(1); + // greenLed.write(1); + // blueLed.write(0); + // wait_ms(200); + // redLed.write(1); + // greenLed.write(0); + // blueLed.write(1); + // wait_ms(200); + // redLed.write(0); + // greenLed.write(1); + // blueLed.write(1); + + // }else if((ir3 < IRP_3.sensorAvg/(averageDivR)) && (ir2 < 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); + // }else if (ir3 < IRP_3.sensorAvg/averageDivR){// right wall gone + // // RED RED RED RED RED + // redLed.write(0); + // greenLed.write(1); + // blueLed.write(1); + // }else if (ir2 < IRP_2.sensorAvg/averageDivL){// left wall gone + // // BLUE BLUE BLUE BLUE + // redLed.write(1); + // greenLed.write(1); + // blueLed.write(0); + // }else if ((ir2 > ((IRP_2.sensorAvg/initAverageL)*averageDivUpper)) && (ir3 > ((IRP_3.sensorAvg/initAverageR)*averageDivUpper))){ + // // both walls there + // redLed.write(1); + // greenLed.write(0); + // blueLed.write(1); + // } + + + + nCellEncoderAndIR(1); - wait_ms(500); + wait_ms(1000); + + // turnRight(); // wait_ms(500); // nCellEncoderAndIR(1); @@ -862,9 +945,9 @@ // nCellEncoderAndIR(5); // break; // turnRight180(); - - - + + + // int number = rand() % 4 + 1; // switch(number){ // case(1):{//turn right @@ -876,7 +959,7 @@ // break; // } // case(3):{// keep going - + // break; // } // case(4):{// turnaround @@ -887,27 +970,34 @@ // break; // } // } - + // float irbase2 = IRP_2.sensorAvg/initAverageL/averageDivL; // float irbase3 = IRP_3.sensorAvg/initAverageR/averageDivR; - + // float ir3 = IRP_2.getSamples(100)/initAverageL; // float ir2 = IRP_3.getSamples(100)/initAverageR; - - - + //serial.printf("%f, %f \n", IRP_1.sensorAvg, IRP_4.sensorAvg); + //serial.printf("%f, %f \n", IRP_2.sensorAvg, IRP_3.sensorAvg); + //break; + + //serial.printf("IRS= >: %f, %f \r\n", IRP_2.getSamples(100), IRP_3.getSamples(100)); + //serial.printf("IRS= >: %f, %f \r\n", IRP_1.getSamples(100), IRP_4.getSamples(100)); + + + + /* counter2++; counter3++; ir2tot += IRP_2.getSamples(100); ir3tot += IRP_3.getSamples(100); - - + + ir2 = ir2tot/counter2; ir3 = ir3tot/counter3; - + serial.printf("IRS= >: %f, %f \r\n", ir2, ir3); */ //serial.printf("%f, %f \n", IRP_2.sensorAvg/initAverageL/averageDivL, IRP_3.sensorAvg/initAverageR/averageDivR); @@ -916,20 +1006,20 @@ //serial.printf("IRS= >: %f, %f \r\n", IRP_2.getSamples(100), IRP_3.getSamples(100)); //serial.printf("IRSAvg= >: %f, %f \r\n", ir2, ir3); //serial.printf("IRSAvg= >: %f, %f \r\n", IRP_2.sensorAvg, IRP_3.sensorAvg); - - + + //////////////////////////////////////////////////////////////// - + //nCellEncoderAndIR(3); //break; - + //serial.printf("IRS= >: %f, %f, %f, %f \r\n", IRP_1.getSamples( 100 ), IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 ), IRP_4.getSamples(100)); - - + + //serial.printf("IRS= >: %f, %f \r\n", IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 )); - - + + //break; // moveForwardCellEncoder(1); // wait(0.5); @@ -945,8 +1035,8 @@ //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 ); - + //reading = Rec_4.read(); // serial.printf("reading: %f\n", reading); } -} \ No newline at end of file +}