Arduino Balance Bot Code

gistfile1.txt

/*
Coded by Santiago Jaramillo 2012

Connection setup:

 === ITG-3200 ===
  VDD  ->  3.3V
  VIO  ->  3.3V
  GND  ->  GND
  SDA  ->  A4
  SCL  ->  A5

 === ADXL335 ===
  VCC  ->  3.3V
  GND  ->  GND
   X   ->  A0
   Y   ->  A1
   Z   ->  A2

Sketch is set up to respond to rotation about the x-axis
*/

#include <Wire.h>
#include <Servo.h>

/*
=======================
 General configuration
=======================
*/
const float WT_ACCEL = 0.03; // weight of the accelerometer in inclination calculation
const float WT_GYRO = 0.97; // weight of the gyroscope in inclination calculation

const int K_P = 50; // proportional coefficient
const int K_D = 120; // derivative coefficient

const int LOOP_DELAY = 10; // For gyro output rate of 100hz, delay = 10ms.

/*
==================
 ITG-3200 config
==================
*/
const char itgAddress = 0x69;

const char WHO_AM_I = 0x00;
const char SMPLRT_DIV= 0x15;
const char DLPF_FS = 0x16;
const char GYRO_XOUT_H = 0x1D;
const char GYRO_XOUT_L = 0x1E;
const char GYRO_YOUT_H = 0x1F;
const char GYRO_YOUT_L = 0x20;
const char GYRO_ZOUT_H = 0x21;
const char GYRO_ZOUT_L = 0x22;

const char DLPF_CFG_0 = (1<<0);
const char DLPF_CFG_1 = (1<<1);
const char DLPF_CFG_2 = (1<<2);
const char DLPF_FS_SEL_0 = (1<<3);
const char DLPF_FS_SEL_1 = (1<<4);

// empirically determined offsets
const int GYRO_XOFF = 21;
const int GYRO_YOFF = 3;
const int GYRO_ZOFF = 30;

const float GYRO_RAW_TO_DPMS = 1 / 14375.0;

/*
=================
 ADXL335 config
=================
*/
const int ACCEL_X = A0;
const int ACCEL_Y = A1;
const int ACCEL_Z = A2;

const int ACCEL_XOFF = -510; // x-offset
const int ACCEL_YOFF = -490; // y-offset
const int ACCEL_ZOFF = -500; // z-offset


/*
===================
 Sensor functions
===================
*/

void itgWrite(char registr, char data) {
  // I2C helper function
  Wire.beginTransmission(itgAddress);
  Wire.write(registr);
  Wire.write(data);
  Wire.endTransmission();
}

unsigned char itgRead(char registr) {
  // I2C helper function
  unsigned char data;
  
  Wire.beginTransmission(itgAddress);
  Wire.write(registr);
  Wire.endTransmission();
  Wire.beginTransmission(itgAddress);
  Wire.requestFrom(itgAddress, 1);
  //Wait for a response from the I2C device and save it
  if (Wire.available())
    data = Wire.read();
  //We're done getting the data
  Wire.endTransmission();
  
  return data;
}

void itgSample(int gyro[]) {
  // x-axis
  gyro[0] = ((itgRead(GYRO_YOUT_H) << 8) | itgRead(GYRO_YOUT_L)) + GYRO_XOFF;
  // y-axis
  gyro[1] = ((itgRead(GYRO_XOUT_H) << 8) | itgRead(GYRO_XOUT_L)) + GYRO_YOFF;
  // z-axis
  gyro[2] = ((itgRead(GYRO_ZOUT_H) << 8) | itgRead(GYRO_ZOUT_L)) + GYRO_ZOFF;
}

void adxlSample(int accel[]) {
  accel[0] = analogRead(ACCEL_X) + ACCEL_XOFF;
  accel[1] = analogRead(ACCEL_Y) + ACCEL_YOFF;
  accel[2] = analogRead(ACCEL_Z) + ACCEL_ZOFF;
}

void accelToAngle(int accel[], int angle[]) {
  // teh funk
  angle[0] = -(90 + RAD_TO_DEG * atan2(-accel[2], accel[1]));
  angle[1] = 90 + RAD_TO_DEG * atan2(-accel[2], accel[0]);
}

void combineAngle(int accelAngle[], int gyro[], int angle[], int dt) {
  // accel must be in degrees, dt is in milliseconds
  angle[0] = WT_GYRO * (angle[0] + gyro[0] * GYRO_RAW_TO_DPMS * dt) + WT_ACCEL * accelAngle[0];
  angle[1] = WT_GYRO * (angle[1] + gyro[1] * GYRO_RAW_TO_DPMS * dt) + WT_ACCEL * accelAngle[1];
}

/*
===================
 Servo stuff
===================
*/

Servo servoLeft, servoRight;

void setVelocity(int v) {
  if (v > 100)
    v = 100;
  else if (v < -100)
    v = -100;
  
  servoLeft.write(map(-v, -100, 100, 0, 179));
  servoRight.write(map(v, -100, 100, 0, 179));
}

/*
===================
 Main stuff
===================
*/

int gyro[3];
int accel[3];
int accelAngle[2];
int angle[2];

int time, lastTime;

int lastAngle;

void setup() {
  Wire.begin(); // Initialize I2C
  
  //Configure the gyroscope
  //Set the gyroscope scale for the outputs to +/-2000 degrees per second
  itgWrite(DLPF_FS, (DLPF_FS_SEL_0|DLPF_FS_SEL_1|DLPF_CFG_0));
  itgWrite(SMPLRT_DIV, 9); //Set the sample rate to 100 hz
  
  // Attach servos
  servoLeft.attach(10);
  servoRight.attach(11);

  // Get initial measurement
  lastTime = millis();
  itgSample(gyro);
  adxlSample(accel);
  accelToAngle(accel, angle);
}

void loop() {
  // get delta time (in milliseconds)
  time = millis();
  int dt = time - lastTime;
  lastTime = time;
  
  // get inclination data
  itgSample(gyro); // angle difference
  adxlSample(accel); // gravity vector
  accelToAngle(accel, accelAngle); // gravity angle
  combineAngle(accelAngle, gyro, angle, dt); // combine
  
  // use inclination data
  // get total error signal from proportional and derivative parts
  setVelocity( -(K_P * angle[0] + K_D * (angle[0]-lastAngle) / dt) ); // angle[0] is rotation about x-axis
  lastAngle = angle[0]; // store current angle for next derivative calculation
  
  delay(LOOP_DELAY);
}