mazerunner.c

/* CMSC 31400 Final Project
 * Arman Bhalla & Matthew Asir >> Cloud Test
 * 29.11.2022
 * Version: v5, 04.12.2022
 */

#include <Servo.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <Wire.h>
#include <SCMD.h>
#include <SCMD_config.h>

// I2C master-slave constants for the ultrasonic sensor
#define SLAVE_BROADCAST_ADDR 0x00
#define SLAVE_ADDR 0x00

// The motor controllers for the front and back of the car
#define RIGHT_MOTOR 0
#define LEFT_MOTOR 1
SCMD front_driver;
SCMD back_driver;

// The panning/turning servo-controlled mount holding the ultrasonic sensor
Servo mount_servo;

// Analogue output values from the ultrasonic sensor
uint8_t distance_H = 0;
uint8_t distance_L = 0;
uint16_t distance = 0;

byte motorSpeed = 55;                             //The maximum motor speed
int motorOffset = 0;                              //Factor to account for one side being more powerful
int turnSpeed = 50;                               //Amount to add to motor speed when turning


void setup() 
{
  rightBackSpeed(motorSpeed);                 //Set the motors to the motor speed
  rightFrontSpeed(motorSpeed);
  leftFrontSpeed(motorSpeed+motorOffset);
  leftBackSpeed(motorSpeed+motorOffset);
  rightBackSpeed(0);                              //Ensure all motors are stopped
  rightFrontSpeed(0);
  leftBackSpeed(0);
  leftFrontSpeed(0);
  mount.attach(10);                               //Assign the servo pin
}


void loop() 
{
  mount_servo.write(90);                            //Set the servo to look straight ahead
  delay(750);
  int distance = getDistance();                   //Check that there are no objects ahead
  if(distance >= stopDist)                        //If there are no objects within the stopping distance, move forward
  {
    moveForward();
  }
  while(distance >= stopDist)                     //Keep checking the object distance until it is within the minimum stopping distance
  {
    distance = getDistance();
    delay(250);
  }
  stopMove();                                     //Stop the motors
  int turnDir = checkDirection();                 //Check the left and right object distances and get the turning instruction
  Serial.print(turnDir);
  switch (turnDir)                                //Turn left, turn around or turn right depending on the instruction
  {
    case 0:                                       //Turn left
      turnLeft (400);
      break;
    case 1:                                       //Turn around
      turnLeft (700);
      break;
    case 2:                                       //Turn right
      turnRight (400);
      break;
  }
}

// gradually reduces speed (be careful of using this in loop() , leads to jerky motion)
void accelerate() {
  for (int i=0; i<motorSpeed; i++)
  {
    rightBackSpeed(i);
    rightFrontSpeed(i);
    leftFrontSpeed(i+motorOffset);
    leftBackSpeed(i+motorOffset);
    delay(10);
  }
}


// gradually reduces speed (be careful of using this in loop() , leads to jerky motion)
void decelerate() {
  for (int i=motorSpeed; i!=0; i--)
  {
    rightBackSpeed(i);
    rightFrontSpeed(i);
    leftFrontSpeed(i+motorOffset);
    leftBackSpeed(i+motorOffset);
    delay(10);
  }
}

// sets all the motors in the forward direction (DOES NOT CHANGE SPEED OR SWITCH THEM ON)
void moveForward() {
  rightBackForward();
  rightFrontForward();
  leftBackForward();
  rightFrontForward();
}


// stops all motors
void stopMove() {
  rightBackSpeed(0);
  leftBackSpeed(0);
  rightFrontSpeed(0);
  leftFrontSpeed(0);
}


// this turns left for a given time in MILISECONDS
void turnLeft(int duration) {
  rightBackSpeed(motorSpeed+turnSpeed);
  rightFrontSpeed(motorSpeed+turnSpeed);
  leftFrontSpeed(motorSpeed+motorOffset+turnSpeed);
  leftBackSpeed(motorSpeed+motorOffset+turnSpeed);
  rightBackForward();
  rightFrontForward();
  leftFrontReverse();
  leftBackReverse();
  delay(duration);
  rightBackSpeed(motorSpeed);
  rightFrontSpeed(motorSpeed);
  leftFrontSpeed(motorSpeed+motorOffset);
  leftBackSpeed(motorSpeed+motorOffset);
  stopMove();
}


// this turns right for a given time in MILISECONDS
// motor offset is simply if one side of motors is more powerful than the other
void turnRight(int duration) {
  rightBackSpeed(motorSpeed+turnSpeed);
  rightFrontSpeed(motorSpeed+turnSpeed);
  leftFrontSpeed(motorSpeed+motorOffset+turnSpeed);
  leftBackSpeed(motorSpeed+motorOffset+turnSpeed);
  rightBackReverse();
  rightFrontReverse();
  leftFrontForward();
  leftBackForward();
  delay(duration);
  rightBackSpeed(motorSpeed);
  rightFrontSpeed(motorSpeed);
  leftFrontSpeed(motorSpeed+motorOffset);
  leftBackSpeed(motorSpeed+motorOffset);
  stopMove();
}

// this function is responsible for actually getting the distance from the ultrasonic sensor
// if we wanted to switch to time of flight for example, we'd change this to the code required to get
// distance from a time of flight sensor
int getDistance() {
  // Begin ultrasonic sensing
  Wire.beginTransmission(SLAVE_ADDR);               // transmit to device #
  Wire.beginTransmission(SLAVE_ADDR);               // transmit to device #8
  Wire.write(1);                                    // measure command: 0x01
  Wire.endTransmission();                           // stop transmitting
  Wire.requestFrom(SLAVE_ADDR, 2);                  // request 6 bytes from slave device #8
  while (Wire.available()) {                        // slave may send less than requested
    distance_H = Wire.read();                       // receive a byte as character
    distance_L = Wire.read();
    distance = (uint16_t)distance_H << 8;
    distance = distance | distance_L;
    int d1 = distance;
    delay(100);
    distance_H = Wire.read();                       // receive a byte as character
    distance_L = Wire.read();
    distance = (uint16_t)distance_H << 8;
    distance = distance | distance_L;
    delay(100);
    int d2 = distance;
    int d_avg = (d1 + d2) / 2;
    distance = d_avg;
  }
  return distance;
}

// this function does the distance comparison from whatever sensor we are using
// the distance array holds on 2 distances (from the sides), and based on the value
// of these directions turns the car
int checkDirection() {
  int distances [2] = {0,0};
  int turnDir = 1;
  servoLook.write(180);
  delay(500);
  distances [0] = getDistance();
  servoLook.write(0);
  delay(1000);
  distances [1] = getDistance();
  if (distances[0]>=200 && distances[1]>=200) {
    turnDir = 0;
  } else if (distances[0]<=stopDist && distances[1]<=stopDist) {
    turnDir = 1;
  } else if (distances[0]>=distances[1]) {
    turnDir = 0;
  } else if (distances[0]<distances[1]) {
    turnDir = 2;
  }
  return turnDir;
}



// Don't look too much into this besides the title ---
// these functions all do one thing (change speed for a given motor)

inline void leftFrontSpeed(int speed) {
  front_driver.setDrive(LEFT_MOTOR, 0, speed);
}
inline void rightFrontSpeed(int speed) {
  front_driver.setDrive(RIGHT_MOTOR, 0, speed);
}
inline void leftBackSpeed(int speed) {
  back_driver.setDrive(LEFT_MOTOR, 0, speed);
}
inline void rightBackSpeed(int speed) {
  back_driver.setDrive(RIGHT_MOTOR, 0, speed);
}


// These functions reverse the direction of a specific motor

inline void leftFrontReverse() {
  front_driver.inversionMode(LEFT_MOTOR, 1);
}
inline void rightFrontReverse() {
  front_driver.inversionMode(RIGHT_MOTOR, 1);
}
inline void leftBackReverse() {
  back_driver.inversionMode(LEFT_MOTOR, 1);
}
inline void rightBackReverse() {
  back_driver.inversionMode(RIGHT_MOTOR, 1);
}


// These functions make a certain motor change to forward (0) direction

inline void leftFrontForward() {
  front_driver.inversionMode(LEFT_MOTOR, 0);
}
inline void rightFrontForward() {
  front_driver.inversionMode(RIGHT_MOTOR, 0);
}
inline void leftBackForward() {
  back_driver.inversionMode(LEFT_MOTOR, 0);
}
inline void rightBackForward() {
  back_driver.inversionMode(RIGHT_MOTOR, 0);
}