Skip to content

Instantly share code, notes, and snippets.

@dahmadjid

dahmadjid/main.cpp

Created June 24, 2022 20:18
Show Gist options

  • Select an option

    Learn more about clone URLs
  • Save dahmadjid/25f64ac6ed073bd6bbc72ddea68cf4b3 to your computer and use it in GitHub Desktop.

Select an option

Learn more about clone URLs
Save dahmadjid/25f64ac6ed073bd6bbc72ddea68cf4b3 to your computer and use it in GitHub Desktop.
Download ZIP

Revisions

  1. dahmadjid created this gist Jun 24, 2022.
    219 changes: 219 additions & 0 deletions main.cpp
    View file
    Original file line number Diff line number Diff line change
    @@ -0,0 +1,219 @@
    // 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';
    }
    }
    }
    169 changes: 169 additions & 0 deletions motors.h
    View file
    Original file line number Diff line number Diff line change
    @@ -0,0 +1,169 @@

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