I was pretty humbled today, when I was contacted with a question. I wrote quite a long answer, so I thought I’d share a little of what I’ve learnt over the past 7 years of Vex programming with you all.
How do you do a ‘good’ turn?
Well, this is basically the only question when it comes to programming robots. It’s simple to program a robot in a perfect world, however the difficulty comes in when you have real-world factors to deal with. The major factors are:
• Battery power: consecutive runs have diminished battery voltage, resulting in reduced motor power
• Motor/physical inconsistencies: motors are not all built the same, and friction in the robot’s drive trains are generally different also.
In a perfect world, programming robots would be simple. You could, for example, pick any motor you wanted, install it in your robot, go forward for 1 second, then turn off. This would put your robot in the same spot every single time you did it, no matter which motor you chose. That would just be magic, right? Therefore, the major goal is to learn techniques to counteract some of these pesky real-world issues.
First of all, don’t use timed movements. It sounds like a great idea at first, because of its simplicity, but it is literally not worth your time. Sensors are the key here, and in particular, my favourite vex sensor, encoders. Encoders count the number of wheel rotations, allowing you to effectively track where your robot is. Quadrature Encoders, like those used in Vex, have the ability to track the direction of rotation as well as the magnitude. One thing to note about encoders is that they should be placed as close to the wheels as possible, because the further away they are, the more ‘slack’ they will have. Preferably, the encoders will be on the same axle as the wheel, and the wheel’s normal square inserts are replaced with metal inserts used for gears. If you haven’t done this already to your wheels, you really should – it makes a huge difference, in both programming and driver control.
So, the question now becomes, how do I use encoders to turn/drive straight?
1. Translate your units into something easily understandable
Many people skip this stage, and end up spending many hours more than they need to, developing their routine – all because they are attempting to use abstract units. Do not fall into this trap. In this case, the output from the encoders is not any recognised unit, so we will say its output is in “ticks”. It is very easy to visualise measurements in degrees and centimetres (or inches if you prefer), and makes adjustments much easier. To begin setting up your units, turn on the robot and spin around exactly 10 times on the spot, then record the encoder values you got back for each wheel. Absolute these values and take the average, and lets call this value X. What you are looking for is the number of encoder “ticks” per degree of robot rotation, which is (X/10)/360, or, X/3600, or the number of ticks recorded by the encoders divided by the number of degrees turned. In your code, assign this variable to a constant variable as something like ticksPerDegree. From here, you can say, “I want to turn 45 degrees”, therefore, “I want my encoders to move 45*ticksPerDegree ticks each”.
You can do this same method again with driving straight as well. Drive forwards 3m and record the ticks required for that distance. Using the same maths you can calculate ticksPerCM (or ticksPerInch if you prefer).
2. Functions. Functions everywhere.
Functions are building blocks that you create for yourself. If you ever think to yourself, “Hmm, perhaps I should make a function for this”. You might as well do it, you may find it’s more useful than you first thought. As an example, I often use a function called driveOn(int leftSpeed, int rightSpeed), which simply sets the respective motor speeds. It sounds simple but is really quite useful. It also means that re-using code on new projects is easy. I can copy and paste a ton of code, and then only change the contents of this driveOn function. It will work exactly the way I want. Similarly, what we are looking at creating now is a function which allows us to program, with little care for the real-world factors. To understand how to do this, I will quickly go over a P (Proportional) Controller.
3. P Controller:
When you turn on an oven/hot water tap, it starts cold and warms up over time. No doubt you’ve learnt over the years, that if you turn the hot water on full blast, it warms up faster, when it is at the temperature you want, you can adjust it to the warm temperature you originally wanted. What you are doing here, is being a P controller, you are taking into account how much it needs to warm up, and telling it to ‘overshoot’ your target, then as it gets closer to the target, lowering the amount you overshoot, as it reaches your desired temperature. The same goes for robots, if you want to drive forwards until your encoders reach an average of 1000 ticks, you could drive at max speed till you reach it, then cut your motors. That will cause you robot to overshoot your target, as it will still have forwards momentum when the desired distance is reached. You could drive at the minimum power needed to move your robot, but your autonomous would end before you drove a metre, so clearly, we need to do something more intelligent here.
4. Implementing a (distance) P controller on encoders
What we need to do here is the same as you would do if you were running up to a line, the further away from the line you are, the faster you move – and as you get closer to the line, you slow down so you can stop on it accurately. The equation for this is therefore logically very simple, you want to set the motors to P*(T-E) where T is the target number of ticks you want, E is the current number of ticks, and P is the proportional constant, such as 0.2. Essentially, what this P variable states, is that for every tick the encoders are off by, increase motor power by this much. Naturally, this won’t work by itself, because the motors need a certain power in order to start moving, and as you get close to your target, P*(T-E) will give a value lower than this value, therefore, you will need to find the lowest motor values needed to move your robot, both for turning and driving straight, and add the outcome of P*(T-E) to it – so, as your reach the target value, you will be moving at the minimum speed needed to move – meaning you will be accurate near the end. However, there is the larger issue of driving straight that still plagues your code.
5. Implementing a (wheel difference) P controller on encoders
We have now essentially removed the real-world effect of the battery slowing our drive, as we are working with distance. Now, it’s time to remove the other real-world effect, mechanical differences between the two sides of the drive. This is actually very similar. In a perfect world, we would like the two encoders to have the same value when driving, so, logically, the solution is to fight to keep them at the same value. To do this, we simply use another P controller at the same time as the previous one. This time, we are going to take the difference between the two encoders, and use that. Let’s assume we are driving forward, and both values are going up, we can do set ‘diff’ to leftEnc – rightEnc, which will give us the amount the left wheel is ahead of the right wheel. From there, we can simply subtract diff*P2 from the left motor, and add diff*P2 to the right motor. Currently, the line of code implementing the function that sets motor speeds could look something like:
driveOn(minSpeed + distPValue - diffPValue, minSpeed + distPValue + diffPValue)
6. Making sure the values work nicely together
If you think about it, when you are going max speed, 127 forwards because of your distance proportion, your diffPValue may not even have a chance to do anything, therefore, it’s a good idea to ‘cap’ how much these values can actually affect your motors. You may want to limit distPValue in this case, to 120-minSpeed, so that the maximum value minSpeed + distPValue can be, is 120. This means diffPValue can take effect all the time.
7. Writing the overall function
So, when everything is said and done, you should now be able to write a function that allows you to negate real-world variables, and drive around easily, using encoders in this case. You may want to increase speed over time, using a timer, or, you may want to use a timer to accelerate up, over the first half-second, it’s really up to you. The beefy bit of code I wrote for the vexU programming skills champion robot used both of these and everything else mentioned here, but nothing else. No Integral and no Derivative (as are used in PID control). Just P, plain and (mostly) simple. The main trouble is using these formulae when turning and moving backwards, but I’ll leave that for you to work out 😉
8. Code smartly
People love to over-complicate code. They love their ternary operators. They love saving bytes of memory by not using variables – which could have descriptive names to make the logic, well, logical. Don’t be this person. Use comments, make simple variables which allow you to code using real-world units. Avoid ternary operations like the plague, write functions for reused code, and use constants/strings to help determine what you want to do, e.g. autoDrive("left") or autoDrive(LEFT). Encoders are, in my mind, the most reliable sensor. They don’t drift, I’ve never seen one break, they are accurate if you drive correctly and they work in any light. Use them.
Hopefully this gives a little insight into what the world champion code will have looked like, the major drive function (called autoDrive) was about 89 lines in total, and was: autoDrive(float dist, int dir, bool flip) (flip in this case simply reversed left/right, to make blue/red auton easy) and could be called like:
autoDrive(100, FORWARD, false) // Drive forward 1 metre
Good luck for “Nothing But Net”! I’m planning on putting up more tutorials in the coming months, I want to see programming skills be even fiercer.
– Kerey, AURA 2015