ROBOTC.net forums
http://www.robotc.net/forums/

HiTechnic DC Motor Controller I2C Programming
http://www.robotc.net/forums/viewtopic.php?f=1&t=5846
Page 1 of 1

Author:  gcronin [ Mon May 13, 2013 9:03 pm ]
Post subject:  HiTechnic DC Motor Controller I2C Programming

I am hoping to help my students understand how to access some of the built in capabilities of the HiTechnic DC Motor Controller, such as move at constant speed and move to encoder position, using I2C programming. I've read the HiTechnic FIRST Motor Controller Specifications, http://www.legoeducation.us/etc/supportFiles/TETRIX/739413/HiTechnicMotorControllerBriefv1.3.pdf, looked briefly at Xander's driver suites which use I2C calls, and read through the nice packet from StormingRobots on I2C. http://stormingrobots.com/prod/tutorial/I2C%20RobotC%20Tutorial.pdf. I am using RobotC 3.51. My code is listed below, and works correctly to read and display the current encoder value if the encoder is plugged into the second motor port. However, if I uncomment the line

//sendI2CMsg(S1, mOutput.nMsgSize, 0);

then the encoder value initially reads "0", as expected, but then reads only -260 when the motor is turned in one direction, or some 2 digital positive numbers when the motor is spun in the opposite direction. In either case, as soon as the motor stops spinning, the reported encoder value is "0".

I have tried with both the SensorType as sensorI2CCustom or as sensorI2CCustomFast. This doesn't make a difference.

I do not understand this behavior. Can someone guide me? Thanks.

Code:
typedef struct{
   byte nMsgSize;
   byte nDeviceAddress;
   byte nRegisterLocation;
   byte nValue;
} I2C_Output_Mode;
I2C_Output_Mode mOutput;

task main()
{
   SetSensorType(S1, sensorI2CCustom);
   wait1Msec(5); //initialize time
   nI2CBytesReady[S1] = 0;  //clear buffer
   
   long encoderposition = 0;
   byte replyMessage[4];
   
   //wait until port is ready for use
   while(nI2CStatus[S1] == STAT_COMM_PENDING)
      wait1Msec(2);

   //reset encoder motor2
   mOutput.nMsgSize = 3;
   mOutput.nDeviceAddress = 0x02;
   mOutput.nRegisterLocation = 0x47;
   mOutput.nValue = 3;

   //sendI2CMsg(S1, mOutput.nMsgSize, 0);
   
   while(nI2CStatus[S1] == STAT_COMM_PENDING)
            wait1Msec(2);
   
   while(true)
   {
      
      nI2CBytesReady[S1] = 0;  //clear buffer
   
      //read encoder motor2
      mOutput.nMsgSize = 2;
      mOutput.nDeviceAddress = 0x02;
      mOutput.nRegisterLocation = 0x50;
   
      sendI2CMsg(S1, mOutput.nMsgSize, 4);   
      
      while(nI2CStatus[S1] == STAT_COMM_PENDING)
            wait1Msec(2);
      
      
      if(nI2CStatus[S1] == NO_ERR)
      {
         readI2CReply(S1, &replyMessage[0], 4);
         encoderposition = replyMessage[3] + (replyMessage[2]<<8) + (replyMessage[1]<<16) + (replyMessage[0]<<24);
         nxtDisplayString(5, "  %i         ", encoderposition);
      }
      else
      {
         nxtDisplayString(3, "i2c err %d", nI2CStatus[S1]);
      }
      
   }
   
      
}

Author:  gcronin [ Tue May 14, 2013 2:38 pm ]
Post subject:  Re: HiTechnic DC Motor Controller I2C Programming

Here are a couple updated attempts. First, I upgrade to RobotC 3.60. Here's a program which is similar to my original, but more generalized. It exhibits exactly the same behavior as the original depending on whether the line "//I2CResetEncoders(S1, 0x02, 1); " is uncommented or not.

//////////RESETS ENCODERS/////////////////////////////////////////////
//
// Sample Usage: I2CResetEncoders(S1, 0x02, 1);
//
// that would be for first daisy chained motor controller, motor #1
///////////////////////////////////////////////////////////////////////

void I2CResetEncoders(tSensors nPortIndex, sbyte I2Caddress, int MotorNumber)
{
sbyte I2Cmessage[4];
sbyte I2CReply[1];
I2Cmessage[0] = 3; // sending 3 bytes
I2Cmessage[1] = I2Caddress; //byte 1 = I2C address of motor controller
if(MotorNumber == 1)
I2Cmessage[2] = 0x44; //byte 2 = address for motor 1 Mode
else if(MotorNumber == 2)
I2Cmessage[2] = 0x47; //byte 2 = address for motor 2 Mode
I2Cmessage[3] = 3; // byte 3 = 11 for mode "reset encoder"
sendI2CMsg(nPortIndex, &I2Cmessage[0], 1);
readI2CReply(nPortIndex, &I2CReply[0], 1);
}

//////////READ ENCODERS/////////////////////////////////////////////
//
// Sample Usage: long currentPosition = I2CReadEncoders(S1, 0x02, 1);
//
// that would be for first daisy chained motor controller, motor #1
///////////////////////////////////////////////////////////////////////

long I2CReadEncoders(tSensors nPortIndex, sbyte I2Caddress, int MotorNumber)
{
sbyte I2Cmessage[3];
sbyte I2CReply[4];
long encoder1position = 0;
I2Cmessage[0] = 2; // sending 2 bytes
I2Cmessage[1] = I2Caddress; //byte 1 = I2C address of motor controller
if(MotorNumber == 1)
I2Cmessage[2] = 0x4C; //byte 2 = address for encoder motor 1
else if(MotorNumber == 2)
I2Cmessage[2] = 0x50; //byte 2 = address for encoder motor 2
sendI2CMsg(nPortIndex, &I2Cmessage[0], 4);
readI2CReply(nPortIndex, &I2CReply[0], 4);
encoder1position = I2CReply[3] + (I2CReply[2]<<8) + (I2CReply[1]<<16) + (I2CReply[0]<<24);
return encoder1position;
}



task main()
{
SetSensorType(S1, sensorI2CCustomFast);
//I2CResetEncoders(S1, 0x02, 1);
wait1Msec(500);
long currentPosition = 0;

while(true)
{
eraseDisplay();
currentPosition = I2CReadEncoders(S1, 0x02, 1);
nxtDisplayString(5, " %i ", currentPosition);
}
}

Here's a program which incorporates several of the overhead error checking functions which Xander has coded for his common.h file in his RobotC 3rd party drivers. This program prints "0" continuously. I was hoping that his error checking would eliminate problems I was seeing, but really this is working worse than the first set of programs.

/**
* Clear out the error state on I2C bus by sending a bunch of dummy
* packets.
* @param nPortIndex the port number
* @param address the I2C address we're sending to
*/
void clearI2CError(tSensors nPortIndex, sbyte address) {
sbyte error_array[2];
error_array[0] = 1; // Message size
error_array[1] = address; // I2C Address

for (int i = 0; i < 5; i++) {
sendI2CMsg(nPortIndex, &error_array[0], 0);
wait1Msec(5);
}
}


/**
* Wait for the I2C bus to be ready for the next message
* @param link the port number
* @return true if no error occured, false if it did
*/
bool waitForI2CBus(tSensors nPortIndex)
{
//TI2CStatus i2cstatus;
while (true)
{
//i2cstatus = nI2CStatus[link];
switch (nI2CStatus[nPortIndex])
//switch(i2cstatus)
{
case NO_ERR:
return true;

case STAT_COMM_PENDING:
break;

case ERR_COMM_CHAN_NOT_READY:
break;

case ERR_COMM_BUS_ERR:
return false;
}
EndTimeSlice();
}
}


//////////RESETS ENCODERS/////////////////////////////////////////////
//
// Sample Usage: I2CResetEncoders(S1, 0x02, 1);
//
// that would be for first daisy chained motor controller, motor #1
//
///////////////////////////////////////////////////////////////////////
bool I2CResetEncoders(tSensors nPortIndex, sbyte I2Caddress, int MotorNumber)
{
sbyte I2Cmessage[4];
I2Cmessage[0] = 3; // sending 3 bytes
I2Cmessage[1] = I2Caddress; //byte 1 = I2C address of motor controller
if(MotorNumber == 1)
I2Cmessage[2] = 0x44; //byte 2 = address for motor 1 Mode
else if(MotorNumber == 2)
I2Cmessage[2] = 0x47; //byte 2 = address for motor 2 Mode
I2Cmessage[3] = 3; // byte 3 = 11 for mode "reset encoder"

if (!waitForI2CBus(nPortIndex))
{
clearI2CError(nPortIndex, I2Caddress);

// Let's try the bus again, see if the above packets flushed it out
if (!waitForI2CBus(nPortIndex))
return false;
}

// send the message
sendI2CMsg(nPortIndex, &I2Cmessage[0], 0);

//check to see if the message got sent correctly, try again once if it did not
if (!waitForI2CBus(nPortIndex))
{
clearI2CError(nPortIndex, I2Caddress);
sendI2CMsg(nPortIndex, &I2Cmessage[0], 0); //send the message
if (!waitForI2CBus(nPortIndex))
return false;
}
return true;
}

//////////READ ENCODERS/////////////////////////////////////////////
//
// Sample Usage: long currentPosition = I2CReadEncoders(S1, 0x02, 1);
//
// that would be for first daisy chained motor controller, motor #1
// function returns encoder position OR -1 for error
//
///////////////////////////////////////////////////////////////////////

long I2CReadEncoders(tSensors nPortIndex, sbyte I2Caddress, int MotorNumber)
{
sbyte I2Cmessage[3];
sbyte I2CReply[4];
long encoder1position = 0;
I2Cmessage[0] = 2; // sending 2 bytes
I2Cmessage[1] = I2Caddress; //byte 1 = I2C address of motor controller
if(MotorNumber == 1)
I2Cmessage[2] = 0x4C; //byte 2 = address for encoder motor 1
else if(MotorNumber == 2)
I2Cmessage[2] = 0x50; //byte 2 = address for encoder motor 2

if (!waitForI2CBus(nPortIndex))
{
clearI2CError(nPortIndex, I2Caddress);

// Let's try the bus again, see if the above packets flushed it out
if (!waitForI2CBus(nPortIndex))
return -1;
}

sendI2CMsg(nPortIndex, &I2Cmessage[0], 4); //send the message
readI2CReply(nPortIndex, &I2CReply[0], 4); //read the reply

//check to see if the message got sent correctly, try again once if it did not
if (!waitForI2CBus(nPortIndex))
{
clearI2CError(nPortIndex, I2Caddress);
sendI2CMsg(nPortIndex, &I2Cmessage[0], 4); //send the message
readI2CReply(nPortIndex, &I2CReply[0], 4); //read the reply
if (!waitForI2CBus(nPortIndex))
return -1;
}
encoder1position = I2CReply[3] + (I2CReply[2]<<8) + (I2CReply[1]<<16) + (I2CReply[0]<<24);
return encoder1position;
}



task main()
{
SetSensorType(S1, sensorI2CCustom);
wait1Msec(2);
nI2CBytesReady[S1] = 0; //clear buffer

long currentPosition = 0;

while(true)
{
nI2CBytesReady[S1] = 0; //clear buffer
currentPosition = I2CReadEncoders(S1, 0x02, 1);
nxtDisplayString(5, " %i ", currentPosition);
}
}

Author:  mightor [ Tue May 14, 2013 2:50 pm ]
Post subject:  Re: HiTechnic DC Motor Controller I2C Programming

I would love to help you, but I don't have any motors/encoders to test this with. I have the controllers but that won't help me much. I have asked HT for some motors so I can debug this kind of thing.

Btw, why are you using sbyte and not ubyte?

= Xander

Author:  mightor [ Tue May 14, 2013 3:01 pm ]
Post subject:  Re: HiTechnic DC Motor Controller I2C Programming

I'll see if I can make the controllers talk, een if there are no motors connected to it.

= Xander

Author:  mightor [ Tue May 14, 2013 3:22 pm ]
Post subject:  Re: HiTechnic DC Motor Controller I2C Programming

I modified your program to make it use my I2C framework. Let me know if this works for you. I only get 0 back for obvious reasons.

= Xander

PS: enclose your code in [ code] [/ code] tags (minus the spaces) to make it more readable.

Code:
#pragma config(Sensor, S1,     M_CONTROL,      sensorI2CCustom)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "common.h"

tByteArray I2Crequest;
tByteArray I2Cresponse;

bool I2CResetEncoders(tSensors nPortIndex, ubyte I2Caddress, int MotorNumber)
{
  I2Crequest[0] = 3;
  I2Crequest[1] = I2Caddress;
  I2Crequest[2] = (MotorNumber == 1) ? 0x44 : 0x47;
  I2Crequest[3] = 3;

  return writeI2C(nPortIndex, I2Crequest);
}


long I2CReadEncoders(tSensors nPortIndex, ubyte I2Caddress, int MotorNumber)
{
  I2Crequest[0] = 2;
  I2Crequest[1] = I2Caddress;
  I2Crequest[2] = (MotorNumber == 1) ? 0x4C : 0x50;

  if (!writeI2C(nPortIndex, I2Crequest, I2Cresponse, 4))
    return -1;

  return I2Cresponse[3] + (I2Cresponse[2]<<8) + (I2Cresponse[1]<<16) + (I2Cresponse[0]<<24);
}


task main()
{
  long currentPosition = 0;

  while(true)
  {
    currentPosition = I2CReadEncoders(M_CONTROL, 0x02, 1);
    nxtDisplayString(5, " %i ", currentPosition);
    wait1Msec(50);
  }
}

Author:  maths222 [ Wed May 15, 2013 1:00 pm ]
Post subject:  Re: HiTechnic DC Motor Controller I2C Programming

This is rather complicated full-robot teleop use of HT motor and servo controllers, but here it is: https://code.google.com/p/hprobotics/source/browse/trunk/2011-2012/3785/Teleop.c

Author:  gcronin [ Wed May 15, 2013 8:12 pm ]
Post subject:  Re: HiTechnic DC Motor Controller I2C Programming

Thanks Xander, for the code and for the tip about HTML formatting for code. I tested out your code briefly today and found that I have the same basic issue; the function to reset encoders, once called, screws up subsequent encoder readings. So, to be clearer, the program you wrote works perfectly. However, if I add a call to reset the encoders, either at the beginning of task main, or at some later point in the program, all subsequent encoder readings default to zero as soon as the motor has stopped spinning.

For example, consider the code below. The pragma config and the functions are the ones you wrote. At the beginning, the NXT displays 0, then displays an increasingly positive number if the motor is turned clockwise. The number on the NXT remains the same if the motor is stopped spinning. When the encoder reaches 5000, it does reset to zero, but at that point spinning the motor no longer results in an increasingly positive number being displayed... the display jumps between some small number if the motor is actively spinning, and zero once the motor has stopped.

Interestingly, if I power cycle the 12V battery running the motor controller, then the code works again; eg I see encoder values which makes sense until the resetencoder function is called again.

I'll keep playing with this, as this could be a simple bug which doesn't manifest itself in other ways. But I hate behavior I don't understand... it makes me paranoid :) :)

Code:
task main()
{
   long currentPosition = 0;

  while(true)
  {
    currentPosition = I2CReadEncoders(M_CONTROL, 0x02, 1);
    nxtDisplayString(5, " %i ", currentPosition);
    if(currentPosition > 5000)
       I2CResetEncoders(M_CONTROL, 0x02, 1);
    wait1Msec(50);

  }
}

Author:  gcronin [ Wed May 15, 2013 9:26 pm ]
Post subject:  Re: HiTechnic DC Motor Controller I2C Programming

So the weird encoder reset issue was the only bug I could find. For anyone out there who is interested, the following functions seem to work great. I haven't done extensively testing, but I saw all these function work with both motors 1 and 2.

Code:
///MOVE TO ENCODER POSITION////////////////////////////
// Power should be set 0 to 100
// Position can be positive or negative
///////////////////////////////////////////////////////////////

bool I2CMoveToPositionMotor(tSensors nPortIndex, ubyte I2Caddress, int MotorNumber, int power, long position)
{
  I2Crequest[0] = 8;
  I2Crequest[1] = I2Caddress;
  I2Crequest[2] = (MotorNumber == 1) ? 0x40 : 0x46;
  I2Crequest[3] = (MotorNumber == 1) ? (position>>24)&0xFF : power&0xFF;
  I2Crequest[4] = (MotorNumber == 1) ? (position>>16)&0xFF : 2;
  I2Crequest[5] = (MotorNumber == 1) ? (position>>8)&0xFF : (position>>24)&0xFF;
  I2Crequest[6] = (MotorNumber == 1) ? position&0xFF : (position>>16)&0xFF;
  I2Crequest[7] = (MotorNumber == 1) ? 2 : (position>>8)&0xFF;
  I2Crequest[8] = (MotorNumber == 1) ? power&0xFF : position&0xFF;

  return writeI2C(nPortIndex, I2Crequest);
}

///MOVE CONSTANT SPEED////////////////////////////
// Speed should be set -100 to 100
///////////////////////////////////////////////////////////////

bool I2CMoveConstantSpeed(tSensors nPortIndex, ubyte I2Caddress, int MotorNumber, int speed)
{
   I2Crequest[0] = 4;
  I2Crequest[1] = I2Caddress;
  I2Crequest[2] = (MotorNumber == 1) ? 0x44 : 0x46;
  I2Crequest[3] = (MotorNumber == 1) ? 1 : speed&0xFF;
  I2Crequest[4] = (MotorNumber == 1) ? speed&0xFF : 1;

  return writeI2C(nPortIndex, I2Crequest);
  }

///////////READ BUSY MODE BIT////////////////////
// 
//  Returns 1 if moving to a position
//  Returns 0 if reached position
//  Returns -1 as error code for failed attempt
//
///////////////////////////////////////////////////////////////

int I2CReadBusyBit(tSensors nPortIndex, ubyte I2Caddress, int MotorNumber)
{
   I2Crequest[0] = 2;
  I2Crequest[1] = I2Caddress;
  I2Crequest[2] = (MotorNumber == 1) ? 0x44 : 0x47;

  if (!writeI2C(nPortIndex, I2Crequest, I2Cresponse, 1))
    return -1;

  return (I2Cresponse[0]>>7)&0x01;
}

Author:  Ernest3.14 [ Sat May 18, 2013 2:15 pm ]
Post subject:  Re: HiTechnic DC Motor Controller I2C Programming

Another option is to store your encoder value in a variable (`int prevEncoderValue`) just before you reset the encoder, and add that to your current encoder value when you need to use it.

Page 1 of 1 All times are UTC - 5 hours [ DST ]
Powered by phpBB © 2000, 2002, 2005, 2007 phpBB Group
http://www.phpbb.com/