PID-ultrasound.ino

#define DEBUG

#include <PID_v1.h>
//Define Variables we’ll be connecting to
double Setpoint, Input, Output;
//Specify the links and initial tuning parameters
PID myPID(&Input, &Output, &Setpoint, 5, 0, 0.5, REVERSE);
const int speedPin_M1 = 5; //M1 Speed Control
const int speedPin_M2 = 6; //M2 Speed Control
const int directionPin_M1 = 4; //M1 Direction Control
const int directionPin_M2 = 7; //M1 Direction Control
const int trigPin = 2;
const int echoPin = 3;
const int led = 13;

// Generally, you should use "unsigned long" for variables that hold time
// The value will quickly become too large for an int to store
unsigned long previousMillis = 0;        // will store last time LED was updated

const long interval = 100;           // interval at which to calculate the PID (milliseconds)

const int stopDistance = 30;        // when do I decide to stop

void setup() {
  pinMode(echoPin, INPUT);
  pinMode(trigPin, OUTPUT);
  pinMode(led, OUTPUT);
  //initialize the variables we’re linked to
  Input = measureDistance();
  Setpoint = 45;
  //turn the PID on
  myPID.SetMode(AUTOMATIC);
#ifdef DEBUG
  Serial.begin(9600);
#endif
}

void loop() {
  int distance = measureDistance();

#ifdef DEBUG
  Serial.print( distance );
  Serial.println( " cm " );
#endif

  // the PID shouldn't be calculated at irregular intervals, hence I use the MCU timing as a reference
  unsigned long currentMillis = millis();
  if (currentMillis - previousMillis >= interval) {
    previousMillis = currentMillis; // save the last time you blinked the LED

    Input = distance;
    myPID.Compute();
    int speed = Output;

    if (distance < stopDistance) { // This is where the LED On/Off happens
      carMove( speed );
      digitalWrite(led, HIGH);
    }
    else {
      digitalWrite(led, LOW);
      if (distance > 80) {
        carStop();
      }
    }
  }
}

void carMove( int speed ) {
  if (speed > 0) {
    carBack( speed, speed );
  }
  else if ( speed < 0) {
    carAdvance( speed, speed );
  }
  else {
    carStop();
  }
}



void carStop() { // Motor Stop
  digitalWrite(speedPin_M2, 0);
  digitalWrite(directionPin_M1, LOW);
  digitalWrite(speedPin_M1, 0);
  digitalWrite(directionPin_M2, LOW);
}
void carBack(int leftSpeed, int rightSpeed) { //Move backward
  analogWrite (speedPin_M2, leftSpeed); //PWM Speed Control
  digitalWrite(directionPin_M1, HIGH);
  analogWrite (speedPin_M1, rightSpeed);
  digitalWrite(directionPin_M2, HIGH);
}
void carAdvance(int leftSpeed, int rightSpeed) { //Move forward
  analogWrite (speedPin_M2, leftSpeed);
  digitalWrite(directionPin_M1, LOW);
  analogWrite (speedPin_M1, rightSpeed);
  digitalWrite(directionPin_M2, LOW);
}
void carTurnLeft(int leftSpeed, int rightSpeed) { //Turn Left
  analogWrite (speedPin_M2, leftSpeed);
  digitalWrite(directionPin_M1, LOW);
  analogWrite (speedPin_M1, rightSpeed);
  digitalWrite(directionPin_M2, HIGH);
}
void carTurnRight(int leftSpeed, int rightSpeed) { //Turn Right
  analogWrite (speedPin_M2, leftSpeed);
  digitalWrite(directionPin_M1, HIGH);
  analogWrite (speedPin_M1, rightSpeed);
  digitalWrite(directionPin_M2, LOW);
}

int measureDistance() {
  long duration, distance;
  digitalWrite(trigPin, LOW);  // Added this line
  delayMicroseconds(2); // Added this line
  digitalWrite(trigPin, HIGH);
  //  delayMicroseconds(1000); - Removed this line
  delayMicroseconds(10); // Added this line
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH);
  distance = (duration / 2) / 29.1;
  return (int) ( distance < 100 ) ? distance : 0;
}