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

[HaWe]

I wish I had one of each of these controllers so I could test this code

so you need only 1 controller (muxer)?
and what about the encoders attached to that controller?

but back to the error code -32:
what does that mean?

[mightor]

Try putting a "ResetSensor(<PORT>);" after the SetSensorType() call.

I have two encoders that came with the set, not sure if those are the ones I'm supposed to have.

- Xander

[HaWe]

thx Xander, thas was the crucial clue I guess!
Nevertheless some commands didn't execute everytime, although I tried an intermediate Wait(10);

[HaWe]

I substituted the Yield() in your code by Wait(1) -
Now all motors work fine even simultaneously and without intermediate aditional Wait - and no error messages appear any more!
This is the final source code so far for TXMotorOn (= Tetrix motor on fwd /rev with PWM):

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); \
}

void TXMotorOn(byte NXTport, byte TXmotor, char percentage)
{
  byte retLen = 0;
  int i, j;
  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);

}

task main(){

  SetSensorType(S1, SENSOR_TYPE_LOWSPEED);
  ResetSensor(S1);
  Wait(20);


  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); // coast
  TXMotorOn(0, 1, -128);   // brake
  Wait(2000);
  while (true);
}

[mightor]

Glad you got it all working in the end

- Xander

[HaWe]

yeah, although sometimes it's still shaky with those motor comands.

But now the last question of the Old Year:

How do I get the encoder values?

Code: Select all

reg        v.type   meaning
---------------------------------------------------------------
44H      byte     Motor 1 mode
45H      s/byte   Motor 1 power
46H      s/byte   Motor 2 power
47H      byte     Motor 2 mode 

40H–43H  s/long   Motor 1 target encoder value, high byte first
48 –4BH  s/long   Motor 2 target encoder value, high byte first
4C –4FH  s/long   Motor 1 current encoder value, high byte first
50 –53H  s/long   Motor 2 current encoder value, high byte first

my approach:

(edit: see below!)

============
already now:
all the best for the New Year to all!

[HaWe]

Happy New Year to all!


thinking about my last approach I found some mistakes. This is my new try to read the encoder values (4 bytes all at once) -
is this correct?
And what's the best way to transfer the values to a long integer that can be returned to main()?

Code: Select all

long GetTXMotorCounter(byte NXTport, byte TXmotor)
{
  byte devAddr;
  byte bufLen = 4;      // <===== 4 byte for long ?????
  byte encbuf[];        // <===== array of 4 bytes ?????
  long IOresult;
  byte i2cMsg[];
  long counter;

  // addresses and registers
  devAddr = (TXmotor+2)&14;
  
  if (TXmotor==0) {
    ArrayBuild(i2cMsg, devAddr,  0x4C,0x4D,0x4E,0x4F, encbuf);         // <===== ?????
  }
  else
  if (TXmotor==1) {
    ArrayBuild(i2cMsg, devAddr,  0x50,0x51,0x52,0x53, encbuf);         // <===== ?????
  }
  
  while ((I2CCheckStatus(NXTport) == STAT_COMM_PENDING) && (I2CCheckStatus(NXTport) != NO_ERR)) Wait(1);

  IOresult= I2CRead(NXTport, bufLen, i2cMsg);  // <===== ????? need to wait until no i2c IOresult error?

  counter=.....?        // <===== ????? long counter out of 4 single bytes?

  return(counter);

}

[mightor]

If you have 4 bytes that go from LSB to MSB then you can add them together like this:
long_value = B1 + (B2 << 8) + (B3 << 16) + (B4 << 24)

- Xander

[HaWe]

thanks for your quick reply!
actually I'm very unsure if my i2cRead is correct to poll 4 register bytes:

Code: Select all

*SNIP*
  if (TXmotor==0) {
    ArrayBuild(i2cMsg, devAddr,  0x4C,0x4D,0x4E,0x4F, encbuf);
//...
  while ((I2CCheckStatus(NXTport) == STAT_COMM_PENDING) && (I2CCheckStatus(NXTport) != NO_ERR)) Wait(1);
  IOresult= I2CRead(NXTport, bufLen, i2cMsg); 
}
*SNIP*

is this correct how I wrote this?

[mightor]

Here is some example code for reading multiple bytes from a sensor:

Code: Select all

long _DGPSreadRegister(byte link, unsigned byte command) {
  const byte msgLen = 4;
  byte sendMsg[];
  byte replyMsg[];

  ArrayBuild(sendMsg, DGPS_I2C_ADDR, command);   // <-- you will need to modify that into something like this:
  // ArrayBuild(sendMsg, DGPS_I2C_ADDR, START_OF_ENCODER_REGISTER);   // Only -one- register, the first one, is requested

  while ((I2CCheckStatus(link) == STAT_COMM_PENDING) && (I2CCheckStatus(link) != NO_ERR)) Yield();
  if(!I2CBytes(link, sendMsg, msgLen, replyMsg))
    return 0;

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