Using SPIDApplication library

warriorridersumrell · November 4, 2022, 9:10pm

I am trying to understand where to plug in input and output to use this library to steer a rc car. i have 9 axis BNO085 and Adafruit ultimate GPS. I have it all programmed to give me bearing and current vehicle direction among other things. Currently I've programed it to turn left and right if beyond 5* either direction and have applied the wrapping code to deal with 360/0. I want to apply the PID controller and thought this library would work but I don't understand how to apply it.

I do know that my error will be bearing - current heading so I should be providing variables for Yaw, Bearing and error, but where do they go in the code?


#include <SPID.h>
#include<ConFunct.h>
Takt100 takt100;
Takt1000 takt1000;
SPID con1;   
PT pT1;
//
//variables for time sampling
//

int takt1;
int takt2;

void setup() {
//define voltage for analog input
analogReference(DEFAULT);
//define digital pin 5 and 6 as digital output
pinMode(5, OUTPUT);
pinMode(6, OUTPUT);
 
Serial.begin(9600); //start serial commmunication

//filter time constant

pT1.T=2;


// step controller parameter

con1.gain=6;  
con1.Ti=20;  
con1.Td=5;
con1.onvalue=0.5;
con1.offvalue=0;
con1.mimp=0.1;
con1.motime=10;

}

void loop() {

takt1=takt100.fupdate();   //calling 100ms takt
takt2=takt1000.fupdate();  //calling 1s takt

//switch controller auto/man

con1.auto_man=digitalRead(2);
//push buttons to manipulate controller output
con1.butt_plu=digitalRead(3);
con1.butt_min=digitalRead(4);
//limit switches
con1.limit_plu=digitalRead(6);
con1.limit_min=digitalRead(7);


//provide actual value and setpoint

pT1.in=analogRead(0)/10.23; //convert from range 0 - 1023 to 0 - 100;
con1.act=pT1.out;   //provide actual value
con1.set=50; //provide setpoint


//controller digital outputs
digitalWrite(5, con1.motor_plu);
digitalWrite(6, con1.motor_min);


///===========time sampling 100ms
if (takt1==1){

con1.cupdate( );   //call controller CPID
pT1.fupdate( );    //call filter
}

///===========time sampling 1s
if (takt2==0){

Serial.println(con1.set+String(" ")+con1.act+String(" ")+con1.motor_min*10+String("  ")+con1.motor_plu*10);  //look for simulation results in plotter

            }
}

jremington · November 4, 2022, 9:39pm

The posted library code does not appear to be useful.

Post code that reads the BNO055 and performs steering, and forum members will be happy to help you implement PID control.

warriorridersumrell · November 4, 2022, 10:20pm

this is my full code I'm building. Servo control is at the bottom. i want to change it to use PID but am not sure how to do it. that is why I posted the library sample of SPID. i think i just need some help in where to plug in the variables.

#include "heltec.h"
#include "Simpletimer.h"
#include <Adafruit_BMP280.h>
#include <Arduino.h>
#include <Adafruit_BNO08x.h>
#include "BluetoothSerial.h"
#include <ESP32Servo.h>
#include <TinyGPSPlus.h>
#include <PID_v1.h>

#if !defined(CONFIG_BT_ENABLED) || !defined(CONFIG_BLUEDROID_ENABLED)
#error Bluetooth is not enabled! Please run `make menuconfig` to and enable it
#endif

#if !defined(CONFIG_BT_SPP_ENABLED)
#error Serial Bluetooth not available or not enabled. It is only available for the ESP32 chip.
#endif

#ifdef FAST_MODE
// Top frequency is reported to be 1000Hz (but freq is somewhat variable)
sh2_SensorId_t reportType = SH2_GYRO_INTEGRATED_RV;
long reportIntervalUs = 2000;
#else
// Top frequency is about 250Hz but this report is more accurate
sh2_SensorId_t reportType = SH2_ARVR_STABILIZED_RV;
long reportIntervalUs = 5000;
#endif

String rssi = "RSSI --";
String packSize = "--";
String packet;

Adafruit_BMP280 bmp;

TinyGPSPlus gps;
unsigned long last = 0UL;

Servo myservo;
int pos = 0;  // variable to store the servo position
int servoPin = 12;



BluetoothSerial SerialBT;
boolean confirmRequestPending = true;
char Incoming_value = 0;

struct euler_t {
  float yaw;
  float pitch;
  float roll;
} ypr;

#define BNO08X_RESET -1
#define BNO08X_CS 10
#define BNO08X_INT 9
#define BMP_SCK (13)
#define BMP_MISO (12)
#define BMP_MOSI (11)
#define BMP_CS (10)
#define BAND 915E6


Simpletimer timer1;
Simpletimer timer2;
Simpletimer timer3;
Simpletimer timer4;
Simpletimer timer5;
Simpletimer timer6;

Adafruit_BNO08x bno08x(BNO08X_RESET);
sh2_SensorValue_t sensorValue;

void loRaPackageSend() {
  int packetSize = LoRa.parsePacket();
  if (packetSize) { cbk(packetSize); }
}

void GPSDataLoad() {
  GPSloop();
  callToGPSLAT();
  callToGPSLONG();
  callToGPSALT();
  callToGPSAGE();
  callToGPSSPEED();
}

void BMP280Load() {
  BMPloop();
}

void BNO085Load() {
  BNOloop();
}

void DistanceWithAltitude() {
  distanceWithAltitude();
}

void PtoP_bearing() {
  PtoP_Bearing();
  vehicleBearingAdjustment();
}

void setup() {
  Heltec.begin(true /*DisplayEnable Enable*/, true /*Heltec.Heltec.Heltec.LoRa Disable*/, true /*Serial Enable*/, true /*PABOOST Enable*/, BAND /*long BAND*/);
  BTsetup();
  GPSsetup();
  BMPsetup();
  BNOsetup();
  Heltec.display->init();
  Heltec.display->flipScreenVertically();
  Heltec.display->setFont(ArialMT_Plain_10);
  timer1.register_callback(loRaPackageSend);
  timer2.register_callback(GPSDataLoad);
  timer3.register_callback(BMP280Load);
  timer4.register_callback(BNO085Load);
  timer5.register_callback(DistanceWithAltitude);
  timer6.register_callback(PtoP_bearing);
  Heltec.display->clear();
  Heltec.display->drawString(0, 0, "Heltec.LoRa Initial success!");
  Heltec.display->drawString(0, 10, "Wait for incoming data...");
  Heltec.display->display();
  LoRa.receive();
}

void loop() {
  BTloop();
  timer1.run(100);
  timer2.run(250);
  timer3.run(500);
  timer4.run(100);
  timer5.run(500);
  timer6.run(100);
  steerloop();
  phoneInput(); 
}

void BMPsetup() {
  Serial.println(F("BMP280 Forced Mode Test."));
  if (!bmp.begin()) {
    Serial.println(F("Could not find a valid BMP280 sensor, check wiring or "
                     "try a different address!"));
    while (1) delay(10);
  }

  bmp.setSampling(Adafruit_BMP280::MODE_FORCED,     /* Operating Mode. */
                  Adafruit_BMP280::SAMPLING_X2,     /* Temp. oversampling */
                  Adafruit_BMP280::SAMPLING_X16,    /* Pressure oversampling */
                  Adafruit_BMP280::FILTER_X16,      /* Filtering. */
                  Adafruit_BMP280::STANDBY_MS_500); /* Standby time. */
}

double BMPloop() {
  if (bmp.takeForcedMeasurement()) {
  } else {
    Serial.println("Forced measurement failed!");
  }
  double Altitude = (bmp.readAltitude(1013.25));
  return (Altitude);
}

void setReports(sh2_SensorId_t reportType, long report_interval) {
  Serial.println("Setting desired reports");
  if (!bno08x.enableReport(reportType, report_interval)) {
    Serial.println("Could not enable stabilized remote vector");
  }
}

void BNOsetup(void) {
  while (!Serial) delay(10);
  Serial.println("Adafruit BNO08x test!");
  if (!bno08x.begin_I2C()) {
    Serial.println("Failed to find BNO08x chip");
    while (1) { delay(10); }
  }
  Serial.println("BNO08x Found!");
  setReports(reportType, reportIntervalUs);
  Serial.println("Reading events");
  //delay(10);
}

void quaternionToEuler(float qr, float qi, float qj, float qk, euler_t* ypr, bool degrees = false) {
  float sqr = sq(qr);
  float sqi = sq(qi);
  float sqj = sq(qj);
  float sqk = sq(qk);
  ypr->yaw = atan2(2 * -1 * (qi * qj + qk * qr), (sqi - sqj - sqk + sqr));
  ypr->pitch = asin(-2.0 * (qi * qk - qj * qr) / (sqi + sqj + sqk + sqr));
  ypr->roll = atan2(2.0 * (qj * qk + qi * qr), (-sqi - sqj + sqk + sqr));
  if (degrees) {
    ypr->yaw *= RAD_TO_DEG;
    ypr->pitch *= RAD_TO_DEG;
    ypr->roll *= RAD_TO_DEG;
  }
}

void quaternionToEulerRV(sh2_RotationVectorWAcc_t* rotational_vector, euler_t* ypr, bool degrees = false) {
  quaternionToEuler(rotational_vector->real, rotational_vector->i, rotational_vector->j, rotational_vector->k, ypr, degrees);
}

void quaternionToEulerGI(sh2_GyroIntegratedRV_t* rotational_vector, euler_t* ypr, bool degrees = false) {
  quaternionToEuler(rotational_vector->real, rotational_vector->i, rotational_vector->j, rotational_vector->k, ypr, degrees);
}

void BNOloop() {
  if (bno08x.wasReset()) {
    Serial.print("sensor was reset ");
    setReports(reportType, reportIntervalUs);
  }

  if (bno08x.getSensorEvent(&sensorValue)) {
    switch (sensorValue.sensorId) {
      case SH2_ARVR_STABILIZED_RV:
        quaternionToEulerRV(&sensorValue.un.arvrStabilizedRV, &ypr, true);
      case SH2_GYRO_INTEGRATED_RV:
        // faster (more noise?)
        quaternionToEulerGI(&sensorValue.un.gyroIntegratedRV, &ypr, true);
        break;
    }
    static long last = 0;
    long now = micros();
    last = now;
  }
}

void BTConfirmRequestCallback(uint32_t numVal) {
  confirmRequestPending = true;
  Serial.println(numVal);
}

void BTAuthCompleteCallback(boolean success) {
  confirmRequestPending = false;
  if (success) {
    Serial.println("Pairing success!!");
  } else {
    Serial.println("Pairing failed, rejected by user!!");
  }
}


void BTsetup() {
  Serial.begin(115200);
  SerialBT.enableSSP();
  SerialBT.onConfirmRequest(BTConfirmRequestCallback);
  SerialBT.onAuthComplete(BTAuthCompleteCallback);
  SerialBT.begin("ESP32test");  //Bluetooth device name
  Serial.println("The device started, now you can pair it with bluetooth!");
  myservo.setPeriodHertz(330);
  myservo.attach(servoPin, 700, 2100);
}

void BTloop() {
  if (confirmRequestPending) {
    if (Serial.available()) {
      int dat = Serial.read();
      if (dat == 'Y' || dat == 'y') {
        SerialBT.confirmReply(true);
      } else {
        SerialBT.confirmReply(false);
      }
    }
  } else {
    if (Serial.available()) {
      SerialBT.write(Serial.read());
    }
    if (SerialBT.available()) {
      Serial.write(SerialBT.read());
    }
    //delay(20);
  }
}

void phoneInput() {
  if (SerialBT.available() > 0) {
    Incoming_value = SerialBT.read();
  }
  if (Incoming_value == '0') {
    Serial.println("Off");
  } else if (Incoming_value == '1') {
    Serial.println("On");
  } else if (Incoming_value == '2') {
    Serial.println("Fwd");
  } else if (Incoming_value == '3') {
    Serial.println("Aft");
  } else if (Incoming_value == '4') {
    Serial.println("Left");
    myservo.write(135);
  } else if (Incoming_value == '5') {
    Serial.println("Right");
    myservo.write(45);
  } else if (Incoming_value == '6') {
    Serial.println("");
  }
}

void GPSsetup() {
  Serial.begin(115200);
  Serial1.begin(9600, SERIAL_8N1, 2, 17);
}

void GPSloop() {
  while (Serial1.available() > 0)
    gps.encode(Serial1.read());

  if (gps.location.isUpdated()) {
    //Serial.print(F("LOCATION   Fix Age="));
    //Serial.print(gps.location.age());
    //Serial.print(F("ms Raw Lat="));
    //Serial.print(gps.location.rawLat().negative ? "-" : "+");
    //Serial.print(gps.location.rawLat().deg);
    //Serial.print("[+");
    //Serial.print(gps.location.rawLat().billionths);
    //Serial.print(F(" billionths],  Raw Long="));
    //Serial.print(gps.location.rawLng().negative ? "-" : "+");
    //Serial.print(gps.location.rawLng().deg);
    //Serial.print("[+");
    //Serial.print(gps.location.rawLng().billionths);
    //Serial.print(F(" billionths],  Lat="));
    //Serial.print(gps.location.lat(), 6);
    //Serial.print(F(" Long="));
    //Serial.println(gps.location.lng(), 6);
  }

  else if (gps.date.isUpdated()) {
    //Serial.print(F("DATE       Fix Age="));
    //Serial.print(gps.date.age());
    //Serial.print(F("ms Raw="));
    //Serial.print(gps.date.value());
    //Serial.print(F(" Year="));
    //Serial.print(gps.date.year());
    //Serial.print(F(" Month="));
    //Serial.print(gps.date.month());
    //Serial.print(F(" Day="));
    //Serial.println(gps.date.day());
  }

  else if (gps.time.isUpdated()) {
    //Serial.print(F("TIME       Fix Age="));
    //Serial.print(gps.time.age());
    //Serial.print(F("ms Raw="));
    //Serial.print(gps.time.value());
    //Serial.print(F(" Hour="));
    //Serial.print(gps.time.hour());
    //Serial.print(F(" Minute="));
    //Serial.print(gps.time.minute());
    //Serial.print(F(" Second="));
    //Serial.print(gps.time.second());
    //Serial.print(F(" Hundredths="));
    //Serial.println(gps.time.centisecond());
  }

  else if (gps.speed.isUpdated()) {
    //Serial.print(F("SPEED      Fix Age="));
    //Serial.print(gps.speed.age());
    //Serial.print(F("ms Raw="));
    //Serial.print(gps.speed.value());
    //Serial.print(F(" Knots="));
    //Serial.print(gps.speed.knots());
    //Serial.print(F(" MPH="));
    //Serial.print(gps.speed.mph());
    //Serial.print(F(" m/s="));
    //Serial.print(gps.speed.mps());
    //Serial.print(F(" km/h="));
    //Serial.println(gps.speed.kmph());
  }

  else if (gps.course.isUpdated()) {
    //Serial.print(F("COURSE     Fix Age="));
    //Serial.print(gps.course.age());
    //Serial.print(F("ms Raw="));
    //Serial.print(gps.course.value());
    //Serial.print(F(" Deg="));
    //Serial.println(gps.course.deg());
  }

  else if (gps.altitude.isUpdated()) {
    //Serial.print(F("ALTITUDE   Fix Age="));
    //Serial.print(gps.altitude.age());
    //Serial.print(F("ms Raw="));
    //Serial.print(gps.altitude.value());
    //Serial.print(F(" Meters="));
    //Serial.print(gps.altitude.meters());
    //Serial.print(F(" Miles="));
    //Serial.print(gps.altitude.miles());
    //Serial.print(F(" KM="));
    //Serial.print(gps.altitude.kilometers());
    //Serial.print(F(" Feet="));
    //Serial.println(gps.altitude.feet());
  }

  else if (gps.satellites.isUpdated()) {
    //Serial.print(F("SATELLITES Fix Age="));
    //Serial.print(gps.satellites.age());
    //Serial.print(F("ms Value="));
    //Serial.println(gps.satellites.value());
  }

  else if (gps.hdop.isUpdated()) {
    //Serial.print(F("HDOP       Fix Age="));
    //Serial.print(gps.hdop.age());
    //Serial.print(F("ms raw="));
    //Serial.print(gps.hdop.value());
    //Serial.print(F(" hdop="));
    //Serial.println(gps.hdop.hdop());
  }

  else if (millis() - last > 5000) {
    Serial.println();
    if (gps.location.isValid()) {
      static const double LONDON_LAT = 51.508131, LONDON_LON = -0.128002;
      double distanceToLondon =
        TinyGPSPlus::distanceBetween(
          gps.location.lat(),
          gps.location.lng(),
          LONDON_LAT,
          LONDON_LON);
      double courseToLondon =
        TinyGPSPlus::courseTo(
          gps.location.lat(),
          gps.location.lng(),
          LONDON_LAT,
          LONDON_LON);

      Serial.print(F("LONDON     Distance="));
      Serial.print(distanceToLondon / 1000, 6);
      Serial.print(F(" km Course-to="));
      Serial.print(courseToLondon, 6);
      Serial.print(F(" degrees ["));
      Serial.print(TinyGPSPlus::cardinal(courseToLondon));
      Serial.println(F("]"));
    }

    Serial.print(F("DIAGS      Chars="));
    Serial.print(gps.charsProcessed());
    Serial.print(F(" Sentences-with-Fix="));
    Serial.print(gps.sentencesWithFix());
    Serial.print(F(" Failed-checksum="));
    Serial.print(gps.failedChecksum());
    Serial.print(F(" Passed-checksum="));
    Serial.println(gps.passedChecksum());

    if (gps.charsProcessed() < 10)
      Serial.println(F("WARNING: No GPS data.  Check wiring."));

    last = millis();
    Serial.println();
  }
}

double distanceWithAltitude() {
  double NewLat = callToGPSLAT();
  double NewLong = callToGPSLONG();
  double lat1 = gps.location.lat();
  double lon1 = gps.location.lng();
  double lat2 = NewLat;
  double lon2 = NewLong;
  lon1 = lon1 * PI / 180;
  lon2 = lon2 * PI / 180;
  lat1 = lat1 * PI / 180;
  lat2 = lat2 * PI / 180;

  // Haversine formula
  double dlon = lon2 - lon1;
  double dlat = lat2 - lat1;
  double a = pow(sin(dlat / 2), 2)
             + cos(lat1) * cos(lat2)
                 * pow(sin(dlon / 2), 2);

  double c = 2 * asin(sqrt(a));

  // Radius of earth in kilometers. Use 3956
  // for miles
  double r = 3956;
  double distance = (c * r);
  //Serial.print("distance ");
  //Serial.println(distance);
  double distanceFeet = distance * 5280;
  return distanceFeet;
}

double callToGPSAGE() {
  String Long = packet.substring(50, 60);
  double AgeNew = Long.toDouble();
  return AgeNew;
}

double callToGPSLONG() {
  String Long = packet.substring(19, 38);
  double LongNew = Long.toDouble();
  return LongNew;
}

double callToGPSALT() {
  String Alt = packet.substring(41, 47);
  double AltNew = Alt.toDouble();
  return AltNew;
}

double callToGPSSPEED() {
  String Speed = packet.substring(50, 55);
  double SpeedNew = Speed.toDouble();
  return SpeedNew;
}

double callToGPSLAT() {
  String Lat = packet.substring(3, 19);
  double LatNew = Lat.toDouble();
  return (LatNew);
}

double PtoP_Bearing() {
  double NewLat = 47.9532887;
  double NewLong = -124.420977;
  double lat = 47.9089450;
  double lon = -122.2439150;
  double lat2 = NewLat;
  double lon2 = NewLong;
  double teta1 = radians(lat);
  double teta2 = radians(lat2);
  double delta1 = radians(lat2 - lat);
  double delta2 = radians(lon2 - lon);
  //==================Heading Formula Calculation================//

  double y = sin(delta2) * cos(teta2);
  double x = cos(teta1) * sin(teta2) - sin(teta1) * cos(teta2) * cos(delta2);
  double brng = atan2(y, x);
  brng = degrees(brng);  // radians to degrees
  brng = (((int)brng + 360) % 360);
  return brng;
}
double vehicleBearingAdjustment() {
  int brng = PtoP_Bearing();
  double YAW = ypr.yaw;
  int DriveHeading = ((((int)YAW - brng) + 540)%360) - 180; //heading error is in range -180 to 179
  Serial.print("DriveHeading: ");
Serial.print(DriveHeading); //heading error is in range -180 to 179
Serial.print("**");
//Serial.print(brng);
if (DriveHeading > -5 && DriveHeading < 5)Serial.println("NOT TURNING: ");
if (DriveHeading < -5)Serial.println("Turn Right: ");
if (DriveHeading > 5)Serial.println("Turn Left: ");
  return DriveHeading;
}

void cbk(int packetSize) {
  packet = "";
  packSize = String(packetSize, DEC);
  for (int i = 0; i < packetSize; i++) { packet += (char)LoRa.read(); }
  rssi = "RSSI " + String(LoRa.packetRssi(), DEC);
  //Serial.println(packet);
  LoRaData();
}

void LoRaData() {
  double AgeNew = callToGPSAGE();
  double DriveHeading = vehicleBearingAdjustment();
  double brng = PtoP_Bearing();
  double distanceFeet = distanceWithAltitude();
  Heltec.display->clear();
  Heltec.display->setTextAlignment(TEXT_ALIGN_LEFT);
  Heltec.display->setFont(ArialMT_Plain_10);
  String a = String(DriveHeading);
  String b = String((ypr.yaw), 2);
  String c = String(brng, 2);
  String d = String(distanceFeet, 1);
  Heltec.display->drawString(0, 0, "Turn to GPS heading " + a);
  Heltec.display->drawString(0, 15, "Feet: " + d);
if (DriveHeading > -5 && DriveHeading < 5)Heltec.display->drawString(0, 30, "No need to turn");
if (DriveHeading < -5)Heltec.display->drawString(0, 30, "turn right");
if (DriveHeading > 5)Heltec.display->drawString(0, 30, "turn left");
  if (gps.location.age() > 1500) {
    Heltec.display->drawString(65, 40, "CK RX GPS ");
  }
  if (AgeNew > 1500) {
    Heltec.display->drawString(0, 40, "Check TX GPS");
  }
  Heltec.display->display();
}

void steerloop() {
  double DriveBearing = vehicleBearingAdjustment();
  if (DriveBearing >= 5) {
    myservo.write(135);
  }
  if (DriveBearing <= -5) {
    myservo.write(45);
  } else if (DriveBearing < 5 && DriveBearing > -5) {
    myservo.write(95);
  }
}

warriorridersumrell · November 5, 2022, 6:44am

I have made an attempt at applying SPID example myself. It is set inside void loop(), sending a myservo.write(output);

I'm doing Serial.print() to get values of output, input and Bearing
I'm getting a value of 255 for output and gap matches Bearing (which is determined by two sets of Lat, lng values.

gap should be difference between input and setpoint but it isn't getting the setpoint I think.
output value doesn't change even if I change input value.

#include <PID_v2.h>

#include "heltec.h"
#include "Simpletimer.h"
#include <Adafruit_BMP280.h>
#include <Arduino.h>
#include <Adafruit_BNO08x.h>
#include "BluetoothSerial.h"
#include <ESP32Servo.h>
#include <TinyGPSPlus.h>

// 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_v2 myPID(consKp, consKi, consKd, PID::Direct);

#if !defined(CONFIG_BT_ENABLED) || !defined(CONFIG_BLUEDROID_ENABLED)
#error Bluetooth is not enabled! Please run `make menuconfig` to and enable it
#endif

#if !defined(CONFIG_BT_SPP_ENABLED)
#error Serial Bluetooth not available or not enabled. It is only available for the ESP32 chip.
#endif

#ifdef FAST_MODE
// Top frequency is reported to be 1000Hz (but freq is somewhat variable)
sh2_SensorId_t reportType = SH2_GYRO_INTEGRATED_RV;
long reportIntervalUs = 2000;
#else
// Top frequency is about 250Hz but this report is more accurate
sh2_SensorId_t reportType = SH2_ARVR_STABILIZED_RV;
long reportIntervalUs = 5000;
#endif

String rssi = "RSSI --";
String packSize = "--";
String packet;

Adafruit_BMP280 bmp;

TinyGPSPlus gps;
unsigned long last = 0UL;

Servo myservo;
int pos = 0;  // variable to store the servo position
int servoPin = 12;



BluetoothSerial SerialBT;
boolean confirmRequestPending = true;
char Incoming_value = 0;

struct euler_t {
  float yaw;
  float pitch;
  float roll;
} ypr;

#define BNO08X_RESET -1
#define BNO08X_CS 10
#define BNO08X_INT 9
#define BMP_SCK (13)
#define BMP_MISO (12)
#define BMP_MOSI (11)
#define BMP_CS (10)
#define BAND 915E6


Simpletimer timer1;
Simpletimer timer2;
Simpletimer timer3;
Simpletimer timer4;
Simpletimer timer5;
Simpletimer timer6;

Adafruit_BNO08x bno08x(BNO08X_RESET);
sh2_SensorValue_t sensorValue;

void loRaPackageSend() {
  int packetSize = LoRa.parsePacket();
  if (packetSize) { cbk(packetSize); }
}

void GPSDataLoad() {
  GPSloop();
  callToGPSLAT();
  callToGPSLONG();
  callToGPSALT();
  callToGPSAGE();
  callToGPSSPEED();
}

void BMP280Load() {
  BMPloop();
}

void BNO085Load() {
  BNOloop();
}

void DistanceWithAltitude() {
  distanceWithAltitude();
}

void PtoP_bearing() {
  PtoP_Bearing();
  vehicleBearingAdjustment();
}

double brng = PtoP_Bearing();

void setup() {
  Heltec.begin(true /*DisplayEnable Enable*/, true /*Heltec.Heltec.Heltec.LoRa Disable*/, true /*Serial Enable*/, true /*PABOOST Enable*/, BAND /*long BAND*/);
  BTsetup();
  GPSsetup();
  BMPsetup();
  BNOsetup();
  Heltec.display->init();
  Heltec.display->flipScreenVertically();
  Heltec.display->setFont(ArialMT_Plain_10);
  timer1.register_callback(loRaPackageSend);
  timer2.register_callback(GPSDataLoad);
  timer3.register_callback(BMP280Load);
  timer4.register_callback(BNO085Load);
  timer5.register_callback(DistanceWithAltitude);
  timer6.register_callback(PtoP_bearing);
  Heltec.display->clear();
  Heltec.display->drawString(0, 0, "Heltec.LoRa Initial success!");
  Heltec.display->drawString(0, 10, "Wait for incoming data...");
  Heltec.display->display();
  LoRa.receive();
  myPID.Start(brng,      // input
              0,         // current output
              ypr.yaw);  // setpoint
}

void loop() {
  BTloop();
  timer1.run(100);
  timer2.run(250);
  timer3.run(500);
  timer4.run(100);
  timer5.run(500);
  timer6.run(100);
  steerloop();
  phoneInput();
  int Bearing = ((((int)brng) + 540) % 360) - 180;
  const double input = Bearing;

  double gap = abs(myPID.GetSetpoint() - 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);
  }

  const double output = myPID.Run(input);
  myservo.write(output);
  Serial.print("output");
  Serial.print(output);
  Serial.print("brng");
  Serial.print(Bearing);
  Serial.print("ypr.yaw");
  Serial.print(ypr.yaw);
  Serial.print("gap");
  Serial.print(gap);
}

void BMPsetup() {
  Serial.println(F("BMP280 Forced Mode Test."));
  if (!bmp.begin()) {
    Serial.println(F("Could not find a valid BMP280 sensor, check wiring or "
                     "try a different address!"));
    while (1) delay(10);
  }

  bmp.setSampling(Adafruit_BMP280::MODE_FORCED,     /* Operating Mode. */
                  Adafruit_BMP280::SAMPLING_X2,     /* Temp. oversampling */
                  Adafruit_BMP280::SAMPLING_X16,    /* Pressure oversampling */
                  Adafruit_BMP280::FILTER_X16,      /* Filtering. */
                  Adafruit_BMP280::STANDBY_MS_500); /* Standby time. */
}

double BMPloop() {
  if (bmp.takeForcedMeasurement()) {
  } else {
    Serial.println("Forced measurement failed!");
  }
  double Altitude = (bmp.readAltitude(1013.25));
  return (Altitude);
}

void setReports(sh2_SensorId_t reportType, long report_interval) {
  Serial.println("Setting desired reports");
  if (!bno08x.enableReport(reportType, report_interval)) {
    Serial.println("Could not enable stabilized remote vector");
  }
}

void BNOsetup(void) {
  while (!Serial) delay(10);
  Serial.println("Adafruit BNO08x test!");
  if (!bno08x.begin_I2C()) {
    Serial.println("Failed to find BNO08x chip");
    while (1) { delay(10); }
  }
  Serial.println("BNO08x Found!");
  setReports(reportType, reportIntervalUs);
  Serial.println("Reading events");
  //delay(10);
}

void quaternionToEuler(float qr, float qi, float qj, float qk, euler_t* ypr, bool degrees = false) {
  float sqr = sq(qr);
  float sqi = sq(qi);
  float sqj = sq(qj);
  float sqk = sq(qk);
  ypr->yaw = atan2(2 * -1 * (qi * qj + qk * qr), (sqi - sqj - sqk + sqr));
  ypr->pitch = asin(-2.0 * (qi * qk - qj * qr) / (sqi + sqj + sqk + sqr));
  ypr->roll = atan2(2.0 * (qj * qk + qi * qr), (-sqi - sqj + sqk + sqr));
  if (degrees) {
    ypr->yaw *= RAD_TO_DEG;
    ypr->pitch *= RAD_TO_DEG;
    ypr->roll *= RAD_TO_DEG;
  }
}

void quaternionToEulerRV(sh2_RotationVectorWAcc_t* rotational_vector, euler_t* ypr, bool degrees = false) {
  quaternionToEuler(rotational_vector->real, rotational_vector->i, rotational_vector->j, rotational_vector->k, ypr, degrees);
}

void quaternionToEulerGI(sh2_GyroIntegratedRV_t* rotational_vector, euler_t* ypr, bool degrees = false) {
  quaternionToEuler(rotational_vector->real, rotational_vector->i, rotational_vector->j, rotational_vector->k, ypr, degrees);
}

void BNOloop() {
  if (bno08x.wasReset()) {
    Serial.print("sensor was reset ");
    setReports(reportType, reportIntervalUs);
  }

  if (bno08x.getSensorEvent(&sensorValue)) {
    switch (sensorValue.sensorId) {
      case SH2_ARVR_STABILIZED_RV:
        quaternionToEulerRV(&sensorValue.un.arvrStabilizedRV, &ypr, true);
      case SH2_GYRO_INTEGRATED_RV:
        // faster (more noise?)
        quaternionToEulerGI(&sensorValue.un.gyroIntegratedRV, &ypr, true);
        break;
    }
    static long last = 0;
    long now = micros();
    last = now;
  }
}

void BTConfirmRequestCallback(uint32_t numVal) {
  confirmRequestPending = true;
  Serial.println(numVal);
}

void BTAuthCompleteCallback(boolean success) {
  confirmRequestPending = false;
  if (success) {
    Serial.println("Pairing success!!");
  } else {
    Serial.println("Pairing failed, rejected by user!!");
  }
}


void BTsetup() {
  Serial.begin(115200);
  SerialBT.enableSSP();
  SerialBT.onConfirmRequest(BTConfirmRequestCallback);
  SerialBT.onAuthComplete(BTAuthCompleteCallback);
  SerialBT.begin("ESP32test");  //Bluetooth device name
  Serial.println("The device started, now you can pair it with bluetooth!");
  myservo.setPeriodHertz(330);
  myservo.attach(servoPin, 700, 2100);
}

void BTloop() {
  if (confirmRequestPending) {
    if (Serial.available()) {
      int dat = Serial.read();
      if (dat == 'Y' || dat == 'y') {
        SerialBT.confirmReply(true);
      } else {
        SerialBT.confirmReply(false);
      }
    }
  } else {
    if (Serial.available()) {
      SerialBT.write(Serial.read());
    }
    if (SerialBT.available()) {
      Serial.write(SerialBT.read());
    }
    //delay(20);
  }
}

void phoneInput() {
  if (SerialBT.available() > 0) {
    Incoming_value = SerialBT.read();
  }
  if (Incoming_value == '0') {
    Serial.println("Off");
  } else if (Incoming_value == '1') {
    Serial.println("On");
  } else if (Incoming_value == '2') {
    Serial.println("Fwd");
  } else if (Incoming_value == '3') {
    Serial.println("Aft");
  } else if (Incoming_value == '4') {
    Serial.println("Left");
    myservo.write(135);
  } else if (Incoming_value == '5') {
    Serial.println("Right");
    myservo.write(45);
  } else if (Incoming_value == '6') {
    Serial.println("");
  }
}

void GPSsetup() {
  Serial.begin(115200);
  Serial1.begin(9600, SERIAL_8N1, 2, 17);
}

void GPSloop() {
  while (Serial1.available() > 0)
    gps.encode(Serial1.read());

  if (gps.location.isUpdated()) {
    //Serial.print(F("LOCATION   Fix Age="));
    //Serial.print(gps.location.age());
    //Serial.print(F("ms Raw Lat="));
    //Serial.print(gps.location.rawLat().negative ? "-" : "+");
    //Serial.print(gps.location.rawLat().deg);
    //Serial.print("[+");
    //Serial.print(gps.location.rawLat().billionths);
    //Serial.print(F(" billionths],  Raw Long="));
    //Serial.print(gps.location.rawLng().negative ? "-" : "+");
    //Serial.print(gps.location.rawLng().deg);
    //Serial.print("[+");
    //Serial.print(gps.location.rawLng().billionths);
    //Serial.print(F(" billionths],  Lat="));
    //Serial.print(gps.location.lat(), 6);
    //Serial.print(F(" Long="));
    //Serial.println(gps.location.lng(), 6);
  }

  else if (gps.date.isUpdated()) {
    //Serial.print(F("DATE       Fix Age="));
    //Serial.print(gps.date.age());
    //Serial.print(F("ms Raw="));
    //Serial.print(gps.date.value());
    //Serial.print(F(" Year="));
    //Serial.print(gps.date.year());
    //Serial.print(F(" Month="));
    //Serial.print(gps.date.month());
    //Serial.print(F(" Day="));
    //Serial.println(gps.date.day());
  }

  else if (gps.time.isUpdated()) {
    //Serial.print(F("TIME       Fix Age="));
    //Serial.print(gps.time.age());
    //Serial.print(F("ms Raw="));
    //Serial.print(gps.time.value());
    //Serial.print(F(" Hour="));
    //Serial.print(gps.time.hour());
    //Serial.print(F(" Minute="));
    //Serial.print(gps.time.minute());
    //Serial.print(F(" Second="));
    //Serial.print(gps.time.second());
    //Serial.print(F(" Hundredths="));
    //Serial.println(gps.time.centisecond());
  }

  else if (gps.speed.isUpdated()) {
    //Serial.print(F("SPEED      Fix Age="));
    //Serial.print(gps.speed.age());
    //Serial.print(F("ms Raw="));
    //Serial.print(gps.speed.value());
    //Serial.print(F(" Knots="));
    //Serial.print(gps.speed.knots());
    //Serial.print(F(" MPH="));
    //Serial.print(gps.speed.mph());
    //Serial.print(F(" m/s="));
    //Serial.print(gps.speed.mps());
    //Serial.print(F(" km/h="));
    //Serial.println(gps.speed.kmph());
  }

  else if (gps.course.isUpdated()) {
    //Serial.print(F("COURSE     Fix Age="));
    //Serial.print(gps.course.age());
    //Serial.print(F("ms Raw="));
    //Serial.print(gps.course.value());
    //Serial.print(F(" Deg="));
    //Serial.println(gps.course.deg());
  }

  else if (gps.altitude.isUpdated()) {
    //Serial.print(F("ALTITUDE   Fix Age="));
    //Serial.print(gps.altitude.age());
    //Serial.print(F("ms Raw="));
    //Serial.print(gps.altitude.value());
    //Serial.print(F(" Meters="));
    //Serial.print(gps.altitude.meters());
    //Serial.print(F(" Miles="));
    //Serial.print(gps.altitude.miles());
    //Serial.print(F(" KM="));
    //Serial.print(gps.altitude.kilometers());
    //Serial.print(F(" Feet="));
    //Serial.println(gps.altitude.feet());
  }

  else if (gps.satellites.isUpdated()) {
    //Serial.print(F("SATELLITES Fix Age="));
    //Serial.print(gps.satellites.age());
    //Serial.print(F("ms Value="));
    //Serial.println(gps.satellites.value());
  }

  else if (gps.hdop.isUpdated()) {
    //Serial.print(F("HDOP       Fix Age="));
    //Serial.print(gps.hdop.age());
    //Serial.print(F("ms raw="));
    //Serial.print(gps.hdop.value());
    //Serial.print(F(" hdop="));
    //Serial.println(gps.hdop.hdop());
  }

  else if (millis() - last > 5000) {
    Serial.println();
    if (gps.location.isValid()) {
      static const double LONDON_LAT = 51.508131, LONDON_LON = -0.128002;
      double distanceToLondon =
        TinyGPSPlus::distanceBetween(
          gps.location.lat(),
          gps.location.lng(),
          LONDON_LAT,
          LONDON_LON);
      double courseToLondon =
        TinyGPSPlus::courseTo(
          gps.location.lat(),
          gps.location.lng(),
          LONDON_LAT,
          LONDON_LON);

      Serial.print(F("LONDON     Distance="));
      Serial.print(distanceToLondon / 1000, 6);
      Serial.print(F(" km Course-to="));
      Serial.print(courseToLondon, 6);
      Serial.print(F(" degrees ["));
      Serial.print(TinyGPSPlus::cardinal(courseToLondon));
      Serial.println(F("]"));
    }

    Serial.print(F("DIAGS      Chars="));
    Serial.print(gps.charsProcessed());
    Serial.print(F(" Sentences-with-Fix="));
    Serial.print(gps.sentencesWithFix());
    Serial.print(F(" Failed-checksum="));
    Serial.print(gps.failedChecksum());
    Serial.print(F(" Passed-checksum="));
    Serial.println(gps.passedChecksum());

    if (gps.charsProcessed() < 10)
      Serial.println(F("WARNING: No GPS data.  Check wiring."));

    last = millis();
    Serial.println();
  }
}

double distanceWithAltitude() {
  double NewLat = callToGPSLAT();
  double NewLong = callToGPSLONG();
  double lat1 = gps.location.lat();
  double lon1 = gps.location.lng();
  double lat2 = NewLat;
  double lon2 = NewLong;
  lon1 = lon1 * PI / 180;
  lon2 = lon2 * PI / 180;
  lat1 = lat1 * PI / 180;
  lat2 = lat2 * PI / 180;

  // Haversine formula
  double dlon = lon2 - lon1;
  double dlat = lat2 - lat1;
  double a = pow(sin(dlat / 2), 2)
             + cos(lat1) * cos(lat2)
                 * pow(sin(dlon / 2), 2);

  double c = 2 * asin(sqrt(a));

  // Radius of earth in kilometers. Use 3956
  // for miles
  double r = 3956;
  double distance = (c * r);
  //Serial.print("distance ");
  //Serial.println(distance);
  double distanceFeet = distance * 5280;
  return distanceFeet;
}

double callToGPSAGE() {
  String Long = packet.substring(50, 60);
  double AgeNew = Long.toDouble();
  return AgeNew;
}

double callToGPSLONG() {
  String Long = packet.substring(19, 38);
  double LongNew = Long.toDouble();
  return LongNew;
}

double callToGPSALT() {
  String Alt = packet.substring(41, 47);
  double AltNew = Alt.toDouble();
  return AltNew;
}

double callToGPSSPEED() {
  String Speed = packet.substring(50, 55);
  double SpeedNew = Speed.toDouble();
  return SpeedNew;
}

double callToGPSLAT() {
  String Lat = packet.substring(3, 19);
  double LatNew = Lat.toDouble();
  return (LatNew);
}



double PtoP_Bearing() {
  double NewLat = 46.5596814;
  double NewLong = -122.2908778;
  double lat = 47.9089450;
  double lon = -122.2439150;
  double lat2 = NewLat;
  double lon2 = NewLong;
  double teta1 = radians(lat);
  double teta2 = radians(lat2);
  double delta1 = radians(lat2 - lat);
  double delta2 = radians(lon2 - lon);
  //==================Heading Formula Calculation================//

  double y = sin(delta2) * cos(teta2);
  double x = cos(teta1) * sin(teta2) - sin(teta1) * cos(teta2) * cos(delta2);
  double brng = atan2(y, x);
  brng = degrees(brng);  // radians to degrees
  brng = (((int)brng + 360) % 360);
  return brng;
}
double vehicleBearingAdjustment() {
  int brng = PtoP_Bearing();
  double YAW = ypr.yaw;
  int DriveHeading = ((((int)YAW - brng) + 540) % 360) - 180;  //heading error is in range -180 to 179
  Serial.print("DriveHeading");
  Serial.print(DriveHeading);  //heading error is in range -180 to 179
  //Serial.print("brng ");
  //Serial.print(brng);
  if (DriveHeading > -5 && DriveHeading < 5) Serial.println("NOT TURNING: ");
  if (DriveHeading < -5) Serial.println("Turn Right: ");
  if (DriveHeading > 5) Serial.println("Turn Left: ");
  return DriveHeading;
}

void cbk(int packetSize) {
  packet = "";
  packSize = String(packetSize, DEC);
  for (int i = 0; i < packetSize; i++) { packet += (char)LoRa.read(); }
  rssi = "RSSI " + String(LoRa.packetRssi(), DEC);
  //Serial.println(packet);
  LoRaData();
}

void LoRaData() {
  double AgeNew = callToGPSAGE();
  double YAW = vehicleBearingAdjustment();
  double brng = PtoP_Bearing();
  double distanceFeet = distanceWithAltitude();
  Heltec.display->clear();
  Heltec.display->setTextAlignment(TEXT_ALIGN_LEFT);
  Heltec.display->setFont(ArialMT_Plain_10);
  String a = String(YAW);
  String b = String((ypr.yaw), 2);
  String c = String(brng, 2);
  String d = String(distanceFeet, 1);
  Heltec.display->drawString(0, 0, "Turn to GPS heading " + a);
  Heltec.display->drawString(0, 10, "GPS Heading " + c);
  Heltec.display->drawString(0, 20, "Feet " + d);

  if (YAW - brng < 5 && YAW - brng > -5) Heltec.display->drawString(0, 30, "No need to turn");
  else if (YAW - brng > 5 && YAW - brng < 180) Heltec.display->drawString(0, 30, "turn left");
  else if (YAW - brng < -5 || YAW - brng > 180) Heltec.display->drawString(0, 30, "turn right");
  //Serial.print(DriveBearing);
  if (gps.location.age() > 1500) {
    Heltec.display->drawString(65, 40, "CK RX GPS ");
  }
  if (AgeNew > 1500) {
    Heltec.display->drawString(0, 45, "Check TX GPS");
  }
  Heltec.display->display();
}

void steerloop() {
  double DriveBearing = vehicleBearingAdjustment();
  if (DriveBearing >= 5) {
    myservo.write(135);
  }
  if (DriveBearing <= -5) {
    myservo.write(45);
  } else if (DriveBearing < 5 && DriveBearing > -5) {
    myservo.write(95);
  }
}

warriorridersumrell · November 5, 2022, 7:45am

I took this out of the setup() and stuck it in the loop() and i am getting a proper gap reading now.
I know this doesn't belong in loop and 0 in current output is just keeping output at 0.

it looks like this library was intended for a setpoint at setup that stays as that number? i don't see any way to set the setpoint to my bno085 compass.

myPID.Start(Bearing, 0, ypr.yaw);  // input, current output, setpoint

warriorridersumrell · November 9, 2022, 11:10am

i got PID working for steering via a servo. I used PIDController.h library. Rather than using bearing for setpoint and current heading for input and subtracting to get error I used setpoint 0 and input of :

int DriveHeading = ((((int)YAW - brng) + 540) % 360) - 180; //heading error is in range -180 to 179.

This way i am always moving to zero. seems to work good though I am confused how the servo is able to take an output of a negative number and move. i thought it needed PWM angle input "numbers between 0 and 360 or micro inputs 1000 to 2000?

my project ultimately wont steer with a servo but with two wheels. So I'll need to understand how to translate the single pid output to input control of two motors.

//************************************************************************//
///////////////////////////////LIBRARIES//////////////////////////////////
//***********************************************************************//
#include <PIDController.h>
#include "heltec.h"
#include "Simpletimer.h"
#include <Adafruit_BMP280.h>
#include <Arduino.h>
#include <Adafruit_BNO08x.h>
#include "BluetoothSerial.h"
#include <ESP32Servo.h>
#include <TinyGPSPlus.h>

//************************************************************************//
///////////////////////////////DEFINITIONS//////////////////////////////////
//***********************************************************************//
#if !defined(CONFIG_BT_ENABLED) || !defined(CONFIG_BLUEDROID_ENABLED)
#error Bluetooth is not enabled! Please run `make menuconfig` to and enable it
#endif

#if !defined(CONFIG_BT_SPP_ENABLED)
#error Serial Bluetooth not available or not enabled. It is only available for the ESP32 chip.
#endif

#ifdef FAST_MODE
// Top frequency is reported to be 1000Hz (but freq is somewhat variable)
sh2_SensorId_t reportType = SH2_GYRO_INTEGRATED_RV;
long reportIntervalUs = 2000;
#else
// Top frequency is about 250Hz but this report is more accurate
sh2_SensorId_t reportType = SH2_ARVR_STABILIZED_RV;
long reportIntervalUs = 5000;
#endif

#define BNO08X_RESET -1
#define BNO08X_CS 10
#define BNO08X_INT 9
#define BMP_SCK (13)
#define BMP_MISO (12)
#define BMP_MOSI (11)
#define BMP_CS (10)
#define BAND 915E6
//************************************************************************//
///////////////////////////////OBJECTS/////////////////////////////////////
//***********************************************************************//
PIDController pid;  // Create an instance of the PID controller class, called "pid"
Adafruit_BMP280 bmp;
TinyGPSPlus gps;
Servo myservo;
BluetoothSerial SerialBT;
Adafruit_BNO08x bno08x(BNO08X_RESET);
sh2_SensorValue_t sensorValue;

//************************************************************************//
////////////////////////////////VARIABLES//////////////////////////////////
//***********************************************************************//
unsigned long last = 0UL;
int pos = 0;  // variable to store the servo position
int servoPin = 12;
int random_variable;
int a;
boolean confirmRequestPending = true;
char Incoming_value = 0;
String rssi = "RSSI --";
String packSize = "--";
String packet;

//************************************************************************//
///////////////////////////////PRE FUNCTIONS///////////////////////////////
//***********************************************************************//


struct euler_t {
  float yaw;
  float pitch;
  float roll;
} ypr;

void loRaPackageSend() {
  int packetSize = LoRa.parsePacket();
  if (packetSize) { cbk(packetSize); }
}

void GPSDataLoad() {
  GPSloop();
  callToGPSLAT();
  callToGPSLONG();
  callToGPSALT();
  callToGPSAGE();
  callToGPSSPEED();
}

void BMP280Load() {
  BMPloop();
}

void BNO085Load() {
  BNOloop();
}

void DistanceWithAltitude() {
  distanceWithAltitude();
}

void PtoP_bearing() {
  PtoP_Bearing();
  vehicleBearingAdjustment();
  SteeringServo();
}

//************************************************************************//
/////////////////////////////////TIMERS////////////////////////////////////
//***********************************************************************//
Simpletimer timer1;
Simpletimer timer2;
Simpletimer timer3;
Simpletimer timer4;
Simpletimer timer5;
Simpletimer timer6;

//************************************************************************//
////////////////////////SET UP FOR PROGRAM/////////////////////////////////
//***********************************************************************//
void setup() {
  Heltec.begin(true /*DisplayEnable Enable*/, true /*Heltec.Heltec.Heltec.LoRa Disable*/, true /*Serial Enable*/, true /*PABOOST Enable*/, BAND /*long BAND*/);
  BTsetup();
  GPSsetup();
  BMPsetup();
  BNOsetup();
  Heltec.display->init();
  Heltec.display->flipScreenVertically();
  Heltec.display->setFont(ArialMT_Plain_10);
  timer1.register_callback(loRaPackageSend);
  timer2.register_callback(GPSDataLoad);
  timer3.register_callback(BMP280Load);
  timer4.register_callback(BNO085Load);
  timer5.register_callback(DistanceWithAltitude);
  timer6.register_callback(PtoP_bearing);
  Heltec.display->clear();
  Heltec.display->drawString(0, 0, "Heltec.LoRa Initial success!");
  Heltec.display->drawString(0, 10, "Wait for incoming data...");
  Heltec.display->display();
  LoRa.receive();
  pid.begin();         // initialize the PID instance
  pid.setpoint(0);    // The "goal" the PID controller tries to "reach"
  pid.tune(8, 0, 1);   // Tune the PID, arguments: kP, kI, kD
  pid.limit(-180, 180);  // Limit the PID output between 0 and 255, this is important to get rid of integral windup!
}


//************************************************************************//
/////////////////////////////LOOPING VOID//////////////////////////////////
//***********************************************************************//
void loop() {
  BTloop();
  timer1.run(100);
  timer2.run(250);
  timer3.run(500);
  timer4.run(100);
  timer5.run(500);
  timer6.run(100);
  phoneInput();
}


//************************************************************************//
//////////////////////////FUNCTIONS SUB PROGRAMS////////////////////////////
//***********************************************************************//

int SteeringServo() {
  a = vehicleBearingAdjustment();
  //if (a < 180 && a > -1) a = a + 90;
  //else if (a < 0 && a >180) a = abs(90 + a);
  int sensorValue = a;  // Read the value from the sensor
  int output = pid.compute(sensorValue);  // Let the PID compute the value, returns the optimal output
                                          // Write the output to the output pin
  Serial.print(0);
  Serial.print(" ");
  Serial.print(a);
  Serial.print(" ");
  //if (output >=0) output = output +90;
  //else if (output <0) output = abs(90+output);
  Serial.println(output);
  output = (output +90);

  myservo.write(output);
  return output;
}

void BMPsetup() {
  Serial.println(F("BMP280 Forced Mode Test."));
  if (!bmp.begin()) {
    Serial.println(F("Could not find a valid BMP280 sensor, check wiring or "
                     "try a different address!"));
    while (1) delay(10);
  }

  bmp.setSampling(Adafruit_BMP280::MODE_FORCED,     /* Operating Mode. */
                  Adafruit_BMP280::SAMPLING_X2,     /* Temp. oversampling */
                  Adafruit_BMP280::SAMPLING_X16,    /* Pressure oversampling */
                  Adafruit_BMP280::FILTER_X16,      /* Filtering. */
                  Adafruit_BMP280::STANDBY_MS_500); /* Standby time. */
}

double BMPloop() {
  if (bmp.takeForcedMeasurement()) {
  } else {
    Serial.println("Forced measurement failed!");
  }
  double Altitude = (bmp.readAltitude(1013.25));
  return (Altitude);
}

void setReports(sh2_SensorId_t reportType, long report_interval) {
  Serial.println("Setting desired reports");
  if (!bno08x.enableReport(reportType, report_interval)) {
    Serial.println("Could not enable stabilized remote vector");
  }
}

void BNOsetup(void) {
  while (!Serial) delay(10);
  Serial.println("Adafruit BNO08x test!");
  if (!bno08x.begin_I2C()) {
    Serial.println("Failed to find BNO08x chip");
    while (1) { delay(10); }
  }
  Serial.println("BNO08x Found!");
  setReports(reportType, reportIntervalUs);
  Serial.println("Reading events");
  //delay(10);
}

void quaternionToEuler(float qr, float qi, float qj, float qk, euler_t* ypr, bool degrees = false) {
  float sqr = sq(qr);
  float sqi = sq(qi);
  float sqj = sq(qj);
  float sqk = sq(qk);
  ypr->yaw = atan2(2 * -1 * (qi * qj + qk * qr), (sqi - sqj - sqk + sqr));
  ypr->pitch = asin(-2.0 * (qi * qk - qj * qr) / (sqi + sqj + sqk + sqr));
  ypr->roll = atan2(2.0 * (qj * qk + qi * qr), (-sqi - sqj + sqk + sqr));
  if (degrees) {
    ypr->yaw *= RAD_TO_DEG;
    ypr->pitch *= RAD_TO_DEG;
    ypr->roll *= RAD_TO_DEG;
  }
}

void quaternionToEulerRV(sh2_RotationVectorWAcc_t* rotational_vector, euler_t* ypr, bool degrees = false) {
  quaternionToEuler(rotational_vector->real, rotational_vector->i, rotational_vector->j, rotational_vector->k, ypr, degrees);
}

void quaternionToEulerGI(sh2_GyroIntegratedRV_t* rotational_vector, euler_t* ypr, bool degrees = false) {
  quaternionToEuler(rotational_vector->real, rotational_vector->i, rotational_vector->j, rotational_vector->k, ypr, degrees);
}

void BNOloop() {
  if (bno08x.wasReset()) {
    Serial.print("sensor was reset ");
    setReports(reportType, reportIntervalUs);
  }

  if (bno08x.getSensorEvent(&sensorValue)) {
    switch (sensorValue.sensorId) {
      case SH2_ARVR_STABILIZED_RV:
        quaternionToEulerRV(&sensorValue.un.arvrStabilizedRV, &ypr, true);
      case SH2_GYRO_INTEGRATED_RV:
        // faster (more noise?)
        quaternionToEulerGI(&sensorValue.un.gyroIntegratedRV, &ypr, true);
        break;
    }
    static long last = 0;
    long now = micros();
    last = now;
  }
}

void BTConfirmRequestCallback(uint32_t numVal) {
  confirmRequestPending = true;
  Serial.println(numVal);
}

void BTAuthCompleteCallback(boolean success) {
  confirmRequestPending = false;
  if (success) {
    Serial.println("Pairing success!!");
  } else {
    Serial.println("Pairing failed, rejected by user!!");
  }
}

void BTsetup() {
  Serial.begin(115200);
  SerialBT.enableSSP();
  SerialBT.onConfirmRequest(BTConfirmRequestCallback);
  SerialBT.onAuthComplete(BTAuthCompleteCallback);
  SerialBT.begin("ESP32test");  //Bluetooth device name
  Serial.println("The device started, now you can pair it with bluetooth!");
  myservo.setPeriodHertz(330);
  myservo.attach(servoPin, 700, 2100);
}

void BTloop() {
  if (confirmRequestPending) {
    if (Serial.available()) {
      int dat = Serial.read();
      if (dat == 'Y' || dat == 'y') {
        SerialBT.confirmReply(true);
      } else {
        SerialBT.confirmReply(false);
      }
    }
  } else {
    if (Serial.available()) {
      SerialBT.write(Serial.read());
    }
    if (SerialBT.available()) {
      Serial.write(SerialBT.read());
    }
  }
}

void phoneInput() {
  if (SerialBT.available() > 0) {
    Incoming_value = SerialBT.read();
  }
  if (Incoming_value == '0') {
    Serial.println("Off");
  } else if (Incoming_value == '1') {
    Serial.println("On");
  } else if (Incoming_value == '2') {
    Serial.println("Fwd");
  } else if (Incoming_value == '3') {
    Serial.println("Aft");
  } else if (Incoming_value == '4') {
    Serial.println("Left");
    myservo.write(135);
  } else if (Incoming_value == '5') {
    Serial.println("Right");
    myservo.write(45);
  } else if (Incoming_value == '6') {
    Serial.println("");
  }
}

void GPSsetup() {
  Serial1.begin(9600, SERIAL_8N1, 2, 17);
}

void GPSloop() {
  while (Serial1.available() > 0)
    gps.encode(Serial1.read());

  if (millis() - last > 5000) {
    if (gps.charsProcessed() < 10)
      Serial.println(F("WARNING: No GPS data.  Check wiring."));

    last = millis();
  }
}

double distanceWithAltitude() {
  double NewLat = callToGPSLAT();
  double NewLong = callToGPSLONG();
  double lat1 = gps.location.lat();
  double lon1 = gps.location.lng();
  double lat2 = NewLat;
  double lon2 = NewLong;
  lon1 = lon1 * PI / 180;
  lon2 = lon2 * PI / 180;
  lat1 = lat1 * PI / 180;
  lat2 = lat2 * PI / 180;
  // Haversine formula
  double dlon = lon2 - lon1;
  double dlat = lat2 - lat1;
  double a = pow(sin(dlat / 2), 2)
             + cos(lat1) * cos(lat2)
                 * pow(sin(dlon / 2), 2);
  double c = 2 * asin(sqrt(a));
  double r = 3956;  // Radius of earth in kilometers. Use 3956 for miles
  double distance = (c * r);
  //Serial.print("distance ");
  //Serial.println(distance);
  double distanceFeet = distance * 5280;
  return distanceFeet;
}

double callToGPSAGE() {
  String Long = packet.substring(50, 60);
  double AgeNew = Long.toDouble();
  return AgeNew;
}

double callToGPSLONG() {
  String Long = packet.substring(19, 38);
  double LongNew = Long.toDouble();
  return LongNew;
}

double callToGPSALT() {
  String Alt = packet.substring(41, 47);
  double AltNew = Alt.toDouble();
  return AltNew;
}

double callToGPSSPEED() {
  String Speed = packet.substring(50, 55);
  double SpeedNew = Speed.toDouble();
  return SpeedNew;
}

double callToGPSLAT() {
  String Lat = packet.substring(3, 19);
  double LatNew = Lat.toDouble();
  return (LatNew);
}

double PtoP_Bearing() {
  double NewLat = 46.5596814;
  double NewLong = -122.2908778;
  double lat = 47.9089450;
  double lon = -122.2439150;
  double lat2 = NewLat;
  double lon2 = NewLong;
  double teta1 = radians(lat);
  double teta2 = radians(lat2);
  double delta1 = radians(lat2 - lat);
  double delta2 = radians(lon2 - lon);
  //==================Heading Formula Calculation================//
  double y = sin(delta2) * cos(teta2);
  double x = cos(teta1) * sin(teta2) - sin(teta1) * cos(teta2) * cos(delta2);
  double brng = atan2(y, x);
  brng = degrees(brng);  // radians to degrees
  brng = (((int)brng + 360) % 360);
  return brng;
}
double vehicleBearingAdjustment() {
  int brng = PtoP_Bearing();
  double YAW = ypr.yaw;
  int DriveHeading = ((((int)YAW - brng) + 540) % 360) - 180;  //heading error is in range -180 to 179
  /*Serial.print("DriveHeading");
  Serial.print(DriveHeading);  //heading error is in range -180 to 179
  //Serial.print("brng ");
  //Serial.print(brng);
  if (DriveHeading > -5 && DriveHeading < 5) Serial.println("NOT TURNING: ");
  if (DriveHeading < -5) Serial.println("Turn Right: ");
  if (DriveHeading > 5) Serial.println("Turn Left: ");*/
  return DriveHeading;
}

void cbk(int packetSize) {
  packet = "";
  packSize = String(packetSize, DEC);
  for (int i = 0; i < packetSize; i++) { packet += (char)LoRa.read(); }
  rssi = "RSSI " + String(LoRa.packetRssi(), DEC);
  //Serial.println(packet);
  LoRaData();
}

void LoRaData() {
  double AgeNew = callToGPSAGE();
  double YAW = vehicleBearingAdjustment();
  double brng = PtoP_Bearing();
  double distanceFeet = distanceWithAltitude();
  Heltec.display->clear();
  Heltec.display->setTextAlignment(TEXT_ALIGN_LEFT);
  Heltec.display->setFont(ArialMT_Plain_10);
  String a = String(YAW);
  String b = String((ypr.yaw), 2);
  String c = String(brng, 2);
  String d = String(distanceFeet, 1);
  Heltec.display->drawString(0, 0, "Turn to GPS heading " + a);
  Heltec.display->drawString(0, 10, "GPS Heading " + c);
  Heltec.display->drawString(0, 20, "Feet " + d);

  if (YAW - brng < 5 && YAW - brng > -5) Heltec.display->drawString(0, 30, "No need to turn");
  else if (YAW - brng > 5 && YAW - brng < 180) Heltec.display->drawString(0, 30, "turn left");
  else if (YAW - brng < -5 || YAW - brng > 180) Heltec.display->drawString(0, 30, "turn right");
  //Serial.print(DriveBearing);
  if (gps.location.age() > 1500) {
    Heltec.display->drawString(65, 40, "CK RX GPS ");
  }
  if (AgeNew > 1500) {
    Heltec.display->drawString(0, 45, "Check TX GPS");
  }
  Heltec.display->display();
}

system · May 8, 2023, 11:11am

This topic was automatically closed 180 days after the last reply. New replies are no longer allowed.

Topic		Replies	Views
PID para motores Software	18	9442	May 7, 2021
Arduino PID Library News	123	146970	May 6, 2021
[gelöst:] PID library - funktioniert [-nicht-] Deutsch	61	18161	May 6, 2021
Controllo PID Generale	17	5889	May 7, 2021
PID control of Quadcopter Programming	29	10857	May 5, 2021

Using SPIDApplication library

Related topics