Hello. So in my school we did a workshop to build a little robot (mine's named Oreo), and this robot has 2 motors, an IR sensor, some LEDs, and a Sparkfun motor controller shield. We went through a bunch of stuff to build the robot, but once we got to the end, everyone was tired and the programming for the compass was pretty complicated and a majority of it we just copy-pasted into our code. The compass gave me a heading for a while, but now that I've tried integrating it into how the wheels spin, something isn't working and I'm almost 100% sure it has to do with the compass code.
Here's the code:
void compassSetup(){
compass.settings.device.commInterface = IMU_MODE_I2C;
compass.settings.device.mAddress = LSM9DS1_M;
compass.settings.device.agAddress = LSM9DS1_AG;
// The above lines will only take effect AFTER calling
// compass.begin(), which verifies communication with the compass
// and turns it on.
if (!compass.begin())
{
Serial.println("Failed to communicate with LSM9DS1.");
Serial.println("Double-check wiring.");
Serial.println("Default settings in this sketch will " \
"work for an out of the box LSM9DS1 " \
"Breakout, but may need to be modified " \
"if the board jumpers are.");
while (1)
;
}
}
float getHeading(){
float x,y;
// Once you have your heading, you must then add your 'Declination Angle',
// which is the 'Error' of the magnetic field in your location.
// Find yours here: http://www.magnetic-declination.com/ Mine is:
// +8° 29' West, which is 8.483 Degrees, or (which we need) 0.14805628 radians, I will use 8.483
// degrees since we convert it to Radians later in the code.
// If you cannot find your Declination, comment out these two lines, your compass will be slightly off
float declinationAngle = 7.75; //Declination in Boulder, Colorado
// Calibration parameters for accuracy
float Xoffset = .31;
float Yoffset = 0.41;
float Xscale = 1.18;
float Yscale = 0.87;
// Get Magnetic field readings
compass.readMag();
// Subtract calculated offsets from magnetometer data
x = compass.calcMag(compass.mx)-Xoffset;
y = compass.calcMag(compass.my)-Yoffset;
// Scaling correction
x *= Xscale;
y *= Yscale;
// Begin to calculate heading
float heading;
// Calculate the angle
if (y == 0)
heading = (x < 0) ? PI : 0;
else
heading = atan2(y,x);
// Correct for Declination
heading -= declinationAngle * (PI/180);
// Correct for sign errors
if (heading > 2*PI) heading -= (2 * PI);
else if (heading < -PI) heading += (2 * PI);
else if (heading < 0) heading += (2 * PI);
// Convert everything from radians to degrees:
heading *= (180.0 / PI);
//Return the heading
return heading;
//Serial.print("Heading: "); Serial.println(heading, 2);
}
When I run it, there's no direct error but the serial port just reads "Can't connect with the LSM9DS1
Here's main tab of the code if something's wrong there:
//Includes all the required libraries
#include <LSM9DS1_Registers.h>
#include <LSM9DS1_Types.h>
#include <Wire.h>
#include <SPI.h>
#include <SparkFunLSM9DS1.h>
//Sets up pins
const int pwm_right = 3;
const int pwm_left = 11;
const int dir_right = 2;
const int dir_left = 4;
const int distance = A3;
//Sets up IR values
int distanceRead = 0;
float distanceVoltage = 0.0;
//Sets up compass
LSM9DS1 compass;
//Sets up addresses for the compass
#define LSM9DS1_M 0x1E
#define LSM9DS1_AG 0x68
//Sets up LED pins
#define BLUE 6
#define GREEN 5
#define RED 9
int redValue;
int greenValue;
int blueValue;
//Sets up the compass heading
float currentHeading = 0;
float goalHeading = 180;
//Runs once before the loop code
void setup() {
//Sets motor pins as outputs
pinMode(pwm_right, OUTPUT);
pinMode(pwm_left, OUTPUT);
pinMode(dir_right, OUTPUT);
pinMode(dir_left, OUTPUT);
pinMode(distance, INPUT);
//Sets up LED pins as outputs
pinMode(RED, OUTPUT);
pinMode(GREEN, OUTPUT);
pinMode(BLUE, OUTPUT);
//Defaults LEDs to be off
digitalWrite(RED, LOW);
digitalWrite(GREEN, LOW);
digitalWrite(BLUE, LOW);
//Starts serialization
Serial.begin(9600);
//Defaults the motors to be off
forward (0,0);
//Runs the compassSetup code on Compass_Tools
compassSetup();
}
//Runs indefinitely forever
void loop() {
//Sets the heading and prints it
currentHeading = getHeading();
Serial.println("");
Serial.print("Compass: ");
Serial.print(currentHeading);
Serial.print("\t");
//Sets the IR values
distanceRead = analogRead(distance);
distanceVoltage = distanceRead*(5.0/1023.0);
//This is for an RGB LED, you can ignore this if you just have a red LED
redValue = map(distanceRead, 0, 1024, 0, 255);
//Again, this is only for my RGB LED.
//If you have a normal LED, use digitalWrite(Red, HIGH) or whatever
analogWrite(RED, redValue);
analogWrite(GREEN, greenValue);
analogWrite(BLUE, blueValue);
//If there's an obstacle, stop, if not, go.
if(distanceVoltage < 2) {
forward(100, 100);
} else {
forward(0,0);
}
if(currentHeading > goalHeading) {
forward(100, (int)(100-(currentHeading - goalHeading)));
} else if (currentHeading < goalHeading) {
forward((int)(100-(goalHeading-currentHeading)), 100);
}
}
I know this is a lot, but thanks for the help.
*Edit: Also I know it's a programming issue because the example code for the LSM9DS1 works fine.
Thanks guys.