FTC: Taking your code to the next level

For two years now my school has had an FTC team. I have been the head programmer for both of those years. Last year my team, ΩVoltz, won the Fontys Control award for the best software at the FTC Dutch open tournament. I would like to share some of the techniques I used in the software, so other people can use them to take their software to the next level.

Before I start, I want to say that this article is primarily focussed on FTC teams, but if you do anything with robotics this article may interest you as well.

Tele-operation: the good, the bad and the ugly

First, let’s look at the worst type of teleop program, which I have encountered many times, in pseudocode:

if buttonPressed(forward) {
   motor[motor1] = 100;
   motor[motor2] = 100;
}
// and so on and so forth

This is the worst you can do. The drivers will have no control over the driving and turning speed of the robot, and thus will find it really tough to position it precisely. Because of this, most teams use more advanced code than this, like:

motor[motor1] = joystick.joy1_y1*100/127;
motor[motor2] = joystick.joy1_y2*100/127;

This code solves the above problems and gives the drivers full control over the robot, but isn’t really elegant. It requires the driver to think about how he or she is going to drive the robot, which reduces the user experience of driving. This is where a lot of teams stop. Some teams improve a bit more on this, and write something like this:

motor[motor1] = joystick.joy1_y1*100/127 + joystick.joy1_x1*100/127;
motor[motor2] = joystick.joy1_y1*100/127 - joystick.joy1_x1*100/127;

I’ve seen very few teams go beyond this point, which is a pity as there are a lot more improvements to be made. I assume that you are a member of one, and thus have understood all coding examples so far, but want to improve your code from this level. Let’s look at some improvements.

Tele-operation: the good, the bad and the ugly

Better controls

A ‘dead zone’

You may have noticed that your motors are whining when you have your joystick in the zero position, which means that they must be on, but not enough to actually move. This is because although your joystick may look to be in its zero position, it’s never exactly there, and there is also some noise on the data from said joystick. This means that it will return low values if the driver doesn’t mean to do so, which can be bad. To counteract this, you can add a ‘dead zone’, a zone of values in which you consider the joystick value to be 0. This can be implemented like this:

x1 = joystick.joy1_x1*100/127;
if(abs(x1)<=5) {
   x1 = 0;
}// now use x1 instead of joystick.joy1_x1.
// do this for other joystick inputs as well.

You just look if the absolute value, that is the distance to what the joystick considers to be its center, of the value is lower than a certain number (I have found 5 to be a good limit) and if so make the value 0.

Logarithmic controls

This is a small touch that isn’t really noticeable, but will make your controls much more polished. When you use logarithmic controls, your driver will have more control over the lower speed, and less absolute control over the higher speeds. This may not sound like much at first, but consider this: a small change at a high speed will get lost in the noise, while it will be quite significant at a lower speed. This will give the driver more control over your robot. You can implement logarithmic controls like this:

x1 = joystick.joy1_x1*100/127;
x1 = x1*x1*sgn(x1)/100;
// now use x1 instead of joystick.joy1_x1.
// do this for other joystick inputs as well.

This maps your controls into the logarithmic domain, by first squaring them, and then dividing by the orignal maximum value, so it fits in the range again. This is then multiplied by the sign of the original value to give you control value its direction back, which was lost in the squaring. If you think that this is to subtle, you could also cube your controls, or is use an even higher exponent. Just remember to multiply by the sign of the number for even powers and to divide enough times by the original maximum value.

Automatic positioning

Drivers will lose a considerable amount of time positioning a mechanisms so they’re perfect for scoring, and making sure it doesn’t go beyond its ranges. These problems can be solved by creating predetermined positions. You could, for example, make buttons that set the lift to certain heights, and make the robot go there. You can check if it has reached its target by using sensors such as limit switches, but there are a lot more possibilities. For example, my team used an ultrasound sensor to measure the height of the lift.

Closed-loop control systems

If you want to automatically position a mechanism using sensor data, you are essentially building a closed-loop control system. This means that you are using the data from the sensor to see if the positioning of a certain element is right, and adjusting its position accordingly.

Let’s look at an example.

Say you want to position a lift at 60 cm, of which you can manipulate the position by a motor, and have an ultrasound sensor measuring the height of it. You could just run the motor for a certain amount of time that you determined earlier, but since the speed of the motors is dependent upon a whole lot of factors, you cannot be certain about the accuracy of the position of the lift.

If however, compare the current position to the position target, and make the lift go up if the current position is lower than the position target and make the lift go down if it is higher than the position target, the accuracy of your position is only dependent on the accuracy of your sensor.

The first example didn’t use feedback, and so we call it an open-loop control system, but the second example did use feedback, so it is a closed-loop control system.

Dampening a control system

As you can see from the example above, closed-loop control systems are essential to automatic positioning. There is, however, one problem with the approach in the example. Let’s take a look at it.

If you take the naive approach of moving your lift to your position target at full speed, it won’t stop immediately upon reaching its target due to inertia, which will make your control system oscillate. Because of this, it will take a long time before your mechanism has stably reached its target.

Preventing oscillation from happening is called the dampening of your control system, and it is quite an advanced and complex topic. I’m absolutely not an expert in this field, as I’ve only really been dealing with control systems for a year or so, but I do know some tricks that can help dampen your control system.

Add hysteresis

Hysteresis is basically a fancy term for a switching limit: in the lift example, the lift could go beyond its target, without the control system compensating, and when the control system starts compensating, the lift could go a bit below its target. You could implement a basic version of hysteresis by adding a ‘dead zone’ around your target: the lift will stop moving at a certain distance from your target.

If you add a hysteresis around your target, your control system can overshoot a bit, without swinging back. You’re basically trading in accuracy for damping. It is always a good idea to have some small amount of hysteresis, but not to make it too big, or you’ll lose a significant amount of accuracy.

Distance-dependent speed

If you slow down the motors when they get closer to the target, the overshoot will be less, which will dampen your control system. Please note that to make this work it is only necessary to slow down a short distance from the target. I’ve found it more effective to slow down depending on the square of the distance, so the  mechanism slows down faster. Higher exponents could also be used. Experimentation is key here.

Predict the next position

Although the above tricks will probably be sufficient for sloppy control systems, there are some other tricks to use when the control system is more critical, such as anything to do with driving.

If you subtract the previous measured position of a mechanism from the current measured position, you can calculate the speed with which it moves. This can then be used to calculate what the position will be in the future. If this position is undesired, the speed can be changed. You could also test if this speed is desired for this position.Again, experimentation really is key here.

You should not directly set the power level of the motors, but rather adjust it depending on the calculated speed to keep the speed at a certain level. Hysteresis should be used here. This is basically a closed-loop control system in a closed-loop control system. You can’t directly influence the position, but you can directly influence the speed, so it is a more logical thing to control.

Experiment

It can take a lot of time to tune a control system, but you should definitely try to do so. Control systems can greatly improve your robot. There are all sorts of things that you can try to make your control loop more stable. Try to make the adjustments in the speed dependent of the distance to your target for example and don’t forget you could use all of these techniques for your autonomous code as well. Controlling the position of the robot on a field is also a control system. Just be sure to use common sense to find what works for you.

Check out my code!

My team has used all of these techniques in last year’s software, and made it freely available on github. While we put it on there under an open source license, which basically means that you are free to copy and modify the code as long as you provide attribution and share it under the same license, we strongly discourage you to carbon copy it. You will learn a lot more if you program yourself, and the code will be much better suited to your robot.

Anyhow, feel free to check out the code, and please leave a comment if you have any questions or if this article was of any use to you.