Thank you
This code was copied from forum and I am trying to develop it for my project
#include <Wire.h>
#include <PID_v1.h>
#include <quaternionFilters.h>
#include <MPU9250.h>
#include <TinyGPS++.h>
#include <math.h>
#include <Servo.h>
#include <Maxbotix.h>
const int pinLD = 12; // Digital output to left motor for direction
const int pinLA = 11; // Analog output to left motor for speed control
const int pinRD = 13; // Digital output to right motor for direction
const int pinRA = 3; // Analog output to right motor for speed control
const int gasAvg = 155; // Average analog output to both motors... FLOOR IT BABY!!!
const int servoPin = 9;
const int pwSonarPin = 5; // The digital pin for MaxSonar in PW mode
const float coastClear = 70.0; // Threshold distance to begin avoidance
const float frontMinDist = 20.0; // Threshold distance to take "evasive action"
const int servoDelay = 150; // Time for servo to swing ~180 deg
const int sonarDelay = 250; // Time to scan
const int headA1 = 0; // Servo position looking 85 deg right
const int headA2 = 15; // Servo position looking 60 deg right
const int headA3 = 35; // Servo position looking 30 deg right
const int headCenter = 65;
const int headA4 = 95; // Servo position looking -30 deg (left)
const int headA5 = 125; // Servo position looking -60 deg
const int headA6 = 150; // Servo position looking ~90 deg left
const float alpha1 = 85; // Angle that's 90 degrees to right
const float alpha2 = 60;
const float alpha3 = 30;
const float alphaTol = 20;
const float tol = 10; // 10 degree angle tolerance for turning
const double Kp = 6;
const double Ki = 0.2;
const double Kd = 1; // PID control constants
const int magBias[] = { 125, 45, -50 };
const double R = 6371008.8;
const double range = 10; // Within 7 meters call it good
double currHeading, Kgas; // PID Setpoint, input and output vars
const double waypoint[] = { 29.540158, -95.110300 };
double coord[2];
double currDist;
double currBearing;
int dist[6];
unsigned long prevMillis = 0;
unsigned long currMillis = 0;
byte counter = 0; // For lookAround
bool looking = false;
bool isObstacle = false;
bool finishedLooking = false; // For the distance measurement
bool newResults = false; // For the distance measurement
bool imReady = false; // for the distance measurement
MPU9250 myIMU;
PID st(double *currHeading, double *Kgas, double *currBearing, double Kp, double Ki, double Kd, int DIRECT);
TinyGPSPlus gps;
Maxbotix sonar(5, Maxbotix::PW, Maxbotix::LV, Maxbotix::BEST);
Servo head;
void setup() {
Wire.begin();
pinMode(pinLD, OUTPUT);
pinMode(pinRD, OUTPUT);
Serial.begin(9600); // GPS attached to Serial1 runs at 9600 baud
head.attach(servoPin);
head.write(headCenter);
myIMU.MPU9250SelfTest(myIMU.selfTest);
myIMU.calibrateMPU9250(myIMU.gyroBias, myIMU.accelBias);
delay(1000);
myIMU.initMPU9250();
myIMU.initAK8963(myIMU.factoryMagCalibration);
myIMU.getAres();
myIMU.getGres();
myIMU.getMres();
delay(1000);
getIMUdata();
calcYawPitchRoll();
getGPSinfo();
currHeading = -myIMU.yaw;
st.SetOutputLimits(-100, 100);
st.SetMode(AUTOMATIC);
}
void loop() {
getGPSinfo(); // gets coordinates, calcs currDist and currBearing
getIMUdata(); // gets accel, gyro and magnetometer data
calcYawPitchRoll(); // calcs yaw, pitch and roll
currHeading = -myIMU.yaw; // compass heading is negative of yaw
if (!isObstacle) { checkForObstacles(); }
if (isObstacle) { avoidObstacles(); }
if ((currDist > range) && (!looking)) {
st.Compute();
pidDrive(Kgas);
} else {
stopCar();
}
} // end loop()
void checkForObstacles() {
float frontDist = 0;
frontDist = sonar.getRange(); // Head pointing forward, measures distance in cm
if (frontDist < coastClear) {
isObstacle = true;
} else {
isObstacle = false;
}
}
void avoidObstacles() {
stopCar();
if (!looking && !imReady) {
imReady = true; // I'M READY! I'M READY! I'M READY!
}
if (imReady) {
counter = 1; // Keeps track of place in switch/case statement
prevMillis = currMillis; // Going to use this to time my servos
imReady = false;
looking = true;
finishedLooking = false; // Yes, this may be extraneous
}
if (!finishedLooking) { lookAround(); }
if (finishedLooking && newResults) {
turnTowardsFreedom();
}
}
// Turn head six different angles and take distance at each
void lookAround() {
currMillis = millis();
switch (counter) {
case 1:
head.write(headA1);
if ((currMillis - prevMillis) > servoDelay) {
counter = 2;
prevMillis = millis();
}
break;
case 2:
dist[0] = round(sonar.getRange());
if ((currMillis - prevMillis) > sonarDelay) {
counter = 3;
prevMillis = millis();
}
break;
case 3:
head.write(headA2);
if ((currMillis - prevMillis) > servoDelay) {
counter = 4;
prevMillis = millis();
}
break;
case 4:
dist[1] = round(sonar.getRange());
if ((currMillis - prevMillis) > sonarDelay) {
counter = 5;
prevMillis = millis();
}
break;
case 5:
head.write(headA3);
if ((currMillis - prevMillis) > servoDelay) {
counter = 6;
prevMillis = millis();
}
break;
case 6:
dist[2] = round(sonar.getRange());
if ((currMillis - prevMillis) > sonarDelay) {
counter = 7;
prevMillis = millis();
}
break;
case 7:
head.write(headA4);
if ((currMillis - prevMillis) > servoDelay) {
counter = 8;
prevMillis = millis();
}
break;
case 8:
dist[3] = round(sonar.getRange());
if ((currMillis - prevMillis) > sonarDelay) {
counter = 9;
prevMillis = millis();
}
break;
case 9:
head.write(headA5);
if ((currMillis - prevMillis) > servoDelay) {
counter = 10;
prevMillis = millis();
}
break;
case 10:
dist[4] = round(sonar.getRange());
if ((currMillis - prevMillis) > sonarDelay) {
counter = 11;
prevMillis = millis();
}
break;
case 11:
head.write(headA6);
if ((currMillis - prevMillis) > servoDelay) {
counter = 12;
prevMillis = millis();
}
break;
case 12:
dist[5] = round(sonar.getRange());
if ((currMillis - prevMillis) > sonarDelay) {
counter = 13;
prevMillis = millis();
}
break;
case 13:
head.write(headCenter);
if ((currMillis - prevMillis) > (3 * servoDelay)) {
counter = 14;
prevMillis = millis();
}
break;
case 14:
counter = 0;
finishedLooking = true;
looking = false;
newResults = true;
break;
}
}
void turnTowardsFreedom() {
float turnAngle = bubbleAngle();
getIMUdata();
calcYawPitchRoll();
currHeading = -myIMU.yaw;
currBearing = currHeading + turnAngle;
if (currBearing > 180) { currBearing = currBearing - 360; }
if (currBearing < -180) { currBearing = currBearing + 360; }
if (turnAngle > tol) {
while (turnAngle > tol) {
turnRight();
getIMUdata();
calcYawPitchRoll();
currHeading = -myIMU.yaw;
turnAngle = currBearing - currHeading;
if (turnAngle > 180) { turnAngle = turnAngle - 360; }
if (turnAngle < -180) { turnAngle = turnAngle + 360; }
}
stopCar();
}
if (turnAngle < -tol) {
while (turnAngle < -tol) {
turnLeft();
getIMUdata();
calcYawPitchRoll();
currHeading = -myIMU.yaw;
turnAngle = currBearing - currHeading;
if (turnAngle > 180) { turnAngle = turnAngle - 360; }
if (turnAngle < -180) { turnAngle = turnAngle + 360; }
}
stopCar();
}
newResults = false;
isObstacle = false;
}
// Bubble Rebound Algorithm turn angle calculator
// I. Susnea, V. Minzu, G. Vasiliu, "Simple, real-time obstacle
// avoidance algorithm for mobile robots," Recent Advances in
// Computational Intelligence, Man-machine Systems and Cybernetics,
// WSEAS Press, 2009, pp 24-29, ISBN: 978-960-474-144-1
float bubbleAngle() {
float sumDist = 0;
float alphaR = 0;
float alphaAdj = 0;
for (int i = 0; i < 6; i++) {
sumDist = sumDist + dist[i];
}
alphaR = (alpha1 * (dist[0] - dist[5])
+ alpha2 * (dist[1] - dist[4])
+ alpha3 * (dist[2] - dist[3]))
/ sumDist;
if (abs(alphaR) < alphaTol) {
if (alphaR <= 0)
alphaAdj = -45;
else if (alphaR > 0)
alphaAdj = 45;
} else
alphaAdj = alphaR;
return alphaAdj;
}
void pidDrive(double gasDiff) {
int rightGas = round(gasAvg - gasDiff);
int leftGas = round(gasAvg + gasDiff);
digitalWrite(pinLD, LOW);
digitalWrite(pinRD, LOW);
analogWrite(pinLA, leftGas);
analogWrite(pinRA, rightGas);
} // end pidDrive(...)
void stopCar() {
digitalWrite(pinLD, HIGH);
digitalWrite(pinRD, HIGH);
analogWrite(pinLA, 0);
analogWrite(pinRA, 0);
}
void turnRight() {
digitalWrite(pinLD, HIGH);
digitalWrite(pinRD, LOW);
digitalWrite(pinLA, gasAvg);
digitalWrite(pinRA, gasAvg);
}
void turnLeft() {
digitalWrite(pinLD, LOW);
digitalWrite(pinRD, HIGH);
digitalWrite(pinLA, gasAvg);
digitalWrite(pinRA, gasAvg);
}
void getIMUdata() {
myIMU.readAccelData(myIMU.accelCount);
myIMU.ax = (float)myIMU.accelCount[0] * myIMU.aRes;
myIMU.ay = (float)myIMU.accelCount[1] * myIMU.aRes;
myIMU.az = (float)myIMU.accelCount[2] * myIMU.aRes;
myIMU.readGyroData(myIMU.gyroCount);
myIMU.gx = (float)myIMU.gyroCount[0] * myIMU.gRes;
myIMU.gy = (float)myIMU.gyroCount[1] * myIMU.gRes;
myIMU.gz = (float)myIMU.gyroCount[2] * myIMU.gRes;
myIMU.readMagData(myIMU.magCount);
myIMU.mx = (float)myIMU.magCount[0] * myIMU.mRes
* myIMU.factoryMagCalibration[0]
- magBias[0];
myIMU.my = (float)myIMU.magCount[1] * myIMU.mRes
* myIMU.factoryMagCalibration[1]
- magBias[1];
myIMU.mz = (float)myIMU.magCount[2] * myIMU.mRes
* myIMU.factoryMagCalibration[2]
- magBias[2];
// Must be called before updating quaternions!
myIMU.updateTime();
// This is for (x, y, z) axes of magnetometer aligned with (y, x, -z) axes
// of gyro/accelerometer.
MahonyQuaternionUpdate(myIMU.ax, myIMU.ay, myIMU.az, myIMU.gx * DEG_TO_RAD,
myIMU.gy * DEG_TO_RAD, myIMU.gz * DEG_TO_RAD, myIMU.my,
myIMU.mx, myIMU.mz, myIMU.deltat);
}
void calcYawPitchRoll() {
myIMU.yaw = atan2(2 * (*(getQ() + 1) * *(getQ() + 2) + *getQ() * *(getQ() + 3)), *getQ() * *getQ() + *(getQ() + 1) * *(getQ() + 1) - *(getQ() + 2) * *(getQ() + 2) - *(getQ() + 3) * *(getQ() + 3));
myIMU.pitch = -asin(2 * (*(getQ() + 1) * *(getQ() + 3) - *getQ() * *(getQ() + 2)));
myIMU.roll = atan2(2 * (*getQ() * *(getQ() + 1) + *(getQ() + 2) * *(getQ() + 3)), *getQ() * *getQ() - *(getQ() + 1) * *(getQ() + 1) - *(getQ() + 2) * *(getQ() + 2) + *(getQ() + 3) * *(getQ() + 3));
myIMU.pitch *= RAD_TO_DEG;
myIMU.yaw *= RAD_TO_DEG;
// myIMU.yaw -= 2.4; //Declination of Webster, TX - not needed due to offset mount
myIMU.roll *= RAD_TO_DEG;
}
void getGPSinfo() {
while (Serial.available() > 0)
if (gps.encode(Serial.read())) {
coord[0] = gps.location.lat();
coord[1] = gps.location.lng();
currDist = distance(coord, waypoint);
currBearing = bearing(coord, waypoint);
}
}
double distance(double *Coord, double *Waypt) {
double rCoord[2];
double rWaypt[2];
rCoord[0] = Coord[0] * PI / 180;
rCoord[1] = Coord[1] * PI / 180;
rWaypt[0] = Waypt[0] * PI / 180;
rWaypt[1] = Waypt[1] * PI / 180;
double sin2Lats = sin((rWaypt[0] - rCoord[0]) / 2) * sin((rWaypt[0] - rCoord[0]) / 2);
double sin2Lngs = sin((rWaypt[1] - rCoord[1]) / 2) * sin((rWaypt[1] - rCoord[1]) / 2);
double cosLat1 = cos(rCoord[0]);
double cosLat2 = cos(rWaypt[0]);
double sqRt = sqrt(sin2Lats + cosLat1 * cosLat2 * sin2Lngs);
double d = 2 * R * asin(sqRt);
return d;
}
double bearing(double *Coord, double *Waypt) {
double rCoord[2];
double rWaypt[2];
rCoord[0] = Coord[0] * PI / 180;
rCoord[1] = Coord[1] * PI / 180;
rWaypt[0] = Waypt[0] * PI / 180;
rWaypt[1] = Waypt[1] * PI / 180;
double sinLngs = sin(rWaypt[1] - rCoord[1]);
double cosLngs = cos(rWaypt[1] - rCoord[1]);
double cosLat2 = cos(rWaypt[0]);
double cosLat1 = cos(rCoord[0]);
double sinLat2 = sin(rWaypt[0]);
double sinLat1 = sin(rCoord[0]);
double term1 = sinLngs * cosLat2;
double term2 = cosLat1 * sinLat2 - sinLat1 * cosLat2 * cosLngs;
double b = atan2(term1, term2) * 180 / PI;
return b;
}