DASL Wiki

This page is read only. You can view the source, but not change it. Ask your administrator if you think this is wrong.
====== NXTWay: Wheeled Inverted Pendulum with Gyro Sensor ======

===== Motivation =====
The LEGO Mindstorms NXT based Wheeled Inverted Pendulum has been proven to work using various types of sensors including stock light sensors and after market gyro sensors.

{{youtube>UnqKIOawlIk?medium}}{{youtube>7OfJwG0EhBU?medium}}\\ 

**Note:** If the videos do not load, just refresh the page.

The light sensor control sceme relies on a relatively solid colored texture free surface to perform well. Changes in light intensity at the sensor cause the robot to move forward or backward in response the sensed tilt. The gyro based model is more robust as it senses angular velocity of the robot directly. Being Gyro controlled, this robot can handle any colored surface, including ramps and small bumps.
The gyro seen in the video about is sold by [[http://www.hitechnic.com/|HiTechnic]]
HiTechnic also provides an assembly manual for their "HiTechnic HTWay" and sample code that allows it to balance and be controlled by and infrared transmitting controller.

{{dylanw:HiTechnicHTWayAssembly.pdf}} {{dylanw:HiTechnicHTWayNXCcode.nxc.zip}}

The goal of this project was to verify HiTechnic's controller using their gyro, then modify the controller to work with an analog [[http://www.sparkfun.com/products/9431|Sparkfun 6-axis IMU]] (sku: SEN-09431, Retired):

{{dylanw:razorIMU.jpeg}}\\ 

===== Original HTWay =====

{{dylanw:HTway.jpg}}\\ 

===== Building Instructions =====

{{dylanw:HiTechnicHTWayAssembly.pdf}}
These instructions are also available from [[http://www.hitechnic.com/models|HiTechnic's Models page]]

In DASL, we have only the HiTechnic Gyro. If you omit the IR receiver, you will have to comment out the corresponding sensor calls in the original code. The edited code in the following section will account for this omission.

===== Code (NXC) =====

{{dylanw:HiTechnicHTWayNXCcode.nxc.zip}}
This code is also available from [[http://www.hitechnic.com/models|HiTechnic's Models page]]

===== NXTway using Sparkfun IMU =====

My method of retrofitting the Sparkfun IMU using the HiTechnic HTway design and NXC code:
  * Build HiTechnic design HTway
  * Test HiTechnic code with HiTechnic gyro (IR control omitted)
  * Connect IMU and log data from both the HiTechnic and Sparkfun gyros for comparison
  * Using both sets of data, find the equation relating IMU data to HiTechnic Gyro data so that the gyro input to the controller effectively looks the same.
  * Alter code to read only IMU gyro data and remove the HiTechnic gyro
  * Edit PID gains to account for slower update frequency.

===== Build HTway =====

Building instruction can be found on HiTechnics site or right here:
{{dylanw:HiTechnicHTWayAssembly.pdf}}
When building this model, you can omit the IR sensor and rubber bumpers. The IR sensor use will be omitted from the code and the rubber bumpers are unavailable in the DASL Mindstorms kits.<br><br>
When adding the Sparkfun IMU into the mix, you should match the position, orientation and wiring seen in the following pictures. This way, all the code will work. You can change things up later. As seen on the underside of the IMU, the 3V in can be wired to the 3.3V output on the HT proto-board and the ground should be wired to ground. The 4x-amplified X-axis gyro output should be wired to the first analog input, A0.

{{dylanw:NXTway_front.jpg}}{{dylanw:nxtway_back.jpg}}\\ 

===== Test HiTechnic Code w/o IR =====

Before you can run HiTechnics HTway code on your wheeled inverted pendulum, you must comment out the calls to the IR sensor.
Comment out lines **273** and **511**
{{dylanw:nxtway_HTgyro_noIR.nxc.zip}}

===== IMU gyro and HT gyro data logging =====

The functions for writing data to a file can be found in the code provided below. The functions initialize a file, prevent overwriting of previously saves files, and create a comma separated CSV file usable by MS Excel or another spreadsheet program. The provided reads each gyro every iteration and stores the raw data read from each.
{{dylanw:nxtway_datalog.nxc.zip}}

The data obtained before filtering the IMU gyro data is found to look a lot like the HT gyro data but is offset by almost 400 units and flipped about the x-axis (depending on gyro orientation).

{{dylanw:GyroDataRaw.jpg}}\\ 

===== IMU Data Massaging =====

To find the offset necessary to center the IMU gyro data about the y=0 axis (the same center as the HT gyro data), the average of the differences between the two data sets at each point was found (-384.525).

  Offset = Mean( IMUgyro_i - HTgyro_i );
  
where i = 1..Number of data points.

Adding this value to the raw IMU gyro data at every point pulls the data down and centers it about the same central axis as the HT gyro data. After that, it can be seen that the IMU gyro data must be scaled slightly so that the local data minimums and maximums correspond in amplitude. This scaling factor can be found by examining corresponding peaks in HT gyro data and IMU gyro data. This scaling factor is found to be -0.98. This scalar to be multiplied by the offset IMU gyro data both decreases the amplitude slightly and flips the data over the x-axis. The data from both sensors now lines up and can be verified by applying filtering when the data is read in the code.

  IMUgyro = -0.98*(IMUgyro_raw - 384.525)

{{dylanw:GyroDataFiltered.jpg}}\\ 

===== Wheeled Inverted Pendulum with IMU gyro =====

At this point, the gyros can effectively be swapped. This means that the HT gyro can be omitted from the control loop and removed from the robot. If you replace every part of the code where the controller calls data from the HT gyro with a read of the analog port connected to the IMU gyro, you will find that the robot ''almost'' works but is extremely unstable.

This instability is due to a much slower control loop and therefore a need for a more extreme reaction. The robot will fall further before the wheels are given a command to react. While the HT gyro can be read at up to 300Hz, the low speed proto-board board connected to the IMU can only be read at about 20Hz max.

Luckily, it is easy to compensate for this newly introduced lag in the controller. This compensation comes in the form of cranking up a gain on the effect of sensed angle (integrated angular velocity from gyro) on the reactionary motor speed. This increases causes smaller tilt angles to cause greater motor speeds than the original gain. With a slower reaction time, a reaction of greater magnitude is necessary to account for lost time!

  #define KGYROANGLE  10    // originally 7.5
  #define KGYROSPEED  1.15
  #define KPOS        0.07
  #define KSPEED      0.1

The completed code for controlling the HTway with the alternative Sparkfun razor IMU can be downloaded here:
{{dylanw:nxtway_justIMU.nxc.zip}}

**Keep in mind:** For this to work, the sensor should be mounted in the same orientation and at the same distance from the wheel axis. Also, the sensor should be wired to the first analog input (A0) and be wire on the same size breadboard to maintain a similar moment of inertia about the wheel axis as the my modified HTway.

===== Video =====



**Note:** If the videos do not load, just refresh the page.<br>
Thanks for reading!

===== NXC Code =====

<code c HTWay.nxc>
//=====================================================================
// HiTechnic HTWay Version 1.1
// Copyright (c) 2010 HiTechnic
//
// Gyro based sample Segway type robot. This NXC program uses the LEGO
// 2.0 firmware which has floating point support.  It has been tested
// with LEGO NXT firmware 1.29.
//
// To use the floating point math available in the 2.0 firmware,
// BricxCC must be setup to tell the compiler to target that firmware.
// From the Edit menu, select Preferences.  Select the Compiler tab,
// and then NBC/NXC sub-tab and check 'NXT 2.0 compatible firmware.'

// Modified by Alex Alspach for use with Sparkfun Razor 6-Axis IMU. 2012.


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

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

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

// 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
#define WHEEL_RATIO_NXT1 1.0
#define WHEEL_RATIO_NXT2 0.8
#define WHEEL_RATIO_RCX  1.4

// 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.
//#define KGYROANGLE  7.5
//#define KGYROSPEED  1.15
//#define KPOS        0.07
//#define KSPEED      0.1

#define KGYROANGLE  10
#define KGYROSPEED  1.15
#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      10

// 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     1000

//---------------------------------------------------------------------
// 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 Variable for File Handing and Gyro Data Logging
unsigned int result; // flag returned when handling files
byte fileHandle; // handle to the data file
short fileSize; // size of the file
short bytesWritten; // number of bytes written to the file
string fileHeader; // column header for data in the file
int fileNumber, filePart; // integers to split up data file names
string fileName; // name of the file
int filesize = 30240;
string strFileNumber; // file number e.g myDataFile 1, 2, 3
string strFilePart; // file part e.g. myDataFile1-1, 1-2, 1-3
string text; // string to be written to file i.e. data values

string strgyroRaw;
string strgOffset;
string strgyroSpeed;
string strgyroAngle;
string strtInterval;
string strA0raw;
string strA1raw;

float A0raw;
float A1raw;
  

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

// Time that robot first starts to balance.  Used to calculate tInterval
long 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.  RCX 2.0 wheel has ratio of 0.7 while large RCX wheel is 1.4
float ratioWheel;

// Gyro globals
float gOffset;
float gAngleGlobal = 0;

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


//=====================================================================

#include "NXCDefs.h"

#define PROTO_PORT IN_2

int inputdata;
int outputdata;
int count;
byte cmndbuf[];                 // buffer for outbound I2C command
byte respbuf[];                 // buffer for inbound I2C response

/* protoboard I/O map
   42,43 - A0 input
   44,45 - A1 input
   46,47 - A2 input
   48,49 - A3 input
   4A,4B - A4 input
   4C    - B inputs
   4D    - B outputs
   4E    - B controls
*/

void readdataA0()
  {
  ArrayInit(cmndbuf, 0, 2);     // set the buffer to hold 2 values
  cmndbuf[0] = 0x02;            // set write to channel
  cmndbuf[1] = 0x42;            // to set read address
  count=2;                      // 2 bytes to read
  I2CBytes(PROTO_PORT, cmndbuf, count, respbuf);  // issue I2C write command and read the byte back
  A0raw=respbuf[0]*4+respbuf[1];              // create input value by reading the high order byte,
                                                  // shift it left 3 bits and add the low order byt to
                                                  //create a full 10 bit value
  }
  
void readdataA1()
  {
  ArrayInit(cmndbuf, 0, 2);     // set the buffer to hold 2 values
  cmndbuf[0] = 0x02;            // set write to channel
  cmndbuf[1] = 0x44;            // to set read address
  count=2;                      // 2 bytes to read
  I2CBytes(PROTO_PORT, cmndbuf, count, respbuf);  // issue I2C write command and read the byte back
  A1raw=respbuf[0]*4+respbuf[1];              // create input value by reading the high order byte,
                                                  // shift it left 3 bits and add the low order byt to
                                                  //create a full 10 bit value
}

//Functions for File Handling and Gyro Data Logging

void InitWriteToFile() {
  fileNumber = 0;  // set first data file to be zero
  filePart = 0; // set first part of first data file to zero
  fileName = "myMotorSpeed.csv"; // name of data file
  result=CreateFile(fileName, filesize, fileHandle);
  // NXT Guide Section 9.100 pg. 1812 and Section 6.59.2.2 pg. 535
  // returns file handle (unsigned int)

  // check if the file already exists
  while (result==LDR_FILEEXISTS) // LDR_FILEEXISTS is returned if file pre-exists
     {
      CloseFile(fileHandle);
      fileNumber = fileNumber + 1; // if data file already exists, create new one
      fileName=NumToStr(fileNumber);
      fileName=StrCat("myMotorSpeed", fileName, ".csv");
      result=CreateFile(fileName, filesize, fileHandle);
      } // end while
  // play a tone every time a file is created
  PlayTone(TONE_B7, 5);
  fileHeader = "strgyroRaw, strgOffset, strgyroSpeed , strgyroAngle , strtInterval, A0raw, A1raw"; // header for myData file
  
  WriteLnString(fileHandle, fileHeader, bytesWritten);
  // NXT Guide Section 6.59.2.43 pg. 554
  // Write string and new line to a file
  // bytesWritten is an unsigned int.  Its value is # of bytes written

} // end InitWriteToFile


void WriteToFile(string strTempText) {
  // strTempText stores the text (i.e. ticks and motorRpm to be written to file
  // write string to file
  result=WriteLnString(fileHandle, strTempText, bytesWritten);
  // if the end of file is reached, close the file and create a new part
  if (result==LDR_EOFEXPECTED) // LDR_EOFEXPECTED is flagged when end-of-file
    {
    // close the current file
    CloseFile(fileHandle);
    // NXT Guide Section 6.59.2.1 pg. 535
    // Closes file associated with file handle

    // create the next file name
    filePart = filePart + 1;
    strFileNumber = NumToStr(fileNumber);
    strFilePart = NumToStr(filePart);
    fileName = StrCat("myMotorSpeed", strFileNumber,"-", strFilePart ,".csv");
    // delete the file if it exists
    DeleteFile(fileName);
    // NXT Guide Section 6.59.2.5 pg. 537
    // Delete the file specified by the string input

    // create a new file
    CreateFile(fileName, filesize, fileHandle);
    // play a tone every time a file is created
    PlayTone(TONE_B7, 5);
    WriteLnString(fileHandle, strTempText, bytesWritten);
    } // end if
} // end WriteToFile

// Close the file
void StopWriteToFile() {
  // close the file
  CloseFile(fileHandle);
} // end StopWriteToFile



// void SetWheelRatio()
//
// This function implements a UI so that the user can choose the
// size of the wheels used on the robot.
void SetWheelRatio()
{
  int nSelect;

  // Start of with Medium (NXT 1.0) selected
  nSelect = 2;

  do {
    // Display base text of user interface on screen
    ClearScreen();
    TextOut(0, LCD_LINE1, "HiTechnic-HTWayC");
    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;
    }

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

    // Get button press
    while(true) {
      if (ButtonPressed(BTNLEFT, false)) {
        nSelect--;
        if (nSelect < 1) nSelect = 3;
        break;
      }
      if (ButtonPressed(BTNRIGHT, false)) {
        nSelect++;
        if (nSelect > 3) 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 one 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, "HiTechnic-HTWayC");

  TextOut(0, LCD_LINE4, "Lay robot down");
  TextOut(0, LCD_LINE5, "flat to get gyro");
  TextOut(0, LCD_LINE6, "offset.");

  // Ensure that the motor controller is active since this affects
  // the gyro values.
  Off(MOTORS);

  do {
    gSum = 0.0;
    gMin = 1000;
    gMax = -1000;
    for (i=0; i<OFFSET_SAMPLES; i++) {
    readdataA0();
      g = (A0raw-384.524)*(-0.98);

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

      gSum += g;
      Wait(5);
    }
  } while ((gMax - gMin) > 1);   // Reject and sample again if range too large
      PlayTone(440,1000);
  //Average the sum of the samples.
  gOffset = gSum / OFFSET_SAMPLES + 1.0;

  // Even with motor controller active, the initial offset appears to
  // be off from the actual needed offset to keep robot from wondering.
  // This +1 helps keep robot from wondering when it first starts to
  // balance.
}

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

  ClearScreen();
  TextOut(0, LCD_LINE1, "HiTechnic-HTWayC");

  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()
{

 InitWriteToFile();
 
 // Initialize Proto Board
 SetSensorLowspeed(PROTO_PORT); // set sensor port 1 to low speed serial (I2C)
  Wait(100);

  ArrayInit(cmndbuf, 0, 3);     // set the buffer to hold 3 values
  cmndbuf[0] = 0x02;            // set write to channel
  cmndbuf[1] = 0x4E;            // to set write address
  cmndbuf[2] = 0x3F;            // to write 111111
  count=0;                      // no bytes to read
  I2CBytes(PROTO_PORT, cmndbuf, count, respbuf);  // issue I2C write command
  Wait(100);

  // Initialize the sensors
  SetSensorHTGyro(GYRO);
  //SetSensorLowspeed(IR_RECEIVER);
  Wait(50);

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

  // Get the initial gyro offset
  GetGyroOffset();

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

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

//=====================================================================
// GetGyroData(float &gyroSpeed, float &gyroAngle)
//
// Get the data from the gyro.
// Fills the pass by reference gyroSpeed and gyroAngle based on
// updated information from the Gyro Sensor
//
// Maintains an automatically adjusted gyro offset as well as
// the integrated gyro angle.
inline void GetGyroData(float &gyroSpeed, float &gyroAngle)
{
  float gyroRaw;

  //gyroRaw = SensorHTGyro(GYRO);
  
  readdataA0();
  gyroRaw = (A0raw-384.524)*(-0.98);
  //readdataA1();
  
  NumOut(20, LCD_LINE5, gyroRaw);
  ClearScreen();
  
  gOffset = EMAOFFSET * gyroRaw + (1-EMAOFFSET) * gOffset;
  gyroSpeed = gyroRaw - gOffset;
  NumOut(20, LCD_LINE6, gyroSpeed);
  NumOut(20, LCD_LINE7, A0raw);

  gAngleGlobal += gyroSpeed*tInterval;
  gyroAngle = gAngleGlobal;
  
  
  //strgyroRaw = FormatNum("%5.6f", gyroRaw);
  //strgOffset = FormatNum("%5.6f", gOffset);
  //strgyroSpeed = FormatNum("%5.6f", gyroSpeed);
  //strgyroAngle = FormatNum("%5.6f", gyroAngle);
  //strtInterval = FormatNum("%5.6f", tInterval);
  

  
  //strA0raw = FormatNum("%5.6f", A0raw);
  //strA1raw = FormatNum("%5.6f", A1raw);
  
  //text=StrCat(strgyroRaw,",", strA0raw);
  //text=StrCat(strgyroRaw, ",", strgOffset,",", strgyroSpeed, ",", strgyroAngle,",", strtInterval, ",", strA0raw); // ",", strA1raw );
  //WriteToFile(text);
}

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

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.0);
  }
}

//---------------------------------------------------------------------
// 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 gyroSpeed, gyroAngle;
  float motorSpeed;

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

  ClearScreen();
  TextOut(0, LCD_LINE1, "HiTechnic-HTWayC");
  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) {
     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 = 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);
}

//=====================================================================
// 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);

  char pfData[8];

  while(true) {
    //ReadSensorHTIRReceiver(IR_RECEIVER, pfData);
    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);
  }
  // Wait to allow user time to see screen.
  Wait(10000);
}

</code>

{{dylanw:nxtway_justIMU.nxc.zip}}