NXC: I²C drivers for Hitechnic Tetrix motor muxer ?

[HaWe]

ok,
but what about this line:

Code: Select all

  if(!I2CBytes(link, sendMsg, msgLen, replyMsg))    return 0;

shouldn't this line be deleted as I don't want an encoder value of zero to be returned if the i2c function fails? (That would cause faulty odometric readings and calculations!)

[mightor]

It was an example piece of code of how to read multiple bytes and reassemble them into a long value. If you prefer to return -1 or whatever when you have a failed I2C call, by all means change the code. As it was, it wasn't suitable for your needs anyway, not without some tweaks. This came from the Dexter Industries GPS sensor driver that I wrote a while back.

- Xander

[HaWe]

uuuhhh - the server again is unbearable slow (1 minute to load "edit" or "reply"...
so you again have been faster than me....

the point is, it must return only correct values and must repeat polling as long as it needs until it gets them...

if it should be deleted, the function would look like this:

Code: Select all

long GetTXMotorCounter(byte NXTport, byte TXmotor)
{
  byte devAddr;
  const byte msgLen = 4;
  byte sendMsg[];
  byte replyMsg[];
  byte encStartReg;

  devAddr = (TXmotor+2)&14;
  if ((TXmotor%2)==0) encStartReg = 0x4C;
  else encStartReg = 0x50;

  ArrayBuild(sendMsg, devAddr, encStartReg);
  // ArrayBuild(sendMsg, DGPS_I2C_ADDR, START_OF_ENCODER_REGISTER);   // Only -one- register, the first one, is requested

  while ((I2CCheckStatus(NXTport) == STAT_COMM_PENDING) && (I2CCheckStatus(NXTport) != NO_ERR)) Wait(1);

  /*  deleted  */

  // Reassemble the messages, depending on their expected size.
  return replyMsg[3] + (replyMsg[2] << 8) + (replyMsg[1] << 16) + (replyMsg[0] << 24);

}

ok, and link = NXTport...

[HaWe]

if I run this program, I get File Error -1 (?????)

Code: Select all

#define printf5( _x, _y, _format1,_format2,_format3,_format4,_format5,_value1,_value2,_value3,_value4,_value5) { \
  string sval1 = FormatNum(_format1, _value1); \
  string sval2 = FormatNum(_format2, _value2); \
  string sval3 = FormatNum(_format3, _value3); \
  string sval4 = FormatNum(_format4, _value4); \
  string sval5 = FormatNum(_format5, _value5); \
  string s =sval1+sval2+sval3+sval4+sval5; \
  TextOut(_x, _y, s); \
}

#define printf2( _x, _y, _format1, _format2, _value1, _value2) { \
  string sval1 = FormatNum(_format1, _value1); \
  string sval2 = FormatNum(_format2, _value2); \
  string s =sval1+sval2; \
  TextOut(_x, _y, s); \
}

void TXMotorOn(byte NXTport, byte TXmotor, char percentage)
{
  byte retLen = 0;
  char modeMsg[];
  char powerMsg[];
  byte devAddr, modeReg, powerReg;
  char IOresult;

  // addresses and registers
  devAddr = (TXmotor+2)&14;
  modeReg = 0x44 + (TXmotor % 2)*3;
  powerReg= 0x45 + (TXmotor % 2);

  ArrayBuild(modeMsg, devAddr, modeReg, 0);
  ArrayBuild(powerMsg, devAddr, powerReg, percentage);
  
  // Send the first message as soon as the bus is ready

  TextOut(0,56,"M Rg md pow error");
  while ((I2CCheckStatus(NXTport) == STAT_COMM_PENDING) && (I2CCheckStatus(NXTport) != NO_ERR)) Wait(1);
  IOresult=I2CWrite(NXTport, retLen, modeMsg);
  if (IOresult != NO_ERR)
    { printf5(0,48-(8*TXmotor),"%d","%3d","%2d","%5d","%4d",TXmotor,modeMsg[1],modeMsg[2],powerMsg[2],IOresult);
      return;  } // for Error debug
  printf5(0,48-(8*TXmotor),"%d","%3d","%2d","%5d","%4d",TXmotor,modeMsg[1],modeMsg[2],powerMsg[2],IOresult);


  // Send the second message when the first one is done
  ArrayBuild(powerMsg, devAddr, powerReg, percentage);
  while ((I2CCheckStatus(NXTport) == STAT_COMM_PENDING) && (I2CCheckStatus(NXTport) != NO_ERR)) Wait(1);
  IOresult=I2CWrite(NXTport, retLen, powerMsg);
  if (IOresult != NO_ERR)
    { printf5(0,48-(8*TXmotor),"%d","%3d","%2d","%5d","%4d",TXmotor,modeMsg[1],modeMsg[2],powerMsg[2],IOresult);
    return;  } // for Error debug

  printf5(0,48-(8*TXmotor),"%d","%3d","%2d","%5d","%4d",TXmotor,modeMsg[1],modeMsg[2],powerMsg[2],IOresult);

}

long GetTXMotorCounter(byte NXTport, byte TXmotor)
{
  byte devAddr;
  const byte msgLen = 4;
  byte sendMsg[];
  byte replyMsg[];
  byte encStartReg;

  devAddr = (TXmotor+2)&14;
  if (TXmotor==0) encStartReg = 0x4C;
  else
  if (TXmotor==1) encStartReg = 0x50;

  ArrayBuild(sendMsg, devAddr, encStartReg);
  // ArrayBuild(sendMsg, DGPS_I2C_ADDR, START_OF_ENCODER_REGISTER);   // Only -one- register, the first one, is requested

  while ((I2CCheckStatus(NXTport) == STAT_COMM_PENDING) && (I2CCheckStatus(NXTport) != NO_ERR)) Wait(1);

  /*  deleted  */

  // Reassemble the messages, depending on their expected size.
  return replyMsg[3] + (replyMsg[2] << 8) + (replyMsg[1] << 16) + (replyMsg[0] << 24);

}

task main(){
  long encoder;
  
  SetSensorType(S1, SENSOR_TYPE_LOWSPEED);
  ResetSensor(S1);
  Wait(20);
  
  encoder= GetTXMotorCounter(0, 0);
  printf("%7d", encoder);

  TXMotorOn(0, 0, 50);
  TXMotorOn(0, 1, 20);
  Wait(2000);
  encoder= GetTXMotorCounter(0, 0);
  printf("%7d", encoder);

  TXMotorOn(0, 0,  0);  // brake
  TXMotorOn(0, 1,  0);  // brake
  Wait(2000);
  encoder= GetTXMotorCounter(0, 0);
  printf("%7d", encoder);

  TXMotorOn(0, 0, -50);
  TXMotorOn(0, 1, -20);
  Wait(2000);
  encoder= GetTXMotorCounter(0, 0);
  printf("%7d", encoder);

  TXMotorOn(0, 0,  0); // coast
  TXMotorOn(0, 1, -128);   // brake
  Wait(2000);
  encoder= GetTXMotorCounter(0, 0);
  printf("%7d", encoder);
  
  while (true);
}

[mightor]

Where's the code that does the I2CBytes() call?

- Xander

[HaWe]

is

Code: Select all

!I2CBytes(...)

the i2C call? I simply didn't recognize this!

if I take this line in again it's working somehow...:

Code: Select all

#define printf5( _x, _y, _format1,_format2,_format3,_format4,_format5,_value1,_value2,_value3,_value4,_value5) { \
  string sval1 = FormatNum(_format1, _value1); \
  string sval2 = FormatNum(_format2, _value2); \
  string sval3 = FormatNum(_format3, _value3); \
  string sval4 = FormatNum(_format4, _value4); \
  string sval5 = FormatNum(_format5, _value5); \
  string s =sval1+sval2+sval3+sval4+sval5; \
  TextOut(_x, _y, s); \
}

#define printf2( _x, _y, _format1, _format2, _value1, _value2) { \
  string sval1 = FormatNum(_format1, _value1); \
  string sval2 = FormatNum(_format2, _value2); \
  string s =sval1+sval2; \
  TextOut(_x, _y, s); \
}

#define printf1( _x, _y, _format1, _value1) { \
  string sval1 = FormatNum(_format1, _value1); \
  TextOut(_x, _y, sval1); \
}

void TXMotorOn(byte NXTport, byte TXmotor, char percentage)
{
  byte retLen = 0;
  char modeMsg[];
  char powerMsg[];
  byte devAddr, modeReg, powerReg;
  char IOresult;

  // addresses and registers
  devAddr = (TXmotor+2)&14;
  modeReg = 0x44 + (TXmotor % 2)*3;
  powerReg= 0x45 + (TXmotor % 2);

  ArrayBuild(modeMsg, devAddr, modeReg, 0);
  ArrayBuild(powerMsg, devAddr, powerReg, percentage);
  
  // Send the first message as soon as the bus is ready

  TextOut(0,56,"M Rg md pow error");
  while ((I2CCheckStatus(NXTport) == STAT_COMM_PENDING) && (I2CCheckStatus(NXTport) != NO_ERR)) Wait(1);
  IOresult=I2CWrite(NXTport, retLen, modeMsg);
  if (IOresult != NO_ERR)
    { printf5(0,48-(8*TXmotor),"%d","%3d","%2d","%5d","%4d",TXmotor,modeMsg[1],modeMsg[2],powerMsg[2],IOresult);
      return;  } // for Error debug
  printf5(0,48-(8*TXmotor),"%d","%3d","%2d","%5d","%4d",TXmotor,modeMsg[1],modeMsg[2],powerMsg[2],IOresult);


  // Send the second message when the first one is done
  ArrayBuild(powerMsg, devAddr, powerReg, percentage);
  while ((I2CCheckStatus(NXTport) == STAT_COMM_PENDING) && (I2CCheckStatus(NXTport) != NO_ERR)) Wait(1);
  IOresult=I2CWrite(NXTport, retLen, powerMsg);
  if (IOresult != NO_ERR)
    { printf5(0,48-(8*TXmotor),"%d","%3d","%2d","%5d","%4d",TXmotor,modeMsg[1],modeMsg[2],powerMsg[2],IOresult);
    return;  } // for Error debug

  printf5(0,48-(8*TXmotor),"%d","%3d","%2d","%5d","%4d",TXmotor,modeMsg[1],modeMsg[2],powerMsg[2],IOresult);

}

long GetTXMotorCounter(byte NXTport, byte TXmotor)
{
  byte devAddr;
  const byte msgLen = 4;
  byte sendMsg[];
  byte replyMsg[];
  byte encStartReg;

  devAddr = (TXmotor+2)&14;
  if (TXmotor==0) encStartReg = 0x4C;
  else
  if (TXmotor==1) encStartReg = 0x50;

  ArrayBuild(sendMsg, devAddr, encStartReg);
  // ArrayBuild(sendMsg, DGPS_I2C_ADDR, START_OF_ENCODER_REGISTER);   // Only -one- register, the first one, is requested

  while ((I2CCheckStatus(NXTport) == STAT_COMM_PENDING) && (I2CCheckStatus(NXTport) != NO_ERR)) Wait(1);

  if(!I2CBytes(NXTport, sendMsg, msgLen, replyMsg))
    return 0;
  //I2CBytes(NXTport, sendMsg, msgLen, replyMsg);

  // Reassemble the messages, depending on their expected size.
  return replyMsg[3] + (replyMsg[2] << 8) + (replyMsg[1] << 16) + (replyMsg[0] << 24);

}

task main(){
  long encoder;
  
  SetSensorType(S1, SENSOR_TYPE_LOWSPEED);
  ResetSensor(S1);
  Wait(20);
  
  encoder= GetTXMotorCounter(0, 0);
  printf1(0,8,"M1=%7d", encoder);


  TXMotorOn(0, 0, 50);
  TXMotorOn(0, 1, 20);
  Wait(2000);
  encoder= GetTXMotorCounter(0, 0);
  printf1(0,8,"M1=%7d", encoder);


  TXMotorOn(0, 0,  0);  // brake
  TXMotorOn(0, 1,  0);  // brake
  Wait(2000);
  encoder= GetTXMotorCounter(0, 0);
  printf1(0,8,"M1=%7d", encoder);


  TXMotorOn(0, 0, -50);
  TXMotorOn(0, 1, -20);
  Wait(2000);
  encoder= GetTXMotorCounter(0, 0);
  printf1(0,8,"M1=%7d", encoder);


  TXMotorOn(0, 0,  0); // coast
  TXMotorOn(0, 1, -128);   // brake
  Wait(2000);
  encoder= GetTXMotorCounter(0, 0);
  printf1(0,8,"M1=%7d", encoder);

  
  Wait(2000);
  encoder= GetTXMotorCounter(0, 0);
  printf1(0,8,"M1=%7d", encoder);

  
  while (true);
}

- how must it be written if I want only valid values to be returned?

Code: Select all

...while(!I2CBytes(NXTport, sendMsg, msgLen, replyMsg));

?
- and why don't you use I2cRead?

[HaWe]

I did some experiments for a while and managed to implement a new function to reset the encoder counters:

Code: Select all

void ResetTXMotorCounter(byte NXTport, byte TXmotor)
{
  byte retLen = 0;
  char modeMsg[];
  byte devAddr, modeReg;
  char IOresult;

  // addresses and registers
  devAddr = (TXmotor+2)&14;
  modeReg = 0x44 + (TXmotor % 2)*3;

  ArrayBuild(modeMsg, devAddr, modeReg, 3);      // 3 = reset enc value

   // Send the reset message as soon as the bus is ready

  while ((I2CCheckStatus(NXTport) == STAT_COMM_PENDING) && (I2CCheckStatus(NXTport) != NO_ERR)) Wait(1);
  IOresult=I2CWrite(NXTport, retLen, modeMsg);
  if (IOresult != NO_ERR)   { return;  };

}

The good news: it works though it needs a Wait(20) to take effect.

The bad news:
the TXMotorOn function not always starts or brakes both motors - sometimes 1 of the motors simply keeps idle or keeps on running.

Code: Select all

// program Tetrix_Motor_Driver ver. 0.14

#define printf5( _x, _y, _format1,_format2,_format3,_format4,_format5,_value1,_value2,_value3,_value4,_value5) { \
  string sval1 = FormatNum(_format1, _value1); \
  string sval2 = FormatNum(_format2, _value2); \
  string sval3 = FormatNum(_format3, _value3); \
  string sval4 = FormatNum(_format4, _value4); \
  string sval5 = FormatNum(_format5, _value5); \
  string s =sval1+sval2+sval3+sval4+sval5; \
  TextOut(_x, _y, s); \
}

#define printf2( _x, _y, _format1, _format2, _value1, _value2) { \
  string sval1 = FormatNum(_format1, _value1); \
  string sval2 = FormatNum(_format2, _value2); \
  string s =sval1+sval2; \
  TextOut(_x, _y, s); \
}

#define printf1( _x, _y, _format1, _value1) { \
  string sval1 = FormatNum(_format1, _value1); \
  TextOut(_x, _y, sval1); \
}

void TXMotorOn(byte NXTport, byte TXmotor, char percentage)
{
  byte retLen = 0;
  char modeMsg[];
  char powerMsg[];
  byte devAddr, modeReg, powerReg;
  char IOresult;

  // addresses and registers
  devAddr = (TXmotor+2)&14;
  modeReg = 0x44 + (TXmotor % 2)*3;
  powerReg= 0x45 + (TXmotor % 2);

  ArrayBuild(modeMsg, devAddr, modeReg, 0);    // 0= PWM percentage mode
  ArrayBuild(powerMsg, devAddr, powerReg, percentage);
  
  // Send the first message as soon as the bus is ready

  while ((I2CCheckStatus(NXTport) == STAT_COMM_PENDING) && (I2CCheckStatus(NXTport) != NO_ERR)) Wait(1);
  IOresult=I2CWrite(NXTport, retLen, modeMsg);
  if (IOresult != NO_ERR)
    { printf5(0,48-(8*TXmotor),"%d","%3d","%2d","%5d","%4d",TXmotor,modeMsg[1],modeMsg[2],powerMsg[2],IOresult);
      return;  }
  printf5(0,48-(8*TXmotor),"%d","%3d","%2d","%5d","%4d",TXmotor,modeMsg[1],modeMsg[2],powerMsg[2],IOresult);
  Wait(1);

  // Send the second message when the first one is done
  ArrayBuild(powerMsg, devAddr, powerReg, percentage);
  while ((I2CCheckStatus(NXTport) == STAT_COMM_PENDING) && (I2CCheckStatus(NXTport) != NO_ERR)) Wait(1);
  IOresult=I2CWrite(NXTport, retLen, powerMsg);
  if (IOresult != NO_ERR)
    { printf5(0,48-(8*TXmotor),"%d","%3d","%2d","%5d","%4d",TXmotor,modeMsg[1],modeMsg[2],powerMsg[2],IOresult);
    return;  }

  printf5(0,48-(8*TXmotor),"%d","%3d","%2d","%5d","%4d",TXmotor,modeMsg[1],modeMsg[2],powerMsg[2],IOresult);

  Wait(1);
}

void ResetTXMotorCounter(byte NXTport, byte TXmotor)
{
  byte retLen = 0;
  char modeMsg[];
  byte devAddr, modeReg;
  char IOresult;

  // addresses and registers
  devAddr = (TXmotor+2)&14;
  modeReg = 0x44 + (TXmotor % 2)*3;

  ArrayBuild(modeMsg, devAddr, modeReg, 3);      // 3 = reset enc value

   // Send the reset message as soon as the bus is ready

  while ((I2CCheckStatus(NXTport) == STAT_COMM_PENDING) && (I2CCheckStatus(NXTport) != NO_ERR)) Wait(1);
  IOresult=I2CWrite(NXTport, retLen, modeMsg);
  if (IOresult != NO_ERR)   { return;  };
  Wait(10);

}

long GetTXMotorCounter(byte NXTport, byte TXmotor)
{
  byte devAddr;
  const byte msgLen = 4;
  byte sendMsg[];
  byte replyMsg[];
  byte encStartReg;

  devAddr = (TXmotor+2)&14;
  if (TXmotor==0) encStartReg = 0x4C;
  else
  if (TXmotor==1) encStartReg = 0x50;

  ArrayBuild(sendMsg, devAddr, encStartReg);
  // ArrayBuild(sendMsg, DGPS_I2C_ADDR, START_OF_ENCODER_REGISTER);   // Only -one- register, the first one, is requested

  while ((I2CCheckStatus(NXTport) == STAT_COMM_PENDING) && (I2CCheckStatus(NXTport) != NO_ERR)) Wait(1);

  // if(!I2CBytes(NXTport, sendMsg, msgLen, replyMsg)) return ;

  if(I2CBytes(NXTport, sendMsg, msgLen, replyMsg)) {
     return replyMsg[3] + (replyMsg[2] << 8) + (replyMsg[1] << 16) + (replyMsg[0] << 24);
  }

}


task showValues()  {
  long encoder0, encoder1;
  while(1) {
    encoder0= GetTXMotorCounter(0, 0);
    encoder1= GetTXMotorCounter(0, 1);
    Wait(10);
    printf1(0,8,"M1=%7d", encoder0);
    printf1(0,0,"M2=%7d", encoder1);
  }
}

task main(){

  TextOut(0,56,"M Rg md pow error");

  SetSensorType(S1, SENSOR_TYPE_LOWSPEED);
  Wait(20);
  ResetSensor(S1);
  Wait(50);
  StartTask(showValues);

  while (true) {                // test loop
  
    TXMotorOn(0, 0, 50);
    TXMotorOn(0, 1, 20);
    Wait(2000);

    TXMotorOn(0, 0,  0);  // brake
    TXMotorOn(0, 1,  0);  // brake
    Wait(2000);

    TXMotorOn(0, 0, -50);
    TXMotorOn(0, 1, -20);
    Wait(2000);

    TXMotorOn(0, 0,  0);   // brake
    TXMotorOn(0, 1, -128);   // coast
    Wait(2000);

    ResetTXMotorCounter(0,0);
    ResetTXMotorCounter(0,1);
    Wait(2000);
  }
}

Waits in between have no effect, the mistake stays as it is.

Any ideas how to start the motors and keep on trying until it's succesful (not break and return in case of IOresult!= NO_ERROR)?

[HaWe]

it's really weird.
I tried and changed some lines of the code above (current version: 0.14),
but it always remains the same: in about 20 percent of all cases one of the motor commands is not executed - either if it's a run fwd, or a run rev, or a stop command (sometimes it's motor 1, sometimes motor 2, sometimes both).
Enlarging the Wait(1) to Wait(10) doesn't make it better.
EDIT: SOMETIMES an error -32 is displayed, but most of the time it's not.
This is the excerpt:

Code: Select all

void TXMotorOn(byte NXTport, byte TXmotor, char percentage)
{
  byte retLen = 0;
  char modeMsg[];
  char powerMsg[];
  byte devAddr, modeReg, powerReg;
  char IOresult;

  // addresses and registers
  devAddr = (TXmotor+2)&14;
  modeReg = 0x44 + (TXmotor % 2)*3;
  powerReg= 0x45 + (TXmotor % 2);

  ArrayBuild(modeMsg, devAddr, modeReg, 0);    // 0= PWM percentage mode
  ArrayBuild(powerMsg, devAddr, powerReg, percentage);
  
  // Send the first message as soon as the bus is ready

  while ((I2CCheckStatus(NXTport) == STAT_COMM_PENDING) && (I2CCheckStatus(NXTport) != NO_ERR)) Wait(1);
  IOresult=I2CWrite(NXTport, retLen, modeMsg);
  if (IOresult != NO_ERR)
    { printf5(0,48-(8*TXmotor),"%d","%3d","%2d","%5d","%4d",TXmotor,modeMsg[1],modeMsg[2],powerMsg[2],IOresult);
      return;  }
  printf5(0,48-(8*TXmotor),"%d","%3d","%2d","%5d","%4d",TXmotor,modeMsg[1],modeMsg[2],powerMsg[2],IOresult);
  Wait(1);

  // Send the second message when the first one is done
  ArrayBuild(powerMsg, devAddr, powerReg, percentage);
  while ((I2CCheckStatus(NXTport) == STAT_COMM_PENDING) && (I2CCheckStatus(NXTport) != NO_ERR)) Wait(1);
  IOresult=I2CWrite(NXTport, retLen, powerMsg);
  if (IOresult != NO_ERR)
    { printf5(0,48-(8*TXmotor),"%d","%3d","%2d","%5d","%4d",TXmotor,modeMsg[1],modeMsg[2],powerMsg[2],IOresult);
    return;  }

  printf5(0,48-(8*TXmotor),"%d","%3d","%2d","%5d","%4d",TXmotor,modeMsg[1],modeMsg[2],powerMsg[2],IOresult);

  Wait(1);
}

Unfortunately I got no source code examples by Hitechnic since I asked them for NXC drivers more than 4 weeks ago, and so I have no idea what's actually faulty -
maybe it's the Hitechnic Tetrix controller itself?
Does anybody own such a controller and can test it by himself?

[HaWe]

I wish I had one of each of these controllers so I could test this code. It would make debugging this a lot easier.
- Xander

Xander, shall I send you the controller and motors/encoders?

[mightor]

I am not sure I will have the time to work on it. I have a couple of new projects coming my way soon and some things I'm working on right now. I appreciate the offer of you wanting to send them to me, but I just don't have the time for it at the moment.

- Xander