Good day!
I am using an MiniIMU9 v2 as a measuring unit to stabilize my Pan and Tilt platform. I am currently trying to stabilize the Tilt Platform by using the gyro sensor for the stability purposes and the accelerometer to prevent gyro drift as well as platform position accuracy.
My problem: as soon as I add either the gyro or accelerometer output readings to the Tilt PID control calculations, the platform jumps occasionally very violently ( I assume it is because of gyro noise that causes an output in which the motors then rotates, where as the accelerometer then reads and tries to compensate for this, then the gyro receives a reading that the accelerometer then creates and then tries to compensate for…thus the 2 sensors are working against each other). If I only use the gyro sensor, the platform does not jump but I end up with gyro drift and an inaccurate platform position. With the accelerometer only, the platform does not jump as well and it seems pretty accurate, but then I’ll only end up with a “pendulum-effect” platform instead of a platform that is stable via the gyro sensor. How do I use both sensors without the platform jumping so much? Please find my code below. (excuse the amount of commented code, I have been using this code for a while) Please help me create some peace between my 2 sensors! Any suggestions would be helpful!
[code]#include <PID_v1.h> // library for PID Controller
#include <Wire.h>
#include <L3G.h> // library for Gryro sensors
#include <LSM303.h> // library for Accelerometer and Magnetometer sensors
int PIN_DIRECTION_0 = 5; // Direction Pin for Motor 1
int PIN_ENABLE_0 = 6; //Enable Pin for Motor 1
int PIN_PWM_0 = 2; //PWM Pin for Motor 1
int PIN_DIRECTION_1 = 11; // Direction Pin for Motor 2
int PIN_ENABLE_1 = 10; //Enable Pin for Motor 2
int PIN_PWM_1 = 8; //PWM Pin for Motor 2
LSM303 compass;
L3G gyro;
String MyVar("");
//----------------------------------------------------------------//
double SetpointT, InputT, OutputT; //Define Variables we’ll be connecting to
double KpT=1.0, KiT=0.0, KdT=0.0;
PID myPIDT (&InputT, &OutputT, &SetpointT,KpT,KiT,KdT, DIRECT); //Specify the links and initial tuning parameters for the TILT axis
//----------------------------------------------------------------//
double SetpointP, InputP, OutputP;
//double KpP=3.0, KiP=1.0, KdP=0.0;
double KpP=1.0, KiP=0.0, KdP=0.0;
PID myPIDP (&InputP, &OutputP, &SetpointP,KpP,KiP,KdP, DIRECT); //Specify the links and initial tuning parameters for the PAN axis
//----------------------------------------------------------------//
//int setspeedPAN;
//int setspeedTILT;
//float MAG_setpoint;
//float current_MAG_reading;
//double MAG_Kp = -0.5;
//double MAG_Ki = 0.00;
#define RAD_to_DEG 57.295779513082320876798154814105
float accXangle = 0;
float R = 0;
//float Sigma = 0;
const int sampleRate = 1; //Variable that determines how fast the PID loop will run
void updatevars()
{
int rchar; // = Serial.read();
if (Serial.available() > 0)
{
rchar = Serial.read();
if(rchar == 'p')
{
KpT = MyVar.toInt();
myPIDT.SetTunings(KpT, KiT, KdT);
Serial.print("Kp Tilt=");
Serial.println(KpT);
MyVar = "";
}
else if(rchar == 'i')
{
KiT = MyVar.toInt();
myPIDT.SetTunings(KpT, KiT, KdT);
Serial.print("Ki Tilt=");
Serial.println(KiT);
MyVar = "";
}
else if(rchar == 'd')
{
KdT = (MyVar.toInt()/100.0);
myPIDT.SetTunings(KpT, KiT, KdT);
Serial.print("Kd Tilt=");
Serial.println(KdT);
MyVar = "";
}
else if(rchar == 'a')
{
KpP = MyVar.toInt();
myPIDP.SetTunings(KpP, KiP, KdP);
Serial.print("Kp Pan=");
Serial.println(KpP);
MyVar = "";
}
else if(rchar == 'b')
{
KiP = MyVar.toInt();
myPIDP.SetTunings(KpP, KiP, KdP);
Serial.print("Ki Pan=");
Serial.println(KiP);
MyVar = "";
}
else if(rchar == 'c')
{
KdP = (MyVar.toInt()/100.0);
myPIDP.SetTunings(KpP, KiP, KdP);
Serial.print("Kd Pan=");
Serial.println(KdP);
MyVar = "";
}
else
{
MyVar += char(rchar);
}
}
}
void setMspeed(int M, int V1)
{
int direct=0;
int a;
if (V1 < 0)
{
direct = 1;
V1 = -V1;
}
if (V1 > 200)
{
V1 = 200;
}
a = V1 + 26;
if(M == 0)
{
analogWrite(PIN_PWM_0, a);
digitalWrite(PIN_DIRECTION_0, direct);
}
else
{
analogWrite(PIN_PWM_1, a);
digitalWrite(PIN_DIRECTION_1, direct);
}
}
void setM1speed(int v)
{
setMspeed(0,v);
}
void setM2speed(int v)
{
setMspeed(1,v);
}
void setup()
{
Serial.begin(115200);
Wire.begin();
compass.init();
compass.enableDefault();
pinMode(PIN_DIRECTION_0, OUTPUT);
pinMode(PIN_ENABLE_0, OUTPUT);
pinMode(PIN_PWM_0, OUTPUT);
digitalWrite(PIN_ENABLE_0, HIGH);
pinMode(PIN_DIRECTION_1, OUTPUT);
pinMode(PIN_ENABLE_1, OUTPUT);
pinMode(PIN_PWM_1, OUTPUT);
digitalWrite(PIN_ENABLE_1, HIGH);
//compass.read();
//R=sqrt(pow(compass.a.x,2)+pow(compass.a.y,2)+pow(compass.a.z,2));
//accXangle = acos(compass.a.y/R)*RAD_to_DEG - 91;
//InputT = ((gyro.g.x/256)-1.6)+(accXangle);
//SetpointT = 0;
//compass.m_min = (LSM303::vector<int16_t>){-362, -270, -194};
//compass.m_max = (LSM303::vector<int16_t>){+155, +415, +350};
//MAG_setpoint = compass.heading(); //Get current position reading from compass
//Sigma = 0.0;
InputP = (((gyro.g.z/256)+0.9)+((gyro.g.y/256)-0.8));
SetpointP = 0;
if (!gyro.init())
{
Serial.println(“Failed to autodetect gyro type!”);
while (1);
}
myPIDT.SetOutputLimits(-200, 200);
myPIDP.SetOutputLimits(-200, 200);
gyro.enableDefault();
myPIDT.SetMode(AUTOMATIC); //Turn on PID - Tilt
myPIDT.SetSampleTime(sampleRate); //Sets the sample rate
myPIDP.SetMode(AUTOMATIC); //Turn on PID - Pan
myPIDP.SetSampleTime(sampleRate); //Sets the sample rate
}
void loop()
{
//float Delta;
gyro.read();
/*loopcounter = loopcounter+1;
if (loopcounter >= 1000)
{
Serial.println(" Loop Complete ");
loopcounter = 0;
}
*/
compass.read();
R=sqrt(pow(compass.a.x,2)+pow(compass.a.y,2)+pow(compass.a.z,2));
accXangle = acos(compass.a.y/R)*RAD_to_DEG - 91;
InputT = (gyro.g.x/256)-1.0 + ((accXangle)/5.0);
SetpointT = 0.0;
myPIDT.Compute();
// current_MAG_reading = compass.heading();
// Delta = ((current_MAG_reading + 360) - (MAG_setpoint))*MAG_Kp; // differentiation of compass.heading; substracts current compass reading with old compass reading
// Delta = Delta >= 360 ? Delta - 360 : Delta;
// Sigma = (Sigma + Delta); //integration of Delta
// Sigma = Sigma >= 100 ? 100 : Sigma <= -100 ? -100 : Sigma;
InputP = (((gyro.g.z/256)+0.9)+((gyro.g.y/256)-0.8));
// InputP = 0.0;
SetpointP = 0.0; //SetpointP = (Sigma *MAG_Ki) + (Delta * MAG_Kp);
myPIDP.Compute();
setM1speed(-OutputT);
setM2speed(-OutputP);
//Serial.print("Input Tilt: “);
//Serial.println(InputT);
/*Serial.print(”\t Output Tilt: “);
Serial.print(OutputT);
Serial.print(”\t Input Pan: “);
Serial.print(InputP);
Serial.print(”\t Output Pan: “);
Serial.print(OutputP);
Serial.print(”\t Magnetometer: ");
Serial.println(current_MAG_reading);
/*
Serial.print("MAG_setpoint: “);
Serial.print(MAG_setpoint);
Serial.print(”\t current_MAG_reading: “);
Serial.print(current_MAG_reading);
Serial.print(”\t Delta: “);
Serial.print(Delta);
Serial.print(”\t Sigma: ");
Serial.println(Sigma);
//*/
// Serial.print("Gyro Pan Value: ");
//Serial.println(PIN_PWM_0);
updatevars();
//Serial.println(InputT);
}[/code]
Kind regards
Vince
. I have tidied-up the program and tried to make it more understandable. The objective I am trying to complete is to create a Stabilized Pan and Tilt system that will be mounted on a vehicle that will be moving at ±12 m/s. The platform will be carrying an inspection camera and should be able to aim (and be stable) at a desired view point. Each platform are driven using timing belts and pulleys. The components I’m currently using is as follows:
