Tetrix, and NXC drivers

Wednesday, I was able to buy a second-hand Pitsco Tetrix set. It was actually the Tetrix and 9797 NXT Combo pack.

When I got it home, I cleaned it up a bit (slightly dusty), and took inventory. It turns out that 2 of the 4 wheels are missing, but that seems to be it. It was (supposedly) only built with once, and never used. Many of the parts were still in sealed bags.

I had to cosmetically fix some of the electrical pieces (the switch in the charger had fallen apart, and the terminals on the motor controller were slightly messed up), but that only took an hour or so.

I also built the “Mantis” (the Tetrix version of the tri-bot i guess). Once it was completed, I started the battery packs (12v tetrix and 7.2v NXT) charging, so they would be nice and full for use Thursday :D

As some of my readers may know, there has been a lack of NXC drivers for the controllers. I really like writing NXC drivers, so I got to working on some for the controllers. Despite my fears (based on other’s lack of success), I was happy to find them as easy to write as any other drivers.

I don’t have Tetrix motor encoders, so I didn’t attempt to support them with the drivers.

Here is the library:

#define TETRIX_ADDRESS_1 0x02         //Daisey chain position 1
#define TETRIX_ADDRESS_2 0x04         //Daisey chain position 2
#define TETRIX_ADDRESS_3 0x06         //Daisey chain position 3
#define TETRIX_ADDRESS_4 0x08         //Daisey chain position 4

#define TETRIX_MOTOR_BRAKE 0          //Motor brake
#define TETRIX_MOTOR_FLOAT -128       //0x80, Motor float

#define TETRIX_MOTOR_REV 0x08         //Reverse bit

#define TETRIX_SERVO_TIME_RESET 0x00  //Restart 10 second timer
#define TETRIX_SERVO_OFF 0xFF         //Turn off/float all servos
#define TETRIX_SERVO_STAY_ON 0xAA     //Disable 10 second timer

void TetrixI2CWrite(byte port, byte addr, byte reg, byte data[])
{
  byte cmdbuf[];                         //register data
  int loop, n, nByteReady;

  ArrayBuild(cmdbuf, addr, reg, data);

  loop = STAT_COMM_PENDING;              //Wait until bus is free
  while (loop == STAT_COMM_PENDING ){    //        ''
    loop = I2CStatus(port, nByteReady);  //        ''
  }                                      //        ''

  n = I2CWrite(port, 0, cmdbuf);        //When the I2C bus is ready, send the message you built
  while (I2CStatus(port, nByteReady) ==  STAT_COMM_PENDING); //Wait until bus is free
}

inline void TetrixSetup(byte port){
  asm{
    setin IN_TYPE_LOWSPEED , port, TypeField        // IN_TYPE_LOWSPEED (not 9v)
    setin IN_TYPE_NO_SENSOR , port, InputModeField

    setin IN_TYPE_SWITCH, port, InvalidDataField
  SensorStillInvalid:
    getin __ResetSensorTmp, port, InvalidDataField
    brtst NEQ, SensorStillInvalid, __ResetSensorTmp
  };
}

char TetrixMotorControlData[4];

void TetrixMotors(byte port, byte addr, char motor1, char motor2, byte options1 = 0, byte options2 = 0){
  TetrixMotorControlData[0]=options1;
  TetrixMotorControlData[3]=options2;
  TetrixMotorControlData[1]=motor1;
  TetrixMotorControlData[2]=motor2;
  TetrixI2CWrite(port, addr, 0x44, TetrixMotorControlData);
}

byte TetrixServoControlData[8];

void TetrixServos(byte port, byte addr, byte servo1, byte servo2, byte servo3, byte servo4, byte servo5, byte servo6, byte step_time = 0, byte pwm = TETRIX_SERVO_TIME_RESET){
  TetrixServoControlData[0]=step_time;
  TetrixServoControlData[1]=servo1;
  TetrixServoControlData[2]=servo2;
  TetrixServoControlData[3]=servo3;
  TetrixServoControlData[4]=servo4;
  TetrixServoControlData[5]=servo5;
  TetrixServoControlData[6]=servo6;
  TetrixServoControlData[7]=pwm;
  TetrixI2CWrite(port, addr, 0x41, TetrixServoControlData);
}

And here is an example:

#include "MyTetrix lib.nxc"

#define TETRIX_PORT S1

char Motor1;
char Motor2;

int Servo1, Servo2, Servo3, Servo4, Servo5, Servo6;

task main(){
  TetrixSetup(TETRIX_PORT);
  while(true){

    Motor1=Random(200)-100;
    Motor2=Random(200)-100;

    TetrixMotors(TETRIX_PORT, TETRIX_ADDRESS_1, Motor1, Motor2, 0x00, TETRIX_MOTOR_REV);

    Servo1=Random(200)+27;
    Servo2=Random(200)+27;
    Servo3=Random(200)+27;
    Servo4=Random(200)+27;
    Servo5=Random(200)+27;
    Servo6=Random(200)+27;

    TetrixServos(TETRIX_PORT, TETRIX_ADDRESS_2, Servo1, Servo2, Servo3, Servo4, Servo5, Servo6, 0, TETRIX_SERVO_TIME_RESET);

    ClearScreen();

    NumOut(0, LCD_LINE1, Motor1);
    NumOut(0, LCD_LINE2, Motor2);
    
    NumOut(0, LCD_LINE3, Servo1);
    NumOut(0, LCD_LINE4, Servo2);
    NumOut(0, LCD_LINE5, Servo3);
    NumOut(0, LCD_LINE6, Servo4);
    NumOut(0, LCD_LINE7, Servo5);
    NumOut(0, LCD_LINE8, Servo6);
    
    Wait(1000);
  }
}

The example is just a while(true) loop, that sets the motors’ and servos’ parameters to random values every second (plus code run time). The example program, is just that, an example. It’s just a PoC (Proof of Concept) program.

To use the example, connect the motor controller to input port 1 of the NXT, and daisy chain the servo controller to the other port of the motor controller. Make sure that the motors can physically do anything (because of the Random values), and that you have the controllers connected to the 12v battery (and the switch is on).

In any program, before you attempt to use either controller, you must set the port up as an I2C port. Normally I use the SerSensorLowspeed command, but in this case I do not. It turns out, that the SetSensorLowspeed command not only sets up the port as I2C, but it also (at least by default) sets it to 9v (supply on the analog line). For most things that makes no difference at all (sometimes it even helps the sensor). For the tetrix controllers, when you daisy-chain them, they must use the analog line as a way to know how many are connected to the chain, and in what place that specific controller is (to set the I2C address). If the line is being held at 9v, they have no way of properly identifying where in the daisy-chain they are. Thus, the sensor port must be setup as an I2C sensor port, in non-9v mode. I could have done this very easily with NXC, but since I had already made a more efficient version in asm (NBC), I just used that. So, before using the controllers, you must have this command in the code:

TetrixSetup(TetrixPort);

TetrixPort is the sensor port that the controllers are connected to.

To use the motor controller, use this command:

TetrixMotors(Port, I2CAddress, Motor1, Motor2, Options1, Options2);

For the “Port”, specify the port on the NXT that the controller is connected to.

For “I2CAddress”, specify the I2C address of the controller. If it is the closest to the NXT in the daisy chain, the address is 0×02. Instead of using a number like that though, you can use (for example) TETRIX_ADDRESS_1 because I #define’d them in the library:

#define TETRIX_ADDRESS_1 0x02         //Daisey chain position 1
#define TETRIX_ADDRESS_2 0x04         //Daisey chain position 2
#define TETRIX_ADDRESS_3 0x06         //Daisey chain position 3
#define TETRIX_ADDRESS_4 0x08         //Daisey chain position 4

Motor1 and Motor2 set the speed and direction. The valid values are -100 to 100.

Options1 and Options2 are to set the options for the motors. Because I don’t have encoders, it’s pointless to set bits 0 and 1 (0×01 and 0×02) to anything other than 0,0. Bit 3 (0×08) is to reverse the direction of the motor. It’s not meant to be used as direction control, but rather as a way to simplify the rest of the program. For example, if you have a differential drive robot (skid-steer), the motors will need to run in opposite directions to go straight. That is when you would use the Rev bit. If you want to set the Rev bit to 1 (reverse direction), you can do so with the #define’d constant TETRIX_MOTOR_REV. Also note, that if you don’t want to use the “Options”, you can leave those parameters empty, like this (it will default to 0):

TetrixMotors(TETRIX_PORT, TETRIX_ADDRESS_1, Motor1, Motor2);

To use the servo controller, use this command:

TetrixServos(Port, I2CAddress, Servo1, Servo2, Servo3, Servo4, Servo5, Servo6, speed, PWMenable);

The Port and I2C address field are used in exactly the same manner as the TetrixMotor command. Remember to use the proper I2C address, based on it’s position in the daisy-chain.

The Servo1 through Servo6 parameters are for the servo’ positions. Note however, than some servos have mechanical limits that restrict it from safe use in the full range of 0-255.

The speed parameter is a (servo controller FW) implemented delay before progressing to the next step. For example, if a servo is at 50, and you give it a new position of 200, it will normally go as fast as it can to get to the new position. However, what if you want it to go to 200, but slowly (“Ramp-Up”)? You can set the speed to a value from 0 to 15, so cause it to go slowly. One of the main things it could be useful for, is if you have two servos with different loads, and you want them to be as much in sync as possible. You can set the speed to slow the controller from changing the servo signals instantly. It’s basically a “Ramp-Up” and “Ramp-Down” function for the servo positions.

The PWMenable field is to reset the 10 second time-out timer. If 10 seconds go by without having this field set to 0, the servo controller will automatically stop the signal line to the servos, causing them to float (turn off). Therefore, it is quite necessary to keep writing it back to 0. To do this, you can use the #define’d term “TETRIX_SERVO_TIME_RESET”. Lets say you don’t want it to be able to timeout, you can instead give it the value of 0xAA, or the #define’d term “TETRIX_SERVO_STAY_ON”. That command will be good until a different one is issued, or the power gets reset. Now, you’re probably wondering how to make the servos float. Instead of waiting for the 10 seconds, and having it timeout and float the servos, you can write 0xFF to it, or use the #define’d term “TETRIX_SERVO_OFF”. Unfortunately, it’s all the servos or none. You can’t make just some of them float.

Like with the TetrixMotors command, you don’t have to specify all the fields. All you must do, is this:

TetrixServos(TetrixPort, TETRIX_ADDRESS_2, Servo1, Servo2, Servo3, Servo4, Servo5, Servo6);

It will default both “hidden” parameters to 0. So, it will have the “speed” equal 0, and it will restart the timeout timer (“TETRIX_SERVO_TIME_RESET”).

I haven’t implemented any of the read functions that the controllers support (such as to monitor status…). It’s possible that I will add some of that later, but for now, it does all I need it to (and maybe a little more).

Hopefully this will be of help to some Tetrix + NXC users.

About these ads
This entry was posted in Drivers, Mindstorms, NXC, NXT, Tetrix and tagged , , , , , . Bookmark the permalink.

5 Responses to Tetrix, and NXC drivers

  1. Heitor Campoli says:

    Hey, very interesting your library!
    I’m a student from Brazil, and I’m trying to work it out… But the command TetrixSetup won’t work… The NXC compiler points the following line of the library:

    setin IN_TYPE_LOWSPEED , port, TypeField // IN_TYPE_LOWSPEED (not 9v)

    and says “Error: Invalid constant expression: TypeField”
    Do you know why is it not working?

    Thanks!!

    • mattallen37 says:

      You probably need to be running the enhanced firmware on the NXT. Try using version 1.31 enhanced.

      Also make sure you have the latest version of the NBC compiler; 1.2.1.r4 is what I’m using (with BCC 3.3.8.10).

      Try those two things, and see if it helps. If not, I can try to rewrite that part.

      • Heitor Campoli says:

        I wonder if I can use this library with an NXC compiler, such as Bricx Command Center… Or does it work only with NBC? I’ve updated the firmware already, but the error insists on occuring…

      • mattallen37 says:

        When you compile an NXC program in BCC, it converts it into NBC, and then into machine code. BCC version 3.3.8.10 comes with the latest compiler (and latest FW). If you aren’t using the latest version of BCC, then it’s possible that you’re using an old file for the macros that doesn’t define “TypeField”. If you still can’t get it to work by updating BCC to 3.3.8.10, try replacing the term “TypeField” with the number 0. The line should look like this “setin IN_TYPE_LOWSPEED , port, 0 // IN_TYPE_LOWSPEED (not 9v)”. TypeField is just a macro with the value of 0, so the program should see no difference.

  2. kirkpthompson says:

    Thanks for the great article! It has helped me as I write LeJOS classes for the Tetrix controllers. Keep up the good work.
    -K

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s