[AustinSchuh]

Originally Posted by thatprogrammer

I had some questions about your closed-loop driving as it seems very interesting!

1. Reading your code, it seems like you dynamically generate motion profiles to your goal (Calculated off of joystick input?)

There are 2 controllers in our drivetrain code (SSDrivetrain and PolyDrivetrain).

SSDrivetrain is used for more traditional driving (go 1 meter forwards). It has motion profiles on the inputs, which can be disabled if you want. This lets us send it a goal of +1 meter, and go work on other things while it does it. Or, it lets us feed the goal in directly when we are doing vision alignment and want the controller dynamics without any sort of profile.

PolyDrivetrain runs the teleop driving. It is mostly using feed forwards, but has a proportional loop to do small corrections. It understands the robot's physics, and uses that knowledge to do constant radius turns. The combination of the feed forwards and the feed back makes the driving experience pretty connected.

Both of these are fed by a 7 state Kalman Filter which estimates the positions, velocities, and disturbance forces of each side of the drivetrain. Controls can be split into 2 worlds, the estimation and control worlds. You need good sensors or algorithms to figure out what your system is doing, and then you can apply the algorithms. Once you've split the world this way, you can take a nice estimator, and use it to feed multiple controllers. Generally speaking, the controller ends up being a matrix to multiply into the error to get an output, which is stateless.

Originally Posted by thatprogrammer

1. How do you account for drift? We used a Spartan Board this year and saw something like 2 degrees of drift per a match. Knowing Austin, he wouldn't tolerate misalignment by 2 degrees

Sorry to let you down, but we don't do anything special. 2 degrees/match is better than we need. Drift is only one of the metrics that matters. We are more concerned with bandwidth and linearity, and we have not been let down by the board on those fronts. (We've been using that gyro since 2012)

Originally Posted by thatprogrammer

1. In what file are you actually running your overall robot code? I can't seem to find where you use your 2016 drive classes to actually run the closed-loop drive.

It's deceptively simple. //y2016/control_loops/drivetrain:drivetrain_main.cc

We have 4 robots (assuming I can count...) all driving with the same code. There is a configuration structure passed into the drivetrain controller class for each robot which contains the physics model for each robot and other configuration bits. This means that the year specific code is 100 lines, all of it boilerplate.

Take a look in //y2016/control_loops/python/ for the models of each of our subsystems.

We have our own framework for designing robot code. Our code is broken up into somewhere around 10 processes, each responsible for one part of the robot. 1 process is Autonomous mode, 1 process is the joystick code, 1 process is the hardware interface, 1 process is the drivetrain, 1 process is the shooter, 1 process is the vision udp listener, 1 process is the superstructure code, etc. Each of those processes communicate with our own PubSub message passing code. That means, for example, that the drivetrain will listen for Goal and Position messages, and publish Output and Status messages. (look in //frc971/control_loops/drivetrain:drivetrain.q for the actual definitions). This lets us be resilient to crashes, and keeps the interfaces between modules very well defined. For example, with 0 changes outside the hardware interface layer, we switched from running all our code on a BBB in 2014 to it all running on the roboRIO. We also are able to generate simulated Position messages and listen for simulated Output messages in our unit tests so that we can exercise the code without risking the real robot.

Originally Posted by thatprogrammer

1. If you are generating a motion profile based on every joystick input (or every 5ms cycle?), how are you not overloading the roboRIO?

There are a bunch of ways to do motion profiles. 1 way is to pre-compute the entire profile, and then just execute it. This is common in well defined environments, or where calculating the profile dynamically is expensive. The other is to re-calculate the profile each time you want to make some progress.

We like the flexibility of re-calculating every time we want to move. To do this, we need to advance the requested position each cycle of the control loop. The only way to guarantee that is to do that in the control loop. (Last year, for our drivetrain, we had another process which was calculating the profiles. The added coordination overhead of interacting with that process was enough that we decided to push the profile down into the controller process.) This also means that the joystick code really just sends out goals when buttons are hit, and lets the underlying code do all the rest. //y2016/joystick_reader.cc has all the joystick code, and it's pretty easy to read.

The motion profiles are cheap to calculate. The 7x7 matrix multiplies do add up though

It turns out a large chunk of CPU on our robot goes into context switches between the various tasks. Our drivetrain code uses like 6% CPU, and our shooter uses like 3%. Most of the CPU on our robot either goes into the logger (20% ish), or into WPILib (50%).

Originally Posted by thatprogrammer

Thanks for the help!

Sorry if any of the questions are strange or ignorant, I'm not super familiar with C++ and some of the more advanced syntax and features of it. Following your code has been somewhat difficult because of that. (That said, it is written very well and is pretty well commented.)

Ask more questions as you have them. There is a lot going on in our code which isn't obvious on the first read. Hopefully the background I've given so far will help you understand what is going on.

If you can get a linux box set up, do try to run the tests. It's pretty cool to be able to exercise our collision detection code, zeroing code, and other test cases. (bazel test //y2016/control_loops/...)

We are huge believers of code review and test driven development. I don't think we could do what we do without both of those, and it helps us involve students at all skill levels in the process while maintaining the quality and reliability we require of our code.