View unanswered posts | View active topics It is currently Wed Jul 23, 2014 1:26 pm






Reply to topic  [ 9 posts ] 
HiTechnic DC Motor Controller I2C Programming 
Author Message
Rookie

Joined: Thu Mar 24, 2011 8:45 pm
Posts: 12
Post 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]);
      }
      
   }
   
      
}


Mon May 13, 2013 9:03 pm
Profile
Rookie

Joined: Thu Mar 24, 2011 8:45 pm
Posts: 12
Post 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);
}
}


Tue May 14, 2013 2:38 pm
Profile
Moderator
Moderator
User avatar

Joined: Wed Mar 05, 2008 8:14 am
Posts: 3162
Location: Rotterdam, The Netherlands
Post 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

_________________
| Professional Conduit of Reasonableness
| (Title bestowed upon on the 8th day of November, 2013)
| My Blog: I'd Rather Be Building Robots
| ROBOTC 3rd Party Driver Suite: [Project Page]


Tue May 14, 2013 2:50 pm
Profile WWW
Moderator
Moderator
User avatar

Joined: Wed Mar 05, 2008 8:14 am
Posts: 3162
Location: Rotterdam, The Netherlands
Post 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

_________________
| Professional Conduit of Reasonableness
| (Title bestowed upon on the 8th day of November, 2013)
| My Blog: I'd Rather Be Building Robots
| ROBOTC 3rd Party Driver Suite: [Project Page]


Tue May 14, 2013 3:01 pm
Profile WWW
Moderator
Moderator
User avatar

Joined: Wed Mar 05, 2008 8:14 am
Posts: 3162
Location: Rotterdam, The Netherlands
Post 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);
  }
}

_________________
| Professional Conduit of Reasonableness
| (Title bestowed upon on the 8th day of November, 2013)
| My Blog: I'd Rather Be Building Robots
| ROBOTC 3rd Party Driver Suite: [Project Page]


Tue May 14, 2013 3:22 pm
Profile WWW
Rookie
User avatar

Joined: Wed Dec 01, 2010 5:15 pm
Posts: 30
Post 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


Wed May 15, 2013 1:00 pm
Profile
Rookie

Joined: Thu Mar 24, 2011 8:45 pm
Posts: 12
Post 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);

  }
}


Wed May 15, 2013 8:12 pm
Profile
Rookie

Joined: Thu Mar 24, 2011 8:45 pm
Posts: 12
Post 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;
}


Wed May 15, 2013 9:26 pm
Profile
Professor
User avatar

Joined: Sat May 18, 2013 1:24 pm
Posts: 272
Location: Olympia, WA
Post 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.

_________________
FTC Team 6424, the 'Oly Cow - Chief programmer.
FRC Team 4450, Olympia Robotics Federation (ORF).

and also quadrotors. Quadrotors!


Sat May 18, 2013 2:15 pm
Profile
Display posts from previous:  Sort by  
Reply to topic   [ 9 posts ] 

Who is online

Users browsing this forum: No registered users and 2 guests


You cannot post new topics in this forum
You cannot reply to topics in this forum
You cannot edit your posts in this forum
You cannot delete your posts in this forum
You cannot post attachments in this forum

Search for:
Jump to:  



Powered by phpBB © 2000, 2002, 2005, 2007 phpBB Group.
Designed by ST Software for PTF.