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

[HaWe]

again, you may get one of mine...
(if you want to bury the hatchet)

[mightor]

Doc,

I can assure that it has nothing to do with our colourful past. I just don't feel comfortable borrowing hardware from people in general; it also creates a certain level of pressure and expectation which is something that I don't want. I contacted some folks at Lego Education to see if there is any way for them to give me access to this stuff. I've been told they will look around for some stuff for me. I will keep you posted.

- Xander

[h-g-t]

This link does not have any code but it does describe the contents of the registers - http://nxt-python.googlecode.com/issues ... 7802772f0f.

This one contains Python code - http://code.google.com/p/nxt-python/iss ... ics2.patch&

This is a MyBlock I think -

HiTechnicMoveMotors.zip

Hightechnic Motor Myblock

(643.85 KiB) Downloaded 249 times

This discusses timing problems with the controller - http://robotc.net/forums/viewtopic.php? ... 38&start=0

This is a Myblock (probably same as above) -

HiTechnicMoveMotors.zip

Hightechnic Motor Myblock

(643.85 KiB) Downloaded 249 times

And, finally, this is a link to the Tetrix spec page - http://www.tetrixrobotics.com/Building_ ... px?aid=112

[HaWe]

the problem is:
with the test program the motors sometime don't start running although they are commanded to do theis, and sometimes they keep on running although the command is "stop".
Sometimes it's the left, sometimes the right motor. Sometimes both.
The i2c commands have waits until the bus is not busy and/or no error, nevertheless not every command is executed correctly.
Most of the times also no bus error is indicated (but admittedly in very few cases though a bus error -32)

[HaWe]

it's still the problem as described here:

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: sometimes a motor doen't start running (while the other one does), another time 1 motor doesn't stop but keeps on running (while the other one stops correctly), sometimes both motors don't follow the commands.
Only in very few cases a i2s bus error is indicated.

[h-g-t]

Came across this website - http://mrdshitechnic.codeplex.com/wikip ... Controller

It said

"It takes some time for Servo or Motor update requests to be processed. If you send requests too fast, they will queue up. This results in behavior that causes servos/motors to keep moving long after they should have reached their target. For example, this happens if you try to control servos using a joystick. To get around this problem, you can "throttle" the request. See the ServoControllerTest and MotorControllerTest VPL programs for examples of how to do this.

Sometimes the polling will interfere with update commands and there will be a communications error on the controller. This causes the LEGO Brick service to reset the controller. Unfortunately, this disables the servos. It therefore appears that the service has died. To resolve this problem temporarily, the Disable command has been removed from the initialization code."

[HaWe]

thx, quite interesting what you wrote.
Actually, waiting some 50 ms would not matter if both motor movements were independent. But I'm using both motors for the wheel drive and for going straight forward both motors have to run and stop strictly simultaneously.
So how to start both motors at the same time without having the robot making a small turn when starting?

[mattallen37]

You should be able to write to all the control registers at the same time, and have that "50ms wait" between each complete write. IIRC, there are 12 motor control registers, so instead of only updating 6 at a time (one motor's worth), just update all 12 at the same time, and wait for the time in between each complete write.

Essentially, if I were to do this, I would have a dummy set of registers (an array). I would update that as often as I want, and have the I2C write command in another loop (with the necessary wait), writing the set of real registers with the dummy registers (array). To be sure that it doesn't write the data until you have completely updated the registers, you can use a flag system (guaranteeing that the motors get the power/update at the same time).

[HaWe]

IIRC, that doesn't work with the existing NXC i2c commands yet because (inter alia) for the HT muxer variable register addresses are needed.

[h-g-t]

The same site also said
!You can change one motor at a time using the MotorPowerUpdate operation. Alternatively you can update both at the same time using the DrivePowerUpdate operation.

There are also MotorStateUpdate and DriveStateUpdate operations that change both the motor mode and power at the same time.

Mode has the following format (bits from MSB to LSB):
Busy | Error | Unused | NTO | Rev | Lock | Sel1 | Sel0
NTO means No Timeout. It must be set on BOTH motors in order for it to work.
Rev reverses the direction of the motor.
Lock is not implemented in the firmware.
Sel1 and Sel0 define the Run mode of the motor:

Run in Power Control mode: 0,0
Run in Constant Speed mode: 0,1
Run to Position mode: 1,0 (Requires Encoders)
Reset Encoders: 1,1

See the MotorControllerMode enum for more information.

Changing the PID values is not currently implemented by this service. In any case, the documentation suggests that they should be left alone.

Generic contracts for Motors and a Drive have not yet been implemented.

Encoders are not yet implemented."

In one of the ones I mentioned in a previous post (http://robotc.net/forums/viewtopic.php? ... 38&start=0) it said that the total time to process a command could be as long as 250ms.