EEEE4008: ‘Recon-Scout’ Platform: Arduino MEGA (Slave) Code

//*** Maze Navigation with a Default Left Implementation
//*** including I2C & Route Storage

//include libraries
#include <SPI.h>
#include <Wire.h>
#include <nRF24L01.h>
#include <RF24.h>
#include <AccelStepper.h>
#include <MultiStepper.h>
#include "Ultrasonic.h"

String route = "";
int stepCount = 0;

//variables for each window segment
int distanceLeft;
int distanceFront;
int distanceRight;

//defining the ultrasonic sensor instances
Ultrasonic uL(A1);
Ultrasonic uF(A2);
Ultrasonic uR(A3);

//constraints and other variables
int corridorWidth = 38; //((wall-to-wall width(cm)/2)+tolerance), ensure it is well under 2*wall-to-wall width

bool prevForward = false; //previously moved forward?

int motor1Pos;
int motor2Pos;
int motor3Pos;

//stepper motor variables
const int step1Pin = 8;
const int dir1Pin = 7;
const int step2Pin = 6;
const int dir2Pin = 5;
const int step3Pin = 4;
const int dir3Pin = 3;

AccelStepper motor1(1,step1Pin, dir1Pin); //1 is a driver interface
AccelStepper motor2(1,step2Pin, dir2Pin);
AccelStepper motor3(1,step3Pin, dir3Pin);

MultiStepper motors;

long positions[3]; //array of step positions

void forwardX(){
  stepCount = stepCount + int(200/1.1547);

  //forward 170mm
  positions[0] = 208;
  positions[1] = -208;
  positions[2] = 0;

  motor1.setCurrentPosition(0);
  motor2.setCurrentPosition(0);
  motor3.setCurrentPosition(0);

  motors.moveTo(positions);
  motors.runSpeedToPosition();
}

void stopMotors(){
  //stop
  motor1.setSpeed(0);
  motor2.setSpeed(0);
  motor3.setSpeed(0);
  motors.run();
}

void rotate90CC(){
  // rotate 90 degrees left
  route = route + "L,";
  positions[0] = -183;
  positions[1] = -183;
  positions[2] = -183;

  motor1.setCurrentPosition(0);
  motor2.setCurrentPosition(0);
  motor3.setCurrentPosition(0);

  motors.moveTo(positions);
  motors.runSpeedToPosition();
  stepCount = 0;
}

void rotate90C(){
  route = route + "R,";
  //rotate 90 degrees right
  positions[0] = 183;
  positions[1] = 183;
  positions[2] = 183;

  motor1.setCurrentPosition(0);
  motor2.setCurrentPosition(0);
  motor3.setCurrentPosition(0);

  motors.moveTo(positions);
  motors.runSpeedToPosition();
  stepCount = 0;
}

void goForward(){
  //forwards
  positions[0] = 20;
  positions[1] = -20;
  positions[2] = 0;

  motor1.setCurrentPosition(0);
  motor2.setCurrentPosition(0);
  motor3.setCurrentPosition(0);

  motors.moveTo(positions);
  motors.runSpeedToPosition();
  stepCount = stepCount + int(100/1.1547);
}

void receiveEvent(int x) {
  while (Wire.available()) { // loop through all but the last
    stopMotors();
    while (true){
      //latch
    }

    //uncomment for I2C transmission back to Raspberry Pi
    //Wire.beginTransmission(4);
    //Wire.write(route);
    //Wire.endTransmission();
  }
}


void setup() {
  Serial.begin(115200); //for troubleshooting - slows down the code

  //I2C setup
  Wire.begin(0x8);
  Wire.onReceive(receiveEvent);

  //stepper motor setup
  motor1.setMaxSpeed(200);
  motor2.setMaxSpeed(200);
  motor3.setMaxSpeed(200);
  motor1.setAcceleration(150);
  motor2.setAcceleration(150);
  motor3.setAcceleration(150);

  motors.addStepper(motor1);
  motors.addStepper(motor2);
  motors.addStepper(motor3);
}

void loop() {

    //measure the distances
    distanceLeft = uL.MeasureInCentimeters();
    distanceFront = uF.MeasureInCentimeters();
    distanceRight = uR.MeasureInCentimeters();

    Serial.print(distanceLeft);
    Serial.print(",");
    Serial.print(distanceFront);
    Serial.print(",");
    Serial.println(distanceRight);

    if (distanceLeft > corridorWidth){ //if left path avaliable
      Serial.println("Left");
      prevForward = false;

      //forward X amount
      forwardX();
      route = route + String(stepCount) + (",");

      //stop
      //stopMotors();
      delay(1000);

      //turn left
      rotate90CC();

      //stop
      stopMotors();
      delay(1000);

      //forward X amount
      forwardX();

      //stop
      stopMotors();
      delay(1000);
    }

    else if ((distanceLeft <= corridorWidth) && (distanceFront > corridorWidth)){ //else if a front path is avaliable
      Serial.println("Front");
      if (prevForward == false){
        goForward();
        prevForward = true;
      }
      else {
        goForward();
      }
    }

    else if ((distanceRight > corridorWidth) && (distanceFront <= corridorWidth) && (distanceLeft <= corridorWidth)){ //else if a right path is avaliable
      Serial.println("Right");
      prevForward = false;

      //forward X amount
      forwardX();
      route = route + String(stepCount) + (",");

      //stop
      stopMotors();
      delay(1000);

      //turn right
      rotate90C();

      //stop
      stopMotors();
      delay(1000);

      //forward X amount
      forwardX();
    }

    else { //at a dead end so need to turn around
      Serial.println("Dead End");
      //stop
      stopMotors();
      delay(1000);

      //turn right
      rotate90C();

      //stop
      stopMotors();
      delay(1000);

      //turn right
      rotate90C();

      //stop
      stopMotors();
      delay(1000);
    }
}