I found some of my first code! Annotating and reflecting on robotics code from 2009.

Monday, January 1, 2024

In high school, one of my teachers shattered my plans for my life, in the most beautiful way. Most of my life, I'd intended to become a math professional of some sort: a math teacher, when that was all I saw math for; an actuary, when I started to learn more; and then a mathematician. I knew that to get a math degree, I'd probably have to take computer science, so I signed up for a programming class in high school. If I wanted to be a mathematician, that was a mistake, because it got me hooked.

The first programming classes were good, but didn't change the course of my life: I still saw them as a useful tool. But our programming teacher started a FIRST Robotics Competition team with us. And that ended up sending my life on a different course1. The magic of writing code that controlled a moving actual thing? Yeah, that pushed me toward where I am today.

Recently, I found the code from our second season in 2009. Let's take a look at what the game was and what made our robot special. Then we'll go through the code, and I'll reflect on things at the end.

The 2009 game and robot

The game for the 2009 season of FRC was called Lunacy. The core thing for that competition was that each robot had a trailer you were trying to score balls into, and the playing surface and wheels were both regulated2 to be a low coefficient of friction, similar to playing on the moon.

We went through a few iterations of designs to come up with the robot we had. It was a monstrosity of PVC and other big box hardware store items, because we did not have access to the kinds of machine shops or fabrication many other teams did, and that many teams do today. It worked out and looking back, I'd best describe us as scrappy.

The robot we ended up with had three key design features:

  • An opening at the ground level to allow balls to enter, where they'd be pulled up a sort of shaft via a moving belt; this was how we got them loaded to shoot
  • A hopper and firing chamber where we could use a piston to launch a ball at a particular distance
  • A traction control system to allow smoother operation on the surface

The hopper and firing chamber were something we had to go through the most iterations on to get them reliable, and they ended up failing at the last moment: before our elimination rounds, a valve on the pneumatic piston sheared off, resulting in our robot being largely disabled during those rounds. But before then, the fact that we made the piston adjustable (something we did not see in general, probably because it's not recommended!) made for a repeatable and mostly reliable firing mechanism.

The traction control system is something we thought of when we realized how hard it would be to drive on the surface and control the robot. A simple test showed us that control was very challenging indeed, and so we went about figuring out how to implement traction control. It's simple applied physics at the end of the day: calculate how fast you are allowed to accelerate, and calculate your wheels' acceleration, and don't let those two meet!

We had the only robot in our regional competitions that had traction control and adjustable pneumatics, as far as we know. These allowed our scrappy robot to place third in the qualifying rounds. Unfortunately, we were knocked out in the first round of elimination due to that hardware failure, but we did very well especially given our resources.

Our code annotated

Let's take a look at the code3. I'm not going to take a particularly harsh eye or apply today's standards, because 2009 (and high school) was a very different time.

It starts with importing WPILib. This was new to us. The hardware in the kit of parts had changed for the 2009 season, so while we used robotC in 2008, we had to change for 2009. We opted to use C++ instead of LabVIEW, since we couldn't wrap our heads around visual programming. I still don't get LabVIEW.

1#include "WPILib.h"

Yup, just an import.

Now we have this giant comment block. It's actually not too bad as far as opening comment blocks go, though it probably should be before the import to be a proper header comment. I really like that it has sincere thanks for people, though I'm amused that I was so proud of the traction control that I put credit for that specifically. A few funny things here after we read through it.

4Credit due to:
8Traction control:
9 Implemented by Nicole Tietz
12Thanks to:
13 All members and mentors of 2603
14 All members of the CD community
15 Mr. Mxxxxx: teacher, coach, and mentor.
16 Mr. Kxxxxx: teacher, mentor. He checked my calculations
19 Autonomous code
21Known bugs:
22 Distance per tick is wrong; it should use 0.1524*pi*(15/22), I forgot to put the pi in. Oddly enough, it works wonderfully.
25 Please forward to <REDACTED-EMAIL>
26 I would be glad to hear about it if my code can help anyone. (Or if you find some errors.)
28Anyone is welcome to use this code, but please give due credit.
30 "Mind, Metal, Machine." 2603.

One funny thing is this comment block was apparently my issue tracker. That's where I listed a TODO, and we never did get our autonomous mode working. That's also our bug tracker, but I... it's a weird thing, because the code "worked" but it's listed as a bug, because we were not sure why it worked. That's not great! And we'll be coming back to that.

I also didn't understand licenses, so we just said "feel free to use it!" without any proper license. The intention was something like MIT or BSD, but it wasn't licensed properly. Ending with our team motto is just... amusing, since I didn't even remember it was a thing; clearly not very memorable.

Now we come to the first real code. A rotary encoder is a sort of sensor we used which detects rotation. Specifically, we used a quadrature encoder which also tells you how fast the thing is turning. And we wanted to have some kind of wrapper around the class given to us, so we made that. The first chunk gives us some fields, and is "commented."

33class AugmentedEncoder {
34//augments the functionality of an encoder
35 Encoder *encoder;
36 Timer *timer;
37 float acceleration;
38 float velocity;
39 float delta_v; //change in velocity
40 float delta_d; //change in distance
41 float delta_t; //change in time
42 float distance_per_tick; //distance per tick of the encoder

The comments are all, uh, not necessary and should be removed. Most comments in this code are of that flavor, since I knew I should have comments but not what they should be like. As for fields, we have pointers to an encoder and to a timer, and then some floats to measure velocity, change in velocity, change in distance, change in time, and how far one tick of the encoder indicates we've moved. Pretty sure those did not need to be pointers, but we will see.

One major change that should have been made here: tell us what the class is adding to the encoder! The fields gave us our first clue, and the actual thing we're getting is calculation of velocity and acceleration from changes in our position. Pretty neat, and having those is foundational for our traction control.

Now we have the public methods. The first one is our constructor, a term I did not know at the time. It initializes our fields, passing through 3 of the 4 parameters directly to the wrapped class. The channels are where to read from in the hardware, and reverse is for which direction it's going so we can use outputs without negating them.

44 AugmentedEncoder(int a_channel, int b_channel, float d_p_t, bool reverse = false) {
45 //initializer for the AugmentedEncoder class
46 encoder = new Encoder(a_channel, b_channel, reverse);
47 timer = new Timer();
48 velocity = 0;
49 acceleration = 0;
50 distance_per_tick = d_p_t;
51 } //end AugmentedEncoder(...)

Next up we have this beauty of a method which is never called. It passes through and starts the underlying object.

53 void Start() {
54 //starts the encoder and timer
55 encoder->Start();
56 timer->Start();
57 }

Curious that we never call Start on these things, huh? Well it turns out that later we use Reset which does double duty and starts it if it isn't started, so this just kind of hung out as code I was afraid to delete.

Now we get to the meat of this class: our Recalculate method. This is where the magic math happens. In this aptly named method, we recalculate all of our tracked values.

58 void Recalculate() {
59 //calculates changes of distance, velocity, and time, as well as absolute velocity and acceleration.
60 delta_t = timer->Get(); //time elapsed since last recalculation
61 timer->Reset(); //resets the time elapsed
62 delta_d = encoder->Get() * distance_per_tick / 4; //quadrature gives 4 times resolution but requires division by 4
63 encoder->Reset(); //resets the ticks for the encoder
64 delta_v = delta_d / delta_t - velocity; //delta_d / delta_t is current velocity
65 velocity += delta_v; //current velocity is now set to old velocity plus the change
66 acceleration = delta_v / delta_t; //acceleration is rate of change of velocity
67 }

So we have just position from the encoder, right? We can use the change in position to get figure out our approximate velocity. And the change in velocity gives us the acceleration!

And, yes, the spacing was that bad. And this is after I've corrected the mixing of spaces and tabs...

The rest of the class straightforward, just another unused method and two getter functions.

68 void Reset() {
69 //resets the augmented encoder
70 velocity = acceleration = 0.0;
71 timer->Reset();
72 encoder->Reset();
73 }
74 float GetAcceleration()
75 {
76 return acceleration; //returns a private member
77 }
78 float GetVelocity()
79 {
80 return velocity; //returns a private member
81 }

To recap, so far we've seen monstrous comments and we've seen a wrapper around Encoder which will take the outputs and approximate velocity and acceleration for us. Now we get to move on to the robot itself!

Our base class is IterativeRobot which gives us the main control loop and then we can override hooks into it, which get run periodically. Our robot was named Sting, because we were the Hornets, so we named the class Sting.

84class Sting : public IterativeRobot

We start off with our fields again. robot_drive will let us control our left/right drivetrains, and driver_station is what our joystick is mounted to that we can read remote inputs from. Since we get remote input, we can see which number the packet is, and we used this to perform actions uniquely per packet received. packets_in_second is only set and never read, so I think it was from debugging something.

86 RobotDrive *robot_drive;
88 DriverStation *driver_station;
89 UINT32 prior_packet_number;
90 UINT8 packets_in_second;

Now we have a bunch of constants. We have G since we later compute things based on the friction force between the wheels and the surface. We also have how many ticks we get per revolution—this is the resolution of our encoders, so we can use that to figure out distance.

92 static const float G = 9.806605; //meters per second squared
93 static const float ticks_per_rev = 250;

We come back to the infamous "bug"! This is where I, future math degree-haver, forgot to include pi in our calculation! I think the reason it ended up working out is because some of the other calculations are sloppy in a compensatory way. We also have our coefficient of friction (measured experimentally, in fact!) and we have our adjustment constant which is used to ramp speed up or down gently.

94 static const float distance_per_rev = 0.1524 * (15/22); //6" in meters times 15/22 gear ratio
95 static const float mu = 0.05; //coefficient of friction between wheels and regolith
96 static const float adjustment = 0.05; //coefficient for adjustment of the current wheel speed to match expected acceleration

Just declaring a bunch of fields now. Joysticks, encoders, motor controller, piston, compressor...

98 Joystick *left_stick;
99 Joystick *right_stick;
101 AugmentedEncoder *left_encoder;
102 AugmentedEncoder *right_encoder;
104 Jaguar *shooter;
106 Solenoid *piston;
108 Relay *compressor;

An inline struct for some grouped fields about our drivetrain! The struct is a nice idea, and can't blame a girl for the inline aspect, I was new and it's fine.

110 struct {
111 //describes left and right drive trains
112 float speed; //current speed
113 float adjust; //how much to adjust current speed
114 }left, right;

A ratio for how far to shoot the piston, and an unused variable ratio. This code has a lot of unused variables. Probably a side effect of not using version control!

116 float shoot;
117 float ratio;

Now some constants for how many buttons or solenoid controls exist, and then creating our controls for those. The +1 is probably because I didn't understand that things were 0-indexed, and we didn't use the first or last to run into that.

119 static const int NUM_JOYSTICK_BUTTONS = 16;
120 bool left_stick_button_state[(NUM_JOYSTICK_BUTTONS+1)];
121 bool right_stick_button_state[(NUM_JOYSTICK_BUTTONS+1)];
123 static const int NUM_SOLENOIDS = 8;
124 Solenoid *solenoid[(NUM_SOLENOIDS+1)];

Some more tracking of info for timing purposes. We use these to fire events on particular frequencies.

126 UINT32 auto_periodic_loops;
127 UINT32 disabled_periodic_loops;
128 UINT32 teleop_periodic_loops;

Now we just initialize our fields. The constructor isn't particularly interesting, although I did comment that I was amused 0.0 looks like a face. This comment is a good comment, and you should always comment about things that make you happy. The rest of the comments here are just kind of lacking, they're shorthand notes for my past self that were not useful even then. The most notable thing here might be that on lines 145 and 146 we divide the (incorrect) distance-per-revolution by the number of ticks to get the distance per tick, for computing position, velocity, and acceleration.

132 Sting() {
134 robot_drive = new RobotDrive(1,2); // use ->SetLeftRightMotorSpeeds(float left, float right);
136 driver_station = DriverStation::GetInstance();
137 prior_packet_number = 0;
138 packets_in_second = 0;
140 left_stick = new Joystick(1);
141 right_stick= new Joystick(2);
143 left.speed = left.adjust = right.speed = right.adjust = 0.0;
145 left_encoder = new AugmentedEncoder(1,2,distance_per_rev / ticks_per_rev);
146 right_encoder = new AugmentedEncoder(3,4,distance_per_rev / ticks_per_rev, true);
148 shoot = 0.0; //sorry, this looks like a smiley. I just had to comment.
150 shooter = new Jaguar(3);
151 piston = new Solenoid(1); //piston solenoid is wired into the first output on the relay module
152 compressor = new Relay(5); //in d_io 5
154 UINT8 button_number = 0;
155 for (button_number = 0; button_number < NUM_JOYSTICK_BUTTONS; button_number++) {
156 left_stick_button_state[button_number] = false;
157 right_stick_button_state[button_number] = false;
158 }
160 UINT8 solenoid_number = 1;
161 for (solenoid_number = 1; solenoid_number <= NUM_SOLENOIDS; solenoid_number++) {
162 solenoid[solenoid_number] = new Solenoid(solenoid_number);
163 }
165 auto_periodic_loops = 0;
166 disabled_periodic_loops = 0;
167 teleop_periodic_loops = 0;
168 }

Initializing the robot when it boots, all we need to do is turn on the compressor for our pneumatics. Everything else was handled in the constructor.

170 void RobotInit(void) {
171 compressor->Set(Relay::kOn);
172 }

There are the three modes for our robot: disabled, autonomous, and teleop. Each has three functions (init, periodic, and continuous) that are called when going into that mode, periodically, or you can do your own continuous flow. We just used periodic to simplify our lives.

In disabled mode, all we do is disable the compressor and feed the watchdog so our robot is known to be responsive. I think if we didn't do that, the field or driver station would disconnect it, or the robot itself shuts down for safety reasons.

174 void DisabledInit(void) {
175 disabled_periodic_loops = 0;
176 compressor->Set(Relay::kOff);
177 }
178 void DisabledPeriodic(void) {
179 GetWatchdog().Feed();
180 disabled_periodic_loops++;
181 }
182 void DisabledContinuous(void) {
183 }

The comment at the beginning was not wrong: we had no autonomous mode. The game in the 2009 season wasn't one where our team was particularly equipped to do anything useful autonomously. We would have needed to use sensors more effectively, which we didn't. The one idea we had was attempt to pin another team's robot in autonomous mode, but we ran out of time to try it and we had no other robot to attempt to pin in testing.

185 void AutonomousInit(void) {
186 auto_periodic_loops = 0;
187 compressor->Set(Relay::kOn);
188 }
189 void AutonomousPeriodic(void) {
190 // feed the user watchdog at every period when in autonomous
191 GetWatchdog().Feed();
192 auto_periodic_loops++;
194 if (auto_periodic_loops == 1) {
195 //start doing something
196 }
197 if (auto_periodic_loops == (2 * GetLoopsPerSec())) {
198 //do something else after two seconds
199 }
200 }
201 void AutonomousContinuous(void) {
202 }

Now we get to the teleop mode code, where we have a lot more fun! The meat of it is just inside the TeleopPeriodic function; before then we turn on the compressor and reset some variables.

204 void TeleopInit(void) {
205 teleop_periodic_loops = 0;
206 packets_in_second = 0;
207 compressor->Set(Relay::kOn);
208 }

This function gets called 200 times a second, so we are able to use that frequency to do things which have to happen on a particular interval. The motor controllers have particular frequencies you can update them, so more frequent doesn't really help you any and is wasted work.

209 void TeleopPeriodic(void) {
210 GetWatchdog().Feed();
211 teleop_periodic_loops++;
212 // put 200Hz Jaguar control here
214 if ((teleop_periodic_loops % 2) == 0) {
215 // put 100Hz Victor control here
216 //left_encoder->Recalculate();
217 //right_encoder->Recalculate();
218 }

And then 50 times a second, we recalculate our position/velocity/acceleration and then invoke the ArcadeDrive function to adjust our motor speeds and be able to, well, drive the robot! The implementation of ArcadeDrive is below and we'll see it soon. Its name refers to the drive mode where you control speed and rotation, in contrast to tank drive which controls speed of each drivetrain independently or curvature drive which is like a car.

219 if ((teleop_periodic_loops % 4) == 0) {
220 // put 50Hz servo control here
221 left_encoder->Recalculate();
222 right_encoder->Recalculate();
223 ArcadeDrive(left_stick->GetY(), left_stick->GetX());
224 }

Now we read from the driver station, but only if we haven't handled the current packet before! This lets us avoid setting some of these things multiple times, and doing less work is always good. I don't recall if it actually caused us problems if we do, or if this was some optimization.

The main thing here is looking at the button states and reading the trigger and other distance buttons, so you could adjust the strength of the shot based on either a preset button (one of the top 4 buttons on the joystick) or based on the adjustable Z-axis dial. Then after reading those, it triggers the piston to open.

We were using pneumatics in definitely-not-recommended ways here, opening a pneumatic valve with a PWM controller to modulate the strength of it. This may have ultimately contributed to the connector for the piston shearing off, or that was just our own bad luck and poor engineering (I think there was stress on that connector). At any rate, it was pretty cool and it's another thing we didn't see other regional teams near us doing!

226 if (driver_station->GetPacketNumber() != prior_packet_number) {
227 prior_packet_number = driver_station->GetPacketNumber();
228 packets_in_second++;
229 if (left_stick->GetTrigger() == true) {
230 if (left_stick->GetTop() == true) {
231 shoot = 1.0;
232 } else if (left_stick->GetRawButton(2)) {
233 shoot = 0.70;
234 } else if (left_stick->GetRawButton(3)) {
235 shoot = 0.50;
236 } else if (left_stick->GetRawButton(4)) {
237 shoot = 0.40;
238 } else if (left_stick->GetZ() > 0) {
239 shoot = sq(left_stick->GetZ());
240 }
241 } else {
242 shoot = 0.0;
243 }
244 if (shoot) {
245 shooter->Set(shoot);
246 }
247 else {
248 shooter->Set(0.0);
249 }
250 if (right_stick->GetTop()) {
251 piston->Set(true);
252 } else {
253 piston->Set(false);
254 }
255 }
257 if ((teleop_periodic_loops % (UINT32)GetLoopsPerSec()) == 0) {
258 packets_in_second = 0;
259 }
260 }
261 void TeleopContinuous(void) {
262 }

Here we have a rather confusing comment: mixes arcade input to be tank input??? I think it's saying it's converting from the input to arcade drive and turning it into the inputs that tank drive would expect. We take in the x/y position of the joystick then combine them to get the expected left and right drivetrain speeds. Neat.

264 void ArcadeDrive(float y, float x) {
265 Drive(Limit(y+x), Limit(y-x)); //mixes arcade input to be tank input
266 }

And here's what we were looking for! This is where we control our traction. We check if our acceleration is faster than what we should have according to our coefficient of friction and, if so, we lower our speed4. Otherwise, we still have room to go, so we can increase it! A nice improvement be to clamp the increase such that we don't go over the max acceleration ever; this worked but crosses that threshold often.

267 void Drive(float suggested_left, float suggested_right) {
268 ratio = left_encoder->GetAcceleration() / (mu*G);
269 if (sq(left_encoder->GetAcceleration()) > sq(mu*G)) {
270 left.speed -= adjustment;
271 }
272 else {
273 left.speed += (suggested_left - left.speed)*(adjustment);
274 }
276 if (sq(right_encoder->GetAcceleration()) > sq(mu*G)) {
277 right.speed -= adjustment;
278 }
279 else {
280 right.speed += (suggested_right - right.speed)*(adjustment);
281 }
282 robot_drive->SetLeftRightMotorSpeeds(left.speed,right.speed);
283 }

I want to say again I'm not sure why this code worked, because the calculations are wrong, but I think they're all just wrong in similar ways that cancel each other out. For example, in the traction control code, we don't include the mass of the robot! So we're estimating probably a much lower max acceleration than possible.

I did not know about libraries, nor the clamp function. I'm pretty certain I did not need to implement sq myself (and also, it was fine).

284 float sq(float x)
285 {
286 return x*x;
287 }
288 float Limit(float x)
289 {
290 return (x>1)?1:(x<-1)?-1:x;
291 }

And that's it. 294 lines of high school Nicole's code. The origin of an engineer.

Reflecting back

Reading through this code has been a trip down memory lane for me. I'm remembering the team members I had, our coach, our mentors. I'm remembering the fun we had. I'm remembering the tears we shared when we saw the sheared pneumatics component.

In terms of moments that made me the engineer I am today, I think that this season of FRC ranks as one of the top things that got me there. It's not because it taught me a lot directly (though it did), but because it showed me that I can be—that I am—an engineer.

The problem solving we used in the 2009 season was exactly the kind of problem solving that you do as an engineer, or at least that I do as a software engineer. One of the greatest things we did, I think, is that we figured out what would be difficult for the user, the driver, and added compensatory systems to make the user interface easier. Traction control really was, for us, a UX improvement more than anything else.

Our robot was far from the most impressive one on the field. But getting to go through that design process with a team, getting to build it together, getting to struggle together? Oh yeah, that made me love engineering and made me understand the joys and pains of building things.

I'm not sure that I'd be a software engineer today if not for FRC, if not for the teacher/coach we had who brought it into our school. Thank you, so much. You changed my life for the better.


Our team had better-than-software-industry representation of girls on it, and many of our team alumni have gone into STEM fields. It can be selection bias (who's going to join robotics but the people interested in STEM?), but it also did provide a good supportive environment to show us we can do it.


Normally you have much more flexibility in your choice of wheels. That year, the wheels were chosen for you. It was a fun constraint and led to some fun code!


Every line of code is included here.


Notably, this probably does not work when reversing the robot. That's okay, but not an intentional limitation, so this belongs in the super-useful header comment bug tracker.

If this post was enjoyable or useful for you, please share it! If you have comments, questions, or feedback, you can email my personal email. To get new posts and support my work, subscribe to the newsletter. There is also an RSS feed.

Want to become a better programmer? Join the Recurse Center!
Want to hire great programmers? Hire via Recurse Center!