main.cpp

// RSLB
#include <Arduino.h>


int trig1 = A3; // FRONT SENSOR
int echo1 = A2;

int trig2 = A1; // LEFT SENSOR 1 0
int echo2 = A0;

int trig3 = A5; // RIGHT SENSOR
int echo3 = A4;

int encoderLeft = 8;
int encoderRight = 9;

int sensorIR = 10;
int motorLeft1 = 11; 
int motorLeft2 = 2;
int motorLeftSpeed = 3;   // PWM
int motorRight1 = 4;
int motorRight2 = 5;
int motorRightSpeed = 6;  // PWM


// PARAMETERS TO TUNE BASED ON BEHAVIOR
int steps_90L = 14;    // 90 Degrees Left Turn
int steps_90R = 10;    // 90 Degrees Right Turn
int steps_180R = 22;   // 180 Degrees Turn

int steps_F1 = 10;      // How much it needs to advance after turn 90 degrees to get out of an intersection
int steps_F2 = 15;      // How much it needs to advance when it is going straight to get out of an intersection.

float K = 4.5;

int threshold_F = 10;
int threshold_R = 15;
int threshold_L = 15; 

int basic_speed_L = 110;
int basic_speed_R = 130;
unsigned long distanceFront, distanceLeft, distanceRight;

float err = 0;

bool end_of_maze = false;

String moves = "";

void ultrasonic(int n)
{

    Serial.print(n);
    Serial.print(", ");

    if (n == 1)
    {
        distanceFront = 0;
        while (distanceFront <= 0)
        {
            digitalWrite(trig1, LOW);
            delayMicroseconds(3);
            digitalWrite(trig1, HIGH);
            delayMicroseconds(10);
            digitalWrite(trig1, LOW);
            int duration1 = pulseIn(echo1, HIGH);
            distanceFront = duration1 * 0.034 / 2;
        }
    }
    else if (n == 2)
    {
        distanceRight = 0;
        while (distanceRight <= 0)
        {
            digitalWrite(trig2, LOW);
            delayMicroseconds(3);
            digitalWrite(trig2, HIGH);
            delayMicroseconds(10);
            digitalWrite(trig2, LOW);
            int duration2 = pulseIn(echo2, HIGH);
            distanceRight = duration2 * 0.034 / 2;
        }
    }
    else if (n == 3)
    {
        distanceLeft = 0;
        while (distanceLeft <= 0)
        {
            digitalWrite(trig3, LOW);
            delayMicroseconds(3);
            digitalWrite(trig3, HIGH);
            delayMicroseconds(10);
            digitalWrite(trig3, LOW);
            int duration3 = pulseIn(echo3, HIGH);
            distanceLeft = duration3 * 0.034 / 2;
        }
    }
}

#include "motors.h"

void setup()
{
    pinMode(trig1, OUTPUT);
    pinMode(echo1, INPUT);

    pinMode(trig2, OUTPUT);
    pinMode(echo2, INPUT);

    pinMode(trig3, OUTPUT);
    pinMode(echo3, INPUT);

    pinMode(motorLeft1, OUTPUT);
    pinMode(motorLeft2, OUTPUT);
    pinMode(motorLeftSpeed, OUTPUT);

    pinMode(motorRight1, OUTPUT);
    pinMode(motorRight2, OUTPUT);
    pinMode(motorRightSpeed, OUTPUT);

    pinMode(encoderLeft, INPUT);
    pinMode(encoderRight, INPUT);

    pinMode(sensorIR, INPUT);

    Serial.begin(9600);

    end_of_maze = false;   
}

void loop()
{
    ultrasonic(1);    // Front
    ultrasonic(2);    // Right
    ultrasonic(3);    // Left

    Serial.print("Ultrasonics: Left: ");
    Serial.print(distanceLeft);
    Serial.print(" Front ");
    Serial.print(distanceFront);
    Serial.print(" Right: ");
    Serial.print(distanceRight);

    if (end_of_maze == false)  // Has to be changed based on infrared sensor
    {
        bool wallL = distanceLeft < threshold_L;
        bool wallR = distanceRight < threshold_R;
        bool wallF = distanceFront < threshold_F;
        bool intersection = wallF || (!wallF && (!wallR || !wallL));

        if (!intersection)
        {
            if (distanceFront < 10)
            {
                stopMotors();
            }

            else
            {
                moveForward();
                err =  distanceLeft - distanceRight;
                setSpeed(basic_speed_L - K * err, basic_speed_R + K * err);
            }

            Serial.println("     no intersection");
        }

        else if (wallR == false)
        {
            Serial.println("     turning Right");

            setSpeed(basic_speed_L, basic_speed_R);

            rotateRight(steps_90R);

            moveForwardUntil(steps_F1);

            moves += 'R';

        }

        else if (wallF == false)
        {
            Serial.println("     going Straight");

            setSpeed(basic_speed_L, basic_speed_R);

            moveForwardUntil(steps_F2);

            moves += 'S';

        }

        else if (wallL == false)
        {

            Serial.println("     turning Left");

            setSpeed(basic_speed_L, basic_speed_R);

            rotateLeft(steps_90L);

            moveForwardUntil(steps_F1);

            moves += 'L';
        }

        else
        {
            Serial.println("     going Backward");

            setSpeed(basic_speed_L, basic_speed_R);

            rotateRight(steps_180R);
            
            moves += 'B';
        }
    }
}

motors.h

void rotateLeft(int steps)
{
    int stepLeft = 0;
    int stepRight = 0;
    while (true)
    {
        sensorStateRight = digitalRead(encoderRight);
        sensorStateLeft = digitalRead(encoderLeft);

        if (sensorStateRight != prevRight)
        {
            prevRight = sensorStateRight;
            stepRight++;
        }
        if (sensorStateLeft != prevLeft)
        {
            prevLeft = sensorStateLeft;
            stepLeft++;
        }
        if (stepRight < steps)
        {
            digitalWrite(motorRight2, 1);
            digitalWrite(motorRight1, 0);
        }
        else
        {
            digitalWrite(motorRight2, 0);
            digitalWrite(motorRight1, 0);
        }

        if (stepLeft < steps)
        {
            digitalWrite(motorLeft2, 1);
            digitalWrite(motorLeft1, 0);
        }
        else
        {
            digitalWrite(motorLeft2, 0);
            digitalWrite(motorLeft1, 0);
        }

        if (stepRight >= steps && stepLeft >= steps)
        {
            break;
        }
    }
}

void rotateRight(int steps)
{
    int stepLeft = 0;
    int stepRight = 0;
   
    while (true)
    {
        sensorStateRight = digitalRead(encoderRight);
        sensorStateLeft = digitalRead(encoderLeft);

        if (sensorStateRight != prevRight)
        {
            prevRight = sensorStateRight;
            stepRight++;
        }
        if (sensorStateLeft != prevLeft)
        {
            prevLeft = sensorStateLeft;
            stepLeft++;
        }

        if (stepRight < steps)
        {
            digitalWrite(motorRight2, 0);
            digitalWrite(motorRight1, 1);
        }
        else
        {
            digitalWrite(motorRight2, 0);
            digitalWrite(motorRight1, 0);
        }

        if (stepLeft < steps)
        {
            digitalWrite(motorLeft2, 0);
            digitalWrite(motorLeft1, 1);
        }
        else
        {
            digitalWrite(motorLeft2, 0);
            digitalWrite(motorLeft1, 0);
        }

        if (stepRight >= steps && stepLeft >= steps)
        {
            break;
        }
    }
}

void setSpeed(float leftSpeed, float rightSpeed)
{
    analogWrite(motorLeftSpeed, leftSpeed);
    analogWrite(motorRightSpeed, rightSpeed);
}

void stopMotors()
{
    digitalWrite(motorLeft1, LOW);
    digitalWrite(motorLeft2, LOW);
    digitalWrite(motorRight1, LOW);
    digitalWrite(motorRight2, LOW);
}

void moveForward()
{
    digitalWrite(motorLeft1, HIGH);
    digitalWrite(motorLeft2, LOW);
    digitalWrite(motorRight1, LOW);
    digitalWrite(motorRight2, HIGH);
}

void moveForwardUntil(int steps)
{
    int stepLeft = 0;
    int stepRight = 0;
    while (true)
    {
        sensorStateRight = digitalRead(encoderRight);
        sensorStateLeft = digitalRead(encoderLeft);

        if (sensorStateRight != prevRight)
        {
            prevRight = sensorStateRight;
            stepRight++;
        }
        if (sensorStateLeft != prevLeft)
        {
            prevLeft = sensorStateLeft;
            stepLeft++;
        }
       
        if (stepRight < steps)
        {
            digitalWrite(motorRight2, 1);
            digitalWrite(motorRight1, 0);
        }
        else
        {
            digitalWrite(motorRight2, 0);
            digitalWrite(motorRight1, 0);
        }

        if (stepLeft < steps)
        {
            digitalWrite(motorLeft2, 0);
            digitalWrite(motorLeft1, 1);
        }
        else
        {
            digitalWrite(motorLeft2, 0);
            digitalWrite(motorLeft1, 0);
        }

        if (stepRight >= steps && stepLeft >= steps)
        {
            break;
        }
    }
}