Create a gist now

Instantly share code, notes, and snippets.

#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;
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment