File content as of revision 28:8126a4d620e8:

#include "mbed.h"
 
#include "irpair.h"
#include "main.h"
#include "motor.h"
 
#include <stdlib.h>
#include <stack>          // std::stack
#include <utility>      // std::pair, std::make_pair

#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 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
 
        //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;
 
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;
 
    left_motor.forward(0.125);
    right_motor.forward(0.095);
    wait_ms(1);
    while (encoder0.getPulses() <= desiredCount0 && encoder1.getPulses() <= desiredCount1){
        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){
            // redLed.write(1);
            // blueLed.write(0);
            turnFlag |= RIGHT_FLAG;
        }
        else if (receiverTwoReading < ir2base){
            // redLed.write(0);
            // blueLed.write(1);
            turnFlag |= LEFT_FLAG;
        }
        pidOnEncoders();
    }
 
    left_motor.brake();
    right_motor.brake();
}
 
 
void moveForwardEncoder(double num){
 
    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)*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(     (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;
}
 
 
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.11;
    double speed1 = 0.13;
    right_motor.move(speed0);
    left_motor.move(speed1);

    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(     (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;
        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; 
        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.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
        
        if((IRP_2.getSamples(SAMPLE_NUM) < 0.005 || IRP_3.getSamples(SAMPLE_NUM) < 0.005)) {
            //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
            //moveForwardLeftWall();
        }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);
            } else {
                blueLed.write(1);
            }
            sumError += currentError;
            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
                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);

            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;
    printDipFlag();
}
void enableButton2()
{
    dipFlags |= BUTTON2_FLAG;
    printDipFlag();
}
void enableButton3()
{
    dipFlags |= BUTTON3_FLAG;
    printDipFlag();
}
void enableButton4()
{
    dipFlags |= BUTTON4_FLAG;
    printDipFlag();
}
void disableButton1()
{
    dipFlags &= ~BUTTON1_FLAG;
    printDipFlag();
}
void disableButton2()
{
    dipFlags &= ~BUTTON2_FLAG;
    printDipFlag();
}
void disableButton3()
{
    dipFlags &= ~BUTTON3_FLAG;
    printDipFlag();
}
void disableButton4()
{
    dipFlags &= ~BUTTON4_FLAG;
    printDipFlag();
}
 
void pidOnEncoders()
{
    int count0;
    int count1;
    count0 = encoder0.getPulses();
    count1 = encoder1.getPulses();
    int diff = count0 - count1;
    double kp = 0.00013;
    double kd = 0.00016;
    int prev = 0;
 
    int counter = 0;
    while(1)
    {
        count0 = encoder0.getPulses();
        count1 = encoder1.getPulses();
        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 - 60 ) // count1 is bigger, right wheel pushed forward
        {
            left_motor.move( -d );
            right_motor.move( d );
        }
        else if( x > diff + 60 )
        {
            left_motor.move( -d );
            right_motor.move( d );
        }
        // else
        // {
        //     left_motor.brake();
        //     right_motor.brake();   
        // }
        prev = x;
        counter++;
        if (counter == 10)
            break;
    }
}
 
void nCellEncoderAndIR(double cellCount){
    double currentError = 0;
    double previousError = 0;
    double derivError = 0;
    double sumError = 0;

    double HIGH_PWM_VOLTAGE = 0.12;
    double rightSpeed = 0.12;
    double leftSpeed = 0.12;

    int desiredCount0 = encoder0.getPulses() + oneCellCountMomentum*cellCount;
    int desiredCount1 = encoder1.getPulses() + oneCellCountMomentum*cellCount;

    left_motor.forward(0.12);
    right_motor.forward(0.10);

    float ir2 = IRP_2.getSamples( SAMPLE_NUM );
    float ir3 = IRP_3.getSamples( SAMPLE_NUM );

    // float previr2 = ir2;
    // float previr3 = ir3;

    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);

        if(  receiverOneReading > IRP_1.sensorAvg*0.70 || receiverFourReading > IRP_4.sensorAvg*0.70 ){
            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] |= L_WALL;
            }
            else if (currDir % 4 == 3){
                wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= B_WALL;
            }
            break;
        }

       if((receiverThreeReading <  3*IRP_3.sensorAvg/(averageDivR)) && (receiverTwoReading < 3*IRP_2.sensorAvg/(averageDivL))){
            // 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);
            moveForwardEncoder(valToPass);
        }
        else if (receiverThreeReading < 3*IRP_3.sensorAvg/averageDivR){// right wall gone
            // 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;
            // }
            // RED RED RED RED RED
            state = 1;
            redLed.write(0);
            greenLed.write(1);
            blueLed.write(1);
        }else if (receiverTwoReading < 3*IRP_2.sensorAvg/averageDivL){// left wall gone
            // if (currDir % 4 == 0){
            //     wallArray[MAZE_LEN - 1 - (mouseY + 1)][mouseX] |= R_WALL;
            // }
            // else if (currDir % 4 == 1){
            //     wallArray[MAZE_LEN - 1 - (mouseY)][mouseX+1] |= B_WALL;
            // }
            // else if (currDir % 4 == 2){
            //     wallArray[MAZE_LEN - 1 - (mouseY - 1)][mouseX] |= L_WALL;
            // }
            // else if (currDir % 4 == 3){
            //     wallArray[MAZE_LEN - 1 - (mouseY)][mouseX-1] |= F_WALL;
            // }
            // 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))){
            // if (currDir % 4 == 0){
            //     wallArray[MAZE_LEN - 1 - (mouseY + 1)][mouseX] |= R_WALL;
            //     wallArray[MAZE_LEN - 1 - (mouseY + 1)][mouseX] |= L_WALL;
            // }
            // else if (currDir % 4 == 1){
            //     wallArray[MAZE_LEN - 1 - (mouseY)][mouseX+1] |= F_WALL;
            //     wallArray[MAZE_LEN - 1 - (mouseY)][mouseX+1] |= B_WALL;
            // }
            // else if (currDir % 4 == 2){
            //     wallArray[MAZE_LEN - 1 - (mouseY - 1)][mouseX] |= R_WALL;
            //     wallArray[MAZE_LEN - 1 - (mouseY - 1)][mouseX] |= L_WALL;
            // }
            // else if (currDir % 4 == 3){
            //     wallArray[MAZE_LEN - 1 - (mouseY)][mouseX-1] |= F_WALL;
            //     wallArray[MAZE_LEN - 1 - (mouseY)][mouseX-1] |= B_WALL;
            // }
            // 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);
                break;
            }
        }

        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;
    }
    if (encoder0.getPulses() >= 0.6*desiredCount0 && encoder1.getPulses() >= 0.6*desiredCount1){
        if (currDir % 4 == 0){
        mouseY += 1;
        }
        else if (currDir % 4 == 1){
            mouseX + 1;
        }
        else if (currDir % 4 == 2){
            mouseY -= 1;
        }
        else if (currDir % 4 == 3){
            mouseX -= 1;
        }
    }
 
    left_motor.brake();
    right_motor.brake();
}

bool isWallInFront(int x, int y){
    return (wallArray[MAZE_LEN - y - 1][x]  & F_WALL);
}
bool isWallInBack(int x, int y){
    return (wallArray[MAZE_LEN - y - 1][x]  & B_WALL);
}
bool isWallOnRight(int x, int y){
    return (wallArray[MAZE_LEN - y - 1][x]  & R_WALL);
}
bool isWallOnLeft(int x, int y){
    return (wallArray[MAZE_LEN - y - 1][x]  & L_WALL);
}

int chooseNextMovement(){
    int currentDistance = manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX];
    if (goingToCenter && (currentDistance == 0)){
        goingToCenter = false;
        changeManhattanDistance(goingToCenter);
    }
    else if (!goingToCenter && (currentDistance == 0)){
        goingToCenter == true;
        changeManhattanDistance(goingToCenter);
    }

    visitedCells[MAZE_LEN - 1 - mouseY][mouseX] = 1;
    int currX = 0;
    int currY = 0;
    int currDist = 0;
    int dirToGo = 0;
    if (!justTurned){
        cellsToVisit.push(make_pair(mouseX, mouseY));
        while (!cellsToVisit.empty()) {
            pair<int, int> curr = cellsToVisit.top();
            cellsToVisit.pop();
            currX = curr.first;
            currY = curr.second;
            currDist = manhattanDistances[(MAZE_LEN - 1) - currY][currX];
            int minDist = INT_MAX;

             if (hasVisited(currX, currY)) { // then we want to actually see where the walls are, else we treat it as if there are no walls!
                if (currX + 1 < MAZE_LEN && !isWallOnRight(currX, currY)) {
                    if (manhattanDistances[MAZE_LEN - 1 - currY][currX + 1] < minDist) {
                        minDist = manhattanDistances[MAZE_LEN - 1 - currY][currX + 1];
                    }
                }
                if (currX - 1 >= 0 && !isWallOnLeft(currX, currY)) {
                    if (manhattanDistances[MAZE_LEN - 1 - currY][currX - 1] < minDist) {
                        minDist = manhattanDistances[MAZE_LEN - 1 - currY][currX - 1];
                    }
                }
                if (currY + 1 < MAZE_LEN && !isWallInFront( currX,  currY)) {
                    if (manhattanDistances[MAZE_LEN - 1 - (currY + 1)][currX] < minDist) {
                        minDist = manhattanDistances[MAZE_LEN - 1 - (currY + 1)][currX];
                    }
                }
                if (currY - 1 >= 0 && !isWallInBack(currX,  currY)) {
                    if (manhattanDistances[MAZE_LEN - 1 - (currY - 1)][currX] < minDist) {
                        minDist = manhattanDistances[MAZE_LEN - 1 - (currY - 1)][currX];
                    }
                }
            } else {
                if (currX + 1 < MAZE_LEN) {
                    if (manhattanDistances[MAZE_LEN - 1 - currY][currX + 1] < minDist) {
                        minDist = manhattanDistances[MAZE_LEN - 1 - currY][currX + 1];
                    }
                }
                if (currX - 1 >= 0) {
                    if (manhattanDistances[MAZE_LEN - 1 - currY][currX - 1] < minDist) {
                        minDist = manhattanDistances[MAZE_LEN - 1 - currY][currX - 1];
                    }
                }
                if (currY + 1 < MAZE_LEN) {
                    if (manhattanDistances[MAZE_LEN - 1 - (currY + 1)][currX] < minDist) {
                        minDist = manhattanDistances[MAZE_LEN - 1 - (currY + 1)][currX];
                    }
                }
                if (currY - 1 >= 0) {
                    if (manhattanDistances[MAZE_LEN - 1 - (currY - 1)][currX] < minDist) {
                        minDist = manhattanDistances[MAZE_LEN - 1 - (currY - 1)][currX];
                    }
                }
            }

            if (minDist == INT_MAX)
                continue;
            if (currDist > minDist)
                continue;
            if (currDist <= minDist) {
                manhattanDistances[MAZE_LEN - 1 - currY][currX] = minDist + 1;
            }
            if (hasVisited(currX, currY)) {
                if (currY + 1 < MAZE_LEN && !isWallInFront(currX, currY)) {
                    cellsToVisit.push(make_pair(currX, currY + 1));
                }
                if (currX + 1 < MAZE_LEN && !isWallOnRight(currX, currY)) {
                    cellsToVisit.push(make_pair(currX + 1, currY));
                }
                if (currY - 1 >= 0 && !isWallInBack(currX, currY)) {
                    cellsToVisit.push(make_pair(currX, currY - 1));
                }
                if (currX - 1 >= 0 && !isWallOnLeft( currX, currY)) {
                    cellsToVisit.push(make_pair(currX - 1, currY));
                }
            } else {
                if (currY + 1 < MAZE_LEN) {
                    cellsToVisit.push(make_pair(currX, currY + 1));
                }
                if (currX + 1 < MAZE_LEN) {
                    cellsToVisit.push(make_pair(currX + 1, currY));
                }
                if (currY - 1 >= 0) {
                    cellsToVisit.push(make_pair(currX, currY - 1));
                }
                if (currX - 1 >= 0) {
                    cellsToVisit.push(make_pair(currX - 1, currY));
                }
            }
        }

        int minDistance = INT_MAX;
        if (currDir % 4 == 0) {
            if (mouseX + 1 < MAZE_LEN && !isWallOnRight(mouseX, mouseY)) {
                if (manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX + 1] <= minDistance) {
                    minDistance = manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX + 1];
                    dirToGo = 1;
                }
            }
            if (mouseX - 1 >= 0 && !isWallOnLeft(mouseX, mouseY)) {
                if (manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX - 1] <= minDistance) {
                    minDistance = manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX - 1];
                    dirToGo = 2;
                }
            }
            if (mouseY + 1 < MAZE_LEN && !isWallInFront(mouseX, mouseY)) {
                if (manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX] <= minDistance) {
                    minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX];
                    dirToGo = 3;
                }
            }
            if (mouseY - 1 >= 0 && !isWallInBack(mouseX, mouseY)) {
                if (manhattanDistances[MAZE_LEN - 1 - (mouseY - 1)][mouseX] <= minDistance) {
                    minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY - 1)][mouseX];
                    dirToGo = 4;
                }
            }
        } else if (currDir % 4 == 2) {
            if (mouseX - 1 >= 0 && !isWallOnRight(mouseX, mouseY)) {
                if (manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX - 1] <= minDistance) {
                    minDistance = manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX - 1];
                    dirToGo = 1;
                }
            }
            if (mouseX + 1 < MAZE_LEN && !isWallOnLeft(mouseX, mouseY)) {
                if (manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX + 1] <= minDistance) {
                    minDistance = manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX + 1];
                    dirToGo = 2;
                }
            }
            if (mouseY - 1 >= 0 && !isWallInFront(mouseX, mouseY)) {
                if (manhattanDistances[MAZE_LEN - 1 - (mouseY - 1)][mouseX] <= minDistance) {
                    minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY - 1)][mouseX];
                    dirToGo = 3;
                }
            }
            if (mouseY + 1 < MAZE_LEN && !isWallInBack(mouseX, mouseY)) {
                if (manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX] <= minDistance) {
                    minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX];
                    dirToGo = 4;
                }
            }
        } else if (currDir % 4 == 1) {
            if (mouseY - 1 >= 0 && !isWallOnRight(mouseX, mouseY)) {
                if (manhattanDistances[MAZE_LEN - 1 - (mouseY - 1)][mouseX] <= minDistance) {
                    minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY - 1)][mouseX];
                    dirToGo = 1;
                }
            }
            if (mouseY + 1 < MAZE_LEN && !isWallOnLeft(mouseX, mouseY)) {
                if (manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX] <= minDistance) {
                    minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX];
                    dirToGo = 2;
                }
            }
            if (mouseX + 1 < MAZE_LEN && !isWallInFront(mouseX, mouseY)) {
                if (manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX + 1] <= minDistance) {
                    minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY)][mouseX + 1];
                    dirToGo = 3;
                }
            }
            if (mouseX - 1 >= 0 && !isWallInBack(mouseX, mouseY)) {
                if (manhattanDistances[MAZE_LEN - 1 - (mouseY)][mouseX - 1] <= minDistance) {
                    minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY)][mouseX - 1];
                    dirToGo = 4;
                }
            }
        } else if (currDir % 4 == 3) {
            if (mouseY + 1 < MAZE_LEN && !isWallOnRight(mouseX, mouseY)) {
                if (manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX] <= minDistance) {
                    minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX];
                    dirToGo = 1;
                }
            }
            if (mouseY - 1 >= 0 && !isWallOnLeft(mouseX, mouseY)) {
                if (manhattanDistances[MAZE_LEN - 1 - (mouseY - 1)][mouseX] <= minDistance) {
                    minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY - 1)][mouseX];
                    dirToGo = 2;
                }
            }
            if (mouseX - 1 >= 0 && !isWallInFront(mouseX, mouseY)) {
                if (manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX - 1] <= minDistance) {
                    minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY)][mouseX - 1];
                    dirToGo = 3;
                }
            }
            if (mouseX + 1 < MAZE_LEN && !isWallInBack(mouseX, mouseY)) {
                if (manhattanDistances[MAZE_LEN - 1 - (mouseY)][mouseX + 1] <= minDistance) {
                    minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY)][mouseX + 1];
                    dirToGo = 4;
                }
            }
        }
    }
    else{
        justTurned = false;
        dirToGo = 0;
    }

    return dirToGo;
}

bool hasVisited(int x, int y){
    return visitedCells[MAZE_LEN - 1 - y][x];
}

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 = 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);
            }
        }
    }
    else {
        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);
            }
        }
        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);
            }
        }
        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);
            }
        }
        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);
            }
        }
    }
}
 
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
    // Now TMR: enable clock with timer, APB1ENR
    // set period, autoreload value, ARR value 2^32-1, CR1 - TMR resets itself, ARPE and EN
    //
    // Encoder mode: disable timer before changing timer to encoder
    // CCMR1/2 1/2 depends on channel 1/2 or 3/4, depends on upper bits, depending which channels you use
    // CCMR sets sample rate and set the channel to input
    // CCER, which edge to trigger on, cannot be 11(not allowed for encoder mode), CCER for both channels
    // 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();
    //     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;

    int prevEncoder0Count = 0;
    int prevEncoder1Count = 0;
    int currEncoder0Count = 0;
    int currEncoder1Count = 0;

    bool overrideFloodFill = false;
    //right_motor.forward( 0.2 );
    //left_motor.forward( 0.2 );
    //turnRight180();
    //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!

        //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(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);
        // handleTurns();
        // wait(0.5);
        // moveForwardCellEncoder(1);
        // wait(0.5);
        // handleTurns();
        //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 );
 
        //reading = Rec_4.read();
//        serial.printf("reading: %f\n", reading);
    }
}