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.beginTransmission(HMC6352SlaveAddress);
Wire.write(HMC6352ReadAddress);
Wire.endTransmission();
delay(6);
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
Input=headingInt;
}
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
motorAPID.SetMode(AUTOMATIC);
motorBPID.SetMode(AUTOMATIC);
motorAPID.SetControllerDirection(REVERSE);
motorBPID.SetControllerDirection(DIRECT);
//Setup Compass and initialise the wire class
HMC6352SlaveAddress = HMC6352SlaveAddress >> 1;
Wire.begin();
//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.
updateCompass();
Setpoint=Input;
}
void loop()
{
//Update compass and compute PID values
updateCompass();
motorAPID.Compute();
motorBPID.Compute();
//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();}
}