Hi guys,
I’m busy with a Stabilized Pan and Tilt platform project that will carry a mid-sized camera. The idea is to eliminate all unwanted external movements only in the Pitch and Yaw axis while aiming at a specific target. I am using the following electrical components:
• Arduino UNO R3
• Pololu Dual VNH5019 Motor Driver Shield for the Arduino that powers 2x Pololu Metal Gearmotor 75:1 with a 48 CPR Encoder.
• MiniIMU-9 v2
In my current program I am using the X-axis Gyro sensor for the Tilt-axis and the Y-axis Accelerometer to prevent Gyro drift, but in my Pan-axis I’m only using the Z-axis Gyro sensor whereas I would like to use the Magnetometer to prevent Gyro drift. My problem is that I do not know how to use the Magnetometer readings in my current program to prevent this Gyro drift.
I calibrated the LSM303 sensor as stated in the library, ran the Heading program and it seems to be working. I’ve even ran the AHRS program on Arduino and Python and it seems to be working. I have gone through some examples how the LSM303 where used in other applications but I’m still clueless in how to use the Magnetometer in my program.
I have added my current program for you all to look at. I am quite new in programming so please excuse my programming methods.
#include <PID_v1.h>
#include <Wire.h>
#include <L3G.h>
#include <LSM303.h>
#include <DualVNH5019MotorShield.h>
LSM303 compass;
L3G gyro;
DualVNH5019MotorShield md;
String MyVar("");
//----------------------------------------------------------------//
double SetpointT, InputT, OutputT; //Define Variables we'll be connecting to
double KpT=22.0, KiT=500.0, KdT=0.07;
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=13.0, KiP=100.0, KdP=0.05;
PID myPIDP (&InputP, &OutputP, &SetpointP,KpP,KiP,KdP, DIRECT); //Specify the links and initial tuning parameters for the PAN axis
int setspeedPAN;
int setspeedTILT;
#define RAD_to_DEG 57.295779513082320876798154814105
float accXangle = 0;
float R = 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 stopIfFault()
{
if (md.getM1Fault())
{
Serial.println("M1 fault");
while(1);
}
if (md.getM2Fault())
{
Serial.println("M2 fault");
while(1);
}
}
void setup()
{
Serial.begin(9600);
Wire.begin();
compass.init();
md.init();
compass.enableDefault();
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;
InputP = ((gyro.g.z/256)+0.9)+((gyro.g.y/256)-1.0);
SetpointP = 0;
if (!gyro.init())
{
Serial.println("Failed to autodetect gyro type!");
while (1);
}
compass.read();
myPIDT.SetOutputLimits(-400, 400);
myPIDP.SetOutputLimits(-400, 400);
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()
{
gyro.read();
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;
myPIDT.Compute();
InputP = ((gyro.g.z/256)+0.9)+((gyro.g.y/256)-1.0);
SetpointP = 0;
myPIDP.Compute();
md.setM1Speed(OutputT);
md.setM2Speed(OutputP);
updatevars();
}Thanks
)