MacroRat
Mouse code for the MacroRat
Diff: main.cpp
- Revision:
- 23:690b0ca34ee9
- Parent:
- 22:681190ff98f0
- Child:
- 24:e7063765d6f0
--- a/main.cpp Fri May 19 01:15:36 2017 +0000
+++ b/main.cpp Sat May 20 09:11:08 2017 +0000
@@ -19,12 +19,13 @@
// #define IP_CONSTANT 8.85
// #define II_CONSTANT 0.005
// #define ID_CONSTANT 3.15
-#define IP_CONSTANT 6.85
+#define IP_CONSTANT 13.5
#define II_CONSTANT 0.095
-#define ID_CONSTANT 15.85
+#define ID_CONSTANT 8.85
-const int desiredCountR = 1400;
-const int desiredCountL = 1475;
+const int desiredCount180 = 2900;
+const int desiredCountR = 1500;
+const int desiredCountL = 1575;
const int oneCellCount = 5400;
const int oneCellCountMomentum = 4800; // one cell count is actually approximately 5400, but this value is considering momentum!
@@ -34,16 +35,50 @@
float receiverThreeReading = 0.0;
float receiverFourReading = 0.0;
-float averageDiv = 170;
-float initAverage = 4;
+//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 averageDivL = 23.0; //blue
+float initAverageL = 10.35;
+
+float averageDivR = 24; //red
+float initAverageR = 12.82; //4.5
+
+float averageDivUpper = 0.9;
void pidOnEncoders();
void turnLeft()
{
- double speed0 = 0.15;
- double speed1 = -0.15;
+ double speed0 = 0.11;
+ double speed1 = -0.13;
int counter = 0;
int initial0 = encoder0.getPulses();
@@ -90,8 +125,8 @@
void turnRight()
{
- double speed0 = -0.15;
- double speed1 = 0.15;
+ double speed0 = -0.11;
+ double speed1 = 0.13;
int counter = 0;
int initial0 = encoder0.getPulses();
@@ -184,15 +219,15 @@
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 + desiredCountR*2;
- int desiredCount1 = initial1 - desiredCountR*2;
+ int desiredCount0 = initial0 + desiredCount180;
+ int desiredCount1 = initial1 - desiredCount180;
int count0 = initial0;
int count1 = initial1;
@@ -239,12 +274,12 @@
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 < IRP_3.sensorAvg/averageDiv){
+ if (receiverThreeReading < IRP_3.sensorAvg/averageDivR){
// redLed.write(1);
// blueLed.write(0);
turnFlag |= RIGHT_FLAG;
}
- else if (receiverTwoReading < IRP_2.sensorAvg/averageDiv){
+ else if (receiverTwoReading < IRP_2.sensorAvg/averageDivL){
// redLed.write(0);
// blueLed.write(1);
turnFlag |= LEFT_FLAG;
@@ -334,14 +369,14 @@
- double speed0 = 0.1;
- double speed1 = 0.13;
+ double speed0 = 0.10;
+ double speed1 = 0.12;
right_motor.move(speed0);
left_motor.move(speed1);
- while((IRP_2.getSamples(SAMPLE_NUM) < 0.005 || IRP_3.getSamples(SAMPLE_NUM) < 0.005) && ((IRP_1.getSamples( SAMPLE_NUM ) + IRP_4.getSamples( SAMPLE_NUM ) )/2 < 0.25) ) {
-
+ 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 ));
count0 = encoder0.getPulses() - initial0;
@@ -372,9 +407,69 @@
}
//pidOnEncoders();
- pidBrake();
- //right_motor.brake();
- //left_motor.brake();
+ //pidBrake();
+ right_motor.brake();
+ left_motor.brake();
+ return;
+}
+
+
+void moveForwardWallEncoder(){
+
+ int count0;
+ int count1;
+ count0 = encoder0.getPulses();
+ count1 = encoder1.getPulses();
+ int initial1 = count1;
+ int initial0 = count0;
+ int diff = count0 - count1;
+ 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)) {
+ 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 ));
+
+ count0 = encoder0.getPulses() - initial0;
+ count1 = encoder1.getPulses() - initial1;
+ int x = count0 - count1;
+ //double d = kp * x + kd * ( x - prev );
+ double kppart = kp * x;
+ double kdpart = kd * (x-prev);
+ 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
+ {
+ left_motor.move( speed1-0.8*d );
+ right_motor.move( speed0+d );
+ }
+ else if( x > diff + 40 )
+ {
+ left_motor.move( speed1-0.8*d );
+ right_motor.move( speed0+d );
+ }
+ // else
+ // {
+ // left_motor.brake();
+ // right_motor.brake();
+ // }
+ prev = x;
+ }
+
+ //pidOnEncoders();
+ //pidBrake();
+ right_motor.brake();
+ left_motor.brake();
return;
}
@@ -397,7 +492,7 @@
while ((IRP_1.getSamples( SAMPLE_NUM ) + IRP_4.getSamples( SAMPLE_NUM ) )/2 < 0.05f) { // while the front facing IR's arent covered
if((IRP_2.getSamples(SAMPLE_NUM) < 0.005 || IRP_3.getSamples(SAMPLE_NUM) < 0.005)) {
- //moveForwardEncoder();
+ //moveForwardWallEncoder();
}else if(IRP_2.getSamples(SAMPLE_NUM) < 0.005){ // left wall gone
//moveForwardRightWall();
}else if(IRP_3.getSamples(SAMPLE_NUM) < 0.005){ // right wall gone
@@ -413,7 +508,7 @@
blueLed.write(1);
}
sumError += currentError;
- currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - IRP_2.sensorAvg/initAverage) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg/initAverage) ) ;
+ 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
@@ -552,8 +647,8 @@
int desiredCount0 = encoder0.getPulses() + oneCellCountMomentum*cellCount;
int desiredCount1 = encoder1.getPulses() + oneCellCountMomentum*cellCount;
- left_motor.forward(0.125);
- right_motor.forward(0.10);
+ left_motor.forward(0.17);
+ right_motor.forward(0.15);
float receiverTwoReading = 0.0;
float receiverThreeReading = 0.0;
@@ -561,26 +656,72 @@
float ir2 = IRP_2.getSamples( SAMPLE_NUM );
float ir3 = IRP_3.getSamples( SAMPLE_NUM );
- float prevRec2 = 0.0;
- float prevRec3 = 0.0;
-
+ int state = 0;
+
+
while (encoder0.getPulses() <= desiredCount0 && encoder1.getPulses() <= desiredCount1){
receiverTwoReading = IRP_2.getSamples(100);
receiverThreeReading = IRP_3.getSamples(100);
- if (receiverTwoReading <= IRP_2.sensorAvg/averageDiv)
- currentError = prevRec2 -(receiverThreeReading - IRP_3.sensorAvg/initAverage);
- else if (receiverThreeReading <= IRP_3.sensorAvg/averageDiv)
- currentError = (receiverTwoReading - IRP_2.sensorAvg/initAverage) - prevRec3;
- // else if (receiverTwoReading <= IRP_2.sensorAvg/2 && receiverThreeReading <= IRP_3.sensorAvg/2) // scenario when both left and right wall missing, use encoders only
- // moveForwardCellEncoder(2);
- else{
- currentError = ( receiverTwoReading - IRP_2.sensorAvg/initAverage) - ( receiverThreeReading - IRP_3.sensorAvg/initAverage);
- prevRec2 = receiverTwoReading - IRP_2.sensorAvg/initAverage;
- prevRec3 = receiverThreeReading - IRP_3.sensorAvg/initAverage;
+
+ if ((IRP_1.getSamples(100) > IRP_1.sensorAvg*0.1) || (IRP_4.getSamples(100) > IRP_4.sensorAvg*0.1) ){
+ // almost to the end
+ redLed.write(1);
+ greenLed.write(1);
+ blueLed.write(1);
+ moveForwardWallEncoder();
+ break;
+
+ }else if( (receiverThreeReading < IRP_3.sensorAvg/(averageDivR*0.8)) && (receiverTwoReading < IRP_2.sensorAvg/(averageDivL*0.8)) ){
+ // both sides gone
+ redLed.write(1);
+ greenLed.write(1);
+ blueLed.write(1);
+ 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) - (0.6*IRP_2.sensorAvg/initAverageL);
+ break;
+ }
+ case(2):{// blue
+ currentError = (0.8*IRP_3.sensorAvg/initAverageL) - (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;
+ }
+ }
+
+
+
sumError += currentError;
derivError = currentError - previousError;
@@ -604,20 +745,22 @@
previousError = currentError;
ir2 = IRP_2.getSamples( SAMPLE_NUM );
ir3 = IRP_3.getSamples( SAMPLE_NUM );
+
}
left_motor.brake();
right_motor.brake();
+ return;
}
int main()
{
//Set highest bandwidth.
- gyro.setLpBandwidth(LPFBW_42HZ);
+ //gyro.setLpBandwidth(LPFBW_42HZ);
serial.baud(9600);
- serial.printf("The gyro's address is %s", gyro.getWhoAmI());
+ //serial.printf("The gyro's address is %s", gyro.getWhoAmI());
wait (0.1);
@@ -671,19 +814,98 @@
//right_motor.forward( 0.2 );
//left_motor.forward( 0.2 );
turnRight180();
- wait_ms(60);
+ wait_ms(1500);
while (1) {
-
+ //wait_ms(1500);
+ //turnRight();
+ //wait_ms(1500);
+ //turnLeft();
+ nCellEncoderAndIR(4);
+ wait_ms(2000);
+ // turnRight();
+ // wait_ms(500);
+ // nCellEncoderAndIR(1);
+ // wait_ms(500);
+ // turnRight();
+ // wait_ms(500);
+ // nCellEncoderAndIR(1);
+ // wait_ms(500);
+ // turnLeft();
+ // wait_ms(500);
+ // nCellEncoderAndIR(2);
+ // wait_ms(500);
+ // turnRight();
+ // wait_ms(500);
+ // nCellEncoderAndIR(1);
+ // wait_ms(500);
+ // turnRight();
+ // wait_ms(500);
+ // nCellEncoderAndIR(5);
+ // break;
+ // turnRight180();
+
+
+
+ // int number = rand() % 4 + 1;
+ // switch(number){
+ // case(1):{//turn right
+ // turnRight();
+ // break;
+ // }
+ // case(2):{ // turn left
+ // turnLeft();
+ // break;
+ // }
+ // case(3):{// keep going
+
+ // break;
+ // }
+ // case(4):{// turnaround
+ // turnRight180();
+ // break;
+ // }
+ // default:{// keep going
+ // 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;
- nCellEncoderAndIR(3);
- break;
+
+
+
+ /*
+ counter2++;
+ counter3++;
+ ir2tot += IRP_2.getSamples(100);
+ ir3tot += IRP_3.getSamples(100);
+
- // 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));
+ 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);
+ //serial.printf("IRBASEnowall= >: %f, %f \r\n", irbase2, irbase3);
+ //break;
+ //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;
- //moveForwardEncoder();
- //serial.printf("ded \n");
- //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 ));