Team 1114 2019 Code Release

Below is a link to a repository containing the code for Team 1114’s 2019 robot, Simbot Snowstorm. If you have any questions, please feel free to ask.


Great, now everyone will have a 2-hatch auto next year!


Your two hatch autos are beyond impressive! Would you mind giving me a layman’s rundown of your “DriveToPoint” system? Are you doing actual real-time odometry calculations? Thanks!

1 Like

Real time odometry calculations are actually pretty simple once you know the math behind it. If you have some loop calculating the change in robot position, you can use cos and sin of your gyro measurement to get the component change in x and y respectively

\Delta s = \frac{\Delta l + \Delta r}{2}\\ x_k = x_{k - 1} + \Delta s \cdot \cos (\theta)\\ y_k = y_{k - 1} + \Delta s \cdot \sin (\theta)

Where x and y are your measurements, and theta is your gyro measurement. Just run that in a loop and you have a fairly accurate estimate of your current position relative to your start. In order to get it field relative, you can have preset positions where your robot starts, and set x_0 and y_0 to the required values.


We are doing real time position tracking using the encoders on the drive and a gyro. We use the average speed of the encoders and turn that into X and Y velocity and every cycle we add to our old X,Y position using the calculated velocities.

The DriveToPoint works by first calculating the X,Y error between the robot’s current position and the position of the point, this is rotated so that Y and X are relative to the points heading. Then you simply do a PID on the Y error and based on your X error you steer towards the point. So if the points final angle was 90* and you have an X error of 2ft and an X error constant of 10 you would turn towards 70* as you move more in line with the point and your X error decreases you get closer to your final desired angle of 90*.


Thank you for posting, 1114s code is always a great learning resource.


Sweet. I love how the hardware & FMS specific input and output functionality is grouped into an independent set of classes. This would make it super simple to unit test the actual controls code.

1 Like

Out of curiosity - I watched some of your videos at IRI and you’re doing turns on the fly. With a point to point system you would have essentially “square” or sharp corners but yours tend to be smooth.

How do you handle smoothing the corners on the point to point system?

Usually the heading of one point will lead into the next so the transition is smooth.
Also we try and use as few points as possible so there are less transitions that need to happen, in our rocket auto for example driving to the rocket to score the first panel is only 1 point to drive off the hab and 1 point to drive to the rocket.

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