Custom Bots

I’m trying to build a 2 wheeled balancing robot for fun. I have all of the hardware built and put together, and I think I have it coded as well. I’m using an IMU with Gyro and Accelerometers to find my tilt angle with a Complimentary Filter for smoothing the signal. It seems pretty smooth as in less than 0.7 variance + or – the actual tilt angle.

My IMU sampling rate is 50Hz and I do a PID calculation at 50Hz too, which I think should be fast enough.

Basically, I’m using the PID library found at http://playground.arduino.cc//Code/PIDLibrary

And to tune the loop I have 3 rotary encoders with interrupts that I have set initially to 0. I then increase the P value by 0.1 hoping to see some “Oscillation” in the system, but all I’m seeing is my robot falling on the floor. If I set P to a low value, it doesn’t respond fast enough but drives in the right direction. When I set P to a slightly higher value it starts to fall and the wheels go in the OPPOSITE direction fast for a second before driving in the right direction fast and hard which just won’t work.

I will post the code below. I would really love some help on this as I’ve been working on this for 1.5 years and would love to finally be able to show it off to everyone I’ve been telling about it.

    // LSM303 accelerometer: 8 g sensitivity
// 3.8 mg/digit; 1 g = 256
#define GRAVITY 256  //this equivalent to 1G in the raw data coming from the accelerometer 

#define ToRad(x) ((x)*0.01745329252)  // *pi/180
#define ToDeg(x) ((x)*57.2957795131)  // *180/pi

// L3G4200D gyro: 2000 dps full scale
// 70 mdps/digit; 1 dps = 0.07
#define Gyro_Gain_X 0.07 //X axis Gyro gain
#define Gyro_Gain_Y 0.07 //Y axis Gyro gain
#define Gyro_Gain_Z 0.07 //Z axis Gyro gain
#define Gyro_Scaled_X(x) ((x)*ToRad(Gyro_Gain_X)) //Return the scaled ADC raw data of the gyro in radians for second
#define Gyro_Scaled_Y(x) ((x)*ToRad(Gyro_Gain_Y)) //Return the scaled ADC raw data of the gyro in radians for second
#define Gyro_Scaled_Z(x) ((x)*ToRad(Gyro_Gain_Z)) //Return the scaled ADC raw data of the gyro in radians for second

#define Kp_ROLLPITCH 0.02
#define Ki_ROLLPITCH 0.00002

#define NO_PORTC_PINCHANGES            //this is a workaround for ooPinChangeInt to work with SoftwareSerial. disables port C for interrupts

//these are the interrupt pins for the rotary encoders
#define ENCA_a 2
#define ENCA_b 4
#define ENCB_a 6
#define ENCB_b 7
#define ENCC_a 10
#define ENCC_b 12

#define STATUS_LED 13 

#include <Wire.h>
#include <EEPROM.h>
#include <ooPinChangeInt.h>
#include <AdaEncoder.h>
#include <PID_v1.h>
#include <segSoftwareSerial.h>
#include <SabertoothSimplified.h>

int8_t clicks=0;

int SENSOR_SIGN[9] = {1,1,1,-1,-1,-1,1,1,1}; //Correct directions x,y,z - gyro, accelerometer, magnetometer

// tested with Arduino Uno with ATmega328 and Arduino Duemilanove with ATMega168
float maxi;
float mini;

segSoftwareSerial SWSerial(15, 14); // RX on pin 2 (unused), TX on pin 3 (to S1).
SabertoothSimplified ST(SWSerial); // Use SWSerial as the serial port.

//let's define our PID values
float P = 0;
float I = 0;
float D = 0;

int serialIn = 0;

//set-up the rotary encoders
AdaEncoder encoderA = AdaEncoder('a', ENCA_a, ENCA_b);
AdaEncoder encoderB = AdaEncoder('b', ENCB_a, ENCB_b);
AdaEncoder encoderC = AdaEncoder('c', ENCC_a, ENCC_b);

//Define Variables we'll be connecting to
double Setpoint, Input, Output;

//Specify the links and initial tuning parameters
PID myPID(&Input, &Output, &Setpoint, P, I, D, DIRECT);

float G_Dt=0.02;    // Integration time (DCM algorithm)  We will run the integration loop at 50Hz if possible

long timer=0;   //general purpuse timer
long timer_old;
long timer24=0; //Second timer used to print values
int AN[6]; //array that stores the gyro and accelerometer data
int AN_OFFSET[6]={0,0,0,0,0,0}; //Array that stores the Offset of the sensors

int gyro_x;
int gyro_y;
int gyro_z;
int accel_x;
int accel_y;
int accel_z;

float Accel_Vector[3]= {0,0,0}; //Store the acceleration in a vector
float Gyro_Vector[3]= {0,0,0};//Store the gyros turn rate in a vector
float Omega_Vector[3]= {0,0,0}; //Corrected Gyro_Vector data
float Omega_P[3]= {0,0,0};//Omega Proportional correction
float Omega_I[3]= {0,0,0};//Omega Integrator
float Omega[3]= {0,0,0};

// Euler angles
float roll;
float pitch;

float errorRollPitch[3]= {0,0,0}; 

unsigned int counter=0;
byte gyro_sat=0;

float DCM_Matrix[3][3] = {{1,0,0},{0,1,0 },{0,0,1  } }; 

float Update_Matrix[3][3] = {{0,1,2},{3,4,5},{6,7,8}}; //Gyros here

float Temporary_Matrix[3][3] = {{0,0,0 },{0,0,0},{0,0,0 }};

void setup() {
  Serial.begin(19200);
  Serial.setTimeout(0);
  pinMode (STATUS_LED,OUTPUT);  // Status LED

  //set up SWSerial for the motor_ops
  SWSerial.begin(19200);

  I2C_Init();

  digitalWrite(STATUS_LED,LOW);
  delay(1500);

  Accel_Init();
  Gyro_Init();

  delay(20);

  for(int i=0;i<32;i++)    // We take some readings...
    {
    Read_Gyro();
    Read_Accel();
    for(int y=0; y<6; y++)   // Cumulate values
      AN_OFFSET[y] += AN[y];
    delay(20);
    }

  for(int y=0; y < 6; y++)
    AN_OFFSET[y] = AN_OFFSET[y]/32;

  AN_OFFSET[5]-=GRAVITY*SENSOR_SIGN[5];

  delay(2000);
  digitalWrite(STATUS_LED,HIGH);

  timer=millis();
  delay(20);
  counter=0;

  //read the P value from EEPROM
  EEPROM_ReadFloat(&P, 1);

      //initialize the variables we're linked to
    Input = ToDeg( pitch );
    Setpoint = 0.00;

    //turn the PID on
    myPID.SetMode(AUTOMATIC);
    myPID.SetOutputLimits(-127, 127);
    myPID.SetSampleTime( 20 );

}

void loop() { //Main Loop

  if((millis()-timer)>=20) {  // Main loop runs at 50Hz

    counter++;
    timer_old = timer;
    timer=millis();

    if (timer>timer_old)
      G_Dt = (timer-timer_old)/1000.0;    // Real time of loop run. We use this on the DCM algorithm (gyro integration time)
    else
      G_Dt = 0;

    // *** DCM algorithm
    // Data adquisition
    Read_Gyro();   // This read gyro data
    Read_Accel();     // Read I2C accelerometer

    // Calculations...
    Matrix_update();
    Normalize();
    Drift_correction();
    Euler_angles();
    // ***

  //////////
    AdaEncoder *thisEncoder=NULL;

  thisEncoder=AdaEncoder::genie();
  char encoderID = thisEncoder->getID();

  if (thisEncoder != NULL) {
    Serial.print(encoderID); Serial.print(':');
    clicks=thisEncoder->query();
    if (clicks > 0) {
      if(encoderID == 'a')
        P -= 0.1;
      if(encoderID == 'b')
        I -= 0.1;
      if(encoderID == 'c')
        D -= 0.1;

      //write the P value to EEPROM, location 1
      EEPROM_WriteFloat(&P, 1);
    }
    if (clicks < 0) {
       if(encoderID == 'a')
        P += 0.1;
      if(encoderID == 'b')
        I += 0.1;
      if(encoderID == 'c')
        D += 0.1;

      //write the P value to EEPROM, location 1
      EEPROM_WriteFloat(&P, 1);
    }

    //print out the new values of the PID
    Serial.print("P: ");
    Serial.print(P);
    Serial.print(" I: ");
    Serial.print(I);
    Serial.print(" D: ");
    Serial.println(D);
  } 

  /////////////////

     myPID.SetTunings( P, I, D );

    Input = ToDeg( pitch );

    if (maxi < Input)
      maxi = Input;
    if ( mini > Input )
      mini = Input;

    myPID.Compute();

    //if the angle is small enough, power the motor
    if( Input <= 35 && Input >= -35 )
      motor_ops( -Output );
    else
      motor_ops(0);

  }

}

//this is a function to write a float byte by byte to the EEPROM
void EEPROM_WriteFloat(float *num, int MemPos)
{
  byte ByteArray[4];
  memcpy(ByteArray, num, 4);
  for(int x = 0; x < 4; x++)
  {
    EEPROM.write((MemPos * 4) + x, ByteArray[x]);
  }
}

//this is a function to read a float byte by byte from the EEPROM
void EEPROM_ReadFloat(float *num, int MemPos)
{
  byte ByteArray[4];
  for(int x = 0; x < 4; x++)
  {
    ByteArray[x] = EEPROM.read((MemPos * 4) + x);
  }
  memcpy(num, ByteArray, 4);
}