Team 1519 - 2019 Code Release w/2020 Beta Test Updates (Java)

Despite our best intentions to make our code release earlier, the combination of non-FRC life, other FRC items to do, and wanting to have more time to improve the code, we are again in the week before kickoff when making our code release. Nonetheless, here it is – our 2019 code, including the minor updates we made to support the 2020 beta control system. We may still make some additional check-ins during the next few days to bring the code release up to date with the final pre-kickoff beta software.

Autonomous remains one of our areas of focus; this year we had 25 different autonomous routines (programs or subprograms) in our robot code, many of which were parameterized to allow for different situations, such as right/left starting options for HAB2 or target height for delivering a hatch panel to the rocket. These autonomous programs were made up of smaller building blocks that performed such things as driving portions of the drive path, placement of a hatch panel on the cargo ship or rocket (at various heights), or retrieval of a hatch panel from the human player station, etc. The more complex autonomous programs were sequences of the autonomous program building blocks, all strung together using the Command-based robot features.

All of our programs had “StartRight” and “StartLeft” variants which used the same code for all but the parameter for starting position.

Our mainstay autonomous program was the HAB2HPtoShipSideBackwards(startSide) program, which started on HAB2, delivered the starting hatch panel to the center side port of the cargo ship, then picked up a second HP from the loading station and delivered it to the nearest port of the cargo ship. Placement of the hatch panels was aided by vision tracking, enabling the needed accuracy to deliver and place hatch panels, which required about +/- 1.5" of left/right accuracy. Yes, we generally ran a “fully autonomous” program during the sandstorm period, only using driver control during sandstorm if the autonomous program went badly, generally as a result of unplanned alliance member collisions. This autonomous program can be seen in Qual 44 on Curie:

If seeking to get a head start on the rocket, we would regularly use the StartRightHAB2HPtoRocketLowTwice() program, which would deliver one hatch to the near side of the rocket and one to the far side of the rocket, both on the low level. We had a “High” variant of this program, too, which differed only by instead passing in a different target height parameter. This autonomous program can be seen in Qual 85 on Curie: (That match also happens to show our fast HAB Level 3 climb – we drove onto HAB L1 with only 4 seconds on the clock and made it to L3!)

Our autonomous driving paths are almost exclusively done by a “DriveStraightOnHeading” command which has the robot drive an odometry-measured distance on a given heading (specified in degrees) with closed-loop robot steering to drive on the specified heading. Nearly all of our “turns” in autonomous driving are handled by the closed-loop control of “DriveStraightOnHeading.” In other words, if we “go straight and then turn left,” we would do this as follows: drive forward 100 inches on a heading of zero degrees, and then drive forward 100 inches on a heading of 270 degrees. The robot will drive forward 100 inches and then do an arcing left turn onto a new heading of 270 degrees. No “turn” command is actually specified. This makes for easy, smooth navigation. (Well, as long as the navX gyro works properly, which it did for us very reliably.) This is a different approach than the path-following approach now starting to be adopted by many teams. There are advantages / disadvantages to each.

I should probably also mention our “one-button” HAB climb routines for HAB Level 3 and HAB Level 2. Our 4-bar linkage climber for HAB Level 3 wasn’t particularly complicated to deploy manually, but the automated “one-button” routine which combined driving tasks and linkage deployment made it highly reliable and fast. By the time of the world championship, we also had a quick HAB Level 2 climb routine, even though that had been a secondary capability after a fast and reliable HAB L3 routine. See AutoClimb() and AutoClimbL2() for these routines.

Some videos of our L3 climb development are on our youtube page:
Whole Playlist, from the beginning:
Climb as of Week 1 Event:

(We did make the climb a little faster by the end of the season - down to less then 3 seconds, as shown in the Curie Qual 85 match linked above.)

Another thing I should probably mention is that we used the LED lights on the robot for communication with our drive team – different colors and flashing patterns communicated status of various robot mechanisms – this turned out to be a time-saver for the drivers when trying to shave a few more seconds from cycle times.

This year (like 2018), we are again releasing our code by making public our “2019-Robot” repository on github so that all can see it:

Feel free to post questions here or to email or PM me if I don’t seem to be paying attention to this thread during the build season…


What is the plan for auto in 2020? Will you guys continue using DriveStraightOnHeading? My team is very excited to use the Trajectory API from WPILib/254.

The short answer is that we don’t know. We haven’t yet invested the effort in using the newer API libraries.

In prior years, we’ve been disappointed with the path-following approaches, as they used only odometry and did not take advantage of the additional IMU information (such as the navX) – we’ve found the inclusion of heading to be important to minimize overall error and to increase accuracy in varying conditions, such as robot weight shifts (moving an arm with a game piece), poor traction (such as when in the air driving off of HAB Level 2), or unplanned interaction with other robots or game pieces. However, it now seems that the path-following techniques have improved to include IMU data and may be the way to go. We just haven’t tried them yet.

1 Like

Wow. That’s a lot of autonomous routines…

If I can suggest something: This year we used multiple SendableChoosers to allow us to configure things like the starting position (left, right) and the level to deliver game pieces at (1, 2, 3). This made it to where we only had to have around 6 different autonomous routines that could each be configured differently. Similar to you guys, we probably have somewhere around 25 different possible autonomous routines, we just configure them differently.

Now, sometimes our autonomous would start in the middle and the selection of left/right didn’t make sense, so I made a DynamicSendableChooser class. I think you guys could benefit from something like this so your chooser could be a lot simpler. Here’s the class that handled the state to initialize our autonomous and the [DynamicSendableChooser class](

Although we used SmartDashboard in 2018, we felt like Shuffleboard was definitely an upgrade when we used in in 2019.

Thanks for the suggestion. One of the things we like about the “canned list” of programs, even though some of them are really just different parameter sets of options, is that we know the different settings are compatible with one another, and there’s only one place to choose a setting (which could be set incorrectly) to get the desired program.

I’ll definitely check out the DynamicSendableChooser class, though – it seems like it would be great for debugging, even if we chose to use a single pre-canned list of program alternatives for matches.

Thanks for the recommendation. We’re hoping to give Shuffleboard a try this year.

This topic was automatically closed 365 days after the last reply. New replies are no longer allowed.