TBH for dual flywheel shooters

My partner team’s robot uses 2 double flywheels (4149C-style) and they want me to implement the TBH algorithm for their robot. I use it on my single-flywheel robot, but I’m not sure how to program the robot to use TBH for 2 independent flywheels. Can someone who has done this before show me how, or float an idea as how it would be written?

Basically, if you follow jpearman’s example code you can get one loop written. But for a double flywheel you have to double up everything to ensure that they are both moving at the same speed. You can have one loop, but have two wheels being controlled by two separate sections in the loop.

So I copy each function in the code for each flywheel, such that there is a separate function (and associated variables) for each flywheel?

It’d probably be easier to use structs to do this because so you don’t have to repeat yourself when you need to change something.

If you’d like to see examples, here are links to one of our past TBH implementations. You have to


the TBH Controller.h file in order to get the TBH functions and then use the controller tasks in your main file.
TBH Controller.h
Main file

A struct in C is a collection of variables, kinda sorta like objects in other programming languages. @Team80_Giraffes has a good explanation of this in this post.

If you have any other questions, just let me know.