2WD Platform with compass

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();}	
}