NXShield dIMU Segway

This is a robot I built in attempt to make a working NXShield-M/Arduino Mega segway. It turned out better than I expected (it actually balances), although not perfect, as it is a little jittery at times.

In order to use the dIMU and HiTechnic IR Receiver with the Arduino, I needed drivers, but I couldn’t find any on the web. I decided I should just write them. So with a lot of examining of other drivers, testing, and retesting, I successfully developed them. This was a slight challenge for me, as I had never written classes in C++ before (and I had never used classes of any kind before using the NXShield). I knew that a segway needed to go through the control loop fairly fast to keep it’s balance, so I wanted to write the drivers to be as fast as they could be (within reason of course).

I based the Arduino program off the program I made for the dIMU NXT Segway (programmed in NXC, based on the HTWay program). I had to make quite a number of changes to the API names, variable types, debugging feedback (the Arduino and NXShield-M don’t have an LCD, so I used the serial terminal), and most notably the structure of the program. The Arduino can’t multitask like the NXT can, so I had to combine the user control loop with the balance loop. This makes the loop take a little longer, which probably accounts for some of the jitter.

Considering the Arduino is using I2C (some of which is bit-banged) to talk with all the devices, I think it does a fairly good job of keeping itself upright.

Here is the program the Arduino is running:

/*
*  Program based on HiTechnic HTWay
*  Copyright (c) 2010 HiTechnic
*  with modifications by Matthew Richardson
*  matthewrichardson37<at>gmail.com
*  https://mattallen37.wordpress.com/
*  October 31 2011
*
*  You may use this code as you wish, provided you give credit where it's due.
*
*  NXShield dIMU based Segway program.
*/

#include <Wire.h>
#include <NXShield.h>
#include <dIMUGyro.h>
#include <HTIRR.h>

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

//=====================================================================
// 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    15

// 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.457//1428571428571428571428571429     //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 = 18;    //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 int mrcSum = 0, mrcSumPrev;
long int motorDiff;
long int mrcDeltaP3 = 0;
long int mrcDeltaP2 = 0;
long int mrcDeltaP1 = 0;
//
// declare the NXShield(s) attached to your Arduino.
//
NXShield    nxshield;

//
// declare the i2c devices used on NXShield(s).
//
dIMUGyro       dIMU;
HTIRR   HTIRRec;

void setup()
{
  ratioWheel = WHEEL_RATIO_RCX; //Set the wheel ratio (size)
  
  char str[256];

  Serial.begin(115200);  // start serial for output
  delay(500); // wait, allowing time to activate the serial monitor

  Serial.println (" ");
  Serial.println ("Initializing the devices ...");

  //
  // Initialize the protocol for NXShield
  // It is best to use Hardware I2C (unless you want to use Ultrasonic).
  //
  nxshield.init( SH_SoftwareI2C );

  //
  // Initialize the i2c sensors.
  //
  dIMU.init( &nxshield, SH_BBS2);
  dIMU.Init(DIMU_GYRO_RANGE_250, 0);
  
  HTIRRec.init( &nxshield, SH_BAS2 );

  nxshield.bank_b.motorReset();
  nxshield.bank_b.writeByte(0x7a, 0);
  nxshield.bank_b.writeByte(0x7b, 0);
  
  //
  // Wait until user presses GO button to continue the program
  //
    
  Serial.println ("Press GO button to continue");
	nxshield.waitForButtonPress(BTN_GO);

  FlashLEDs();
}

void FlashLEDs(){
  for(byte i = 5; i; i--){
    nxshield.ledSetRGB(8,8,8);
    delay(250);
    nxshield.ledSetRGB(0,0,0);
    delay(250);
  }
  nxshield.ledSetRGB(8,8,8);
  delay(500);
  nxshield.ledSetRGB(0,0,0);
}

inline void GetMotorData(float &motorSpeed, float &motorPos)
{
  long int mrcLeft, mrcRight, mrcDelta;  //not sure if I need the int FIXME

  // Keep track of motor position and speed
  
//  nxshield.bank_b.motorGetEncoderPositionBoth(mrcRight, mrcLeft);
  mrcRight = -nxshield.bank_b.motorGetEncoderPosition(SH_Motor_1);  // FIXME TEST not tested with individual calls
  mrcLeft  = -nxshield.bank_b.motorGetEncoderPosition(SH_Motor_2);

  // 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;
}

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;
}

inline void GetGyroData(float &gyroSpeed, float &gyroAngle)
{
  float gyroRaw;

  gyroRaw = -dIMU.ReadAxis(AXIS_USED);
  gOffset = EMAOFFSET * gyroRaw + (1-EMAOFFSET) * gOffset;
  gyroSpeed = gyroRaw - gOffset;

  gAngleGlobal += gyroSpeed*tInterval;
  gyroAngle = gAngleGlobal;
}

//millis(); is (i think) CurrentTick();

inline void CalcInterval(long cLoop)
{
  if (cLoop == 0) {
    // First time through, set an initial tInterval time and
    // record start time
    tInterval = 0.0055;
    tCalcStart = millis();
  } else {
    // Take average of number of times through the loop and
    // use for interval time.
    tInterval = (millis() - tCalcStart)/(cLoop*1000);
  }
}
void loop()
{
  long lastmilis = millis();
  byte pfData[8];
  byte OkayToRead = 1;
 
  float gyroSpeed, gyroAngle;
  float motorSpeed;

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

  tMotorPosOK = millis();

  // Reset the motors to make sure we start at a zero position
  nxshield.bank_b.motorResetEncoder(SH_Motor_1);
  nxshield.bank_b.motorResetEncoder(SH_Motor_2);

  while(true) {
    CalcInterval(cLoop++);

    GetGyroData(gyroSpeed, gyroAngle);
    
    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 = millis();
      
    if (millis() >= lastmilis + 27){
      HTIRRec.ReadAllChannels(pfData);
      if(pfData[HTIRR_2A]&&OkayToRead){
        KGYROANGLE += (float)((char)pfData[HTIRR_2A])/100;
        OkayToRead = 0;
      }
      if(pfData[HTIRR_2B]&&OkayToRead){
        KGYROSPEED += (float)((char)pfData[HTIRR_2B])/1000;
        OkayToRead = 0;
      }
      if(!pfData[HTIRR_2A]&&!pfData[HTIRR_2B]&&!OkayToRead){
        OkayToRead = 1;
      }
      
      motorControlDrive = (int)((char)pfData[HTIRR_1A] + (char)pfData[HTIRR_1B]) * 2;
      motorControlSteer = (int)((char)pfData[HTIRR_1A] - (char)pfData[HTIRR_1B]);
      lastmilis = millis();
    }  
      
    SteerControl(power, powerLeft, powerRight);
     // Apply the power values to the motors
    
    if(powerRight>0){ 
      nxshield.bank_b.motorRunUnlimited(SH_Motor_1, SH_Direction_Reverse, powerRight/2 + 50);
    }
    else{
      nxshield.bank_b.motorRunUnlimited(SH_Motor_1, SH_Direction_Reverse, powerRight/2 - 50);
    }
    if(powerLeft>0){
      nxshield.bank_b.motorRunUnlimited(SH_Motor_2, SH_Direction_Reverse, powerLeft/2 + 50);
    }
    else{
      nxshield.bank_b.motorRunUnlimited(SH_Motor_2, SH_Direction_Reverse, powerLeft/2 - 50);
    }
     // Check if robot has fallen by detecting that motorPos is being limited
     // for an extended amount of time.
     if ((millis() - tMotorPosOK) > TIME_FALL_LIMIT) {
       break;
     }
     char str[256];
     //int motorSpeed_i = motorSpeed;
     //int motorPos_i   = motorPos;
     long tInterval_l = tInterval*1000;
     //sprintf (str, "gyroSpeed %4d GyroAngle %4d MLS %4d MRS %4d mSpd %-12ld %10d mPos %-12ld %10d %5ld", (int)gyroSpeed, (int)gyroAngle, powerLeft, powerRight, motorSpeed, motorSpeed_i, motorPos, motorPos_i, tInterval_l);
//     sprintf (str, "PF L: %4d PF R: %4d Drive: %6d Steer: %6d kAng: %3d kSpd * 100: %3d", (char)pfData[HT_CH1_A], (char)pfData[HT_CH1_B], motorControlDrive, motorControlSteer, (int)KGYROANGLE, (int)((float)KGYROSPEED * 100));
     sprintf(str, "tInterval: %-12ld", tInterval_l);
     Serial.println(str);
//     delay(WAIT_TIME);
  }
  nxshield.bank_b.motorStop(SH_Motor_1, SH_Next_Action_Brake);
  nxshield.bank_b.motorStop(SH_Motor_2, SH_Next_Action_Brake);
  while(true);
}

You will need my NXShield Driver Suite in order to use the HT IR Receiver, and you will also need the library for the dIMU.

Here are the necessary files for using the dIMU gyro (no accelerometer support). Note that these files are not a fully developed library, and only support a limited number of features that the dIMU is capable of. The cpp file:

/*
*  Matthew Richardson
*  matthewrichardson37<at>gmail.com
*  https://mattallen37.wordpress.com/
*  October 31 2011
*
*  Based on Xander Soldaat's awesome ROBOTC drivers
*
*  You may use this code as you wish, provided you give credit where it's due.
*/

#include "dIMUGyro.h"

dIMUGyro::dIMUGyro(uint8_t i2c_address)
:NXShieldI2C(i2c_address)
{
}

uint8_t DIMU_Gyro_divisor;

uint8_t dIMUGyro::Init(uint8_t range, uint8_t filter){
	writeByte(DIMU_GYRO_CTRL_REG2, 0x00);
	writeByte(DIMU_GYRO_CTRL_REG3, 0x08);
	writeByte(DIMU_GYRO_CTRL_REG4, range + DIMU_CTRL4_BLOCKDATA);
	writeByte(DIMU_GYRO_CTRL_REG5, filter ? 0x02 : 0x00);
	writeByte(DIMU_GYRO_CTRL_REG1, 0x0F);
	// Set DIMU_Gyro_divisor so that the output of our gyro axis readings can be turned
	// into scaled values.
	///////////////////////////////////////////////////////////////////////////
	if(range == 0)
	  DIMU_Gyro_divisor = 128;      // Full scale range is 250 dps.
    else if (range == 0x10)
      DIMU_Gyro_divisor = 64;       // Full scale range is 500 dps.
	else if (range == 0x30)
	  DIMU_Gyro_divisor = 16;       // Full scale range is 2000 dps.
	return 1;
}

float dIMUGyro::ReadAxis(uint8_t axis){
	uint8_t reg;
	if(axis == 0)
	  reg = DIMU_GYRO_X_AXIS;
    else if (axis == 1)
      reg = DIMU_GYRO_Y_AXIS;
	else if (axis == 2)
	  reg = DIMU_GYRO_Z_AXIS;

	return (float)(readByte(reg) + ((int)readByte(reg+1) << 8)) / DIMU_Gyro_divisor;
}

void  dIMUGyro::ReadAxes(float & x, float & y, float & z){
	x = (float)(readByte(DIMU_GYRO_X_AXIS) + ((int)readByte(DIMU_GYRO_X_AXIS+1) << 8)) / DIMU_Gyro_divisor;
	y = (float)(readByte(DIMU_GYRO_Y_AXIS) + ((int)readByte(DIMU_GYRO_Y_AXIS+1) << 8)) / DIMU_Gyro_divisor;
	z = (float)(readByte(DIMU_GYRO_Z_AXIS) + ((int)readByte(DIMU_GYRO_Z_AXIS+1) << 8)) / DIMU_Gyro_divisor;
}

The header file:

/*
*  Matthew Richardson
*  matthewrichardson37<at>gmail.com
*  https://mattallen37.wordpress.com/
*  October 31 2011
*
*  Based on Xander Soldaat's awesome ROBOTC drivers
*
*  You may use this code as you wish, provided you give credit where it's due.
*/

#ifndef dIMUGyro_H
#define dIMUGyro_H

#define DIMU_GYRO_I2C_ADDR      0xD2  /*!< Gyro I2C address */

#define DIMU_GYRO_RANGE_250     0x00  /*!< 250 dps range */
#define DIMU_GYRO_RANGE_500     0x10  /*!< 500 dps range */
#define DIMU_GYRO_RANGE_2000    0x30  /*!< 2000 dps range */
#define DIMU_CTRL4_BLOCKDATA    0x80

#define DIMU_GYRO_CTRL_REG1     0x20  /*!< CTRL_REG1 for Gyro */
#define DIMU_GYRO_CTRL_REG2     0x21  /*!< CTRL_REG2 for Gyro */
#define DIMU_GYRO_CTRL_REG3     0x22  /*!< CTRL_REG3 for Gyro */
#define DIMU_GYRO_CTRL_REG4     0x23  /*!< CTRL_REG4 for Gyro */
#define DIMU_GYRO_CTRL_REG5     0x24  /*!< CTRL_REG5 for Gyro */

#define DIMU_GYRO_ALL_AXES      0x28  /*!< All Axes for Gyro */
#define DIMU_GYRO_X_AXIS        0x2A  /*!< X Axis for Gyro */
#define DIMU_GYRO_Y_AXIS        0x28  /*!< Y Axis for Gyro */
#define DIMU_GYRO_Z_AXIS        0x2C  /*!< Z Axis for Gyro */

#include "NXShieldI2C.h"

class dIMUGyro : public NXShieldI2C
{
public:
  dIMUGyro(uint8_t i2c_address = DIMU_GYRO_I2C_ADDR);
  
  uint8_t Init(uint8_t range, uint8_t filter);
  float ReadAxis(uint8_t axis);
  void  ReadAxes(float & x, float & y, float & z);
};

#endif

The keywords file:

dIMUGyro	KEYWORD1

Init	KEYWORD2
ReadAxis	KEYWORD2
ReadAxes	KEYWORD2

DIMU_GYRO_I2C_ADDR	LITERAL1
DIMU_GYRO_RANGE_250	LITERAL1
DIMU_GYRO_RANGE_500	LITERAL1
DIMU_GYRO_RANGE_2000	LITERAL1
DIMU_CTRL4_BLOCKDATA	LITERAL1
DIMU_GYRO_CTRL_REG1	LITERAL1
DIMU_GYRO_CTRL_REG2	LITERAL1
DIMU_GYRO_CTRL_REG3	LITERAL1
DIMU_GYRO_CTRL_REG4	LITERAL1
DIMU_GYRO_CTRL_REG5	LITERAL1
DIMU_GYRO_ALL_AXES	LITERAL1
DIMU_GYRO_X_AXIS	LITERAL1
DIMU_GYRO_Y_AXIS	LITERAL1
DIMU_GYRO_Z_AXIS	LITERAL1
Advertisements
This entry was posted in Dexter Industries, Mindsensors, Mindstorms and tagged , , , . Bookmark the permalink.

5 Responses to NXShield dIMU Segway

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

  2. Christian says:

    Hello, Congratulations for this achievement. Is it possible to have the program Arduino for NXShield and Dexter sensor.
    Thank you in advance

  3. Sorin says:

    Hi,
    Very nice project. You did used only the inputs from the gyro sensor to balance the robot? No accelerator inputs used? In many other example i saw both gyro and accel used.

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