MacroRat
Mouse code for the MacroRat
Diff: main.cpp
- Revision:
- 33:68ce1f74ab5f
- Parent:
- 32:69acb14778ea
- Child:
- 34:69342782fb68
diff -r 69acb14778ea -r 68ce1f74ab5f main.cpp
--- a/main.cpp Fri May 26 03:46:03 2017 +0000
+++ b/main.cpp Fri May 26 06:23:19 2017 +0000
@@ -12,25 +12,35 @@
#include "stm32f4xx.h"
#include "QEI.h"
-//IRSAvg= >: 0.143701, 0.128285
+/*LEFT/RIGHT BOTH WALLS
+0.34 0.12
+
+LEFT/RIGHT LEFT WALL GONE
+0.02 0.11
-//facing wall ir2 and ir3
-//0.144562, 0.113971 in maze
+LEFT/RIGHT RIGTH WALL GONE
+0.33 0.008
-// normal hall ir2 and ir3
-//0.013665, 0.010889 in maze
+LEFT/RIGHT BOTH WALLS GONE
+0.02 0.008
+
+LEFT/RIGHT CLOSE TO RIGHT WALL
+0.14 0.47
-//0.014462, 0.009138
-// 0.013888, 0.010530
+LEFT/RIGHT CLOSE TO LEFT WALL
+0.89 0.05
+FRONT IRS NEAR WALL (STOPPING DISTANCE)
+0.41 0.49
-//no walls ir2 and ir3
-//0.003265, 0.002904 in maze9
+FRONT IRS NO WALL AHEAD (FOR ATLEAST 1 FULL CELL)
+0.07 0.06
-//0.003162, 0.003123
-//0.003795,
-
-//0.005297, 0.007064
+*/
+
+#define IP_CONSTANT 1.93
+#define II_CONSTANT 0.00001
+#define ID_CONSTANT 0.175
void pidOnEncoders();
@@ -65,7 +75,6 @@
void moveForwardEncoder(double num)
{
-
int count0;
int count1;
count0 = encoder0.getPulses();
@@ -77,12 +86,12 @@
double kd = 0.00019;
int prev = 0;
- double speed0 = 0.10;
- double speed1 = 0.12;
+ double speed0 = 0.11;
+ double speed1 = 0.13;
right_motor.move(speed0);
left_motor.move(speed1);
- while( ((encoder0.getPulses() - initial0) <= (oneCellCountMomentum-200)*num && (encoder1.getPulses() - initial1) <= (oneCellCountMomentum-200)*num) || IRP_1.getSamples(50) > IRP_1.sensorAvg*0.8 || IRP_4.getSamples(50) > IRP_4.sensorAvg*0.8) {
+ while( ((encoder0.getPulses() - initial0) <= (oneCellCountMomentum-200)*num && (encoder1.getPulses() - initial1) <= (oneCellCountMomentum-200)*num)) {
//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 ));
@@ -304,7 +313,7 @@
count0 = encoder0.getPulses();
count1 = encoder1.getPulses();
int diff = count0 - count1;
- double kp = 0.00013;
+ double kp = 0.00016;
double kd = 0.00016;
int prev = 0;
@@ -340,7 +349,6 @@
void nCellEncoderAndIR(double cellCount)
{
- //serial.printf("I'm inside IR!");
double currentError = 0;
double previousError = 0;
double derivError = 0;
@@ -362,44 +370,41 @@
float ir2 = IRP_2.getSamples( SAMPLE_NUM );
float ir3 = IRP_3.getSamples( SAMPLE_NUM );
- // float previr2 = ir2;
- // float previr3 = ir3;
-
int state = 0;
- //serial.printf("Enter encoder while loop\n");
- //serial.printf("%d, %d\n", encoder0.getPulses(), encoder1.getPulses() );
while (encoder0.getPulses() <= desiredCount0 && encoder1.getPulses() <= desiredCount1) {
- //serial.printf("Entered encoder while loop\n");
+ // serial.printf("The desiredCount0 is: %d \t The desiredCount1 is: %d\t the 0encoderval is :%d\t the 1encoderval is : %d\t\n", desiredCount0, desiredCount1, encoder0.getPulses(), encoder1.getPulses());
receiverTwoReading = IRP_2.getSamples( SAMPLE_NUM );
receiverThreeReading = IRP_3.getSamples( SAMPLE_NUM );
receiverOneReading = IRP_1.getSamples( SAMPLE_NUM );
receiverFourReading = IRP_4.getSamples( SAMPLE_NUM );
-
- if( receiverOneReading > IRP_1.sensorAvg * 3 || receiverFourReading > IRP_4.sensorAvg * 3 ) {
+ // 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 * 3.4 || receiverFourReading > IRP_4.sensorAvg * 3.4) {
break;
}
- if((receiverThreeReading < 3*IRP_3.sensorAvg/(averageDivR)) && (receiverTwoReading < 3*IRP_2.sensorAvg/(averageDivL))) {
+ if((receiverThreeReading < IRP_3.sensorAvg/5) && (receiverTwoReading < IRP_2.sensorAvg/5)) {
// both sides gone
- int prev0 = encoder0.getPulses();
- int prev1 = encoder1.getPulses();
- int diff0 = desiredCount0 - prev0;
- int diff1 = desiredCount1 - prev1;
- int valToPass = ((diff0 + diff1)/2)/(oneCellCountMomentum);
+ double prev0 = encoder0.getPulses();
+ double prev1 = encoder1.getPulses();
+ double diff0 = desiredCount0 - prev0;
+ double diff1 = desiredCount1 - prev1;
+ double valToPass = ((diff0 + diff1)/2)/(oneCellCountMomentum);
+ // serial.printf("Going to go over to move forward with encoder, and passing %f\n", valToPass);
moveForwardEncoder(valToPass);
- } else if (receiverThreeReading < 3*IRP_3.sensorAvg/averageDivR) { // right wall gone
+ continue;
+ } else if (receiverThreeReading < IRP_3.sensorAvg/3.5) { // right wall gone RED
state = 1;
redLed.write(0);
greenLed.write(1);
blueLed.write(1);
- } else if (receiverTwoReading < 3*IRP_2.sensorAvg/averageDivL) { // left wall gone
+ } else if (receiverTwoReading < IRP_2.sensorAvg/3.5) { // 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))) {
+ } else if ((receiverTwoReading > ((IRP_2.sensorAvg)*averageDivUpper)) && (receiverThreeReading > ((IRP_3.sensorAvg)*averageDivUpper))) {
// both walls there
state = 0;
redLed.write(1);
@@ -410,19 +415,19 @@
//serial.printf("Entering switch\n");
switch(state) {
case(0): { // both walls there
- currentError = ( receiverTwoReading - IRP_2.sensorAvg/initAverageL) - ( receiverThreeReading - IRP_3.sensorAvg/initAverageR);
+ currentError = ( receiverTwoReading - IRP_2.sensorAvg) - ( receiverThreeReading - IRP_3.sensorAvg);
break;
}
case(1): { // RED RED RED RED RED
- currentError = (receiverTwoReading - IRP_2.sensorAvg/initAverageL) - (IRP_2.sensorAvg/initAverageL);
+ currentError = (receiverTwoReading - IRP_2.sensorAvg) - (IRP_3.sensorAvg*0.001);
break;
}
case(2): { // blue
- currentError = (IRP_3.sensorAvg/initAverageR) - (receiverThreeReading - IRP_3.sensorAvg/initAverageR);
+ currentError = (IRP_2.sensorAvg*0.001) - (receiverThreeReading - IRP_3.sensorAvg);
break;
}
default: {
- currentError = ( receiverTwoReading - IRP_2.sensorAvg/initAverageL) - ( receiverThreeReading - IRP_3.sensorAvg/initAverageR);
+ currentError = ( receiverTwoReading - IRP_2.sensorAvg) - ( receiverThreeReading - IRP_3.sensorAvg);
break;
}
}
@@ -447,7 +452,7 @@
right_motor.forward(rightSpeed);
left_motor.forward(leftSpeed);
- //pidOnEncoders();
+ pidOnEncoders();
previousError = currentError;
}
@@ -463,12 +468,12 @@
}
// the mouse has moved forward, we need to update the wall map now
- receiverOneReading = IRP_1.getSamples(50);
- receiverTwoReading = IRP_2.getSamples(50);
- receiverThreeReading = IRP_3.getSamples(50);
- receiverFourReading = IRP_4.getSamples(50);
+ receiverOneReading = IRP_1.getSamples(75);
+ receiverTwoReading = IRP_2.getSamples(75);
+ receiverThreeReading = IRP_3.getSamples(75);
+ receiverFourReading = IRP_4.getSamples(75);
- if (receiverOneReading >= 0.5f && receiverFourReading >= 0.5f) {
+ if (receiverOneReading >= 0.3f && receiverFourReading >= 0.3f) {
if (currDir % 4 == 0) {
wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= F_WALL;
} else if (currDir % 4 == 1) {
@@ -479,7 +484,7 @@
wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= B_WALL;
}
}
- if (receiverThreeReading >= 0.1f) {
+ if (receiverThreeReading >= IRP_3.sensorAvg*0.6) {
if (currDir % 4 == 0) {
wallArray[MAZE_LEN - 1 - (mouseY + 1)][mouseX] |= R_WALL;
} else if (currDir % 4 == 1) {
@@ -490,7 +495,7 @@
wallArray[MAZE_LEN - 1 - (mouseY)][mouseX-1] |= F_WALL;
}
}
- if (receiverTwoReading >= 0.1f) {
+ 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) {
@@ -508,20 +513,16 @@
right_motor.brake();
}
-bool isWallInFront(int x, int y)
-{
+bool isWallInFront(int x, int y){
return (wallArray[MAZE_LEN - y - 1][x] & F_WALL);
}
-bool isWallInBack(int x, int y)
-{
+bool isWallInBack(int x, int y){
return (wallArray[MAZE_LEN - y - 1][x] & B_WALL);
}
-bool isWallOnRight(int x, int y)
-{
+bool isWallOnRight(int x, int y){
return (wallArray[MAZE_LEN - y - 1][x] & R_WALL);
}
-bool isWallOnLeft(int x, int y)
-{
+bool isWallOnLeft(int x, int y){
return (wallArray[MAZE_LEN - y - 1][x] & L_WALL);
}
@@ -741,8 +742,7 @@
return dirToGo;
}
-bool hasVisited(int x, int y)
-{
+bool hasVisited(int x, int y){
return visitedCells[MAZE_LEN - 1 - y][x];
}
@@ -842,7 +842,6 @@
// CR1 reenabales
// then read CNT reg of timer
-
dipButton1.rise(&enableButton1);
dipButton2.rise(&enableButton2);
dipButton3.rise(&enableButton3);
@@ -853,20 +852,6 @@
dipButton3.fall(&disableButton3);
dipButton4.fall(&disableButton4);
- // if(dipFlags == 0x1){
- // }else{
- // turnRight();
- // IRP_1.calibrateSensor();
- // IRP_4.calibrateSensor();
- // wallArray[MAZE_LEN - 1 - (mouseY + 1)][mouseX] |= L_WALL;
- // wallArray[MAZE_LEN - 1 - (mouseY + 1)][mouseX] |= R_WALL;
-
- // wait_ms(300);
- // turnLeft();
- // wait_ms(300);
- // }
-
-
// init the wall, and mouse loc arrays:
wallArray[MAZE_LEN - 1 - mouseY][mouseX] = 0xE;
visitedCells[MAZE_LEN - 1 - mouseY][mouseX] = 1;
@@ -877,13 +862,11 @@
//int currEncoder1Count = 0;
//bool overrideFloodFill = false;
- //right_motor.forward( 0.2 );
- //left_motor.forward( 0.2 );
+ right_motor.forward( 0.2 );
+ left_motor.forward( 0.2 );
//turnRight180();
//wait_ms(1500);
//int nextMovement = 0;
- //serial.printf("I'm about to enter!");
- nCellEncoderAndIR(3);
while (1) {
// prevEncoder0Count = encoder0.getPulses();
// prevEncoder1Count = encoder1.getPulses();
@@ -929,112 +912,34 @@
//
// wait_ms(300); // reduce this once we fine tune this!
- //wait_ms(1500);
- //turnRight();
- //wait_ms(1500);
- //turnLeft();
- // nCellEncoderAndIR(1);
- // wait_ms(500);
- // 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;
-
-
-
- /*
- 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);
- //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(4);
- //while(1);
- //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);
- // handleTurns();
- // wait(0.5);
- // moveForwardCellEncoder(1);
- // wait(0.5);
- // handleTurns();
- //break;
- //pidOnEncoders();
- // moveForwardUntilWallIr();
- //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);
+ //////////////////////// TESTING CODE GOES BELOW ///////////////////////////
+ nCellEncoderAndIR(4);
+ wait_ms(200);
+ turnRight();
+ wait_ms(200);
+ nCellEncoderAndIR(3);
+ wait_ms(200);
+ turnRight();
+ wait_ms(200);
+ nCellEncoderAndIR(2);
+ wait_ms(200);
+ turnRight();
+ wait_ms(200);
+ nCellEncoderAndIR(1);
+ wait_ms(200);
+ turnLeft();
+ wait_ms(200);
+ nCellEncoderAndIR(1);
+ wait_ms(200);
+ turnLeft();
+ wait_ms(200);
+ nCellEncoderAndIR(1);
+ wait_ms(200);
+ turnRight180();
+ break;
+ // 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 );
}
}
\ No newline at end of file