dIMU NXT Segway program

By request, today I finally got around to cleaning up the program for the dIMU Segway. It is based on the HTWay by HiTechnic, but I significantly modified it to work with the dIMU instead of the HiTechnic Gyro, and also to make it work better. There is still a lot of unnecessary stuff in the code (that was used for the HT Gyro), but it works really well for the dIMU the way it is (improved a lot since I took the video of the segway).

#include "dIMU lib.nxc"

//=====================================================================
// Program based on HiTechnic HTWay
// Copyright (c) 2010 HiTechnic
// with modifications by Matthew Richardson
//
// dIMU based Segway robot program. It has been tested
// with Enhanced LEGO NXT firmware 1.31 (by John Hansen).

// Port Input and Output
#define dIMU         IN_1
#define IR_RECEIVER  IN_4

#define LEFT_MOTOR   OUT_C
#define RIGHT_MOTOR  OUT_A
#define MOTORS       OUT_AC

#define AXIS_USED    1 //x=0, y=1, z=2

//=====================================================================
// Balancing constants
//
// These are the constants used to maintain balance.
//
// Loop wait time.  WAIT_TIME is the time in ms passed to the
// Wait command.
#define WAIT_TIME    0

// Size of wheel / 56 = WHEEL_RATIO
// Wheel ratio is relative size compared to NXT 1.0 wheels
// For the NXT 1.0 wheels (56x26) use:           1.0
// For the NXT 2.0 wheels (43.2x22) use:         0.7
// For the big white RCX wheels (81.6x15) use:   1.4
//                       X large 94.8
#define WHEEL_RATIO_NXT1    1.0                                   //1.0
#define WHEEL_RATIO_NXT2    0.77142857142857142857142857142857    //0.8
#define WHEEL_RATIO_RCX     1.4571428571428571428571428571429     //1.4
#define WHEEL_RATIO_X_LARGE 1.6928571428571428571428571428571     //1.5

// These are the main four balance constants, only the gyro
// constants are relative to the wheel size.  KPOS and KSPEED
// are self-relative to the wheel size.
float KGYROANGLE = 14.0;  //Defaults, but can be adjusted by the PF remote
float KGYROSPEED = 1.35;  //       ''
#define KPOS        0.07
#define KSPEED      0.1

// This constant aids in drive control.  When the robot starts
// moving because of user control, this constant helps get the
// robot leaning in the right direction.  Similarly, it helps
// bring robot to a stop when stopping.
#define KDRIVE      -0.02

// Power differential used for steering based on difference of
// target steering and actual motor difference.
#define KSTEER      0.25

// Gyro offset control
// The gyro sensor will drift with time.  This constant is used in a
// simple long term averaging to adjust for this drift.
// Every time through the loop, the current gyro sensor value is
// averaged into the gyro offset weighted according to this constant.
#define EMAOFFSET 0.0005

// If robot power is saturated (over +/- 100) for over this time limit
// then robot must have fallen.  In milliseconds.
#define TIME_FALL_LIMIT     2000

//---------------------------------------------------------------------
// IR Control constants
#define CONTROL_WAIT 25

// To use a different IR channel, chage these constants
#define IR_LEFT           HT_CH1_A
#define IR_RIGHT          HT_CH1_B

// This constant is in degrees/second for maximum speed.  Note that
// position and speed are measured as the sum of the two motors, in
// other words, 600 would actually be 300 degrees/second for each
// motor.
#define CONTROL_SPEED  600.0

//=====================================================================
// Global variables
//
// These variables are used to control the movement of the robot.  Both
// are in degrees/second.
//
// motorControlDrive is the target speed for the sum of the two motors
// while motorControlSteer is the target change in difference for two motors.
float motorControlDrive = 0.0;
float motorControlSteer = 0.0;

// This global contains the target motor differential, essentially,
// which way the robot should be pointing.  This value is
// updated every time through the balance loop based on motorControlSteer
float motorDiffTarget = 0.0;

// Use float to start start time to force floating point math for
// average integral calculation.
float tCalcStart;

// tInterval is the time, in seconds, for each iteration of the
// balance loop.
float tInterval;

// ratioWheel stores the relative wheel size compared to a standard NXT
// 1.0 wheel.  NXT 2.0 wheel has ratio of 0.7 while large RCX wheel is 1.4
float ratioWheel;

// Gyro globals
float gOffset = 0;     //Start it out at 0, in case I don't set it.
float gAngleGlobal = 0;

// Motor globals
float motorPos = 0;
long mrcSum = 0, mrcSumPrev;
long motorDiff;
long mrcDeltaP3 = 0;
long mrcDeltaP2 = 0;
long mrcDeltaP1 = 0;


//=====================================================================
// void SetWheelRatio()
//
// This function implements a UI so that the user can choose the
// size of the wheels used on the robot.
//
// I expaned the menu and added the ability to use the X Large wheels
// I also set the default wheel size to RCX wheels
void SetWheelRatio()
{
  int nSelect;

  // Start off with RCX size selected
  nSelect = 3;

  do {
    // Display base text of user interface on screen
    ClearScreen();       //HiTechnic-HTWayC
    TextOut(0, LCD_LINE1, "  dIMU Segway   ");
    TextOut(6, LCD_LINE2, "Set wheel size:");
    TextOut(2, LCD_LINE8, "<   [Select]   >");

    // Display current selection
    switch (nSelect) {
    case 1:
      // Small
      CircleOut(50, 32, 6);
      TextOut(7, LCD_LINE7, "Small (NXT 2.0)");
      ratioWheel = WHEEL_RATIO_NXT2;
      break;
    case 2:
      // Medium
      CircleOut(50, 32, 10);
      TextOut(2, LCD_LINE7, "Medium (NXT 1.0)");
      ratioWheel = WHEEL_RATIO_NXT1;
      break;
    case 3:
      // Large
      CircleOut(50, 32, 14);
      TextOut(17, LCD_LINE7, "Large (RCX)");
      ratioWheel = WHEEL_RATIO_RCX;
      break;
    case 4:
      // X Large
      TextOut(29, LCD_LINE7, "X Large");
      CircleOut(50, 31, 16);
      ratioWheel = WHEEL_RATIO_X_LARGE;
      break;
    }

    // Make sure previous button is released, loop until all released
    while(ButtonPressed(BTNLEFT, false) ||
          ButtonPressed(BTNCENTER, false) ||
          ButtonPressed(BTNRIGHT, false));

    // Get button press to navigate menu
    while(true) {
      if (ButtonPressed(BTNLEFT, false)) {
        nSelect--;
        if (nSelect < 1) nSelect = 4;
        break;
      }
      if (ButtonPressed(BTNRIGHT, false)) {
        nSelect++;
        if (nSelect > 4) nSelect = 1;
        break;
      }
      if (ButtonPressed(BTNCENTER, false))
        break;
    }
  } until (ButtonPressed(BTNCENTER, false));
  ClearScreen();
}

//=====================================================================
// GetGyroOffset
//
// This function returns a suitable initial gyro offset.  It takes
// 100 gyro samples over a time of 1/2 second and averages them to
// get the offset.  It also check the max and min during that time
// and if the difference is larger than the threshold (I set it to 5)
// it rejects the data and gets another set of samples.
#define OFFSET_SAMPLES 100

void GetGyroOffset()
{
  float gSum;
  int  i, gMin, gMax, g;

  ClearScreen();
  TextOut(0, LCD_LINE1, "  dIMU Segway   ");

  TextOut(0, LCD_LINE4, "Lay robot down");
  TextOut(0, LCD_LINE5, "flat to get gyro");
  TextOut(0, LCD_LINE6, "offset.");
  do {
    gSum = 0.0;
    gMin = 1000;
    gMax = -1000;
    for (i=0; i<OFFSET_SAMPLES; i++) {
      g = dIMU_Gyro_Read_Axis(dIMU, AXIS_USED);

      if (g > gMax)
        gMax = g;
      if (g < gMin)
        gMin = g;

      gSum += g;
      Wait(5);
    }
  } while ((gMax - gMin) > 5);   // Reject and sample again if range too large

  //Average the sum of the samples.  The -1 is an adjustment to
  //anticipate a known shift in gyro values once the motors are active.
  gOffset = gSum / OFFSET_SAMPLES - 1;
}

//=====================================================================
void StartBeeps()
{
  int c;

  ClearScreen();
  TextOut(0, LCD_LINE1, "  dIMU Segway   ");

  TextOut(20, LCD_LINE3, "Balance in");

  // Play warning beep sequence to indicate balance about to start
  for (c=5; c>0;c--) {
    NumOut(47, LCD_LINE4, c);
    PlayTone(440,100);
    Wait(1000);
  }
  NumOut(47, LCD_LINE4, 0);
  PlayTone(440,1000);
  Wait(1000);
}

//=====================================================================
// task main - initializes the sensors, Both taskBalance and
// taskController start when main ends.
task main()
{
//0x00 - 250 dps.  Full scale range.
//0x10 - 500 dps.  Full scale range.
//0x30 - 2000 dps.  Full scale range.
  // Initialize the sensors
  SetSensorLowspeed(dIMU);
  dIMU_Gyro_Init(dIMU, 0x00);
  SetSensorLowspeed(IR_RECEIVER);
  Wait(50);

  // Get user input on wheel size
  SetWheelRatio();

  // Get the initial gyro offset
  //GetGyroOffset();  //I don't need this with the dIMU

  // Play warning beep sequence before balance starts
  StartBeeps();

  // When task main ends, both taskBalance and taskControl will start
}

//---------------------------------------------------------------------

inline void GetMotorData(float &motorSpeed, float &motorPos)
{
  long mrcLeft, mrcRight, mrcDelta;

  // Keep track of motor position and speed
  mrcLeft = MotorRotationCount(LEFT_MOTOR);
  mrcRight = MotorRotationCount(RIGHT_MOTOR);

  // Maintain previus mrcSum so that delta can be calculated and get
  // new mrcSum and Diff values
  mrcSumPrev = mrcSum;
  mrcSum = mrcLeft + mrcRight;
  motorDiff = mrcLeft - mrcRight;

  // mrcDetla is the change int sum of the motor encoders, update
  // motorPos based on this detla
  mrcDelta = mrcSum - mrcSumPrev;
  motorPos += mrcDelta;

  // motorSpeed is based on the average of the last four delta's.
  motorSpeed = (mrcDelta+mrcDeltaP1+mrcDeltaP2+mrcDeltaP3)/(4*tInterval);

  // Shift the latest mrcDelta into the previous three saved delta values
  mrcDeltaP3 = mrcDeltaP2;
  mrcDeltaP2 = mrcDeltaP1;
  mrcDeltaP1 = mrcDelta;
}

//---------------------------------------------------------------------
// void SteerControl(int power, int &powerLeft, int &powerRight)
//
// This function determines the left and right motor power that should
// be used based on the balance power and the steering control
inline void SteerControl(int power, int &powerLeft, int &powerRight)
{
  int powerSteer;

  // Update the target motor difference based on the user steering
  // control value.
  motorDiffTarget += motorControlSteer * tInterval;

  // Determine the proportionate power differential to be used based
  // on the difference between the target motor difference and the
  // actual motor difference.
  powerSteer = KSTEER * (motorDiffTarget - motorDiff);

  // Apply the power steering value with the main power value to
  // get the left and right power values.
  powerLeft = power + powerSteer;
  powerRight = power - powerSteer;

  // Limit the power to motor power range -100 to 100
  if (powerLeft > 100)   powerLeft = 100;
  if (powerLeft < -100)  powerLeft = -100;

  // Limit the power to motor power range -100 to 100
  if (powerRight > 100)  powerRight = 100;
  if (powerRight < -100) powerRight = -100;
}

//---------------------------------------------------------------------
// void CalcInterval(long cLoop)
//
// Calculate the interval time from one iteration of the loop to the next.
// Note that first time through, cLoop is 0, and has not gone through
// the body of the loop yet.  Use it to save the start time.
// After the first iteration, take the average time and convert it to
// seconds for use as interval time.
inline void CalcInterval(long cLoop)
{
  if (cLoop == 0) {
    // First time through, set an initial tInterval time and
    // record start time
    tInterval = 0.0055;
    tCalcStart = CurrentTick();
  } else {
    // Take average of number of times through the loop and
    // use for interval time.
    tInterval = (CurrentTick() - tCalcStart)/(cLoop*1000);
  }
}

//---------------------------------------------------------------------
// taskBalance
// This is the main balance task for the HTWay robot.
//
// Robot is assumed to start leaning on a wall.  The first thing it
// does is take multiple samples of the gyro sensor to establish and
// initial gyro offset.
//
// After an initial gyro offset is established, the robot backs up
// against the wall until it falls forward, when it detects the
// forward fall, it start the balance loop.
//
// The main state variables are:
// gyroAngle  This is the angle of the robot, it is the results of
//            integrating on the gyro value.
//            Units: degrees
// gyroSpeed  The value from the Gyro Sensor after offset subtracted
//            Units: degrees/second
// motorPos   This is the motor position used for balancing.
//            Note that this variable has two sources of input:
//             Change in motor position based on the sum of
//             MotorRotationCount of the two motors,
//            and,
//             forced movement based on user driving the robot.
//            Units: degrees (sum of the two motors)
// motorSpeed This is the speed of the wheels of the robot based on the
//            motor encoders.
//            Units: degrees/second (sum of the two motors)
//
// From these state variables, the power to the motors is determined
// by this linear equation:
//     power = KGYROSPEED * gyro +
//             KGYROANGLE * gyroAngle +
//             KPOS       * motorPos +
//             KSPEED     * motorSpeed;
//
task taskBalance()
{
  Follows(main);

  float gyroRaw;
  float gyroSpeed, gyroAngle;
  float motorSpeed;

  int power, powerLeft, powerRight;
  long tMotorPosOK;
  long cLoop = 0;

  ClearScreen();
  TextOut(0, LCD_LINE1, "  dIMU Segway   ");
  TextOut(0, LCD_LINE4, "  Balancing     ");

  tMotorPosOK = CurrentTick();

  // Reset the motors to make sure we start at a zero position
  ResetRotationCount(LEFT_MOTOR);
  ResetRotationCount(RIGHT_MOTOR);

  while(true) {
    gyroRaw = dIMU_Gyro_Read_Axis(dIMU, AXIS_USED);
  
    CalcInterval(cLoop++);
    

    gOffset = EMAOFFSET * gyroRaw + (1-EMAOFFSET) * gOffset;
    gyroSpeed = gyroRaw - gOffset;
    gyroAngle += gyroSpeed*tInterval;

    GetMotorData(motorSpeed, motorPos);

    // Apply the drive control value to the motor position to get robot
    // to move.
    motorPos -= motorControlDrive * tInterval;

     // This is the main balancing equation
     power = (KGYROSPEED * gyroSpeed +               // Deg/Sec from Gyro sensor
              KGYROANGLE * gyroAngle) / ratioWheel + // Deg from integral of gyro
             KPOS       * motorPos +                 // From MotorRotaionCount of both motors
             KDRIVE     * motorControlDrive +        // To improve start/stop performance
             KSPEED     * motorSpeed;                // Motor speed in Deg/Sec

     if (abs(power) < 100)
       tMotorPosOK = CurrentTick();

     SteerControl(power, powerLeft, powerRight);

     // Apply the power values to the motors
     OnFwd(LEFT_MOTOR, powerLeft);
     OnFwd(RIGHT_MOTOR, powerRight);

     // Check if robot has fallen by detecting that motorPos is being limited
     // for an extended amount of time.
     if ((CurrentTick() - tMotorPosOK) > TIME_FALL_LIMIT) {
       break;
     }

     Wait(WAIT_TIME);
  }
  Off(MOTORS);

  TextOut(0, LCD_LINE4, "Oops... I fell");

  TextOut(0, LCD_LINE8, "tInt ms:");
  NumOut(6*8, LCD_LINE8, tInterval*1000);
}


//The array that will hold the data from the IR Receiver
char pfData[8];


//Waits until you are not giving any input through the remote to continue
void WaitUntilNoInput(){
  until((pfData[HT_CH2_A]== -128 && pfData[HT_CH2_B] == -128)||(pfData[HT_CH2_A]== 0 && pfData[HT_CH2_B] == 0)){
    ReadSensorHTIRReceiver(IR_RECEIVER, pfData);
    Wait(10);
  }
}


//=====================================================================
// taskControl
// This task runs in parallel to taskBalance.  This task monitors
// the IR Receiver and sets the global variables motorControlDrive
// and motorControlSteer.  Both of these values are in degrees/second.
//
task taskControl()
{
  Follows(main);

  while(true) {

    KeepAliveType kaArgs;
    SysKeepAlive(kaArgs);  //Don't let the NXT turn off because of a timeout
    
    ReadSensorHTIRReceiver(IR_RECEIVER, pfData);
    
    if (pfData[HT_CH2_A] == 100){           // These are to adjust the balancing Konstants.
      KGYROANGLE++;
      WaitUntilNoInput();
    }
    else{
      if (pfData[HT_CH2_A] == -100){
        KGYROANGLE--;
        WaitUntilNoInput();
      }
    }
    
    if (pfData[HT_CH2_B] == 100){
      KGYROSPEED = KGYROSPEED + 0.1;
      WaitUntilNoInput();
    }
    else{
      if (pfData[HT_CH2_B] == -100){
        KGYROSPEED = KGYROSPEED - 0.1;
        WaitUntilNoInput();
      }
    }
    TextOut(0, LCD_LINE5, "Offset");
    TextOut(0, LCD_LINE6, "kp        ");
    TextOut(0, LCD_LINE7, "ki        ");
    NumOut(42, LCD_LINE5, gOffset);
    NumOut(18, LCD_LINE6, KGYROANGLE);
    NumOut(18, LCD_LINE7, KGYROSPEED);
    
    if (pfData[IR_LEFT] == -128) pfData[IR_LEFT] = 0;
    if (pfData[IR_RIGHT] == -128) pfData[IR_RIGHT] = 0;

    // Set control Drive and Steer.  These are in motor degree/second
    motorControlDrive = (pfData[IR_LEFT] + pfData[IR_RIGHT]) * CONTROL_SPEED / 200.0;
    motorControlSteer = (pfData[IR_LEFT] - pfData[IR_RIGHT]) * CONTROL_SPEED / 200.0;

    Wait(CONTROL_WAIT);
  }
}

You will need this library in order to use the program. Note that I am still developing the library, and so many comments are not right, and names may change by the time I post it again elsewhere.

// By Matthew Richardson
// Based in part on several programs by Dexter Industries

// These bytes set the full scale range of the gyroscope.
// it is important to define full_scale_range.  Values are below.
//0x00 - 250 dps.  Full scale range.
//0x10 - 500 dps.  Full scale range.
//0x30 - 2000 dps.  Full scale range.
#define full_scale_range 0x00

#define dIMU_Accel_Addr 0x3A

// G_Mode not tested yet. Not commented yet. FIXME
void dIMU_Accel_Init(byte port, byte G_Mode = 2){
  byte I2Csnd[3];
  I2Csnd[0] = dIMU_Accel_Addr;  // I2C Address of Accelerometer.

  //Set the Mode Control - P.25 of Documentation
  ////////////////////////////////////////////////////////////////////////////
    I2Csnd[1] = 0x16;                   // Register address of Mode Control
    I2Csnd[2] = 0x05;                   // 0x05 For 2G
    if(G_Mode == 0x01) I2Csnd[2] = 0x09; // 0x09 For 4G
    if(G_Mode == 0x02) I2Csnd[2] = 0x01; // 0x01 For 8G

  byte I2Crec[1];           // We are looking for a single byte returned.
  byte cnt = 1;             // cnt must be a variable, not a constant
  I2CBytes(port, I2Csnd, cnt, I2Crec);
}

void dIMU_Accel_Read(byte port, float & Accel_X, float & Accel_Y, float & Accel_Z){

  byte I2Csnd[2];              // Sending buffer
  I2Csnd[0] = dIMU_Accel_Addr; // I2C Address of accl.
  I2Csnd[1] = 0x00;            // First Register of the data we're requesting.
  byte cnt = 6;                // Number of bytes to request
  byte I2Crec[6];              // Receiving buffer

  I2CBytes(port, I2Csnd, cnt, I2Crec);    //Send the address and the register, and receive the 6 bytes of data.

  Accel_X = I2Crec[1] * 256 + I2Crec[0];  // Convert the returned array into floating point G values
  Accel_Y = I2Crec[3] * 256 + I2Crec[2];  //                ''
  Accel_Z = I2Crec[5] * 256 + I2Crec[4];  //                ''

  if(Accel_X > 512)Accel_X -= 1023;       // Convert into a signed value
  if(Accel_Y > 512)Accel_Y -= 1023;       //                ''
  if(Accel_Z > 512)Accel_Z -= 1023;       //                ''

  Accel_X /= 64;
  Accel_Y /= 64;
  Accel_Z /= 64;

  return;
}

void dIMU_Accel_Write_Calibration(byte port, int x_offset, int y_offset, int z_offset){

  byte I2Csnd[8];
  I2Csnd[0] = dIMU_Accel_Addr; // I2C Address of accel.
  I2Csnd[1] = 0x10;            // Register of the data we're writing.
  I2Csnd[2] = x_offset;         // The drift value to write for calibration.
  I2Csnd[3] = x_offset>>8;      //                 ''
  I2Csnd[4] = y_offset;         //                 ''
  I2Csnd[5] = y_offset>>8;      //                 ''
  I2Csnd[6] = z_offset;         //                 ''
  I2Csnd[7] = z_offset>>8;      //                 ''

  byte I2Crec[1];              // We are looking for a single byte returned.
  byte cnt = 1;                // cnt must be a variable, not a constant
  I2CBytes(port, I2Csnd, cnt, I2Crec);   //Send the address, register, and 6 bytes of calibration data.
}

// Not commented, or entirely tested yet. I also should implement an average of n readings. FIXME
// This assumes that the z axis is placed flat.  When sensor is flat on the ground
// the x and y axis should be zero.
// There may be a better way to calibrate this.  But it's the most user-friendly.
// Gravity direction is 1 (x), 2 (y), 3 (z) and 0 (off).  This is the field direction that gravity is headed.

void dIMU_Accel_Calibrate(byte port, int gravity_direction = 3){

  dIMU_Accel_Write_Calibration(port, 0, 0, 0);    // Clear all of the calibration registers.

  int x_offset;
  int y_offset;
  int z_offset;

  byte I2Csnd[2];              // Sending buffer
  I2Csnd[0] = dIMU_Accel_Addr; // I2C Address of accel.
  I2Csnd[1] = 0x00;            // First Register of the data we're requesting.
  byte cnt = 6;                // Number of bytes to request
  byte I2Crec[6];              // Receiving buffer

  I2CBytes(port, I2Csnd, cnt, I2Crec);     //Send the address and the register, and receive the 6 bytes of data.

  x_offset = I2Crec[1] * 256 + I2Crec[0];  // Convert the returned array into floating point G values
  y_offset = I2Crec[3] * 256 + I2Crec[2];  //                ''
  z_offset = I2Crec[5] * 256 + I2Crec[4];  //                ''
  if(x_offset > 512)x_offset -= 1023;      // Convert into a signed value
  if(y_offset > 512)y_offset -= 1023;      //                ''
  if(z_offset > 512)z_offset -= 1023;      //                ''

  // We will calibrate depending on the axis that's inline with the gravity field.
  // Positive 1G is 64 decimal, or 0x40 Hex, so we need to subtract that from the proper axis.
  switch(gravity_direction){

   case 1: x_offset -= 0x40;   // Subtract out the 1-g that the x-axis is experiencing.
    break;

   case 2: y_offset -= 0x40;   // Subtract out the 1-g that the y-axis is experiencing.
    break;

   default: z_offset -= 0x40;  // Subtract out the 1-g that the z-axis is experiencing.
    break;
  }

  x_offset *= (-2);    // Treated normally here and just zero it out.
  y_offset *= (-2);    // Treated normally here and just zero it out.
  z_offset *= (-2);    // Treated normally here and just zero it out.

  dIMU_Accel_Write_Calibration(port, x_offset, y_offset, z_offset);
}

int divisor = 128;      // This will be the divisor we divide the raw value of the gryscope by
                        // to get a scaled value on output.  Default will be for 250 dps,
                        // but we'll define it again in start_gyro.

void dIMU_Gyro_Init(byte port, byte range){
  byte I2Csnd[3];        // We are writing 3 bytes at a time, and...
  byte I2Crec[1];        // ...we are looking for a single byte returned.
  I2Csnd[0] = 0xD2;      // I2C Address of gyro.

  //Write CTRL_REG1
  ////////////////////////////////////////////////////////////////////////////
    I2Csnd[1] = 0x20;      // Register address of CTRL_REG1
    I2Csnd[2] = 0x0F;      // Enable all axes. Disable power down.
    //I2CBytes(port, I2Csnd, cnt, I2Crec);
    
    I2CWrite(port, 1, I2Csnd);
    Wait(10);
    I2CRead(port, 1, I2Crec);

  //Write CTRL_REG2
  ////////////////////////////////////////////////////////////////////////////
    I2Csnd[1] = 0x21;                  // Register address of CTRL_REG2
    I2Csnd[2] = 0x00;                  // No High Pass Filter
    I2CWrite(port, 1, I2Csnd);
    Wait(10);
    I2CRead(port, 1, I2Crec);

  //Write CTRL_REG3
  ////////////////////////////////////////////////////////////////////////////
    I2Csnd[1] = 0x22;      // Register address of CTRL_REG3
    I2Csnd[2] = 0x08;      // No interrupts.  Date ready.
    I2CWrite(port, 1, I2Csnd);
    Wait(10);
    I2CRead(port, 1, I2Crec);

  //Write CTRL_REG4
  ////////////////////////////////////////////////////////////////////////////
    I2Csnd[1] = 0x23;      // Register address of CTRL_REG4
    I2Csnd[2] = range;     // Full scale range.
    I2CWrite(port, 1, I2Csnd);
    Wait(10);
    I2CRead(port, 1, I2Crec);

  //Write CTRL_REG5
  ////////////////////////////////////////////////////////////////////////////
    I2Csnd[1] = 0x24;      // Register address of CTRL_REG5
    I2Csnd[2] = 0x00;      // Enable all axes. Disable power down.
    I2CWrite(port, 1, I2Csnd);
    Wait(10);
    I2CRead(port, 1, I2Crec);

   // Set divisor so that the output of our gyro axis readings can be turned
   // into scaled values.
   ///////////////////////////////////////////////////////////////////////////
    if(full_scale_range == 0) divisor = 128;      // Full scale range is 250 dps.
    if(full_scale_range == 0x10) divisor = 64;    // Full scale range is 500 dps.
    if(full_scale_range == 0x30) divisor = 16;    // Full scale range is 2000 dps.
}

// Gyro: axis_reading gets a byte of axis reading data
//
byte dIMU_Gyro_Read_Byte(byte port, byte reg){
  byte I2Csnd[2];
  I2Csnd[0] = 0xD2;   // I2C Address of gyro.
  I2Csnd[1] = reg;    // Register of the data we're requesting.

  byte I2Crec[1];    // We are looking for a single byte returned.

  I2CWrite(port, 1, I2Csnd);
  Wait(10);
  I2CRead(port, 1, I2Crec);

  byte result = I2Crec[0];
  return result;
}

// Gyro: gets a full axis reading, scaled to the full scale reading.  Returns
// float in degrees per second.

float dIMU_Gyro_Read_Axis(byte port, byte axis){
  // ubyte axis definitions
  // 0x00 - x-axis
  // 0x01 - y-axis
  // 0x02 - z-axis

  byte hb = 0;
  byte lb = 0;

  // We will set the value of divisor.  This is important to get a scaled output in dps.
  int divisor = 128;
  if(full_scale_range == 0x10) divisor = 64;
  if(full_scale_range == 0x30) divisor = 16;

  if(axis == 0x00) {               // x-axis
    hb = dIMU_Gyro_Read_Byte(port, 0x29);
    lb = dIMU_Gyro_Read_Byte(port, 0x28);
  }
  if(axis == 0x01) {               // y-axis
    hb = dIMU_Gyro_Read_Byte(port, 0x2B);
    lb = dIMU_Gyro_Read_Byte(port, 0x2A);
  }
  if(axis == 0x02) {               // z-axis
    hb = dIMU_Gyro_Read_Byte(port, 0x2D);
    lb = dIMU_Gyro_Read_Byte(port, 0x2C);
  }
  int RAW_val = (lb+(hb<<8));
  float val = RAW_val/divisor;  // Assemble the final number by assembling the two bytes,
                                             // and dividing it by the divisor (defined in the gyro startup,
                                             // to get a properly scaled long.
  return val;
}

Things you should know, to use the program:

– Based on the #include preprocessor in the program, the library must be saved as “dIMU lib.nxc”.

– To use the dIMU segway program, you need to save both of these programs in the same directory (or point the compiler to the directory with the library).

– You can choose whatever ports are most convenient for the sensors and motors, you just need to change the #define preprocessors at the beginning of the main program.

– In the program I am using the Y axis of the gyro to balance, but if you prefer a different axis (for mechanical reasons), you can change that as well.

– You can use NXT 1.0, 2.0, RCX, or extra large wheels (43.2, 56, 81.6, or 94.8). Mechanically anything bigger than RCX wheels (81.6) are a bit of a challenge to use. Because of the large diameter, the axles twist a lot, and the segway oscillates really badly (almost to the point of being unable to balance). The smaller the wheels, the more steady and smooth (especially on a smooth surface), but also the more likely to cause the robot to fall from something like getting bumped or driving over uneven surfaces. You can use just about any size of wheel, you will just need to program it to run with them. To calculate the WHEEL_RATIO, take the size of the wheel and divide by 56.

– When the program is running, you can adjust a couple of the balancing Konstants using channel 2 of the PF remote. The values are displayed on the NXT screen for your reference.

Advertisements
This entry was posted in Dexter Industries, Drivers, Mindstorms, NXC, NXT and tagged , , , , , . Bookmark the permalink.

8 Responses to dIMU NXT Segway program

  1. Pingback: NXShield dIMU Segway | mattallen37

  2. sikko says:

    Thanks for the program.
    but can i download it to?
    i dont use or have nxc, i just would like to run my segway. so could you mayby give my a program that i just can dowload to m nxt and run?
    or the place where i can download nxc?

    thanks in advance

    • mattallen37 says:

      Here is the BCC home page, and here is a link to the official release downloads page. If you would prefer a pre-compiled program (.rxe file), I can email it to you, but you probably also need to be running enhanced FW (included with BCC).

  3. sikko says:

    Thank you for the lniks, and i would like ik when you e-mail my the rxe file

  4. Pingback: Segway Roundup with the dIMU | Dexter Industries Blog

  5. James Summers (McSummation) says:

    Have you considered using the accelerometer to get the robot initially upright?

    • mattallen37 says:

      I have, but it’s unnecessary. The slight offset from vertical by standing it up manually is almost instantly corrected when it starts to balance. Furthermore, you’d have to correct for any physical imbalance (like if the front weighs more than the back).

  6. Pingback: Segway Roundup with the dIMU - Dexter Industries

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s