If i may ask - what was the rationale behind aiming for the top before mid?
PROTOTYPING WOOOOOO
U-shaped Gripper:
Summary
It was a simple design to test the compression and grip of compliant wheels on the game pieces. The two sides of the U were adjustable, allowing us a smaller width to test intaking and outtaking the cone and a larger with to test the cube. After gaining some basic measurements of 35A compliant wheel compression and spacing, we saw that the implementation of this design as a cone intake was questionable due to the way the cone reoriented itself and its distribution of weight after being gripped. At different angles and different speeds, the cone would rotate within the rollers and leave us uncertain about a fixed way to outtake. The outtake was also questionable because the vertical rollers pushed the cone forward at different distances depending on the speed of the rollers, and so outtaking directly above the cone node didn’t work and we couldn’t get a fixed position to outtake reliably from.
On the other hand, this design worked really well for the cube, as it is light and symmetric. We didn’t have to worry about any gripping issues, and since the cube nodes are much wider than the cube itself, outtaking accurately wasn’t an issue. We stopped pursuing this idea further due to more promising manipulator designs we tested below.
Horizontal Roller intake/outtake
Summary
The idea with this prototype was to figure out how effective and efficient two horizontal rollers could be when intaking a cone at a constant fixed angle. Our goal was figuring out this angle, how far apart the rollers should be, and what kinds of rollers would be most effective. Our mechanism could pivot about a shaft, allowing us to test intaking at different angles very easily. Our first iteration of this used pvc lined with horse gauze as grip tape for the rollers. This was pretty effective in both intaking and outtaking cones, but after shaking the intake while a cone was inside, it seemed that the intake wasn’t reliable in holding the cones. After realizing that our lack of roller compression, and therefore constant pressure on the sides of the cone was an issue, our next idea was to test 2" compliant wheels, which could provide the necessary compression and grip. After some discussion, though, we realized that the wheels would add a lot of weight to the lever arm that this intake would be mountain on, so to reduce the weight, we decided to line pvc with foam (providing the compression) and then F4 grip tape. This made our design work nearly a hundred precent of the time for intaking, holding, and outtaking the cone. Since the two rollers are horizontal and rotating at the same speed, outtaking was always at the same angle, directly into the cone node. We noted that having the two rollers at an angle of incline between 30-45 degrees worked really well.
Compression data collected:
Cube side: roller Center-Center distance is 8.5 inches
Cone side: Center-Center 4 inch. Roller Diameter 1.7 +/- 0.1 inch
Cube Ground Intake:
The idea behind this is that we want our intake to span the full width of our robot in order to cover the most potential intake surface area while driving. The cubes will go over the bumper and into the robot, where the arm intake from above will take control (the arm intake was not testing for cubes yet). With this, we wanted our cube to center itself in the middle of our robot so that there was only one fixed place that the arm intake could obtain the cube from. We originally prototyped this for 3 rollers but then later changed the design to only use 2 with a bumper cutout. We are still working to hone in our compression levels for the 2 roller design but the 3 roller videos and compression are linked here.
4 Likes
There were 2 key reasons behind this decision.
- We’re designing our robot to score high to ensure we can form as many links as possible, are aiming to be a successful scorer at DCMP as an alliance captain while also making it easy for our alliance partners focus on forming links on the mid nodes, and because it’s one more point in virtually the same amount of time. In quals, this would allow us to get the sustainability RP as quick as possible—we would never be in a situation where all the middle nodes are filled and what we’re fastest at doing is done. Arguably it would make sense to also design for low since the point differentials between node heights are small but the next reason was a more driving factor towards this decision.
- Cycling using the double substation slider. The single substation chute and the double substation ramp were far too unreliable in dispensing the cone upright. So, we went with cycling from the double substitution slider in an upright orientation (bc dealing with diff cone orientations sucks) to optimize our cycle time. The slider is close enough in height to the high node to prioritize design for high node scoring as it gets us a sweet extra point for every node (in addition to the link reason I gave above).
FWIW, our current design allows us to score on both the high and mid nodes. The plan right now is to primarily focus on scoring cones/cubes on high nodes. Not gonna cap I feel like high node cycling is gonna be the move for us but I expect the ability to score on the mid node will be useful sometime down the line.
6 Likes
We’ve separated our robot cad into specific subsystem files. Onshape be lagging a lot with everything in one file so splitting it before we got too much done felt like it was the move 
.
Expect a more detailed post going over our v1 robot design later 
3 Likes
Prototyping Week 2
In the process of finalizing our mechanisms and subsystems for our robot, our ground intake for cubes proved to give some trouble in terms of the spacing and grippyness of the rollers.
Previous iteration of ground intake with thrifty compliant wheels and vectored mecanum wheels
Assesment of Problem^^^
After iterating from our previous 3-roller design, we tried moving to a two-roller design: the bottom roller being the plastic thrify vectored mecanum wheels and the top being 2" thrifty compliant wheels. The issue we found with this was that the plastic mecanum wheels were not able to sufficiently grip the cube unless the cube was pushed into the wheels with decent force. One idea we had to solve this was to place a roller with compliant wheels right in front and at the same height as the row of mecanum wheels so the cube was stay gripped while being centered. Although we didn't test this idea, we knew from other tests that the grippyness of the compliant wheels often overpowered the centering ability of the mecanum wheels, so we were unsure about the reliability of the idea.New iteration of ground intake with 4" grippy mecanum wheels
Another idea we had to solve this was to replace the plastic mecanum wheels with 4" grippy mecanum wheels, assuming that these wheels would grip the cube immediately and therefore center it much faster without added force.
Assesment of Problem^^^
At first, we used 2 of these wheels on each side and had the roller a bit far from the bumpers. Here, since there was enough space for the cube to go in between the roller and the bumpers, the cube would just go over the bumper as soon as the roller was in control of it and not center.
Summary of successful tests^^^
So we then moved the roller down and back so that the wheels were close to the top corner of the bumpers, ensuring that the cube could not physically go through in between. We also added a third wheel on both sides which reduced the possibility that the cube would get pinched and held in between two wheels. We tested two positions of the roller, both pretty close to the corner of the bumper, but one a bit higher than the other. For the higher one, when the roller was spinning at very fast speeds, the cube would bend the shaft upwards and find a way to go over the bumper, but at slower speeds, the roller was consistent and pretty touch-it-own-it. For the position that was closest to the corner of the bumper, the intaking and centering worked pretty well even at fast speeds.After rewatching the videos, we did notice a discrepancy in how the cube was moving into the center. It seemed that the cube was moving over the top corner of the bumper, and more so on the right side (near side in the video) than the left side. This might be because on the right side, the mecanum wheels were more towards the right side and less near the center, so there was a decent space between the last mecanum roller and the corner of bumper at the center on that side, which gave the cube more leeway to move up. On the left side though, the mecanum wheels were closer to the center and so it kept pulling the cube in. We solved this problem by packing the center more closely and making sure the mecanum wheels on both sides pass the corners of the bumpers at the center.
Compression Numbers:
9.5" - height of center of roller off the ground
4.75" - horizontal distance between center of roller and back of bumper
1.625" - height of bottom of bumper off the ground
6 Likes
Robot Design Update
Woohoo it’s CAD!!
Design members have been hard at work and finished up CADing and reviewing the robot. Bar some prototyping for the Gripper which may change some distances, we now have a good idea of what our robot for this year will look like!
We ultimately decided to split our robot into four different subsystems: Drivetrain, Ground Intake, Lift, and Gripper. Overviews and design decisions for each subsystem can be found below 
Drivetrain
For the Drivetrain, we used a similar design to our 2022 robot with MK4i swerve modules and a 28.5” x 28.5” frame. Last year, we really enjoyed having a brainpan to maintain clean wiring and having an easy way to access the electronics. We were initially going to use a brainpan to mount electronics this year too, but due to our Ground Intake and Lift mounting needing a lot of cutouts on the brainpan, it was difficult for us to maintain sufficient space to mount electronics and wire effectively. Having good electronics placement (such as reducing the distance of the PDH → Main Breaker and PDH → SB120 path as much as possible to reduce the resistance in our system, see CD post here) was a top priority for us this year. Therefore, the brainpan became a less desirable option. Soon after, we switched to bellypan as we realized that it would not be terrible given the open space we have on our robot this year, it would also give us enough space to mount our electronics where we wanted.
To protect our electronics, we have velcroed-on polycarb plates which cover the open areas on our robot. These are also raised to be slightly above the bumpers so that game pieces would not get stuck in our robot.
One mistake we made this year was not finalizing the bellypan fast enough. Since we do not have the manufacturing capabilities to mill the large bellypan, we usually get our bellypans done through SendCutSend. We’ve been very satisfied with their work, but it is one of the first things we prioritize because of the time it takes to ship. However, given the switching from brainpan to bellypan as well as the desire to perfect electronics placement/making sure that things didn’t clip or interfere with anything, we ordered the bellypan later than we would have liked, which blocked our goal of getting the drivetrain wired early.
Our bellypan, finally here!
We went with a bumper cutout this season in order to accommodate how the Ground Intake intakes CUBES. This cutout is only for the bumper as we deemed a frame cutout to be unnecessary; we did not like how it affected the structural integrity of the drivetrain frame and where the center of gravity would be placed.
Going along with the theme of clean wiring, we added numerous grommet holes in the Lift mounting rails, as well as other places on our robot to allow for electronics in the bellypan to be wired easily.
On the corner of our swerve modules, we took inspiration from Spectrum and printed out 3DP corner pieces to push our frame perimeter out by 1/4” on each side. This allowed us to move our bumper mounting plates out of the corner, making them a lot smaller.
Ground Intake
For the Ground Intake, we decided to use a motor-powered pivoting roller system with 4” mecanum wheels. This only intakes CUBES from the ground, as we decided intaking CONES was not worth the extra complexity. Although we do have a Gripper to intake both CUBES and CONES, we thought that a handoff system would speed up our CUBE cycles by a significant amount.
Current Design
Our decision to use rollers instead of a gripper to intake CUBES off the ground was based on the logic of “touch it own it.” With this full-width intake, we are able to run into and intake CUBES without needing to precisely align with it.
The Ground Intake was one of the first things we prototyped, and we actually had a different original design. This design utilized two rollers: a set of 2” mecanum wheels and 2” compliant wheels to center and bring the CUBE into the middle of the robot’s frame. Aluminum tubing was used to ensure the intake was structurally sound because its planned resting state during the match was against the bumpers. However, after prototyping the position and CUBE movement with this design, we realized that the geometry of the two rollers was barely able to intake the CUBE. We iterated on this design, and after successful prototypes with the 4” mecanum wheels, decided on what we currently have.
Original Design
In order to center the CUBES into an easily accessible position for the gripper to pick up, prototyping found that 4” mecanum and Thrifty Squish Wheels would work to center the CUBES into the bumper cutout.
The actuation of the ground intake is powered by a NEO Brushless motor on the right side of the robot, with a hex shaft spanning the robot to connect to the left. The NEO was geared with a 44:1 reduction, providing us with enough torque to lift the intake with only one motor.
The final stage of reduction is done through a 16T RT25 pulley belted to a 32T RT25 pulley on a dead axle. Originally, we had the pivot done with chain and sprocket, but we didn’t want to deal with chain tensioning or the extra weight. We hope that the belt and pulley will hold up to the torque, but if it doesn’t, the RT25 system was used so that we can just swap it out with 25 chain. The pulley is screwed directly to the arm, so as the pulley rotates, the arm will too.
The rollers at the end are powered by another NEO, mounted directly on the left intake arm. We originally had this NEO on the gearbox plate with a double pulley on the dead axle. However, due to concerns over 3D-printed spacers under the double pulley melting due to fast speeds, we decided that mounting it directly onto the arm would be the simplest solution.
We are also trying out 3D printing these bearing retention hats to retain our bearings and make sure that they don’t pop out on the polycarb intake plate.
Lift
For the lift mechanism, we decided on using a tilted elevator to minimize arm extension length. We took heavy inspiration from 125’s 2018 design. We opted for a continuous design over a cascading elevator because we wanted to save weight through 3DP and belts as opposed to chain.
Since we pre-purchased a thrifty elevator kit, we wanted to use things we got from it even though we were opting for a continuous elevator. Both the base and the first stage of the elevator are similar to the original thrifty elevator, using the thrifty elevator bearing blocks.
Carriage
For our carriage, we decided to mill our own plates and have standoffs in between the plates. The design of the carriage is very similar to 125’s carriage, but instead of 3D printing it, we milled a 0.25” aluminum plate. The 3DP carriage was a cool idea that would have saved weight, but we were concerned about its rigidity. Since it had to hold a mechanism with a linear extension at a tilted angle, we thought it was better safe than sorry and traded some extra weight for more rigidity.
For the left-right constraining bearings on the carriage, we half-pocketed enough space to fit an aluminum dowel pin with bearings on it (similar to 125’s carriage). An important thing to note is the “dogbone” corner relief, which we did because our CNC’s 4mm endmill would not be able to cut the corners needed to fit the dowel pin.
For the front-back constraining bearings, we used 1/2” 10-32 shoulder screws with a 3DP spacer to stop the bearing from moving. We plan on tapping the plate for 10-32 screws so that the shoulder screw can directly screw into the plate. We want to do this on the CNC to ensure that we tap the hole completely perpendicular to the plate. The holes for the shoulder screw are half-pocketed so that a portion of the shoulder part of the screw can rest inside the plate. If the elevator ever takes a hit, the shoulder screw wouldn’t be taking all the force and instead the carriage plate would take some of it.
Rigging
We have 2 NEO motors with a 2.08 reduction (see reca.lc). We are using RT25 belts and pulleys just incase we may need to switch to chain, where we can just swap them for chain and sprocket without having to change center distance.
Similar to 125’s rigging, we have inline 3DP pulley blocks to allow for a 20mm wide HTD 5mm pitch timing belt to run through them. They are attached to the tube through screws that go through the tube and 3dp block. We also have an idler near the driving pulley to make sure that enough teeth are contacting the driving pulley that the belt won’t skip teeth.
We plan to drill a hole through and tap a 1/2” hex shaft for #4-40 screws to fasten the end of the belt to the shaft. For tensioning, we have a ratcheting system using a ratcheting wrench so that we can hand-tighten the system.
Cable Carrier
Originally, we were planning on using an energy cable, but we couldn’t get it to fit properly without interfering with other things on our robot. Instead, we are using a polycarbonate sheet that holds the wires on the inside and attaches to the carriage. As the carriage goes up, the polycarbonate sheet will bend up with it so the wires move up and down.
Gripper
For the Gripper design, we took inspiration from team 3847, Spectrum. We aimed for a lightweight manipulator that attaches to our tilted elevator carriage. This would be able to intake CUBES and CONES from the human player station as well as extend outwards to grab CUBES from the Ground Intake in one mechanism. We are still prototyping the roller distances and will iterate based on what mechanical finds to work the best.
We are using a set of three 2” rollers connected by belts with the motor connecting to the middle roller which then connects to both of the other rollers through infinity belts to ensure that the rollers run in opposite directions. The motor in this case is a Neo 550, mounted far back, also for weight-saving purposes, attached to an UltraPlanetary gearbox for a 3:1 reduction.
For all of the rollers, we are using tubes mounted to custom pulleys with bearings on either end mounted on dead shafts to reduce the amount of weight in the Gripper.
All of this is mounted on a linear slide with a 10” extension, made using two 2”x1” tubes and two 2”x2” tubes with Thrifty Elevator bearing blocks in between (essentially just a horizontal, inverted Thrifty elevator). This linear slide moves using another Neo 550 motor mounted far back, also for weight-saving purposes, with another UltraPlanetary gearbox with a 9:1 reduction. All of this is then mounted on the Lift carriage plate.
Feel free to ask if there are any questions about our design, and until next time! (hopefully with more of the robot built 
)
21 Likes
Incredible work! Your robot is looking amazing. Definitely has been one of my favorite build blogs to follow! Thank you for the thorough documentation.
7 Likes
Just pedantry but your squish wheels linked are 2" when I think you meant to link the 4" ones. Looking forward to seeing what comes next, our design is very similar to yours.
1 Like
You are correct, it has been fixed now, thanks!
Really nice design, looking forward to seeing this on einstein
After we finished prototyping our gripper, it was time to construct it. The rollers were quite the challenge to prepare. For context, the rollers are comprised of two elements, a 2 inch diameter polycarb tube and 1 3/8 inch diameter rubber sleeving. The sleeving was chosen to help increase the grip between the rollers and cones/cubes. The rollers are then driven by two pulleys connected with an infinity belt. The middle roller is shared between the cone and cube intake sections.
Actually getting the rubber sleeving over the tubes proved to be quite the challenge however. The sleeving would have to be stretched over, and it wasn’t very stretchy to begin with. Initially, we tried having 3 members individually pull the opening of the sleeving wider to make it easier to insert the tubing, but even with soap and water as a lubricant, there was just way to much friction. Doing further research, we happened across a video from 1678: Citrus Circuits. They utilized an air compressor to more evenly inflate the sleeving to a larger diameter, which made it significantly easier to get the tube inside. To more evenly stretch the sleeving over the tube, we designed and 3D printed a custom “cone” to use as a tool to help with this.
To create a better seal for the air and to further lubricate the surface, we applied soap and water to the cone and polycarb tube which worked quite well. This worked quite well for us (though after 5 hours of struggle 
)
9 Likes
The struggle of learning to stretch rubber tubing on to rollers is very real. No matter how many years in a row we do it, I feel like we have to learn it all over again every year with slightly different tubes and rubbers.
13 Likes
for your intake, could you measure bumper edge to the center of the roller?
This was a difficult measurement to take since the bumpers are going to have some level of defect in them (especially because these were older bumpers taken apart) and so we chose to measure it to the back of the wood since this was a much more consistent and non-compressible measurement.
Based on CAD the distance from the edge of the wood to the front face of the bumper is around 3.25 inches so the Horizontal distance from the bumper edge to the center of the roller is about 1.5". I hope that helps but if not you can make any other measurements based on the bumpers on our onshape.
1 Like
I see, thanks, was just concerned about how well the cube would grab when against the side of the bot but it looks like it works just fine according to one of your videos that I missed.
1 Like
It spins!
Our elevator is rigged but we haven’t attached a motor to the system; 2 quick grip clamps are holding up the carriage right now. Our wonderful mechanical team has a detailed post coming tomorrow going into the specifics of all subsystems so I’ll leave the explanations to them
Something I’m really proud of is that this was pretty much first try. I largely attribute this to our use of simulation this year. For example, we recently replaced our legacy swerve math with WPIlib swerve math and solved a lot of drivetrain logic-related issues right away in sim! We have to do proper PID tuning (stole last year’s values for now) but we zeroed the modules and it pretty much just worked.
Relatively speaking, drivetrain code is the most evergreen but I expect our use of sim to save an immense amount of time in debugging issues for other subsystems–sim is fr fr the move ong.
Looks cooler in real life but here’s the robot also spinning in sim:
More detailed software post to come in the future but for now be on the lookout for a tuff mech one
-Saraansh
11 Likes
Do you guys use AdvantageScope for sim?
Yessir 
I’m not entirely sure if you’re asking about visualizing simulation or simulating the physics for mechanisms so I’ll talk about both.
With respect to visualization, we now pretty much only use AdvantageScope for viewing mechanism and drivetrain simulation. WPIlib simulation visualization tools (Field2d, Mechanism2d, etc.) are seriously incredible and AdvantageScope has helped us take it one step further in visualizing stuff!
With respect to simulation itself, we use AdvantageKit in conjunction with WPIlib physics simulation. We stick most of our control related stuff (motion profiling, tunable setpoints, feedforward control, etc.) in our logic classes and leave lower level controller stuff (setting voltages, commanding setpoints, etc.) to hardware implementations.
To give an example, here’s our Ground Intake subsystem in code. Trapezoidal motion profiles and the setpoints we command our ground intake to go to are all done in this logic class (independent of hardware implementations).
And here is the Ground Intake simulation implementation which the subsystem communicates to using the io object. The only thing the simulation implementation does is simulate the mechanism using WPIlib physics simulation and implement lower level control stuff (configPID(), setArmVoltage(), setArmPosition(), and setRollerVoltage()).
Having a minimal amount of controller specific functions in our hardware implementations helps us be more confident in our code working b/c the logic class is utilized by all hardware implementations (this is also why logic class is like 2x longer than hardware implementations).
Tbh, @jonahb55 has done a much better job of explaining this so if you’re interested in learning more about how IO structures are used to help simulate check out this section on AdvantageKit docs.
Last thing on this is that we generally like to stick PID controllers in hardware implementations to reduce overhead on the roboRIO. To increase our confidence in the robot using the fr fr hardware implementations (i.e. GroundIntakeIONEO.kt) we could add PID controllers in our logic class but we like to take advantage of inbuilt motor controller PID controllers: which is why we have to add PID controllers in simulation implementations.
7 Likes
Sorry it’s been a while, but I’m back with a huge progress update! Here’s what we’ve been up to the past two weeks. Shoutout to everyone on the team who compiled all of this info and pictures!
Drivetrain
Before the season, we decided that we’re going to use the SDS MK4i swerve modules for our drivetrain so we went ahead and assembled those ahead of time so that when we had all the tubes for the frame we could quickly assemble it. After mounting the swerve modules we attached the drivetrain rails. These rails would anchor the lift supports and had grommet holes to allow for cleaner wiring.
They were also reinforced with a gusset on one side and MAXtube endcaps on the other because the gusset would clip with the lift so with the endcap we could just screw it directly to the frame. Soon after putting on the rails, it was time to mount the belly pan, in order to lay out our electronics. We ordered our belly pan a few weeks prior through SendCutSend, as it would’ve been too large to fit on our CNC. Although it took a while to arrive, it arrived safely and was sized accurately.
After getting the bellypan on, it was time to start mounting electronics and wiring up the robot.
While attaching the radio power module, we accidentally overtightened one of the nuts on the corner and the case broke so we decided to just drill two holes on either side of the RPM on the bellypan and just zip-tie across.
With all the electronics and swerve modules being wired and connected to the PDH, drivetrain wiring was finished but still far from the current configuration being finalized.
We then realized that in order to put on the two elevator plates, we would have to take out two of the back swerve modules because they were blocking access to the drivetrain rails and our rivet gun couldn’t fit in the gap. For other small spaces, we used a manual rivet gun, which was much easier to fit in the gaps.
After we got those plates on, we attached the base stage tubes to the plates as well as the support tube to strengthen the structure and then we put the two back swerve modules back on.
Ground Intake
Assembly of the ground intake was more or less a straightforward process, with small adjustments made accordingly along the way to resolve issues that came up.
Assembling the intake arms
There were three main tasks that were necessary to assemble the ground intake: putting the wheels on the roller shaft and attaching it to the polycarb intake arm, assembling and attaching the gearbox to the drivetrain, and attaching the arms to the plates.
To assemble the roller, we put on mecanum wheels and Thrifty squish wheels, similarly to our prototype, in order to center the cubes into the cutout.
This year, we opted to use bearing hats on top of the bearings in order to make sure that they would not move or fall out during matches. Here is a link to the CAD:
Bearing Hats
Washers to hold rivets
An issue we came across while riveting the bearing hat to the polycarb arm was that the holes in the polycarb arm were too large; this prevented the rivet from being able to grab onto the end of the plate and fastening the bearing hat to it. This was an easy fix though, as we just used washers with smaller holes on the plate to prevent the end of the rivet from slipping through.
Initially, we 3D printed the bearing hats with PETG, but later realized after one of the hats broke they would likely be vulnerable when force would be applied to the polycarb arms (ex. if another robot rams into us). In order to alleviate this problem, we reprinted new bearing hats out of GF nylon to improve the stability and structure of the intake.
The last task to assembling the polycarb arms was to attach the roller motor onto the arm. We screwed the motor onto the arm with a spacer extending the distance of the motor shaft so that it would allow the motor pinion to spin freely. This however came with a few issues, one being that the motor and spacer were not directly perpendicular with the arm because it was only fastened with two screws. The lessened distance caused the belt tensioning to be looser than expected, so we reprinted the motor spacer with a third hole in the back to better hold the motor down.
With that, the arms were assembled and were ready to be attached to the gearbox.
Attaching plates and gearbox
The intake plates were milled out of aluminum in order to provide strength and solidity to the intake as they would be responsible for carrying most of the load. We countersunk the outside holes of the outermost plates so that the rivets attaching it to the drivetrain rails would be flush with the bumpers. The outside plates were then riveted into the rails, and then the inside plates on the other side of the drivetrain rails were riveted as well. This was a bit of a struggle due to the spatial constraints with wiring occurring on the drivetrain, but we were eventually able to rivet most of the holes to support the intake.
We then put the middle plate between the inside and outside plates. The three plates were fastened together with 4” screws spanning the entire length of the inner to outer plate, separated by aluminum spacers to keep the distancing accurate. We then assembled the gearbox for the actual actuation of the intake, the dead axle tube nuts were inserted into the ends of the two cut dead axles, and the through bore encoder was screwed into the left plate.
Fastening arms to plates
Before attaching the arms to the intake plates, we screwed the RT25 pulleys into both sides of the polycarb arms. Then, we used two bronze bushings on both dead axles as a rotation point for the intake. We decided to use bushings as opposed to a spacers in order to account for the fact that the point of rotation for the ground intake would heat up during a match due to the constant movement of the intake, which could cause a 3D printed spacer to melt. We then ran the bottom intake shaft through both sides of the attached intake plates, making sure to place the other RT25 actuation pulley at each end of the shaft.
We then put both sides of the polycarb arm onto each bushing on the dead axle, belted the pulleys together on each side (the small RT25 pulley to the RT25 on the intake arm), and screwed in the dead axles into the intake plates. This was a bit difficult given that the length of the dead axle was exactly the distance between the inner and outer plates, but we were eventually able to squeeze the dead axle between the plates and screw them in their corresponding holes.
With the intake arms being connected to the intake plates, the assembly of the ground intake was complete.
Things to look out for
The reason we decided to choose a RT25 pulley/belt system was because it was a lot lighter than using chain and sprocket, and it also simplifies the process of having to mess with chain tensioning. However, after receiving information from OA teams about RT25 belts potentially skipping, we have been reconsidering if we should switch to a chain and sprocket to actuate the intake. Belt skipping could cause inaccuracies with the through bore encoder, which is meant to record the rotations of the shaft, and could therefore lead to inconsistent rotations of the intake.
The good thing about RT25 pulleys is that the center to center distance of the pulleys should be almost the exact distance if we were to switch to chain and sprocket instead of belt and pulleys, so fixing this potential issue should not be too complicated of a process.
Gripper
The assembly of the gripper went quite smoothly for the most part, except for a couple of things, such as the preparation of the motors.
Getting the shafts and rollers on
The rollers of the gripper freely rotate around a shaft, and are driven by pulleys. Attaching the shafts was quite easy, as we had precut and tapped them. One of the shafts had metal shavings in the tapped hole, which made it difficult to screw it in but we cleared it out with an air duster. We used the same infinity belt configuration we prototyped with earlier for the two top rollers which worked out well. However, for the bottom two rollers, we couldn’t find the exact belt we used for our prototype and the older belt of the same size we found was too worn to use so we had to order a new one. This new belt fit quite nicely, and was about the perfect tension to have every roller on the gripper be driven reliably.
Preparing the motors
Preparing the motors was something we thought to be easy, but in fact, it turned out to be a bit of a hurdle in the assembly of the gripper. The motors we were utilizing were two NEO 550’s, one for driving the rollers, and one for driving the extension of the gripper. Note that the rollers had quite a bit of resistance when being free spun, as this will be important later.
The NEO’s would both require REV ultra-planetary gearboxes to get the necessary torque, so we decided to go with 3:1 cartridges. To drive that cartridge, we had to press the necessary pinion for it onto our NEO’s, which we didn’t carry any spares of at the time. However, the planetary gearboxes come with FTC 550 series motors (REV HD Hex), and they have the necessary pinions already pre-pressed onto them. We decided to “steal” these and reuse them on our NEO’s.
To remove the pinions from the HD Hex motors, we used a heat gun to thermally expand the pinion, and this worked quite well to remove the pinion from the motor’s shaft after prying it off with two screwdrivers. Then, once the pinion had cooled to a temperature we could handle it with, we pressed it onto the NEO 550 with a quick-grip clamp. The process of pressing the pinion onto the NEO’s was suspiciously easy and we did research on this. Later we noticed that the shaft of the NEO was undersized in relation to the pinion we were using. We also tried testing it on the gripper, and because of a combination of the resistance that the rollers had as well as the loose pinion, the motor would rotate freely, but not spin the rollers. We saw that this has been an issue for quite a while, until we found this CD thread [link] about solutions. We decided to try the crimping tool idea that essentially deforms the NEO’s shaft, and expands it out in the adjacent direction. After testing this on a spare NEO we had, we liked how it worked, and did this to our gripper motors. Pressing the pinion on felt much more secure now, and we also decided to swap out the 3:1 planetary cartridges with 5:1’s so there would be less torque needed for the motor to rotate, and less of a chance for the pinion to come loose. We then reattached them to the gripper plates and ran a test using our prototyping board, and it was able to spin freely on its own, and we plan on testing with actual cubes and cones once we finish more aspects of the robot.
Assembling the extension stage
Assembly of the extension stage was quite easy for the most part, with the only issue we ran into being bearings not fitting into tubes correctly. We suspect this to be because of the powdercoating making it’s way inside of the hole, as well as not leaving enough tolerance for the bearing. This was easy to solve however, and we just had to deburr the inside of the holes to expand them slightly. One issue we had with the 3D printed clamp was the design and print orientation.
With testing, the layers separated, causing the clamp to fail and required a redesign. Our newly designed clamp was able to be printed in a better orientation, which worked well. Note that the hex bore in this picture is a bit loose, so we’ve reprinted since then.
Changes we want to make
Right now, when intaking cubes there isn’t a mechanical hard stop. We don’t want to solely rely on code for this, so we plan on attaching an additional shaft above the cube intake for the cube to intake into, and not just fly out the top.
Additionally, with further testing, we’ve seen that even with a 5:1 ultra-planetary stage, the NEO 500 still struggles to rotate all three gripper rollers consistently. We plan on either attaching additional stages or switching to a full sized NEO if space permits to resolve this.
Some upLIFTing progress!
The first step in assembling our lift was to assemble the carriage that the gripper is mounted on. The carriage plates were milled and the 10-32 holes for the shoulder screws were tapped on our drill press instead of the CNC. A major issue with using the drill press is the fact that the speed in which the tapping bit goes into the plate had to be just right, or the threads would not be suitable for the screws we had. After messing up and tapping a spare plate incorrectly, we managed to successfully tap the holes for the carriage plates on the second try, with a lower speed and checking the alignment multiple times.
Bearing Block Assembly
Once the plates were milled and tapped, we started to assemble the bearing blocks that would constrain the carriage from moving left and right. The bearings had to be fit onto a dowel a specific distance from the edge which proved to be very challenging to get exact. We tried press fitting the bearings onto the dowels but this was very time consuming as it was mainly just trial and error and the bearings were pretty fragile.
In addition to this, we realized that the tolerances for the dowel slots were too tight, so the bearings and dowel wouldn’t fit perfectly into place as we hoped. The hole for the bearings was too small so there was friction between the bearings and the plate and the half-pocket was too deep which could potentially cause the carriage to wiggle in the middle stage. At first, we tried to widen the holes in the plate with a drill to accommodate the bearings and dowel but this didn’t work very well. After several broken bearings, we decided to sand down the dowels themselves so that the bearings could easily slide on them. In the future, we will probably design a 3D printed jig that we can use to properly align the bearings on the dowel so that this process can be sped up.
In milling, the z-axis was not accurately zeroed, so the depth of the dowel rods’ slots was not accurate. The wobbliness of the carriage is dependent on getting this depth accurately. We 3D-printed shims of various depths (1mm to 2mm) to place inside the slots to bring the dowel to the correct depth.
This worked very well for us and the bearing blocks were able to be completed and installed in the carriage while the rest of the carriage was assembled in parallel.
Once the bearing blocks were somewhat in proper alignment, we decided to finish up the carriage by mounting the springs and standoffs to the plate. We also built our ratcheting axle that the lift belt would attach to. We did this by sawing off half of a ratcheting wrench, and sliding the ratcheting disk onto the axle.
We then zip-tied the wrench down onto the carriage plate. This would ensure that the axle only spins in one direction so that we could later properly tension the belt without cutting it to an exact length.
Assembling stages together
The next step after assembling the carriage was to make sure that it fit within the middle stage properly and attach the gripper assembly to it. When we first slid the carriage into the middle stage, it fit pretty well and it moved up and down smoothly with relatively no wiggle room. However, we notice that some of the bearings weren’t fully contacting the middle stage. We then tried to fit the gripper into the carriage and we realized that it wouldn’t fit in and the carriage and middle stage were slightly bending outwards due to the gripper being too wide. After measuring and remeasuring, we realized that the carriage was an eighth of an inch too small so we decided to recut the shafts and reassemble the carriage.
This allowed the gripper to fit into the carriage perfectly and simultaneously fixed the issue of the bearings not fully contacting the middle stage. With the gripper on the carriage, it still moved very smoothly and there was still little to no wiggle room in the middle stage.
Middle stage was then put onto base stage which was already mounted on the drivetrain and then after we fastened the top tubes for both the middle and base stages, lift was complete! Until we tried moving it 
We realized that there was an undetected clip between one of the carriage standoffs and the pulley at the top of the base stage.
We decided that we could just remove that standoff entirely since there were already several standoffs holding the carriage apart on top of the gripper tubes and other shafts on the carriage. We also noticed that the constant force springs weren’t spaced correctly apart after we widened the carriage so one of them was actually hitting the pulley as well. After moving the spring over and removing the standoff, the lift was pretty much complete! All that was left to do was to rig the belt through the lift and attach the motors.
Rigging
The next day we rigged the lift by feeding the belt through all the pulleys and securing it at the bottom with zip-ties and at the top with tiny 4-40 screws onto the carriage. With the belt needing to be centered on the carriage, we decided to fasten the end of the belt to the center of stand-off. We resorted to using two 4-40 screws side-by-side because they were small enough to fit two end caps on the belt for proper securing and small enough to screw into one of the hex faces of the stand-off. Tapping the holes for these screws was a challenge as the holes had to be very precise and if the tapping bit broke while inside the shaft, we would have to scrap it entirely. In fact, we broke multiple bits while practicing tapping on a spare shaft. The strategy for this that we learned through practice was that the 5/64” pilot bit we used to make the original hole had to be drilled through the whole shaft. If it was drilled through only one half (stopping at the hole in the middle), the 4-40 tapping bit would instantly break, as the tip would hit a solid surface, in this case the second half of the shaft. Apart from this issue, we followed normal practices like keeping the bit as straight as possible and making sure oil was used.
We then used the ratcheting shaft on the carriage to tension the lift properly. Right now, the lift is pretty smooth but there is still a little friction from the powdercoat on the tubes and at the transition between the side and top tubes of the middle stage. This makes it so that its hard to start moving middle stage out of base stage but it’s easy to move after that point.
Over time, the powdercoat will rub off but as for the transition between the side and top tubes, we decided to file down the corners of the tube near the transition in order to allow the bearing to smoothly transition between the tubes. From our testing, when the carriage comes up and hits the top of the middle stage with enough momentum, the resistance caused by the bearing getting caught is negligible. We do plan on adding padding at the top though so when the carriage does slam into the top of the middle stage, it doesn’t damage the tube. The two NEOs on either side and the cable carrier still have to be attached but other than that, lift is looking pretty complete.
At this point the robot was looking almost complete so naturally we decided to have a little photoshoot:
13 Likes
Look at all that Software
Its been a while and a bigger recap post is hopefully coming but we got a W software update for you. We went crazy with our programming this year and did a lot of cool things and we wanna talk about it.
Taking advantage of sim
Sim is pretty valid. We were able to use sim in order to perfect a lot of our codebase before we had the robot. This rapidly decreased the amount of time we needed to do robot bring up. So let’s go over the different parts of sim.
One of the best parts about being able to use AdvantageKit is its ability to handle multiple different IO implementations. When coding a Ground Intake, if we don’t know what type of motor will be used we can code two different classes, a GroundTntakeIOFalcon and GroundIntakeIONeo. Each can be easily used in code. But how is this useful in sim? Well, we can code a GroundIntakeIOSim and take advantage of the motor and mechanism simulation classes within WPILib. Then when we instantiate our subsystems, we can do GroundIntake(GroundIntakeIOSim), and boom we got simulated subsystems running.
How is sim useful though? The biggest thing is testing logic. We talked about this once in our build blog but one of the things we changed in software this year is transitioning from our legacy swerve lib to WPILib Swerve code. Coding this change is relatively easy, the hard part is testing it and fixing bugs. We were able to simulate the motors on our drivetrain, and then see the outputs of this inside of AdvantageScope’s swerve module visualization tool. And the first time we ran this code on the robot, it worked.
We can also log the Pose3D of all of our subsystems and visualize this using the AdvantageScope 3D field viewer.
We were also able to simulate running auto routines in sim by visualizing our robot within AdvantageScope’s 3D viewer. This let us take Ws in comps by being able to generate a similar questionnaire system in Shuffleboard like 6328 and add engages to our auto paths. We could test these routines before in sim and check if they would roughly be under the 15-second time limit then run them IRL at a practice field to see if they work and tune more important values.
Overall sim helped us finish programming and tuning a robot where more time was spent getting it mechanically ready.
MotorChecker / FalconSpin
Over the past year, we have transitioned from a full Falcon500 robot to a Falcon500 drivetrain and a NEO / NEO550 subsystem robot. During this transition, one thing we found was a lack of thermal protection configuration for NEOs that is found in Falcons. After smoking and ruining countless motors we wrote MotorChecker which monitors the temperature of motors in order to move the current limits we have set through stages.
Essentially, every Motor is assigned to a Motor Collection. These groups essentially do the same function, so if we have two elevator motors, they will be assigned to the Elevator Group whereas the drive motors on a swervebase will each be assigned to their own group. Every Motor Collection will be given 4 values
- Base Stator Current Limit
- First-stage Temperature Limit
- First-stage Stator Current Limit
- Shutdown Temperature
MotorChecker.add(
"Elevator", // subsystem name
"Extension", // subcategory
MotorCollection(
mutableListOf(
Neo(leaderSparkMax, "Leader Extension Motor"), // add motors
Neo(followerSparkMax, "Follower Extension Motor")
),
ElevatorConstants.PHASE_CURRENT_LIMIT, // base current limit
30.celsius, // first stage threshold
ElevatorConstants.PHASE_CURRENT_LIMIT - 30.amps, // first stage limit
90.celsius // second stage (kill stage) threshold
),
)
When a motor is acting normally, it will be assigned the Base Stator Current Limit. If a motor is being stalled for long portions of time and the temperature is being raised to sus levels past the First-Stage Temperature Limit, then it will be assigned the First-stage Stator Current Limit. If it gets even more sus and goes past the Shutdown temperature, the motor is shut down. The temperature values are selected from the toastiest motor in the collection. You can pick these values using the graphs REV posted for their locked rotor testing. Let’s walk through picking the current limit for our elevator motors.
When our motors are stalled at the full application of 12V, we know that our stator current will be equal to our supply current. If we are supplying 60 amps on a NEO, we will reach motor failure near 105 seconds. So if we hit the 105 C, we know we may be reaching a point of high thermal failure. This means that if we shift our current limit to something lower than 105 C to be safe, like 70 C, then we can reduce the chances of burning out a motor. So at 70 C, we shift to a 40 amp current limit and then monitor our temperature. If it continues to rise to 85 C (just a number past our previous), we will shut down the motor.
After the implementation of our staged motor checker, we have not burned a motor since.
Additionally, we monitor for any motor faults and issues and log these. In order to make these easy to find and not packed far away into NetworkTables, we made a viewer directly in AdvantageScope to see the data. All of our custom views that we make are in our fork of AdvantageScope on Github and are mostly drop-in.
Superstructure and State Machines
It’s hard to build complex robots, and it’s even harder to control. How do you make sure that the subsystems you built aren’t colliding and hurting each other? The best way in my opinion is to use a superstructure. Superstructures are great since you can control the states and flow in which subsystems move in.
Let’s say Zapdos (2023 Robot) want to go from its IDLE position to a scoring position. The steps it has to take are
- Retract the manipulator to IDLE
- Drop the ground intake to its low position
- Extend the elevator to scoring height
- Extend the manipulator to the scoring distance
- Run the manipulator rollers
That’s a lot of steps and a lot of possible collision space. A superstructure handles all of this by checking the state of each subsystem and requesting movement from each one based on the state of the previous step. Let’s simulate a step 2 and 3.
- The superstructure commands the ground intake to go down by setting a
Requestwith parameters like angle and roller speed (groundIntake.currentRequest = GroundIntakeRequest.TargetingPosition(angle, rollerVoltage)) - Every loop cycle, we monitor the state of the ground intake by calling an attribute
.isAtTargettedPositionwhich is a boolean of if the subsystem is at the position requested - Once the
.isAtTargettedPositionreturns true, then we can move on to the next step - The superstructure commands the elevator to go up by setting a
Requestwith a parameter of height (elevator.currentRequest = ElevatorRequest.TargetingPosition(height)) - We monitor the
Elevator.isAtTargettedPositiontill it turns true and then move on to the next step
The sequential process has the con of being very copy-paste-like but is very readable and lets you easily control the movements of your robot. But how do you tell the superstructure what to do? We use something called a state machine.
State machines are just that, machines with set states. We can request a state to the state machine, and through a set of switch statements, we can start the control portion of our superstructure. It is best understood how a state machine works if we walk through the process of a button press to robot movement (Button Press to Score)
Every button is mapped to a function that returns a command. When our operator presses LB + Y in order to send the robot to the high-scoring position, the trigger calls a function to generate a command, with the appropriate passed-in node height and gamepiece type. Every command looks very similar because of the control scheme of the state machine.
val returnCommand =
runOnce {
currentRequest = SuperstructureRequest.PrepScore(gamePiece, nodeTier)
}
.andThen(
WaitUntilCommand {
isAtRequestedState && currentState == SuperstructureStates.SCORE_PREP
}
)
Essentially, the command sets the Request for the state machine or essentially any of the data the state machine/superstructure needs in order to move. When a request is set, there is a setter that reads the request type and then parses the possible game piece or node values.
Within our periodic, we read the request state and based on the current state of the robot (score prep state will only process score state or idle state while the idle state will process most of the states [switch statements go crazy]). When a request is processed it sets the next state which is then run in the next loop cycle. Essentially, a button press will take two loop cycles for the superstructure to start running the code for it.
To help understand the flow, we made a model of one loop cycle.
We also have state machines within each one of our subsystems and make requests to them within the superstructure. We have found this system of states for each subsystem and superstructure makes debugging any issues within our code very simple. If we are having issues with our subsystem not running, we can open up AdvantageScope and open the entry to see what the state of each of our subsystems is to see if something is stalling the next step in the superstructure process.
Each state machine within a subsystem for movement generally has these states.
- targeting position (during command)
- uninitialized (on init)
- zero/homing (at enable)
- open loop (need to get unstuck. barely used)
Like said before, each request here is its own class that stores the information needed to execute the request. While our Targeting Position may take Length types and a roller Voltage type, our Open Loop request takes an extension Voltage and roller Voltage.
sealed interface ManipulatorRequest : Request {
class TargetingPosition(val position: Length, val rollerVoltage: ElectricalPotential) :
ManipulatorRequest
class OpenLoop(val voltage: ElectricalPotential, val rollerVoltage: ElectricalPotential) :
ManipulatorRequest
class Home() : ManipulatorRequest
}
We recommend using the combination of a superstructure/state machine in order to control your subsystems and we look forward to using this in the future.
AprilTag Showers Bring May Flowers + tape
When they were announced, we played around with PhotonVision on the LL2 and quickly realized the performance wasn’t going to be enough. The testing we did is incredibly outdated but to this day I don’t think the LL2 is strong enough to produce tag data at a high enough resolution with a meaningful level of performance in terms of FPS, latency, and accuracy.
This led us to create our own AprilTag detection solution: an Android application called Hawkeye. We’re not going to use Hawkeye this season because of the limitations of Android camera APIs: you can only receive 30 frames per second from the camera (with the exception of a few phones) which made it at best equivalent in performance to existing solutions that were 3x cheaper. I’m still really glad we made Hawkeye–it was an incredible experience to learn the semantics of AprilTag tag detection and we were able to build wpiutil, ntcore, and wpimath for Android devices, and have them all work fr fr.
Hawkeye utilized ARcore for receiving the camera feed, apriltag3 C library for AprilTag detection and processing, and NTcore for sending the AprilTag data. And because it was NetworkTables based, we sent the data in a way that perfectly mimicked PhotonVision so we could use PhotonLib for parsing the detection data encoded in byte format. This made Hawkeye a drop-in replacement for PhotonVision which was really swag.
In the end, it all worked. We were able to get detection data, send it over NetworkTables, and successfully receive data on the rio side. Unfortunately, when we began testing on real devices we found that the S22 was achieving a max of 30 fps at 480p, 720pm and 1080p (for 720p and 1080p the phone dropped to 20 fps over the course of 2.5 minutes). The precision was also down to fractions of an inch which was really sweet in terms of accuracy.
Ultimately though, with hardware like the Orange PI absolutely demolishing the S22 in terms of performance for like a third of the price, it wasn’t worth running w Hawkeye. I think that if we were to have optimized the S22 by messing around with overclocking, we would get consistent 30 FPS readings on the S22 at 1080p but that wouldn’t have been enough. If we were to overcome camera API limitations, we would consider it but even on the Pixel 7, from which we can access the video feed at 60 fps), the performance was subpar (tensor chip is mid).
The only phone that we haven’t tested that could potentially work is the Xiaomi Mi 11 Ultra. If anyone on CD, has one you should highkey lmk because I’m curious to see Hawkeye performance on a good chip.
The most major benefit of this that I think might come in handy in future years is using a phone as a separate coprocessor. Having WPIlib work for Android devices might be useful for things that were previously too inefficient to do on your roboRIO, like on-the-fly trajectory generation, and then send that data over NetworkTables. If anyone’s more curious about Hawkeye, lmk and I’d be glad to go in more detail about it but as of right now we don’t plan on supporting it this season.
After we realized Hawkeye wasn’t going to work for us, we planned to use PhotonVision on the Orange Pi 5. The results looked promising compared to all other solutions. We ordered the hardware and found 6328’s solution to be insanely more performant and since it used the same hardware we decided to skrt using Northstar.
Jonah has a great write-up on Northstar and stats on it vs something like PhotonVision so we won’t get into that. We setup 4 Orangepi 5s and 4 Arducam OV2311s with Northstar but this is where the fun kind of ended for us with april tags. We faced some issues this season with getting this system to be as robust as we needed it to be.
Issues
- Some arducams had vertical strips, ruining the actual detection of tags with the cameras
- Orangepis would sometimes have their images corrupt which is so not valid
- Cameras would just stop running and getting images which is also not valid
- We have had the JST to USB cables from between the Orangepi and cameras stop working. (The three hardware issues happened at the same time at DCMP
) - Not that big of an issue but Northstar crashes when you show it a ChArUco Board without being in calibration mode
- Our calibration being bad
I don’t really think I need to go into our hardware issues but I will talk about our calibration. Northstar uses 9x6 ChArUco Boards in order to generate Open CV calibration matrices. The two issues with this are the amount of photos needed in order to get a good calibration file and Open CV being weird. During the season, we generated our calibration files via using the inbuilt calibration feature and took about ~20 snapshots. The pose being generated by Northstar after this was about 6-7 inches off.
After the season ended, we realized our calibration files could be the reason we are generating bad poses so we worked on generating our calibration files off of Northstar. We connected each Arducam to a laptop and took close to ~250 photos (rip my storage) per cam. We extracted the calibration logic from Northstar and ran each image into the calibration solver and generated a file. We ran this file and our poses were multiple feet off.
We visualized our camera calibration matrices and saw the amount of distortion in our files was messing up our calibration so we rewrote the script and boom our files were valid. We were generating poses within 1-2 inches of the robot’s location (good enough in my opinion).
One thing to note during this process is that you need to have your Open CV version be the same as the one running on Northstar since we have found different versions to generate different values in both calibration and corner locations.
We wrote our own PoseEstimator class and tuned our standard deviations and tag logic using log replay. We were able to drive our robot around the field, log tag detections, and then replay these detections using log replay with various standard deviations in order to test the best values and/or test logic for throwing poses in order to get the best possible W estimated pose.
Anti Jit(ter)
The calibration issue led to a lot of pose jitter because two different cameras would return slightly different poses. This would end up being enough to throw off our pose in the community by just enough to mess up auto-scoring. At the time, we just thought this was a limitation of the AprilTag setup and so we decided to throw in a limelight for tape detection so we would have another source of vision measurements and have no more jitter!
To briefly explain the workflow, we would first find the distance from the limelight to the target in the XY plane, then use trig and couple that with the node tape height to get the point-to-point distance from the node’s tape to the camera, and then represent that distance with the retroreflective reading’s ty and tx as a Translation3d. We relied on our operator app to figure out what node tier we were looking for (between mid or high), worked out the math with the robot’s current pose + that node tier height, and then passed in a robot pose that was represented as an inverse transformation from the real node’s pose using the Translation3d we calculated earlier. We found it was best to only accept LL readings that were within 5 inches (in the X, Y, and Z plane) of a real node. Limelight math and code can be found in this function LimelightVision.kt.
And it wouldn’t be a 4099 software post without showing sim so here’s a gif of us finally figuring out how 3d geometry works and getting LL math up and running!
It’s worth noting that we were trusting Limelight poses way more than AprilTag poses when we had our Limelight was properly detecting a target. Mathematically, this meant that our XY standard deviation for limelight poses were relatively lower than our AprilTag ones. The word relatively is really important here. Standard Deviations are a proper relative representation of your model and help us define how much we trust measurements relative to other measurements. Because Limelights didn’t have calibration issues 
we trusted its XY pose measurements more than AprilTag ones, but because its theta measurement was inherently going to be less accurate than one a $200 gyroscope would return, our theta stdev was relatively higher. Tuning pose estimators can be tricky but understanding the concept of relative stdevs and equations really helps you get a better idea of where to start after you yoink some starter values for odometry stdevs.
Log replay really helps you tune these stdevs and here is a vid of our current stdevs as the real robot and log replay a different stdev value as the ghost robot.
We got this all working by champs but due to the other AprilTag issues above, we ran very few automated scoring cycles.
OTF Trajectory Generation and why
In order to use the detected pose locations for scoring, we started generating trajectories to drive the robot from its location to a position on the grid in order to use it for auto score. The overall logic for this is pretty simple.
When the driver presses the button to start autoscore, a command is run to generate two poses, the current robot pose and the final robot pose. Our autoscoring sequence works when the robot is parallel to the grid and only has to make side to side movements in order to align so the final pose is just the location the robot has to be in front of the grid.
After generating these poses we pass them into a Drive Path command which takes a series of waypoints and drives to each of them in sequence. We use WPILib in order to generate quintic hermite splines for our trajectories and then use individual PID controllers for both our x, y, and theta in order to make sure we stay on trajectory.
In order to determine the location we want to go to, we utilized 6328’s Operator App and grabbed the location and game piece type from NetworkTables.
FalconUtils and introduction of units.
Unlike many teams, our codebase is written in Kotlin. One advantage that it gives us (other than being so nice to write) is that we can take advantage of receiver types and type definitions in order to make custom units and build wrappers from traditional WPILib classes that utilize these units.
The immediate benefit of this system is that we can easily do math within our useful robotics SI units (lmk when we need mole units) like for length, time, and current.
Within our units library we can write a length in any different unit and have it convert to a standard meter length. These conversions are done automatically because of receiver types and when they are needed to be used, can be converted back into their double form. All the conversions are unit tested so we don’t have bad conversions being done. Let’s make an example
-
val dist: Length = 5.inches + 3.centimeterswill be internally written as0.127 + 0.03since when you use the property.incheson a Double, it will return the value converted to typeLengthwhich is a value in meters. -
0.127 + 0.03=0.157which will then be stored/returned as anotherLengthtype. It is to be noted that a Length type is simply a type alias for theValue<Meters>type which stores the unit being used and can later be used to make sure the right type of unit is being used - If we need to pass this variable
distto a parameter that only takes typeDouble, we can use the property.inInchesto convert this value to inches and aDoubleor.inMetersto convert to meters. This applies to all unit types and we cover almost all useful units (most SI has Yotto to Yocto as well as feet, inches for dist, and other conventional units for other measurements)
Conversions take advantage of inline functions to reduce the overhead of all the conversions being done. We have noted a significant speed increase and decrease in memory usage (no heap allocation for running a function since the code is basically copy-pasted).
We also have derived units like Angle (Radians, Degrees) and characterization/control units for your Feedforward constants and PID constants. This is a key feature that is also something I really like. Many times we forget that PID and FF constants have units relative to the mechanism being used. KP for a linear mechanism can have units as Meters/Volt while an angular mechanism can have units of Degrees/Volt. This brings us to our next point.
We can wrap our WPILib classes to make sure that the values being passed in are of certain units. If we are using a traditional non-units system and are generating a Pose2D class, our code may look like this.
val x: Double = 12.2
val y: Double = 13.1
val theta: Double = 2.4
val pose: Pose2d = Pose2d(x, y, theta)
But this can easily be messed up if our pose is generated by the following line Pose2d(theta, x, y). Since the class itself has no idea about the values, we can’t stop issues like this. But if we write a wrapper for our Pose2d to only accept certain parameters of units Length (Value<Meter>) and Angle (Value<Radian>), then we can write our code like this.
val x: Length = 12.2.meters
val y: Length = 13.1.meters
val theta: Angle = 2.4.degrees
val pose: Pose2d = Pose2d(x, y, theta)
If we pass in the arguments any other way, the code won’t be happy since we are passing type Angle for type Length. Now let’s expand these working examples to something like PID units.
With PID we can check to make sure the right constant is being passed in something like a configPID method
configPID(
kP: ProportionalGain<Meter, Volt>,
kI: IntegralGain<Meter, Volt>,
kD: DerivativeGain<Meter, Volt>
) {...}
val kP = 0.69.volts / 1.inches
val kI = 0.4.volts / (1.inches * 1.seconds)
val kD = 0.20.volts / (1.inches.perSecond)
configPID(kP, kI, kD)
This is a good example of the robustness we have in our units. We can easily use inches within our constants and then have it convert our constant to meters which goes well into our method. Other examples of basic unit usage are shown below. Note that traditional math can be done on values with units.
leaderSparkMax.motorTemperature.celsius // converts to celsius units
leaderSparkMax.outputCurrent.amps // converts to amps
leaderSparkMax.busVoltage.volts * leaderSparkMax.appliedOutput // can easily do the math on values with units like volts (Value<Volt> * Double)
When using ReCalc, it will give you your FF values with units which you can then just write into the units library and use within your FeedForward Controllers. You don’t have to manually convert to native sensor units per native timescale.
Although utilities like this are already powerful, the utils library goes past just normal SI conversions into generating Mechanism Sensors. Mechanism sensors utilize our units library in order to generate real-life measurements based on subsystem configurations and the motor encoder sensors.
A Linear Mechanism Sensor for an elevator can take in the ratio of the input to output for our motors to determine how far it has extended given the counts on an encoder. An Angular Mechanism Sensor works in a similar manner. We can wrap this up again by creating wrappers of the mechanism sensors for both CTRE and REV. This system of wrappers handles the different timescales used by CTRE and REV.
So let’s look at some code using a SparkMAX Linear Mechanism Sensor.
private val leaderSparkMax =
CANSparkMax(
Constants.Elevator.LEADER_MOTOR_ID,
CANSparkMaxLowLevel.MotorType.kBrushless
)
private val leaderSensor =
sparkMaxLinearMechanismSensor(
leaderSparkMax,
ElevatorConstants.GEAR_RATIO.asDrivenOverDriving,
ElevatorConstants.SPOOL_RADIUS * 2,
ElevatorConstants.VOLTAGE_COMPENSATION
)
leaderSensor.position // returns system in meters
leaderSensor.velocity // returns system in meters/sec
leaderSensor.positionToRawUnits(
clamp(
position,
ElevatorConstants.ELEVATOR_SOFT_LIMIT_RETRACTION,
ElevatorConstants.ELEVATOR_SOFT_LIMIT_EXTENSION
)
) // convert units value back to the raw position with encoder for lib use
One of the most powerful parts of this is the utilization of the units to easily do conversion between raw and unit values. We can get a position from our system given the raw values from our encoders. We can also generate raw values for our system given our unit values since we just reverse the equations used for conversion.
Here is a list of some of the stuff within FalconUtils
- Units for SI
- Derived Units
- Math for units
- PID, FF controllers with units
- Drivers
- For hardware control like Blinkens with all LED values enumed
- Getting values from controllers
- All geo classes (Pose2d, Twist2d, …) with units and conversion to/from WPILib geo
- AdvantageKit safety stuff
- Swerve data classes with units (speeds, vels)
- Math Utilities
- Trajectory generation with units
If people are interested in checking this out, this is available in Team4099/FalconUtils and is also available as a package. We use FalconUtils internally but has not been fully tested for release. If people want one, we can write a more detailed write-up on the use of units and the utilities we have built and work on getting a final release ready. If you want to see the units library in use, read our constants folder in our codebase or just read any of our code and you will find some FalconUtils thing. It should be noted that you can only use this in Kotlin but in my opinion, it’s worth the switch. Also, gotta add JitPack to your maven in order to use it.
dependencies{
// other dependencies
implementation 'com.github.team4099:FalconUtils:<release_tag>' // version you want
}
Hope you had fun reading
~ Pranav
17 Likes