With the use of the various libraries the main code loop is very simple. Once all the objects are initialised, the loop reads the robots angle, computes the PID output and uses that to set the motor speed and direction.

void loop()
{
my3IMU.getYawPitchRoll(ypr);
Input = ypr[IMU_ROLL];
myPID.Compute();
md.setSpeeds(Output,Output);
}

The system worked (ish). As mentioned previously, it took a while to find the PID parameters, and they are still not perfect. However I did get a problem.

The robot would balance, but after a few minutes, both motors would just turn on full. Sometimes one motor. The strange thing was sometimes the other motor would still be managed by the PID algorithm, and would change direction when the robot was tilted back and forth, so it wasn’t as if the Arduino had crashed. I initially thought there was a short circuit or lose connection but I couldn’t find one.

I had trouble in the past with other projects, where the code is large and complex, the Arduino sometimes exhibits strange behaviour. Not quite crashing, but being erroneous. I has a suspicion the device is running out of RAM. The programs either had too many variables, or too many recursive functions. I’ve not researched a way to model programs to estimate their RAM usage, or if there is a way to report RAM usage via the code. I’m sure there’s a way. Let me know if you have any ideas.

So, I just threw hardware at the issue, and upgraded the Arduino with a Mega, which has four times the RAM (8k on the Mega verses 2k on the Uno). This solved the problem immediately. Whereas the robot would “crash” every time on the Uno, the same code worked without any issue on the Mega.

One problem was the Mega though is it’s physically longer, so it sticks out of the side of the robot 😦

The other issue was the SPI and TWI pins on the Mega are on different pins to the Uno. Not a huge problem, but until I update the shield PCB, it means jumper wires.

Full code listing below:

#include <ADXL345.h>
#include <HMC58X3.h>
#include <ITG3200.h>
#include <Wire.h>
#include <EEPROM.h>
#include <FreeIMU.h>
#include <CommunicationUtils.h>
#include <DualVNH5019MotorShield.h>
#include <SPI.h>
#include <PID_v1.h>

//#define DEBUG
#include <DebugUtils.h>

// *** Duemilanova / Uno
// IMU uses I2C - pins A4 (SDA) & A5 (SCL)
// Quad counter uses SPI - pins D11 (MOSI), D12 (MISO) & D13 (SCK)

// *** Mega 2560
// IMU uses I2C - pins 20 (SDA) & 21 (SCL).
// Quad counter uses SPI - pins D50 (MISO), D51 (MOSI) & D52 (SCK)

// IMU constansts
#define IMU_YAW 0
#define IMU_PITCH 1
#define IMU_ROLL 2

#define Ku 20    // where is oscilates
#define Pu 0.4   // period of oscilation
#define Kp 0.6 * Ku
#define Ki (2 * Kp) / Pu
#define Kd (Kp * Pu) / 8

//IMU variables
float ypr[3]; // yaw/pitch/roll
FreeIMU my3IMU = FreeIMU();

//PID variables
double Setpoint, Input, Output;
PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT); //or REVERSE

//MotorDriver
DualVNH5019MotorShield md;

void setup()
 {
  Serial.begin(115200);

  // Initialise Motor Driver:
  Serial.println("#Init Motor Driver");
  md.init();
  delay(50);

  // Initialise Wire Library:
  Serial.println("#Init Wire library");
  Wire.begin();
  delay(50);

  // Initialise SPI:
  Serial.println("#Init SPI library");
  SPI.begin();
  SPI.setDataMode(SPI_MODE0);
  SPI.setBitOrder(MSBFIRST);
  SPI.setClockDivider(SPI_CLOCK_DIV8);
  delay(50);

  // Initialise IMU:
  Serial.println("#Init IMU");
  my3IMU.init();
  delay(50);

  // Get Initial variables:
  Serial.println("#Get first YPR");
  my3IMU.getYawPitchRoll(ypr);

  // Initialise PID:
  Serial.println("#Set initial Input variable");
  Input = ypr[IMU_ROLL];
  Serial.println("#Init IMU Setpoint");
  Setpoint = 0;
  myPID.SetMode(AUTOMATIC);
  myPID.SetOutputLimits(-400,400);

  Serial.println("#Exit Setup");
}

void loop()
{
  my3IMU.getYawPitchRoll(ypr);
  Input = ypr[IMU_ROLL];
  myPID.Compute();
  md.setSpeeds(Output,Output);
  stopIfFault();
}

void stopIfFault()
{
  if (md.getM1Fault())
  {
    Serial.println("#M1 fault");
    while(1);
  }
  if (md.getM2Fault())
  {
    Serial.println("#M2 fault");
    while(1);
  }
}