Autonomous Vehicle Project as part of CUED IB coursework

IntegratedDesignProject.ino

#include <Adafruit_MotorShield.h>
#include <BlockNot.h>
#include <math.h>

// Pin IO

const int pinLineSensor1 = A0;
const int pinLineSensor2 = A1;
const int pinLineSensor3 = A2;

const int pinLeftMotor = 1;
const int pinRightMotor = 2;

const int pinHallEffectRedLED = 0;
const int pinHallEffectGreenLED = 1;
const int pinFlashingLED = 2;

const int pinDistanceEcho = 7;
const int pinDistanceTrigger = 6;

const int pinPushButton = 4;

const int pinHallEffectSensor = 5;

//                     Using BlockNot for timings
//                     --------------------------
//
// It is very useful to schedule things to occur in the future. For example, we
// need to update the PID system roughly every 50 milliseconds. This can be
// implemented like so:
//
//   void loop() {
//     doPIDMotorControl();
//     delay(50);
//   }
//
// This is problematic when it comes to scheduling many simultaneous events. For
// example, we need to update PID control every 50 milliseconds and update the
// flashing LED every 250 milliseconds. With a bit of thought, you can implement
// this like so:
//
//   void loop() {
//     for (int i = 0; i < 5; i++) {
//       // this code gets executed once every 50 milliseconds
//       doPIDMotorControl();
//       delay(50);
//     }
//     // this code gets executed once every 250 milliseconds
//     updateFlashingLED();
//   }
//
//
// What if requirement were changed so that the LED operated at 4Hz? What if
// physical testing showed us that PID motor control needed to occur every 30ms?
// What if doPIDMotorControl() took 30ms to fetch data over WiFi?
// The code above does cannot work effectively in any general case.
//
//                     The Solution: BlockNot
//                     ----------------------
//
// BlockNot is a library that manages these sorts of timers for you.  The previous
// code becomes:
//
//   BlockNot updatePIDTimer(50, MILLISECONDS);
//   BlockNot flashingLEDTimer(250, MILLISECONDS);
//
//   void loop() {
//     if (updatePIDTimer.TRIGGERED) {
//       // this code gets executed once every 50 milliseconds
//       doPIDMotorControl();
//     }
//     if (flashingLEDTimer.TRIGGERED) {
//       // this code gets executed once every 250 milliseconds
//       updateFlashingLED();
//     }
//   }
//
// Simply put this method is far more flexible. We could add many more
// components, each executing at arbitary intervals, all with relative ease.

// This timer is responsible for updating PID control every 50 milliseconds
const int timeStep = 50;
BlockNot updatePIDTimer(timeStep, MILLISECONDS);

// This timer allows us to update the LED's status at a frequency of 2Hz. Since
// the LED changes state four times every second, we set the timer to be
// triggered every 250 milliseconds. Every time the timer is triggered, we
// update the value of flashingLEDStatus and write to the LED.
BlockNot flashingLEDTimer(250, MILLISECONDS);
bool flashingLEDStatus = false;

// Unfortunately, we are using a push-switch to switch the robot on and off.
// This is problematic because the "Button Pushed" signal is not debounced, and
// lasts anywhere between 200 to 600 milliseconds. To deal with this, we use
// BlockNot to keep track of when the button was last pushed and ignore button
// presses that occur in close succession to each other.
BlockNot pushButtonTimer(1, SECONDS);
bool robotIsOn = true;

// Unfortunately, the distance sensor tends to pick up erroneous values while
// the vehicle is turning. This is fixed by simply ignoring the distance sensor
// output for the first 70 seconds.
BlockNot distanceSensorTimer(70, SECONDS);

// Rather than detecting junctions, we instead set a timer that gets triggered
// when the robot is (empirically) at the red/green box. The timer begins when
// the robot has picked up the box, and is triggered upon reaching the box
BlockNot greenBoxTimer(52, SECONDS);
BlockNot redBoxTimer(81, SECONDS);

// Global State

bool pickedUpBlock = false;
int numDistanceShort = 0;

// Create the motor shield object with the default address
Adafruit_MotorShield AFMS = Adafruit_MotorShield();

// Select which 'port' M1, M2, M3 or M4
Adafruit_DCMotor *leftMotor = AFMS.getMotor(pinLeftMotor);
Adafruit_DCMotor *rightMotor = AFMS.getMotor(pinRightMotor);

const float MOTOR_SPEED_DEFAULT = 0.6;  // default linear speed of motors
const float MOTOR_ACCELERATION = 0.008; // rate of increase in MOTOR_SPEED per millisecond
float MOTOR_SPEED = 0;                  // the desired MOTOR_SPEED

// At any given time, the motor speeds are given as
//
//    rightMotor:  255 * tanh(MOTOR_SPEED + curvature)
//    leftMotor:   255 * tanh(MOTOR_SPEED - curvature)
//
// this has the following advantages:
//
//  - `tanh()` gaurentees the speed to be between -255 and 255, meaning that we can
//    use PID while still ruling out overflow issues! This eliminates quite a nasty
//    class of bugs where the motors abruptly stutter to zero with little warning.
//
//  - the input `curvature` is not the true curvature of the vehicle, but this
//    doesn't matter for PID tuning. The requirements are that 1) the true
//    curvature is zero for an input curvature of zero 2) the true curvature always
//    increases for increasing values of input curvature
//
//  - `MOTOR_SPEED` allows us to increase the linear speed of the vehicle. This
//    is inceased when the motor is heading in a straight line, and reset when the
//    line following reports an error
void set_motor_curvature(float curve) {
  float motor_right_speed = 255 * tanhf((MOTOR_SPEED + curve) * 0.92);
  float motor_left_speed = 255 * tanhf(MOTOR_SPEED - curve);

  rightMotor->setSpeed(int(abs(motor_right_speed)));
  rightMotor->run((motor_right_speed > 0) ? BACKWARD : FORWARD);
  leftMotor->setSpeed(int(abs(motor_left_speed)));
  leftMotor->run((motor_left_speed > 0) ? BACKWARD : FORWARD);
}

const float Kp = 0.56;  // related to the proportional control term;
const float Ki = 0.002; // related to the integral control term;
const float Kd = 0.4;   // related to the derivative control term;

// error is the approximate number of centimeters between the center of the
// distance sensor and the center of the white line
float error = 0;
// lastError is the previous error, also measured in centimeters
float lastError = 0;
// I is the error integrated over time, in units of centimeter-milliseconds (eeew!!)
float I = 0;

float PID_control() {
  float P = error;
  float D = (error - lastError) / timeStep;
  I = I + error * timeStep;
  lastError = error;
  return P * Kp + I * Ki + D * Kd;
}

// Sets motor curvature to follow the line using the line sensor reading in a
// binary format.
//
// line_sensor_bits is to be an integer in the range 0-7, such that:
//  - 100b corresponds to only the left line detector reading a line
//  - 010b corresponds to only the central line detector reading a line
//  - 011b corresponds to the central and right line detector reading a line
//  - ect. ect.
// 
void follow_line(int line_sensor_bits) {
  // We need to convert line_sensor_bits into error. Since line_sensor_bits is
  // an integer, we can use a table to lookup the approximate number of
  // centimeters of error. This is far nicer than a nested series of if
  // statements. To summarize:
  // - error values at 010b and 000b are 0
  // - error values at 100b and 110b are positive, with 001b being a much larger error
  // - error values at 001b and 011b are negative, with 100b being a much larger error
  // - error values at 101b and 111b are undefined
  float table[] = {0, -5, 0, -1, 5, NAN, 1, NAN};
  error = table[line_sensor_bits];

  // We want the robot to accelerate fast along straight lines, and turn corners
  // slowly. This is achieved by the MOTOR_SPEED variable, which increases
  // while there is a small error, and reset when there is a large error.
  if (error == 0 || error == -1 || error == 1) {
    MOTOR_SPEED += MOTOR_ACCELERATION * timeStep;
  } else {
    MOTOR_SPEED = MOTOR_SPEED_DEFAULT;
  }

  MOTOR_SPEED = MOTOR_SPEED_DEFAULT;
  float curvature = PID_control();
  set_motor_curvature(curvature);
}

// move forward a number of centimeters in a blocking manner
void move_forward_centimetres(float distance) {
  // hardware weirdness: Motor->run(BACKWARD) moves the motors forward
  rightMotor->setSpeed(160);
  rightMotor->run(BACKWARD);
  leftMotor->setSpeed(160);
  leftMotor->run(BACKWARD);
  delay(int(distance * 890.0));
}

// move backward a number of centimeters in a blocking manner
void move_backward_centimetres(float distance) {
  // hardware weirdness: Motor->run(FORWARD) moves the motors backward
  rightMotor->setSpeed(160);
  rightMotor->run(FORWARD);
  leftMotor->setSpeed(160);
  leftMotor->run(FORWARD);
  delay(int(distance * 890.0));
}

// rotate 90 degrees anticlockwise in a blocking manner.
void rotate_90_degrees_anticlockwise() {
  rightMotor->setSpeed(150);
  rightMotor->run(FORWARD);
  leftMotor->setSpeed(150);
  leftMotor->run(BACKWARD);
  delay(3000);
}

// rotate 90 degrees clockwise in a blocking manner.
void rotate_90_degrees_clockwise() {
  rightMotor->setSpeed(150);
  rightMotor->run(BACKWARD);
  leftMotor->setSpeed(150);
  leftMotor->run(FORWARD);
  delay(3000);
}

// stop moving all motors entirely, non-blocking
void stop_all_motors() {
  rightMotor->setSpeed(0);
  rightMotor->run(RELEASE);
  leftMotor->setSpeed(0);
  leftMotor->run(RELEASE);
}

void setup() {
  Serial.begin(9600);
  Serial.println("Running Setup Code!");

  if (!AFMS.begin()) {
    Serial.println("Could not find Motor Shield. Check wiring.");
    while (1);
  }
  Serial.println("Motor Shield found.");

  // setup pin inputs and outputs
  pinMode(pinLineSensor1, INPUT);
  pinMode(pinLineSensor2, INPUT);
  pinMode(pinLineSensor3, INPUT);

  pinMode(pinDistanceEcho, INPUT);
  pinMode(pinDistanceTrigger, OUTPUT);
  pinMode(pinHallEffectRedLED, OUTPUT);
  pinMode(pinHallEffectGreenLED, OUTPUT);

  pinMode(pinHallEffectSensor, INPUT);
  pinMode(pinFlashingLED, OUTPUT);
  pinMode(pinPushButton, INPUT_PULLUP);

  digitalWrite(pinDistanceTrigger, LOW);

  delay(1000);

  while (digitalRead(pinPushButton)) {
    delay(200);
  }

  // this is the "boot sequence"; the code which moves the robot out of the box
  // and rotates it onto the line
  move_forward_centimetres(35.0);
  rotate_90_degrees_anticlockwise();
  Serial.println("Starting loop code");
}

float read_distance_sensor() {
  digitalWrite(pinDistanceTrigger, HIGH);
  delayMicroseconds(10);
  digitalWrite(pinDistanceTrigger, LOW);

  int duration = pulseIn(pinDistanceEcho, HIGH);
  return duration / 58.0;
}

// line detector readings are returned in a binary format, such that:
//  - 100b corresponds to only the left line detector reading a line
//  - 010b corresponds to only the central line detector reading a line
//  - 011b corresponds to the central and right line detector reading a line
// ect. 
int read_line_detectors() {
  // sets a variable for the analog signal as a num between 1 and 1023
  // scales the range to 0-5V so to put out a voltage
  float voltage1 = analogRead(pinLineSensor1) * (5.0 / 1023.0);
  float voltage2 = analogRead(pinLineSensor2) * (5.0 / 1023.0);
  float voltage3 = analogRead(pinLineSensor3) * (5.0 / 1023.0);

  Serial.print("Voltages: ");
  Serial.print(voltage1);
  Serial.print(' ');
  Serial.print(voltage2);
  Serial.print(' ');
  Serial.print(voltage3);

  int bits = 0;

  // hardware issue: the leftmost line sensor (corresponding to voltage1) has a
  // reduced sensitivity to lines, so it has an increased threshhold voltage
  bits += (voltage1 < 2.3) ? 1 : 0;
  bits += (voltage2 < 1.8) ? 2 : 0;
  bits += (voltage3 < 1.8) ? 4 : 0;

  Serial.print("   BITS: ");
  Serial.print(bits, BIN);
  return bits;
}

void loop() {
  //                     CODE WHICH IS EXECUTED EVEN IF ROBOT IS OFF
  // In many cases, it is useful to have function sensors and LEDS even when the
  // robot is "off". This proves to be most useful in testing, where we can keep
  // the robot physically stationary while printing sensor information to the
  // serial port.

  // read the hall effect sensor and update the LEDs
  bool isMagnetic = !digitalRead(pinHallEffectSensor);
  digitalWrite(pinHallEffectGreenLED, isMagnetic ? HIGH : LOW);
  digitalWrite(pinHallEffectRedLED, isMagnetic ? LOW : HIGH);

  if (!digitalRead(pinPushButton)) {
    // We are using a push button to switch the robot on and off, which is
    // problematic because the "Button Pushed" signal lasts anywhere between 200
    // to 600 milliseconds and is not debounced. To deal with this, we use
    // BlockNot to keep track of when the button was last pushed and ignore
    // button presses that occur in close succession to each other.
    if (pushButtonTimer.TRIGGERED) {
      robotIsOn = !robotIsOn;
    }
  }

  // Always read the line detectors, regardless of robotIsOn. Very useful for
  // testing, as `read_line_detectors` prints debugging information over serial
  int line_sensor_bits = read_line_detectors();

  if (!robotIsOn) {
    // Robot is off, set the FlashingLED high to signify that the robot is still
    // plugged in to the battery pack
    digitalWrite(pinFlashingLED, HIGH);
    stop_all_motors();
    return;
  }

  //                     CODE WHICH IS EXECUTED ONLY IF ROBOT IS ON

  // Using the BlockNot library, `flashingLEDTimer.TRIGGERED` evaluates to true
  // once once every 250 milliseconds. This allows us to update the flashing LED
  // without resorting to precarious use of Arduino's `millis()`
  if (flashingLEDTimer.TRIGGERED) {
    flashingLEDStatus = !flashingLEDStatus;
    digitalWrite(pinFlashingLED, (flashingLEDStatus ? HIGH : LOW));
  }

  float distance = read_distance_sensor();

  if (distance < 4 && !pickedUpBlock && distanceSensorTimer.HAS_TRIGGERED) {
    numDistanceShort++;
    if (numDistanceShort > 6) {
      move_forward_centimetres(0.7);
      stop_all_motors();

      bool isMagnetic = !digitalRead(pinHallEffectSensor);
      digitalWrite(pinHallEffectGreenLED, isMagnetic ? HIGH : LOW);
      digitalWrite(pinHallEffectRedLED, isMagnetic ? LOW : HIGH);

      delay(5000);
      rotate_90_degrees_clockwise();
      rotate_90_degrees_clockwise();
      pickedUpBlock = true;
      greenBoxTimer.RESET;
      redBoxTimer.RESET;
    }
  } else {
    numDistanceShort = 0;
  }

  if (updatePIDTimer.TRIGGERED) {
    follow_line(line_sensor_bits);
  }

  if (pickedUpBlock) {
    if (isMagnetic && redBoxTimer.TRIGGERED) {
      rotate_90_degrees_clockwise();
      move_forward_centimetres(30.0);
      move_backward_centimetres(4.0);
      rotate_90_degrees_clockwise();
      move_forward_centimetres(38.0);
      robotIsOn = false;
    } else if (greenBoxTimer.TRIGGERED) {
      rotate_90_degrees_clockwise();
      move_forward_centimetres(30.0);
      move_backward_centimetres(4.0);
      rotate_90_degrees_anticlockwise();
      move_forward_centimetres(38.0);
      robotIsOn = false;
    }
  }
  return;
}

Stats for nerds: tokei

krish@dynabook:~/Desktop/IDP.ino$ tokei
===============================================================================
 Language            Files        Lines         Code     Comments       Blanks
===============================================================================
 Arduino C++             1          427          211          156           60
===============================================================================