Hi All,
I've been working on a project of building a thrust vectoring gimbal system on and off for a while and think I'm nearing a point where things are working,
A brief outline on whats going on. I am using an MPU9250 to gather IMU data and feeding this through a function I created to calculate the PID output. I put it into a function so I could call it with different axis in the loop just for the sake of simplicity (See below)
//Calculate output based on axis
double OutputCalc(double axis){
Input = axis;
myPID.Compute();
double Calc = Output;
if (Calc >= MAXANGLE){
Calc = MAXANGLE;
}
if (Calc <= MINANGLE){
Calc = MINANGLE;
}
return Calc;
}
This being my first time dealing with control systems I'm really just winging it so it's entirely possible this is bad practice, if so please let me know. A couple other bits I've just as logical solutions which may also be poor practice is an if statement to limit the output. I did it this way so that the output wouldn't just be clipped after 90 but would return to 90 each time so as to prevent any 'windup' of the output. I then took this output and mapped the values from the initial -90 - 90 to 0 - 180. I was thinking to perhaps initialise the output to 90 instead and then change the min and max limit to 0 - 180 but this way has worked so far.
The main part I'm struggling with is setting the setpoint for each. My initial plan was to ensure that the starting position of the vehicle was recorded in setup and these positions would be the setpoints. A few problems with that is for some getting an average of the readings has proven to be more difficult for me than expected as you will see in a previous post I've raised. The other thing is how to store the different setpoints. I'm not confident enough in my coding abilities to go in and try to link another variable to library so I was thinking to add this into the function too and add another variable to call.
Below all the code in context.
#include "MPU9250.h"
#include <Arduino.h>
#include <ESP32Servo.h>
#include <SFE_BMP180.h>
#include <esp_now.h>
#include <WiFi.h>
#include <PID_v1.h>
#include "Filter.h"
//GlobalVariables --------------------------------------
//Set Min/Max values for PID Output
int MAXANGLE = 90;
int MINANGLE = -90;
//Set Servo Pins
int XPin = 16;
int YPin = 17;
int ESCPin = 4;
//PID Tuning Parameters
double Kp=0.6;
double Ki=0.5;
double Kd=0.125;
float RollOutput = 0;
float PitchOutput = 0;
float RawPitch = 0;
float RawRoll = 0;
float FilteredPitch = 0;
float FilteredRoll = 0;
//Create PID Variables
double Setpoint, Input, Output;
double PitchAVG = 0;
double RollAVG= 0;
//Initialisers -----------------------------------------
//Create Servos
Servo XAxis;
Servo YAxis;
Servo ESC;
//Create pressure sesor variables
SFE_BMP180 pressure;
//Create IMU Variable
MPU9250 mpu;
//Filter Variables
ExponentialFilter<long> FilterPitch(25, 0);
ExponentialFilter<long> FilterRoll(25, 0);
double baseline;
PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT);
//--------------------------------------------------------
//RX Data struct
typedef struct struct_message {
int val;
} struct_message;
struct_message myData;
//Calculate output based on axis
double OutputCalc(double axis){
Input = axis;
myPID.Compute();
double Calc = Output;
if (Calc >= MAXANGLE){
Calc = MAXANGLE;
}
if (Calc <= MINANGLE){
Calc = MINANGLE;
}
return Calc;
}
//Recieve ESP32 Data from controller
void OnDataRecv(const uint8_t * mac, const uint8_t *incomingData, int len) {
memcpy(&myData, incomingData, sizeof(myData));
}
//Pressure get funcion
double getPressure()
{
char status;
double T,P,p0,a;
status = pressure.startTemperature();
if (status != 0)
{
delay(status);
status = pressure.getTemperature(T);
if (status != 0)
{
status = pressure.startPressure(3);
if (status != 0)
{
delay(status);
status = pressure.getPressure(P,T);
if (status != 0)
{
return(P);
}
else Serial.println("error retrieving pressure measurement\n");
}
else Serial.println("error starting pressure measurement\n");
}
else Serial.println("error retrieving temperature measurement\n");
}
else Serial.println("error starting temperature measurement\n");
}
void print_calibration() {
Serial.println("< calibration parameters >");
Serial.println("accel bias [g]: ");
Serial.print(mpu.getAccBiasX() * 1000.f / (float)MPU9250::CALIB_ACCEL_SENSITIVITY);
Serial.print(", ");
Serial.print(mpu.getAccBiasY() * 1000.f / (float)MPU9250::CALIB_ACCEL_SENSITIVITY);
Serial.print(", ");
Serial.print(mpu.getAccBiasZ() * 1000.f / (float)MPU9250::CALIB_ACCEL_SENSITIVITY);
Serial.println();
Serial.println("gyro bias [deg/s]: ");
Serial.print(mpu.getGyroBiasX() / (float)MPU9250::CALIB_GYRO_SENSITIVITY);
Serial.print(", ");
Serial.print(mpu.getGyroBiasY() / (float)MPU9250::CALIB_GYRO_SENSITIVITY);
Serial.print(", ");
Serial.print(mpu.getGyroBiasZ() / (float)MPU9250::CALIB_GYRO_SENSITIVITY);
Serial.println();
}
void setup() {
Serial.begin(115200);
Wire.begin();
delay(2000);
WiFi.mode(WIFI_STA);
// Init ESP-NOW
if (esp_now_init() != ESP_OK) {
Serial.println("Error initializing ESP-NOW");
return;
}
// Once ESPNow is successfully Init, we will register for recv CB to
// get recv packer info
esp_now_register_recv_cb(OnDataRecv);
if (!mpu.setup(0x68)) { // change to your own address
while (1) {
Serial.println("MPU connection failed. Please check your connection with `connection_check` example.");
delay(5000);
}
}
if (pressure.begin()){
Serial.println("BMP180 init success");
}else{
Serial.println("BMP180 init fail (disconnected?)\n\n");
while(1); // Pause forever.
}
// calibrate anytime you want to
Serial.println("Accel Gyro calibration will start in 5sec.");
Serial.println("Please leave the device still on the flat plane.");
mpu.verbose(true);
delay(5000);
mpu.calibrateAccelGyro();
print_calibration();
mpu.verbose(false);
ESP32PWM::allocateTimer(0);
ESP32PWM::allocateTimer(1);
ESP32PWM::allocateTimer(2);
ESP32PWM::allocateTimer(3);
XAxis.setPeriodHertz(50);
XAxis.attach(XPin, 500, 2400);
YAxis.setPeriodHertz(50);
YAxis.attach(YPin, 500, 2400);
ESC.setPeriodHertz(50);
ESC.attach(ESCPin, 1000, 2000);
baseline = getPressure();
Serial.print("baseline pressure: ");
Serial.print(baseline);
Serial.println(" mb");
Setpoint = 0;
myPID.SetMode(AUTOMATIC);
XAxis.write(90);
YAxis.write(90);
}
void loop() {
if(mpu.update()){
RawPitch = mpu.getPitch();
RawRoll = mpu.getYaw();
FilterPitch.Filter(RawPitch);
FilterRoll.Filter(RawRoll);
FilteredPitch = FilterPitch.Current();
FilteredRoll = FilterRoll.Current();
PitchOutput = OutputCalc(FilteredPitch);
RollOutput = OutputCalc(FilteredRoll);
XAxis.write(map(PitchOutput, -90, 90, 0, 180));
YAxis.write(map(RollOutput, -90, 90, 0, 180));
ESC.write(myData.val);
Serial.print("RawPitch:");
Serial.print(RawPitch);
Serial.print(",");
Serial.print("FilteredPitch:");
Serial.print(FilteredPitch);
Serial.print(",");
Serial.print("Pitch Output:");
Serial.print(map(PitchOutput, -90, 90, 0, 180));
Serial.print(",");
Serial.print("RawRoll");
Serial.print(RawRoll);
Serial.print(",");
Serial.print("FilteredRoll:");
Serial.print(FilteredRoll);
Serial.print(",");
Serial.print("Roll Output:");
Serial.println(map(RollOutput, -90, 90, 0, 180));
// Serial.print(",");
// Serial.print("Pressure:");
// Serial.println(getPressure());
}
}
Any help and constructive criticism of my code would be greatly appreciated
Kind Regards
Aman