Hi,
I have a completed sketch for an autonomous GPS navigation car. Since the GPS module is not very accurate, I was thinking about using PID to get rid of the cars deviation.
I have one sketch for the GPS car and another sketch for the PID tuning.
How do I combine these two programs into one program? The code for each program is below.
/* GPS Car v0.1 by Eric Barch (ttjcrew.com) */
#include <nmea.h>
#include <Servo.h>
#undef abs
#undef round
int wpt = 0;
float dest_latitude;
float dest_longitude;
NMEA gps(GPRMC); // GPS data connection to GPRMC sentence typesss
/* BEGIN EDITABLE CONSTANTS SECTION */
//These define the positions for your steering servo
#define CENTER_SERVO 95
#define MAX_LEFT_SERVO 85
#define MAX_RIGHT_SERVO 108
//When the car is within this range (meters), move to the next waypoint
#define WPT_IN_RANGE_M 12
//These pins are your h-bridge output. If the car is running in reverse, swap these
#define MOTOR_OUTPUT_ONE 5
#define MOTOR_OUTPUT_TWO 9
#define Motor_1 10
#define Motor_2 11
/* DEFINE GPS WPTS HERE - Create more cases as needed */
void trackWpts() {
switch(wpt) {
case 0:
dest_latitude = 40.756054;
dest_longitude = -73.986951;
break;
case 1:
dest_latitude = 37.775206;
dest_longitude = -122.419209;
break;
default:
dest_latitude = 0;
dest_longitude = 0;
break;
}
if (gps.gprmc_distance_to(dest_latitude,dest_longitude,MTR) < WPT_IN_RANGE_M)
wpt++;
}
/* END EDITABLE CONSTANTS SECTION */
Servo steering;
float dir; // relative direction to destination
int servo_pos = CENTER_SERVO;
void setup() {
Serial.begin(4800);
pinMode(MOTOR_OUTPUT_ONE, OUTPUT);
pinMode(MOTOR_OUTPUT_TWO, OUTPUT);
steering.attach(3);
}
void loop() {
trackWpts();
trackGPS();
}
void trackGPS() {
if (dest_latitude != 0 && dest_longitude != 0)
{
if (Serial.available() > 0 ) {
char c = Serial.read();
if (gps.decode(c)) {
// check if GPS positioning was active
if (gps.gprmc_status() == 'A') {
// calculate relative direction to destination
dir = gps.gprmc_course_to(dest_latitude, dest_longitude) - gps.gprmc_course();
if (dir < 0)
dir += 360;
if (dir > 180)
dir -= 360;
if (dir < -75)
hardLeft();
else if (dir > 75)
hardRight();
else
driveToTarget();
}
else //No GPS Fix...Wait for signal
stop();
}
}
}
else
stop();
}
void driveStraight() {
steering.write(CENTER_SERVO);
digitalWrite(MOTOR_OUTPUT_ONE, LOW);
digitalWrite(MOTOR_OUTPUT_TWO, HIGH);
analogWrite(Motor_1, 50);
analogWrite(Motor_2, 50);
Serial.println("Driving straight...");
}
void driveToTarget() {
servo_pos = map(dir, -75, 75, MAX_LEFT_SERVO, MAX_RIGHT_SERVO);
steering.write(servo_pos);
digitalWrite(MOTOR_OUTPUT_ONE, LOW);
digitalWrite(MOTOR_OUTPUT_TWO, HIGH);
analogWrite(Motor_1, 50);
analogWrite(Motor_2, 50);
Serial.print("Driving at ");
Serial.print(dir);
Serial.print(". - ");
Serial.print(gps.gprmc_distance_to(dest_latitude,dest_longitude,MTR));
Serial.print("m to go");
Serial.print("\n");
}
void hardLeft() {
steering.write(MAX_LEFT_SERVO);
digitalWrite(MOTOR_OUTPUT_ONE, LOW);
digitalWrite(MOTOR_OUTPUT_TWO, HIGH);
analogWrite(Motor_1, 50);
analogWrite(Motor_2, 50);
Serial.print("Driving hard left. - ");
Serial.print(gps.gprmc_distance_to(dest_latitude,dest_longitude,MTR));
Serial.print("m to go");
Serial.print("\n");
}
void hardRight() {
steering.write(MAX_RIGHT_SERVO);
digitalWrite(MOTOR_OUTPUT_ONE, LOW);
digitalWrite(MOTOR_OUTPUT_TWO, HIGH);
analogWrite(Motor_1, 50);
analogWrite(Motor_2, 50);
Serial.print("Driving hard right. - ");
Serial.print(gps.gprmc_distance_to(dest_latitude,dest_longitude,MTR));
Serial.print("m to go");
Serial.print("\n");
}
void stop() {
steering.write(CENTER_SERVO);
digitalWrite(MOTOR_OUTPUT_ONE, HIGH);
digitalWrite(MOTOR_OUTPUT_TWO, HIGH);
analogWrite(Motor_1, 50);
analogWrite(Motor_2, 50);
Serial.print("Stopped.");
Serial.print("\n");
}
#include <PID_v1.h>
#define PIN_INPUT 0
#define PIN_OUTPUT 3
//Define Variables we'll be connecting to
double Setpoint, Input, Output;
//Define the aggressive and conservative Tuning Parameters
double aggKp=4, aggKi=0.2, aggKd=1;
double consKp=1, consKi=0.05, consKd=0.25;
//Specify the links and initial tuning parameters
PID myPID(&Input, &Output, &Setpoint, consKp, consKi, consKd, DIRECT);
void setup()
{
//initialize the variables we're linked to
Input = analogRead(PIN_INPUT);
Setpoint = 100;
//turn the PID on
myPID.SetMode(AUTOMATIC);
}
void loop()
{
Input = analogRead(PIN_INPUT);
double gap = abs(Setpoint-Input); //distance away from setpoint
if (gap < 10)
{ //we're close to setpoint, use conservative tuning parameters
myPID.SetTunings(consKP, consKi, consKd);
}
else
{
//we're far from setpoint, use aggressive tuning parameters
myPID.SetTunings(aggKp, aggKi, aggKd);
}
myPID.Compute();
analogWrite(PIN_OUTPUT, Output);
}