Workshop - Robotics - Adding Autonomy

arduino-robot.ino

// Pins
int echoPin = 5;
int trigPin = 6;
int switchPin = 7;  
const int BIN1 = 8; 
const int BIN2 = 9;
const int PWMB = 10;
const int PWMA = 11; 
const int AIN2 = 12; 
const int AIN1 = 13;

// Parameters
const int backupTime = 300;     // ms
const int turnTime = 200;       // ms
const int distanceThresh = 25;  // cm

// Globals
String botDirection;
String distance;

void setup()
{
  // We use the switch to enable motors
  pinMode(switchPin, INPUT_PULLUP); 

  // Set motor driver pins as output
  pinMode(AIN1, OUTPUT);
  pinMode(AIN2, OUTPUT);
  pinMode(PWMA, OUTPUT);
  pinMode(BIN1, OUTPUT);
  pinMode(BIN2, OUTPUT);
  pinMode(PWMB, OUTPUT);

  // Add Serial output
  Serial.begin(9600);
  Serial.print("Autobots, roll out!");
}

void loop()
{
  // Get distance
  int distance = getDistance();
  Serial.print("Distance: ");
  Serial.print(distance);
  Serial.println(" cm");

  // If switch is flipped, run motors
  if ( digitalRead(switchPin) == LOW ) {
    if ( distance < distanceThresh ) {
      Serial.println("Back it up!");

      // Stop
      rightMotor(0);
      leftMotor(0);
      delay(200);

      // Back up
      rightMotor(-255);
      leftMotor(-255);
      delay(backupTime);

      // Turn away from obstacle
      rightMotor(255);
      leftMotor(-255);
      delay(turnTime);
     
    } else {

      // Move forward if no obstacles detected
      rightMotor(255);
      leftMotor(255);
    }
  } else {

    // Stop motors if switch is off
    rightMotor(0);
    leftMotor(0);
  }

  // Wait some time between readings
  delay(50);
}

// Functions
void rightMotor(int motorSpeed) {
  if (motorSpeed > 0) {                               //if the motor should drive forward (positive speed)
    digitalWrite(AIN1, HIGH);                         //set pin 1 to high
    digitalWrite(AIN2, LOW);                          //set pin 2 to low
  } else if (motorSpeed < 0) {                        //if the motor should drive backward (negative speed)
    digitalWrite(AIN1, LOW);                          //set pin 1 to low
    digitalWrite(AIN2, HIGH);                         //set pin 2 to high
  } else {                                            //if the motor should stop
    digitalWrite(AIN1, LOW);                          //set pin 1 to low
    digitalWrite(AIN2, LOW);                          //set pin 2 to low
  }
  analogWrite(PWMA, abs(motorSpeed));                 //now that the motor direction is set, drive it at the entered speed
}

void leftMotor(int motorSpeed) {                      //function for driving the left motor
  if (motorSpeed > 0) {                               //if the motor should drive forward (positive speed)
    digitalWrite(BIN1, HIGH);                         //set pin 1 to high
    digitalWrite(BIN2, LOW);                          //set pin 2 to low
  } else if (motorSpeed < 0) {                        //if the motor should drive backward (negative speed)
    digitalWrite(BIN1, LOW);                          //set pin 1 to low
    digitalWrite(BIN2, HIGH);                         //set pin 2 to high
  } else {                                            //if the motor should stop
    digitalWrite(BIN1, LOW);                          //set pin 1 to low
    digitalWrite(BIN2, LOW);                          //set pin 2 to low
  }
  analogWrite(PWMB, abs(motorSpeed));                 //now that the motor direction is set, drive it at the entered speed
}

float getDistance() {
  unsigned long t;
  float d;
  
  // Tell the sensor to send out ultrasonic pulse
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10); 
  digitalWrite(trigPin, LOW);

  // Read how long it takes for pulse to return to sensor
  t = pulseIn(echoPin, HIGH);

  // Calculate distance of object based on reflected pulse
  d = 0.01715 * t;
  
  return d;
}