FTC Servos


servo 2servo

HiTechnic Servo Controller Specification Specification Rev1.2

HiTechnic First Servo Controller for LEGO NXT

  1. Summary

This specification defines a servo controller for use with the LEGO NXT Mindstorms system. The servo controller will provide six channels each of which can support an FTC servo. The servos will be driven by standard PWM signals.

Power will be supplied from an external 9v – 15v battery via a screw terminal block.

  1. Technical Description

2.1. Operation

The servo controller firmware will support the LEGO company’s sensor memory model. Address Type Contents
00 – 07H chars Sensor version number
08 – 0FH chars Manufacturer
10 – 17H chars Sensor type
18 – 3DH bytes Not used
3E, 3FH chars Reserved
40H byte Status
41H byte Step time
42H byte Servo 1 position
43H byte Servo 2 position
44H byte Servo 3 position
45H byte Servo 4 position
46H byte Servo 5 position
47H byte Servo 6 position
48H byte PWM enable

The Sensor version number field will report a revision number in the format “Vn.m” where n is the major version number and m is the revision level. Revision numbers will typically reflect the firmware level. The version number will be used to indicate the hardware level.

The Manufacturer field will contain “HiTechnc”.

The Sensor type field will contain “ServoCon”.

The Status field returns 1 if there are still servos changing position, else it will return 0.

The Step time field sets the step time for the servo channel which has the furthest to move. Other servo channels which are not at their designated positions yet will run at a slower rate to ensure they reach their destination positions at the same time.

HiTechnic Servo Controller Specification


The Servo 1/2/3/4/5/6 position fields will accept a value between 0 – 255 to set the PWM output from 0.75 – 2.25mS. Note that some servos may hit their internal mechanical limits at each end of this range causing them to consume excessive current.

The PWM enable field is used to control the PWM output. If it is set to zero (00H), it will enable the PWM outputs, causing the servos to be active, and start a ten second timeout running. If it is set to 255 (0FFH), it will disable the PWM outputs, causing the servos to be inactive. If no I2C communication is received during the ten second timeout period, the PWM enable field will automatically revert to 255 (0FFH), disabling the servos. If the PWM enable field is set to 170 (0AAH), it will enable the PWM outputs, causing the servos to be active, and disable the ten second timeout. The PWM enable field is set to 255 (0FFH) during power up and after the timeout has elapsed.

Using LabVIEW and NXT in the FTC competition, Introduces tetrix motors.

Using LabVIEW and NXT in the FTC competition, Introduces the motors and watchdog timers.

Using LabVIEW and NXT in the FTC competition, Introduces servo motors.


Standard Models

You can ignore this tab for general FTC programming

Sensor Management

If you have any of the HiTechnic sensors (such as the compass or IR Seeker) you may want to check the “Allow 3rd Party Sensors from HiTechnic” checkbox.
*When using Xander’s 3rd party driver suite, many of the sensors will be configured as ?

TETRIX Controllers

Here we’ll want to forgo the standard configuration and do a custom one.  The standard configuration limits you to one motor controller and one servo controller which isn’t typical or particularly flexible for a FTC team.

Once you select the Custom radio button, the configuration grid becomes unlocked.  Here we are only going to configure the Tetrix motor and servo controllers.  Most teams have up to four of these and daisy chain them off sensor port 1.  Although you could run some controllers off of one port and others off another, it is a huge waste of your limited sensor ports.  As it is using just one port for all of your controllers, you only have three ports leftover for sensors which means you’ll have to use multiplexers if you want more than 3 sensors on a FTC robot.

Use the dropdowns to configure the controllers exactly as they are wired on your robot.  For our example configuration which has one motor controller and one servo controller connected to sensor port 4, our configuration would look like:

If you had 3 motor controllers followed by a servo controller daisey-chained off sensor port 1 on your NXT, then your configuration would look something like this:

Remember that the order of the devices matters!  If you get a warning window popup like the one below; you can safely ignore it.  It’s just there to make sure you really want to change the configuration as any previously configured motors, servos, or sensors on the port/device will be lost.


The motor tab allows you to setup each one of your NXT and TETRIX 12v motors.  You will see the three NXT ports as well as motor ports any Tetrix motor controllers you configured on the previous tab.  By default, the NXT motors are called motorA, motorB, and motorC.  Interestingly enough, it doesn’t seem to matter whether you set the NXT motors as equipped or not.

You have the following configuration options for each motor:

Name – You will want to give each motor a useful name.  The name should reflect the function of the motor on the robot and be easily understood by anyone reading your code.
Say you have three motors for a particular mechanism such as an arm.  Take a look at the two pieces of functionally equivalent example code below where the only difference is the motor names.  Which would you rather work with?

Type – This is either ‘Motor Equipped’ or ‘No Motor’.  If you do not have any NXT motors, it is OK to leave them as ‘Motor Equiped’ as ROBOTC seems to reset them even if you set them to ‘No Motor’.

Reversed – This is an extremely useful feature.  Motors spin a certain direction based on the direction of current flowing through them.  Two motors wired the same, but mounted in opposite directions (say as drive motors on the left and right side of your robot) will tend to spin in opposite directions when given the same power in ROBOTC.  One fix is to switch the wires to one of the motors, but that’s just plain wrong!  Another solution is to set the backwards motor with negative values in code when you want to go forward, but that’s almost as horrible.
The easy and clean fix is this one little checkbox in the motor setup.  If you want to flip the direction of a given motor so it goes the same direction you’ve mentally mapped with positive & negative values, then just tick the Reversed checkbox here.

Encoder – If you have one of the shaft encoders mounted on the motor and want to be able to read data from it, you’ll need to hit this checkbox.

PID Control – If you have an encoder and want to enable PID control on the motor, make sure this guy is checked.  What’s PID control you say? PID is an acronym for ‘Proportional Integral Derivative’ and is a popular algorithm in process control, usually around maintaining a set-point or hitting a target smoothly without going over.  When combined with the motor encoders, it allows you to hand control of your motor over to the PID controller so that you can do higher-level things like maintain a constant speed, drive a certain distance with accuracy, or synchronize two motors to spin at the exact same speed.
Do not check the PID Control box if you are not planning to use PID functions.

For our example configuration, we have only a single motor connected to motor 1 on the controller:

Below is an example motor configuration for a robot with 3 motor controllers all daisy-chained off sensor port 1 as controllers 1, 2, and 4.  There are 2 drive motors on the first motor controller, one motor on the second controller, and two more motors on the fourth controller.


The servo tab is a little more straightforward than the motor tab.  You just need to set a name and a type for each servo you’ve got on the robot.

Name – Pick a meaningful name as with the motors.

Type – You’ve only got three options:  No Servo, Standard Servo (180 degrees), or Continuous Rotation Servo (360 degrees).  If your not sure which type you’ve got, it is most like a standard servo.

In our example configuration we have only a single servo connected to servo 1 on the controller:


The sensor tab is also fairly straightforward looking, but there are some nuances if you’re using multiplexers or custom sensors.  We may cover multiplexers in another article in the future, but will stick with the simple stuff here.  Just like the servo tab, you’ll need to name each sensor and pick the correct type.  There are only 4 ports here and you’ll usually only have the last 3 to use as S1 is typically taken up by your Tetrix motor and servo controllers.

Name – You guessed it; pick a meaningful and easily understandable name.

Type – You just need to pick the correct type here for ROBOTC to be able to talk to the sensor.
*Note that if you’re using 3rd party drivers such as those available from Xander, you may need to configure the sensor differently!  For most of the 3rd party sensor drivers, you’ll need to set the type as I2C Custom:

In our example configuration, there are no sensors to configure.  Easy enough.

About those pragma statements

What is a pragma statement anyway?  Without going too deep into how ROBOTC compiles your code into something the NXT can understand, you should at least understand that ROBOTC uses a pre-compiler similar to traditional C.  The pre-compiler handles various directives such as #include, #define, #pragma and all your macros before the normal compiler does its thing.
‘Pragma’ is traditionally short for ‘pragmatic information’ and is used as a means to define implementation specific data needed by a compiler.  In the case of ROBOTC, it needs to understand your robot, its hardware, and configuration and it is much simpler to define this information in your code using pragma statements than working with separate config files or other means.

Did you know that the pragma statements are actually quite human readable?  The most interesting details are typically right there in the address configuration.  For example, if you look at any of the Tetrix motor or servo pragma statements, you’ll see something like “mtr_S1_C1_1″.  The format here is ControllerType_SensorPort_ControllerNumber_DeviceNumber.   This means we can decipher mtr_S1_C1_1 to mean:
mtr – This is a motor controller
_S1 – The controller is connected to sensor port 1 on the NXT
_C1 – The controller is the first controller from the NXT in a daisy-chain.
_1 – The motor is the first device on the motor controller (physically wired to motor 1 on the controller)

Once you understand this simple addressing scheme, wiring and configuring the Motors and Sensors Setup in ROBOTC becomes almost trivial and easier to read without even opening up the setup screen.

Putting it all together

If you’ve followed the example configuration above, our generated pragma statements should look something like the following:

Configuring Across Multiple Files

Most FTC teams will have more than one program; usually at least one for an autonomous and one for tele-op.  A common question is how to best manage the motor/sensor configuration so that when you re-wire the motors or add a new sensor, you do not have to remember to update each of your files individually.
Unfortunately, if you try to create a common header file with the pragma statements, the ROBOTC compiler appears to restrict the scope of the motor/servo/sensor variables to the file in which the pragma is defined.
The best approach then is to manually maintain the pragma statements across your various programs.  You can safely update the config in one file and just copy/paste the pragmas to the others.