Hello everybody,
My new project is rc car with compass, maybe in future I will attach GPS module. This code is all so good for robot. One of the biggest problem is data that sends compass.. Compass send correct data only than when it is on ideal horizontal and vertical level. But when compass is tilted in some direction data comes incorrect... I have tried to correct this problem with code, but the result can be better..
I have found good title (https://www.loveelectronics.co.uk/Tutorials/13/tilt-compensated-compass-arduino-tutorial) that describes how to solve this problem with additional component (https://www.loveelectronics.co.uk/products/136/3-axis-accelerometer---adxl345-breakout-board).
But maybe there are possibility to solve the problem only with code?
The components that I use:
compass - HMC5883L
Arduino UNO
What makes code:
I control my car with rc transmitter(left and right, forward and stop). When I have chosen drive direction and do not touch the steering wheel more, the code begins to work. It knows the direction which the car must go, and then compares it to current direction. If current direction is not equal to main direction it begins to steering. So if the car goes left from the way, goes the signal from arduino to servo (turn right) and so on.
If you have idea how to make better this code, please let me know!
Thank you very much!
P.S. Why MSP and not ESP? Because: Magnetic Stabilization Program.. and not Electronic Stabilization Program.. like on VW or AUDI...
#include <Servo.h>
#include <Wire.h>
#include <HMC5883L.h>
Servo rudder;
HMC5883L compass;
int error = 0;
unsigned long RCrudder = 6;
int currentdirection;
int maindirection;
int state = 1;
int firststandart = 0;
int zstandart = 0;
int zlimit = 5;
int P = 5;
int left = 20;
int right = 160;
int forward = 90;
long rcrudderneutral;
void setup()
{
pinMode (RCrudder, INPUT);
Serial.begin(9600);
rudder.attach(3);
Serial.println("Starting the I2C interface.");
Wire.begin();
Serial.println("Constructing new HMC5883L");
compass = HMC5883L();
Serial.println("Setting scale to +/- 1.3 Ga");
error = compass.SetScale(1.3);
if(error != 0)
Serial.println(compass.GetErrorText(error));
Serial.println("Setting measurement mode to continous.");
error = compass.SetMeasurementMode(Measurement_Continuous);
if(error != 0)
Serial.println(compass.GetErrorText(error));
}
void loop()
{
if(state == 1) {
////////////////calibration///////////////////////////
compas();
zstandart = firststandart;
RCrudder = (pulseIn (6, HIGH, 100000))/10;
rcrudderneutral = RCrudder;
Serial.println("calibration");
//////////////////////////////////////////////////////
if(millis() >= 5000) {
state = 0;
}
}
else
{
RCrudder = (pulseIn (6, HIGH, 100000))/10;
if(RCrudder >= (rcrudderneutral-5) && RCrudder <= (rcrudderneutral+5)) {
pidcontroll();
}
else if(RCrudder > (rcrudderneutral+6) || RCrudder < (rcrudderneutral-6))
{
maualcontroll();
}
}
}
void pidcontroll()
{
compas();
if ((currentdirection - maindirection > P) || (maindirection - currentdirection > P)) {
if (currentdirection > maindirection)
{
if((currentdirection-maindirection) < 180)
rudder.write(right);
else
rudder.write(left);
}
else
{
if((maindirection-currentdirection) < 180)
rudder.write(left);
else
rudder.write(right);
}
}
else
{
rudder.write(forward);
}
}
void maualcontroll()
{
RCrudder = map(RCrudder, 97, 209, 0, 179);
rudder.write(RCrudder);
compas();
maindirection = currentdirection;
}
void compas()
{
// Retrive the raw values from the compass (not scaled).
MagnetometerRaw raw = compass.ReadRawAxis();
// Retrived the scaled values from the compass (scaled to the configured scale).
MagnetometerScaled scaled = compass.ReadScaledAxis();
// Values are accessed like so:
int MilliGauss_OnThe_XAxis = scaled.XAxis;// (or YAxis, or ZAxis)
// Calculate heading when the magnetometer is level, then correct for signs of axis.
float heading = atan2(raw.YAxis, raw.XAxis);
// Your mrad result / 1000.00 (to turn it into radians).
float declinationAngle = 96.87 / 1000.0;
// If you have an EAST declination, use += declinationAngle, if you have a WEST declination, use -= declinationAngle
heading += declinationAngle;
// Correct for when signs are reversed.
if(heading < 0)
heading += 2*PI;
// Check for wrap due to addition of declination.
if(heading > 2*PI)
heading -= 2*PI;
// Convert radians to degrees for readability.
float headingDegrees = heading * 180/M_PI;
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
firststandart = raw.ZAxis;
if(state == 0) {
if((raw.ZAxis >= zstandart - zlimit) && (raw.ZAxis <= zstandart + zlimit)) { //zstandart 515
currentdirection = headingDegrees;
}
}
}
