#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