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 course¹. 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 regulated² 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 code³. 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"
2

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.

3
/*
4
Credit due to:
5
 TEAM HORNET: 2603
6
 (<REDACTED-WEBSITE>)
7

8
Traction control:
9
 Implemented by Nicole Tietz
10
 (<REDACTED-EMAIL>)
11

12
Thanks 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
17

18
Todo:
19
 Autonomous code
20

21
Known 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.
23

24
Questions/comments:
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.)
27

28
Anyone is welcome to use this code, but please give due credit.
29

30
 "Mind, Metal, Machine." 2603.
31
*/
32

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."

33
class 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.

43
public:
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(...)
52

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
    }
82
};
83

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.

84
class Sting : public IterativeRobot
85
{

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;
87

88
    DriverStation *driver_station;
89
    UINT32 prior_packet_number;
90
    UINT8 packets_in_second;
91

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
97

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

98
    Joystick *left_stick;
99
    Joystick *right_stick;
100

101
    AugmentedEncoder *left_encoder;
102
    AugmentedEncoder *right_encoder;
103

104
    Jaguar *shooter;
105

106
    Solenoid *piston;
107

108
    Relay *compressor;
109

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;
115

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;
118

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)];
122

123
    static const int NUM_SOLENOIDS = 8;
124
    Solenoid *solenoid[(NUM_SOLENOIDS+1)];
125

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;
129

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.

130
public:
131

132
    Sting() {
133

134
        robot_drive = new RobotDrive(1,2); // use ->SetLeftRightMotorSpeeds(float left, float right);
135

136
        driver_station = DriverStation::GetInstance();
137
        prior_packet_number = 0;
138
        packets_in_second = 0;
139

140
        left_stick = new Joystick(1);
141
        right_stick= new Joystick(2);
142

143
        left.speed = left.adjust = right.speed = right.adjust = 0.0;
144

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);
147

148
        shoot = 0.0; //sorry, this looks like a smiley. I just had to comment.
149

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
153

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
        }
159

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
        }
164

165
        auto_periodic_loops = 0;
166
        disabled_periodic_loops = 0;
167
        teleop_periodic_loops = 0;
168
    }
169

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
    }
173

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
    }
184

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++;
193

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
    }
203

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
213

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
        }
225

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
        }
256

257
        if ((teleop_periodic_loops % (UINT32)GetLoopsPerSec()) == 0) {
258
            packets_in_second = 0;
259
        }
260
    }
261
    void TeleopContinuous(void) {
262
    }
263

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 speed⁴. 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
        }
275

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
    }
292
};
293

294
START_ROBOT_CLASS(Sting);

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.

---