Arduino Power Functions remote for NXT

Here is a project I did for making an Arduino a Power Functions remote for the NXT. I took advantage of my NXTI2C library, as well as my PFIR library (more info, and download links for both here).

I setup the I2C register of the Arduino system as follows:

0x00-0x07 - version number - "V1.0"      - read only
0x08-0x0F - manuf. name    - "Mt Allen"  - read only
0x10-0x17 - device type    - "PFIR"      - read only

0x41 - Command         - write only
0x42 - Nibble 1        - write/read
0x43 - Nibble 2        - write/read
0x44 - Nibble 3        - write/read
0x45 - ActiveChannel   - write/read

If you write, starting at Nibble 1, and going through at least Nibble 3, the Arduino will load the data into the appropriate (PF channel specific) buffers, and automatically enable sending messages on that channel. You can optionally write to the ActiveChannel buffer, to remote control the permissions bits (either by writing to 0x45, or by writing a 4th byte, starting at 0x42).

There is a fairly extensive list of commands you can issue to the command register. In order to issue a command, you must write it to 0x41 (e.g. you can’t write 2 bytes starting at 0x40, and have the command take effect).

Here are the supported commands, as defined in the program:

#define COMMAND_E_STOP     0x73  // s  Command to float all motors immediately. Applies only to active channels.
#define COMMAND_E_STOP_ALL 0x53  // S  Command to float all motors immediately. Applies to all channels. Takes control of all channels.
#define COMMAND_DIS_ASS    0x64  // d  Disassociate from all IR channels.
#define COMMAND_DIS_ASS_CC 0x44  // D  Disassociate from all IR channels, and completely clear the ActiveChannel byte.
#define COMMAND_DISABLE    0x4E  // N  Disable messages.
#define COMMAND_ENABLE     0x45  // E  Enable messages.
#define COMMAND_LOAD_DATA  0x6C  // l (L) Load data as if it was sent directly to REG_NIBBLE_1.
#define COMMAND_DISABLE_TO 0x4F  // O Disable timeout.
#define COMMAND_ENABLE_TO  0x54  // T Enable timeout. (default when booted).
#define COMMAND_COMP_CON   0x43  // C Enable complete control of IR.
#define COMMAND_COMP_CON_O 0x63  // c Disable complete control of IR.
#define COMMAND_FAST_OFF   0x46  // F Enable really short blank messages.
#define COMMAND_FAST_OFF_O 0x66  // f Disable really short blank messages.

You will need to give the user-program running on the Arduino some time to respond to any messages sent (commands or PF control messages). With the simple loop I made, 10ms between sending messages seems to be more than enough. If I integrated the two libraries into one, specific for this purpose, it would probably eliminate this potential drawback (although 10ms really isn’t much of a wait).

Here’s the schematic of the circuit I’m using:

I used a transistor to increase the current for the IR LED. The IR LED I’m using is rated 1.6v forward drop, up to 100ma continuous current, and 940nm wavelength.

For R3, I used a 47 ohm resistor, but the appropriate value is specific to the LED you use.

For R4, I just used a common value of 330, but 220 to 1k should work just fine.

Download the program from here. You will also need the NXTI2C library and the PFIR library.

Posted in Arduino, Electronics, Mindstorms, NXT | Tagged , , , , | 5 Comments

DIWIFI Weather with ROBOTC

During DIWIFI week 2 presented by Dexter Industries, one of the projects was a ROBOTC program that would use the DIWIFI to read the current weather, provided by Google.

The program worked fine, but I made some modifications to improve it.

The way the original program would read the incoming data limited speed to about 9600 Baud. I like to be able to run communications faster than that, so I made some modifications to allow up to 57600 Baud.

The data provided by Google includes more than just the current conditions, temp in C and F, and wind conditions. I added several things to allow the NXT to parse and display the humidity, and the data for the next forecasted day. The debug stream outputs all this, as well as the other three forecasted days.

I also improved several other things to speed up the process of getting the data. Now it takes about 1 1/2 seconds to connect, request, receive, parse, and display the weather.

The program appears to work similarly, but several of the internal functions are totally redesigned to allow the extra functionality.

DIWIFI-Weather.c

// Read Google Weather
//
// DIWFI Weather uses the Dexter Industries Wifi Sensor to get the weather
// from Google.com
//
// Run this program with the RobotC Debugging Stream On to view any errors.
// Run AFTER connecting to a Wifi network!
// For more information visit http://www.dexterindustries.com/
//
// This program uses HTTP GET examples to retrieve the weather and
// display the current conditions on the NXT.
//
// See more about the Dexter Industries Wifi Sensor for the Lego Mindstorms NXT here:  http://www.dexterindustries.com/wifi.html
//
//  With modifications by Matthew Richardson.
//  matthewrichardson37<at>gmail.com
//  https://mattallen37.wordpress.com/
//
//////////////////////////////////////////////////////////////////////////////////////////////////////

#include "drivers/common.h"
#include "DIWIFI-Weather.h"

void weather(string location){

  closeAllConns();                  // Housekeeping: Close any open connections.
  clear_read_buffer();              // Housekeeping: Clear out the buffer.
  int CID = 0;
  CID = start_TCP_client();         // We start the HTTP client here.  Now we're acting as a client, not a server.
  wait1Msec(10);
  GetWeather(CID, location);        // This kicks off the meat of the project.  We're starting to send a tweet through the connection ID.
  read_weather_data();              // Load the weather data into an array so we can deal with it.

  writeDebugStreamLine("");         // New line in the debug stream.
  writeDebugStreamLine("");         // New line in the debug stream.

  print_weather();
}

task main()
{
  clear_read_buffer();

  while(true){                      // Repeat forever
    weather("Kabul");               // Connect, request, receive, parse, and display the current and forcasted weather
//    wait10Msec(100);                // Optionally slow down.
  }
}

DIWIFI-Weather.h

////////////////////////////////////////////////////////////////////////////////////////////////////////
//
//  Header file for reading the weather using the DIWIFI
//
//  With modifications by Matthew Richardson
//  matthewrichardson37<at>gmail.com
//  https://mattallen37.wordpress.com/
//
////////////////////////////////////////////////////////////////////////////////////////////////////////

string IP_num = "173.194.73.106";       // Google IP Number
ubyte byteStart[] = {27, 'S', '0'};     // Begin a Transmission
ubyte byteEnd[] = {27, 'E'};            // End a transmission
ubyte BytesRead[1];                     //
char weather_data[2000];                // Weather Data goes into this array.
int index = 0;                          // This will be the position index in weather_data

////////////////////////////////////////////////////////////////////////////////////////////////////////
//      Clear Read Buffer
//      Run this to clear out the reading buffer.
//      Simply sends a carriage return, then clears the buffer out.
////////////////////////////////////////////////////////////////////////////////////////////////////////

void clear_read_buffer()
{
    ubyte nData[] = {13};
    nxtWriteRawHS(nData[0], 1);   // Send the carriage return
    wait1Msec(50);
    while(nxtGetAvailHSBytes()){
      nxtReadRawHS(BytesRead[0], 1);    // Read the response.  Probably an error.
    }
}

// This function writes a string to the DebugStream and Port4
// We wrote this out because it is easier to recognize and see
// strings; easier than working with arrays of characters.
void writeStr(string sData)
{
  ubyte byteData = 0;
  for(int i = 0; i < strlen(sData); i++)
  {
    byteData = strIndex(sData, i);
    nxtWriteRawHS(byteData, 1);
    writeDebugStream("%c", byteData);     // Critical for debugging.  Make sure you're getting some time in there
                                          // to read what you wrote.
    while(nxtHS_Status == HS_SENDING) wait1Msec(1);
    wait1Msec(1);                         // With
  }
}

//
// This function just writes a line break to DebugStream and Port4.
// It is necessary to have this in RobotC because strings are limited to 17 characters
// and sometimes you will need to send commands longer than 17 characters.
void writeLineBreak()
{
  writeDebugStream("%c", 13);
  ubyte newline = 0x0D;
  nxtWriteRawHS(newline, 1);
}

////////////////////////////////////////////////////////////////////////////////////////////////////////
// Send Carriage Return, New Line
// This is used in the HTTP request
////////////////////////////////////////////////////////////////////////////////////////////////////////
void crln()
{
  writeDebugStream("%c", 13);
  writeDebugStream("%c", 10);              // Display this on the debug for our information.
  ubyte newline[] = {0x0D, 0x0A};          // Carriage return and then line feed.
  nxtWriteRawHS(newline, 2);
  wait1Msec(5);                            // ABSOLUTELY CRITICAL.  We added this because
                                           // we found that in line-feeds, the first bytes of the next transmission get
                                           // dropped.  VERY IMPORTANT TO HAVE THIS DELAY IN HERE.
}

////////////////////////////////////////////////////////////////////////////////////////////////////////
// Receive Bytes
// Reads whatever is in the buffer and prints to debug
////////////////////////////////////////////////////////////////////////////////////////////////////////
/*void Receive(bool wait=false)
{
  if (wait)
    while (nxtGetAvailHSBytes() == 0) wait1Msec(1);

  while (nxtGetAvailHSBytes() > 0) {
    nxtReadRawHS(BytesRead[0], 1);
    writeDebugStream("%c", BytesRead[0]);
    wait1Msec(1);
  }
}*/

void Receive(bool wait=false)
{
  if (wait)
    while (nxtGetAvailHSBytes() == 0) wait1Msec(1);

  time1[T1] = 0;
  int LastTick = 0;
  while (LastTick + 10 > time1[T1]){                 // Keep looping until it hasn't received any data for at least 10ms
    if(nxtGetAvailHSBytes() > 0) {                   // If there is data to receive
	    nxtReadRawHS(BytesRead[0], 1);                 // Grab a byte
	    writeDebugStream("%c", BytesRead[0]);          // Send it to the debug stream
      LastTick = time1[T1];                          // Reset the timer offset
    }
  }
}

////////////////////////////////////////////////////////////////////////////////////////////////////////
// Housekeeping: Close down the Connections
////////////////////////////////////////////////////////////////////////////////////////////////////////
void closeAllConns() {
  writeDebugStreamLine("closeAllCons");
  clear_read_buffer();
  writeStr("at+ncloseall");
  writeLineBreak();
//  wait10Msec(10);
  Receive(true);
}

////////////////////////////////////////////////////////////////////////////////////////////////////////
// Connect to google via TCP.
// Open up a client connection with the Google Server.
////////////////////////////////////////////////////////////////////////////////////////////////////////
int start_TCP_client() {
  int CID = 0;
  bool connected = false;   // This will tell us if we're connected or not.
  string port = ",80";

  while(!connected){        // Do this over and over again until we get a connection.
	  writeStr("AT+NCTCP=");
	  writeStr(IP_num);
	  writeStr(port);
	  writeLineBreak();

	  // Don't replace the following code with a Receive function.
	  // This code picks out the CID number.
	  while (nxtGetAvailHSBytes() == 0) wait1Msec(5);
	  while (nxtGetAvailHSBytes() > 0) {
	    nxtReadRawHS(BytesRead[0], 1);
	    writeDebugStream("%c", BytesRead[0]);
	    if(BytesRead[0] < 58 && BytesRead[0] > 47){  // So if it's a number . . .
	      CID = BytesRead[0]-48;  // Works for connections 0 through 9.
	      connected = true;
	    }
	    wait1Msec(2);
	  }
	}
  return CID;
}

////////////////////////////////////////////////////////////////////////////////////////////////////////
// Send information to Google to get weather.
////////////////////////////////////////////////////////////////////////////////////////////////////////
void GetWeather(int CID, string location) {
  nxtWriteRawHS(byteStart, 3);
  writeDebugStream("%c", byteStart, 2);
  writeDebugStreamLine("%c", (CID+48));

  wait1Msec(10);

  writeStr("GET /ig/api?");
  writeStr("weather=");
  writeStr(location);
  writeStr(" HTTP/1.1");
  crln();

  writeStr("User-Agent: ");
  writeStr("DIWifi");
  crln();

  writeStr("Host: ");
  writeStr("www.google.com");
  crln();
  crln();

  wait1Msec(10);
  nxtWriteRawHS(byteEnd, 2);
}

///////////////////////////////////////////////////////////////////////////////////////////////////
// Receive the stream of data sent to the NXT at the request of GetWeather
///////////////////////////////////////////////////////////////////////////////////////////////////
void read_weather_data(){
  while(true){                                                       // Repeat forever, or until break
	  while (nxtGetAvailHSBytes() == 0) wait1Msec(1);                  // Wait until data has been received
	  time1[T1] = 0;                                                   // Reset timer 1
	  int LastTick = 0;                                                // Set the timer offset to 0
	  while (LastTick + 10 > time1[T1]){                               // Repeat while it's been less than 10ms since the last byte was received
	    if(nxtGetAvailHSBytes() > 0) {                                 // If data has been received
		    nxtReadRawHS(BytesRead[0], 1);                               // Read a byte
		    writeDebugStream("%c", BytesRead[0]);                        // Write it to the debug stream
		    weather_data[index] = BytesRead[0];                          // Add the byte to the weather_data
		    index++;                                                     // Increment the weather_data index
	      LastTick = time1[T1];                                        // Reset the timer offset
	    }
	  }
    if(weather_data[index-1] == 69 && weather_data[index-2] == 27) break;  // This looks for the <ESC> E Sequence that signals the end of a transmission.
  }
}

///////////////////////////////////////////////////////////////////////////////////////////////////
// Search for a string in the weather_data array
///////////////////////////////////////////////////////////////////////////////////////////////////
int StringFindWeather(string F_String, int Begin_Here = 0){          // Input a string to look for in the weather data array, optionally a place to start, and return the position
  byte F_Size = strlen(F_String);                                    // Get the length of the string to look for
  byte F_Array[20];                                                  // An array to hold the string of chars to look for
  memcpy(F_Array, F_String, F_Size);                                 // Copy the string into the array
  for (int i = Begin_Here; i < sizeof(weather_data); i++){           // repeat for the number of times the weather_data array is long
    if (weather_data[i]==F_Array[0]){                                // If the current char is the same as the first char in the string to look for
      for (int ii = 0; ii < F_Size; ii++){                           // Repeat for the number of times the find string is long
        if (weather_data[i+ii]!=F_Array[ii]) goto LookForFirstChar;  // If the chars don't match up, go back to looking for the first char again
      }
      return i;                                                      // It found the matching string, so return the start char position
    }
    LookForFirstChar:                                                // Label to jump to to look again for the first char of the string to search for
  }
  return -1;                                                         // It didn't find the string, so return -1
}

///////////////////////////////////////////////////////////////////////////////////////////////////
// Find data, send it to the debug stream, and optionally display it on the NXT LCD
///////////////////////////////////////////////////////////////////////////////////////////////////
int find_and_print_XML(string findme, int DispLine, string Header = "", int Begin_Here = 0){
  int Start_Location = StringFindWeather(findme, Begin_Here);                      // Find the starting position of the findme string
  if(Start_Location == -1) return -1;                                              // If it didn't exist, return -1
  int End_Location = StringFindWeather("/>", Start_Location);                      // Find the next "/>", starting at the Start_Location
  if(End_Location == -1) return -1;                                                // If it didn't exist, return -1
  int findme_Length = strlen(findme);                                              // Determine the length of the string it's looking for

  int Info_Length = End_Location - Start_Location - findme_Length - 2;             // Determine the length of the information string

  byte Info[20];                                               // Create an array to temporarily hold the information string
  memset(Info, 0, 20);                                         // Set all elements to 0
  for(int i = 0; i < Info_Length; i++){                        // Repeat once for each character in the information string
    Info[i] = weather_data[Start_Location+findme_Length+1+i];  // Load the information array with data from the wather_data array
  }

  string strCond;                                              // Create a string to hold the information
  StringFromChars(strCond, Info);                              // Copy the information array into the information string

  if(StringFind(strCond, "%")!=-1){                            // If the character "%" exists (will only be at the end of the humidity infomation)
    strCond += "%";                                            // Append another one. The diplay and debug stream functions use the "%" symbol as a formatter, so the first one isn't displayed.
  }

  writeDebugStream(Header);                                    // Write the header string to the debug stream
  writeDebugStreamLine(strCond);                               // Write the information string to the debug stream, and create another line

  if(DispLine>-1){                                             // If it is supposed to display to the NXT LCD
	  string combined = Header+strCond;                          // Combine the header with the information string
	  nxtDisplayString(DispLine, combined);                      // Display the combined strings
  }
  return 1;
}

////////////////////////////////////////////////////////////////////////////////////////////////////////
// Print the weather.
////////////////////////////////////////////////////////////////////////////////////////////////////////
void print_weather(){

  writeDebugStreamLine("");                               // New line in the debug stream.

  find_and_print_XML("condition data=" ,0 , ""        );  // General Conditions
  find_and_print_XML("temp_f data="    ,1 , "Temp F: ");  // Temp in F
  find_and_print_XML("temp_c data="    ,-1, "Temp C: ");  // Temp in C
  find_and_print_XML("_condition data=",2 , ""        );  // WIND!  "wind_condition data="
  find_and_print_XML("humidity data="  ,3 , ""        );  // Humidity %

  writeDebugStreamLine("");                               // New line in the debug stream.

  int parser_pointer = StringFindWeather("cast_conditions");                       // Go to the first day of forcast
  find_and_print_XML("_of_week data="  , 4, "Forecast for ", parser_pointer);      // The day of the week, for the first forcasted day
  find_and_print_XML("condition data=" , 5, ""             , parser_pointer);      // General Conditions for the first forcasted day. "day_of_week data="
  find_and_print_XML("low data="       , 6, "Low  F: "     , parser_pointer);      // Forcasted low for the first forcased day
  find_and_print_XML("high data="      , 7, "High F: "     , parser_pointer);      // Forcasted high for the first forcased day
  parser_pointer = StringFindWeather("</forecast_cond"     , parser_pointer+1);    // End of the first day of forcast

  writeDebugStreamLine("");                                                        // New line in the debug stream.

  parser_pointer = StringFindWeather("cast_conditions"     , parser_pointer+10);   // Go to the beginning of the second day of forcast
  find_and_print_XML("_of_week data="  ,-1, "Forecast for ", parser_pointer);      // The day of the week, for the second forcasted day. "day_of_week data="
  find_and_print_XML("condition data=" ,-1, ""             , parser_pointer);      // General Conditions for the second day
  find_and_print_XML("low data="       ,-1, "Low  F: "     , parser_pointer);      // Low for the second day
  find_and_print_XML("high data="      ,-1, "High F: "     , parser_pointer);      // High for the second day
  parser_pointer = StringFindWeather("</forecast_cond"     , parser_pointer+1);    // End of the second day of forcast

  writeDebugStreamLine("");                                                        // New line in the debug stream.

  parser_pointer = StringFindWeather("cast_conditions"     , parser_pointer+10);   // Go to the beginning of the third day of forcast
  find_and_print_XML("_of_week data="  ,-1, "Forecast for ", parser_pointer);      // The day of the week, for the third forcasted day. "day_of_week data="
  find_and_print_XML("condition data=" ,-1, ""             , parser_pointer);      // General Conditions for the third day
  find_and_print_XML("low data="       ,-1, "Low  F: "     , parser_pointer);      // Low for the third day
  find_and_print_XML("high data="      ,-1, "High F: "     , parser_pointer);      // High for the third day
  parser_pointer = StringFindWeather("</forecast_cond"     , parser_pointer+1);    // End of the third day of forcast

  writeDebugStreamLine("");                                                        // New line in the debug stream.

  parser_pointer = StringFindWeather("cast_conditions"     , parser_pointer+10);   // Go to the beginning of the fourth day of forcast
  find_and_print_XML("_of_week data="  ,-1, "Forecast for ", parser_pointer);      // The day of the week, for the fourth forcasted day. "day_of_week data="
  find_and_print_XML("condition data=" ,-1, ""             , parser_pointer);      // General Conditions for the fourth day
  find_and_print_XML("low data="       ,-1, "Low  F: "     , parser_pointer);      // Low for the fourth day
  find_and_print_XML("high data="      ,-1, "High F: "     , parser_pointer);      // High for the fourth day
  parser_pointer = StringFindWeather("</forecast_cond"     , parser_pointer+1);    // End of the fourth day of forcast

  writeDebugStreamLine("");                                                        // New line in the debug stream.

  index = 0;                                            // Reset the count so we can do this over and over again.
  memset(weather_data, 0, 2000);                        // Reset the weather data array to zeros.  Good housekeeping.
}
Posted in Dexter Industries, Drivers, NXT, ROBOTC | Tagged , , , , | 1 Comment

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
Posted in Dexter Industries, Mindsensors, Mindstorms | Tagged , , , | 5 Comments

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.

Posted in Dexter Industries, Drivers, Mindstorms, NXC, NXT | Tagged , , , , , | 8 Comments

NXShield line follower robot

This is a robot I made to test, and experiment with, the OpenElectrons NXShield-M. I am using an Arduino Mega as the controller, so I program it using the standard Arduino programming environment (with the NXShield library). The robot uses the mindsensors’ LineLeader to “see” the line. I programmed it to use a PD algorithm to calculate the motor speeds, based on the line position.

Here is the Arduino program it’s running:

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

#include <LineLeader.h>
#include <PSPNx.h>

#define Clip(in,min,max) (in<min?min:(in>max?max:in))

//
// declare the NXShield(s) attached to your Arduino.
//
NXShield    nxshield;

//
// declare the i2c devices used on NXShield(s).
//
LineLeader LL_Nx;
PSPNx PSP_Nx;

void setup()
{
  delay(500);
  char str[256];
  Serial.begin(115200);  // start serial for output
  Serial.println (" ");
  Serial.println ("Initializing the devices ...");
  //
  // NXShield provides multiple protocols
  // 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.
  //
  
  LL_Nx.init( &nxshield, SH_BBS2 );
  PSP_Nx.init( &nxshield, SH_BBS1 );
  
  // initialize the analog sensors.

  //
  // reset motors.
  //
  nxshield.bank_a.motorReset();
  nxshield.bank_b.motorReset();
  Serial.println ("Press < to calibrate black");
  Serial.println ("Press > to calibrate white");
  Serial.println ("Press GO button to continue");
  while(true){
    if(nxshield.getButtonState(BTN_GO)){
      break;
    }
    
    if(nxshield.getButtonState(BTN_LEFT)){
      while(nxshield.getButtonState(BTN_LEFT)){delay(1);}
      Serial.print("Calibrating black ...");
      LL_Nx.calibrateBlack();
      delay(500);
      Serial.println(" Done");      
      Serial.println ("Press < to calibrate black");
      Serial.println ("Press > to calibrate white");
      Serial.println ("Press GO button to continue");      
    }
    
    if(nxshield.getButtonState(BTN_RIGHT)){
      while(nxshield.getButtonState(BTN_RIGHT)){delay(1);}
      Serial.print("Calibrating white ...");
      LL_Nx.calibrateWhite();
      delay(500);
      Serial.println(" Done");
      Serial.println ("Press < to calibrate black");
      Serial.println ("Press > to calibrate white");
      Serial.println ("Press GO button to continue");      
    }
  }
  randomSeed(analogRead(0));  
}

//Scales a range of numbers to a new range.
//Input value, lowest possible, highest possible, scale from this, to this. It returns the result.
float ScaleRange(float Value, float ValueMin, float ValueMax, float DesiredMin, float DesiredMax)
{
  float Result = (DesiredMax - DesiredMin) / (ValueMax - ValueMin) * (Value - ValueMin) + DesiredMin;
  return Result;
}

byte kp = 9;
byte kd = 14;
float error;
int error_i;
int error_last;
int derivitive;

float turn;

int MLS, MRS;

byte LL_Bool;
byte LL_Avg;

byte ly, ry;
byte ButtonSet1, ButtonSet2;

byte LL_Active_Control;
byte LED_On = 1;

void ButtonSet1NotPressed(){
  while (PSP_Nx.readByte(PSPNx_ButtonSet1) * (-1) + 255){
    delay(10);
  }   
}

void loop()
{
  char str[256];
  nxshield.bank_b. writeByte(0x7a, 0);
  nxshield.bank_b. writeByte(0x7b, 0);
  LL_Active_Control = 0;
  ButtonSet2 = PSP_Nx.readByte(PSPNx_ButtonSet2) * (-1) + 255;
  while (true) {   
    
    ButtonSet2 = PSP_Nx.readByte(PSPNx_ButtonSet2) * (-1) + 255;

    if (ButtonSet2 & 0x80) {LL_Active_Control = 1;}
    else {
      if (ButtonSet2 & 0x40) {
        LL_Active_Control = 0;
      }
    }
    
    ButtonSet1 = PSP_Nx.readByte(PSPNx_ButtonSet1) * (-1) + 255;
    
    if (ButtonSet1 & 0x10) {
      kp++;
      ButtonSet1NotPressed();
    }
    if (ButtonSet1 & 0x20) {
      kp--;
      ButtonSet1NotPressed();
    }
    if (ButtonSet1 & 0x40) {
      kd--;
      ButtonSet1NotPressed();
    }
    if (ButtonSet1 & 0x80) {
      kd++;
      ButtonSet1NotPressed();
    }    
    
    
    if (ButtonSet2 & 0x10) {
      while (ButtonSet2 & 0x10){
        delay(10);
        ButtonSet2 = PSP_Nx.readByte(PSPNx_ButtonSet2) * (-1) + 255;
      }      
      if(LED_On)LED_On = 0;
      else LED_On = 1;
    }
    
    if (LL_Active_Control){
      LL_Bool = LL_Nx.getResult();
      LL_Avg = LL_Nx.getAverage();
      
      error = LL_Avg - 10;  //The error, between -5 and 5
      error /= 7;
      error -= 5;
      error *= (-1);
      
      if (LL_Avg==0){                       //if it is not all white, scale to -5 to 5
        error = 0;
      }
      
      error = error * (abs(error)/3*2);
      error_i = error;
      derivitive = error - error_last;
      turn = error_i * kp + derivitive * kd;
      
      error_last = error;
      
      MLS = 75 - turn;
      MRS = 75 + turn;
      
      MLS = Clip(MLS, 0, 100);
      MRS = Clip(MRS, 0, 100);      
    }
    else{
      error_last = 0;      //Reset so that it is fresh for when it resumes following the line
      ly = PSP_Nx.getYLJoy();
      ry = PSP_Nx.getYRJoy();      
      MLS = ScaleRange(ly, 0, 255, 100, -100);
      MRS = ScaleRange(ry, 0, 255, 100, -100);
    }
    
    if (MLS>=3||MLS<=(-3)){    
    //nxshield.bank_b.motorSetBothSpeed(MRS, MLS);
      nxshield.bank_b.motorRunUnlimited(SH_Motor_1, SH_Direction_Forward, MLS);
    }
    else{
      nxshield.bank_b.motorRunUnlimited(SH_Motor_1, SH_Direction_Forward, 0);
      nxshield.bank_b.motorStop(SH_Motor_1, SH_Next_Action_Brake);
    }
    if (MRS>=3||MRS<=(-3)){      
      nxshield.bank_b.motorRunUnlimited(SH_Motor_2, SH_Direction_Forward, MRS);
    }
    else{
      nxshield.bank_b.motorRunUnlimited(SH_Motor_2, SH_Direction_Forward, 0);
      nxshield.bank_b.motorStop(SH_Motor_2, SH_Next_Action_Brake);
    }    
    
    sprintf (str, "LL-Nx Average: %d Error: %d LL-Nx Bits: %d%d%d%d,%d%d%d%d", LL_Avg, error_i, LL_Bool&0x80?1:0, LL_Bool&0x40?1:0, LL_Bool&0x20?1:0, LL_Bool&0x10?1:0, LL_Bool&0x08?1:0, LL_Bool&0x04?1:0, LL_Bool&0x02?1:0, LL_Bool&0x01?1:0);
    Serial.println(str);
    
    sprintf (str, "ly %d MLS %d ry %d MRS %d kp %d kd %d", ly, MLS, ry, MRS, kp, kd);
    Serial.println(str);
    
    if (LED_On) nxshield.setRGB(random(9), random(9), random(9));
    else nxshield.setRGB(0,0,0);
  }
}
Posted in Electronics, Mindsensors, Mindstorms, Technic | Tagged , , , , | Leave a comment

NXTTouchPanel calculator

mindsensors is soon coming out with a very cool new product; a touch panel overlay for the NXT screen. Deepak sent me one to beta test (big thank you!), so right away I started programming with it. I started out with the basics of reading the X and Y positions, and then started making virtual blocks on the screen I could “click” on. I wanted something more than just a grid where the segments turned black when I touched them, so I started programming the NXT to be a calculator.

It really isn’t anything fancy as far as calculators go, but it has the potential to be a lot more than it is right now.

So far, the calculator supports things as follows:
– C (clear out the values).
– 12*5+3-5/3 (operations right on top of the last one). It displays the results as it goes. So, you press 1, it displays 1, you press 2, it displays 12, you press *, it displays 12 and also indicates you pressed *, you press 5, it displays 5, you press +, it displays 60 and also indicates you pressed +…
– You can also do something like 10+16=+4/6 (where you press = in the middle to see the result before entering a new operator).
– You can do a totally different operation right after the first one like this 17-47=3+8 it will do the 17-47, and when you press =, it will give the answer. Then, since you enter a number next (3), it does the same thing as clearing it before the 3+8.

For the future:
– Add a <- for un-entering a digit or operator
– Add parentheses support
– Add decimal point support (the variables already support it, just not the display or buttons)
– Add more advanced math functions (like trig)
– Add shortcut for PI
– Make a scrolling menu for more room for buttons

Posted in Mindsensors, Mindstorms, NXC, NXT | Tagged , , , , | 2 Comments

dIMU 6 DOF accelerometer and gyroscope

Dexter Industries is working on a new sensor for the NXT. It is a 6 DOF (Degrees Of Freedom) IMU (Inertial Measurement Unit), called a dIMU. It consists of an accelerometer, and a gyroscopic sensor.

An Accelerometer (often referred to as an “accel”) is a gravity sensor. It measures the strength of gravity (in g forces). If the accel sensor is stationary, flat, “upright”, and on the earth, it should read 1g in the Z axis, and 0g in both the X and the Y axis. If you were to tip the sensor, the X and/or the Y axis would then begin to feel the pull of gravity, and the Z axis would start to feel it less. However, being moved around causes it to sense more than just the pull of the earth; it now is sensing acceleration and deceleration, potentially in all three axis. Earth has a g force of 1, but the reason an accelerometer will often have a range more like 2, 4, or 8 g forces is because it senses acceleration due to motion.

The accelerometer used in the dIMU has a full 3 axis, and has a selectable range of sensitivity (2g, 4g, or 8g). It has 8 bit resolution for all ranges, but it also has a 10-bit resolution RAW value. The range chooses which 8 bits you are reading. Reading all 10 bits makes it unnecessary to worry about the range. The only advantage I can think of to using the range, and only 8 bits, is that you only need to read one I2C register per axis (instead of two). This could be a slight time saver if you really need speed. The down-side, is that in 4 or 8 g ranges you lose 1 or 2 least significant bits (you get lower resolution), and in 2 or 4 g ranges, the sensor value maxes out at 2 or 4 g. One more cool feature of the accel sensor, is being able to write an offset to the sensor itself. No sensor is perfect, and being so, they need to be calibrated. Being able to write an offset to the accelerometer means you potentially need less user code in the program to get good values.

A gyroscopic sensor (typically called a “gyro” for short), is a sensor that measures rate of rotation in dps (degrees per second). If you hold a gyro “upright” in a chair, and you spin the chair, the value from the sensor will be in direct proportion to the speed the chair is spinning (it will return how many degrees per second the chair is turning). If you want to get values that are of absolute degrees (so you know how many times you have spun around in the chair, for example), you can get an idea of the position by using an integral. To do that, you need to multiply the speed in dps (degrees per second), by the time since the last reading, and adding that to a grand sum. It would be something like this: Position = Position + (dps * TimeFrame). An integral is very susceptible to errors, because any little error in the reading will be added up every time you update the integral. Also, the longer between reads, the more likely you are to have missed important information (such as a short jerk to the sensor).

The gyroscopic sensor used in the dIMU has 3 axis. The gyro has a selectable range of 250, 500, and 2000 dps. The resolution is 16-bit. That means that in 2000 dps mode, you get readings precise to 1/16 of a dps. In 500 and 250 dps modes, you get readings precise to 1/64 and 1/128 dps. Now, in case you didn’t realize it already, that is very precise. Integrated into the gyro, is also a temperature sensor. I have yet to use it, but it is supposed to have a resolution of 8 bits.

John Cole from Dexter Industries sent me a dIMU to test, and experiment with (Thank you!). I can say from personal experience that it is a great device, and the possibilities that this combination of sensors brings are endless. I have already (as has Laurens Valk) built a segway that uses this as the sensor to balance, and I have many ideas for other robots that would use the dIMU.

Posted in Dexter Industries, Mindstorms, NXT | Tagged , , , | Leave a comment