As you are using the Arduino, you should leverage the most from the community libraries. My advice would be to use the PID library.
There is a detailed explanation about how the library works in that page. It is a link to the creators's blog. Also with explanations about how to tune up the Kp, Ki and Kd.
I have a similar project and it turned out to be too imprecise for us to use only a magnetometer. The worst scenario for a robot is to be next to a source of interference ( like a TV or a big metallic structure ) and then move several meters based on the decision taken when turning.
To solve these issues, we used a gyroscope ( L3G4200D in our case ). This is the code that we ended up using:
unsigned long amount_off_target ;
L3G4200D gyroscope;
while (!gyroscope.begin(L3G4200D_SCALE_250DPS, L3G4200D_DATARATE_400HZ_50)) // Set scale 250 dps and 400HZ Output data rate (cut-off 50)
{
Serial.println("Could not find a valid L3G4200D sensor, check wiring!");
return;
}
gyroscope.calibrate(100);
// here it goes some code referred to our encoders
double Setpoint = 0, Input = 0, Output = 0;
const double scale_factor = 7000000.0f;
PID myPID(&Input, &Output, &Setpoint, 9.5 / scale_factor, 4 / scale_factor, 3.5 / scale_factor, DIRECT);
myPID.SetMode(AUTOMATIC);
myPID.SetOutputLimits(-50, 50); // these parameters are related to your particular motor PWM values
int min_power = MN_POWER+35;
long z_total = 0;
Input = 0;
const long target_z = long(float(degs) * (90000000.0f / 90.0f));
unsigned long last_off_target = millis();
unsigned long last_read_sensor = micros();
const unsigned long started_turn = millis();
Setpoint = target_z;
// Grace period to start moving
long last_not_moving = millis() + 50;
int direction;
while ((millis() - last_off_target) < 100L && (millis() - started_turn) < 2000L)
{
// more code about our encoders goes here
const long now_millis = millis();
const long bump_min_speed_frequency = 10;
amount_off_target = abs(z_total - target_z);
if (amount_off_target > 2506516L) { // here you decide that you are on target
last_off_target = millis();
}
const unsigned long read_sensor_time = micros();
Vector norm = gyroscope.readNormalize();
const long time_since_read = (read_sensor_time - last_read_sensor);
last_read_sensor = read_sensor_time;
long yaw = (long)norm.ZAxis;
z_total += yaw * time_since_read;
Input = double(z_total);
myPID.Compute();
if (Output > 0) {
direction = 1;
}
else {
direction = -1;
}
int mtr_pwr = min_power + abs(Output);
motorRight( mtr_pwr, -direction);
motorLeft( mtr_pwr, direction);
}
I hope that this works as a general principle of implementation. The main code structure can be easily understood if you look at this PDF explaining it.
The code was tuned up by a real rockstar coder that worked with our team and it works surprisingly well in many diverse surfaces. You might notice that it overshoots a bit when it's on glass and undershoots when it's on a thick rug.
