2WD Platform with compass


I've using the DFRoboto 2WD platform with Uno and the Arduino Motor Shield. Due to lack of encoders I've been having trouble driving in a straight line so I purchased a HMC 6352 compass to have an experiment with this.

Currently I start with both motors running at 255, then monitor the compass reading, if it deviates I slow one of the motors to compensate by -1 on each loop, then when we're back at the desired heading I bring both motors to 255 again.

Its "sort of" working but does tend to wander, I've had varying success by changing the increments and also using the timer on the Arduino to make sure changes are made less often. When its within 15 degrees it'll drop one of the motor speeds down but if it ever gets outside of this, ie. WAY off course, it'll stop and turn on the spot until it hits the required heading then it trundles off forwards.

Just wondering if anyone has had a similar thing working. The compass is well mounted and out the way of interference and the running surface is very flat. If this doesn't work then its time for some encoders, anyone know what can be fitted to the DFRobot 2WD chassis?



A few suggestions that might help:

look into PID logic for your speed adjustment calculations You might want to use a running average of the last 3-5 compass outputs to smooth any jitter in its output

Does your robot always pull left or right? If so you might want to look at trying different ratios of speeds - IE 253 and 255 at max speed

Thanks, I'll have a read up on PID logic.

With regards to the compass jitter, its difficult to know if I'm getting any as its rock solid when attached to the computer and I can't read it while its "unplugged", not sure what other people do, maybe onboard SD logging will speed up debugging of this and any future projects.

That said, I have experimented with compass jitter smoothing, however looking at the maths I don't think it'll help. Currently I have an array of 50 integers and that's updated at each read and then an average is taken by cycling the array and dividing by array size. Problem is that lets say I record a heading of 220 49 times and then get a bad one of 100, that's still an average of 217 which will cause it to speed up/slow down one motor, if I then keep this for 50 more cycles it'll affect my heading for a whole second. Drop that to 5 in the array and the average is then 196 (((220 x 4)+100)/5).

I was thinking I might try keeping 5 but junking anything which is "unrealistic", ie. if I get a reading more than is feasible to achieve given the previous reading and turn speed of the platform.

All that said, I don't know if the jitter is +/- 2-3 or if its as bad as I describe. Given the flat surface, I suspect that my problems are due to the rate at which I'm updating the motors so I'll do some more reading and experiment some more.

If you are averaging the last 50, you are lagging your adjustments to much.

From the description on how you are adjusting your speeds, it sounds like you are only using the I part of a PID circuit, if you wanted to go "round one" simplification of PID configuration I would do the following:

Run a 3 sample rolling average for your sample set. Instead of having your loop change speeds by 1 increment, each cycle you are off direction, change your speed based on the difference between expected direction and actual direction: IE:

If your expected heading is 180 degrees, but you are going 160 degrees, drop the speed for the side needed proportional the error amount. For example, drop the speed by 20 creating speeds of 255, 235. You will probably need to work with your multiple to find a smooth offset amount - IE you may want 255, 245 (1 per 2 deg) or 255, 205 (2 per 1 deg) or some formula. This would be the P part of a PID logic.

Thanks, I was using the 50 as a test, my code had it at 5 before.

Earlier this evening I started a fresh project file focussed on straight line driving using the PID library with the two motors and compass heading. I found that I had to use DIRECT for one and REVERSE for the other as they needed to react the opposite depending on whether I was above or below the desired compass heading. Also I found that the outputs ran from 0-255 and started at 0 so I've inverted these (255-output), that gets things running but I think it needs to be much more aggressive to keep track of the fast heading changes. I'm using the tuning values from the example code and reading up now on how to tune, I'll post some code if I get anything working (once I've cleaned it up for public consumption).

With regards to adjusting speed offset based on how far out I am from my desired heading, I had thought of this but haven't implemented it yet, previous to the PID test code, I wrote a heading helper function which basically returned 1 of 5 modes depending on how far out I was, 1 = within tolerance (tolerance is passed in), 2 = within 15 degrees CW, 3 = within 15 degrees CCW, 4 = outside of 15 degrees CW and 5 = outside of 15 degrees CCW. To aid the startup process and debugging, I always initialise the desired heading with the heading read from the compass, so basically it shoots off forwards and then tries to keep on the heading which it was placed on.

This is a great hobby for these winter nights. Its strangely satisfying sorting something as making a bot drive in a straight line using maths.

Thanks for your help guys. I'll be back with more later :)


ok, so a couple of hours later and some PID tuning (trial and error), I have it going in a straight line. The only problem now is that the longest run I have is in my kitchen/diner and the fridge gives off a magnetic field and it goes straight down the room until it gets within 1 metre of the fridge!

This was a fun POC but in practice I don't think compass led steering is the way forwards! At least I've learnt how PID logic works (well the basics anyway).

I'll tidy it up and post in case it helps anyone else, it'll be a basic 2 motor with compass and a PID loop.

I'll integrate the code into my larger project tomorrow too and see how it works when issuing new headings after the sonar detects a collision. I may be able to use the PID logic again to make it turn more and more aggressively the closer it gets to an object to see if I can achieve some smooth turning around obstacles :)

That sounds great. Glad you learned something. Looking forward to seeing your code.

I believe a gyroscope can be used for dead reaconing and would be less influenced by the magnetic field.

The only problem now is that the longest run I have is in my kitchen/diner and the fridge gives off a magnetic field and it goes straight down the room until it gets within 1 metre of the fridge!

It's probably going for a beer. I would. 8)

Just finished tidying the code up.

The compass code came from various sources but the rest is all original.

#include <Wire.h>
#include <PID_v1.h>

//Motor pin definitions and topspeed
int dirA = 12;
int dirB = 13;
int speedPinA = 3;
int speedPinB = 11;
int topSpeed = 100;

//Compass Variables
int HMC6352SlaveAddress = 0x42;
int HMC6352ReadAddress = 0x41;

//Used to record interval between ramping speed up
long speedTimer = 0;

//Define Variables we'll be connecting to
double Setpoint, Input, OutputA, OutputB;

//Create the PID objects and pass pointers to the working variables and set tuning parameters, one for each motor
PID motorAPID(&Input, &OutputA, &Setpoint,5,0.01,0.25, DIRECT);
PID motorBPID(&Input, &OutputB, &Setpoint,5,0.01,0.25, DIRECT);

void updateCompass() {
	//Setup and read from compass
	Wire.requestFrom(HMC6352SlaveAddress, 2);

	//The heading output data will be the value in tenths of degrees
	//from zero to 3599 and provided in binary format over the two bytes."
	byte MSB = Wire.read();
	byte LSB = Wire.read();
	float headingSum = (MSB << 8) + LSB; //(MSB / LSB sum)
	float headingInt = headingSum / 10;

	//Put the heading into the Input variable which is used for PID calculation

void setup()
	//Set the mode for the PIC and set A to reverse and B to DIRECT, this makes each PID work on each side of centre
	//ie. one motor is slowed when we're left of desired heading and the other when we're right so the PIDs have 
	//to operate opposite to each other

	//Setup Compass and initialise the wire class
	HMC6352SlaveAddress = HMC6352SlaveAddress >> 1;

	//Setup motor power, speed and direction
	pinMode (dirA, OUTPUT);
	pinMode (dirB, OUTPUT);
	pinMode (speedPinA, OUTPUT);
	pinMode (speedPinB, OUTPUT);
	digitalWrite (dirA, HIGH);
	digitalWrite (dirB, LOW);

	//Get the compass heading at startup and set the Setpoint (desired heading), to this value, this will insure
	//that the robot will drive in the direction it was switched on at.

void loop()
	//Update compass and compute PID values
	//Write the PWM using the Output values from the two PID classes
	//The outputs from the PID are 0 when heading is achieved and 255 when it is not,
	//therefore the values need deducting from the top speed to get the desired motor speeds
	analogWrite(speedPinA,topSpeed - OutputA);
	analogWrite(speedPinB,topSpeed - OutputB);
	//This basically increments the topspeed variable from its initial value to 255 over a period of time to ensure
	//a smooth motor startup as I found the differences between physical motor startup speeds caused it to start
	//one motor first which causes it to immediately lose heading.
	if ((speedTimer + 15 < millis()) && topSpeed < 255) {topSpeed=topSpeed+1; speedTimer=millis();}