Glad you brought that robot up, Nathan
I was the coder for Scorpion. I also helped out with a 2-jointed arm in 2019, which performed similarly complex motions.
If your team is looking to get into multi-jointed arms, the first thing to tackle is the mechanical design. There are a couple of tricks that will make controlling it a whole lot easier. The first is using what I can best describe as an actively controlled virtual-four bar. What this does is make it so that moving joint x does not affect the angle of all joints y>x. For example with a 2-jointed arm, if the wrist is parallel the to the ground and the motor controlling the wrist is held still, it will stay parallel to the ground even as the shoulder moves up and down. This decoupling makes control a lot easier.
The second mechanical trick is counter-balancing each joint in a way that makes it as close to being effectively weightless as possible. Because the joint is moving in a circle, the effect gravity has on it is proportional to the cosine of the angle, so the counter-balance should behave similarly. The virtual four bar makes this much more practical for multi-jointed arms.
Now on to the software. 971 does some insane work to achieve beautifully smooth motion on their mechanisms. With the resources currently available in FRC, there are only a handful of teams which could successfully pull off many of the things they do. But if your arm is designed to have the two things I mentioned above, then doing waht 971 does isn’t really necessary. PID or motion magic are both good enough for the controls side of things, especially with the 1kHz update rates on modern FRC controllers. Scorpion arm’s was running 3 PID loops at a 50Hz update rate without any feedforward.
The other aspect of this performing complex motions without breaking the arm or any extension/height rules. Running through a sequence of setpoints is the simple way to go. It can be a bit of an imprecise science to get the right setpoints, and it won’t look at pretty as fancy path planners, but it will get the job done. For determining when to consider one setpoint completed and move onto the next, look into use the root mean squared error of the joint angles. The larger the error threshold, the more fluid and less precise the motions will be.
Our 2019 code can be found here. It uses motion magic, but the setpoint sequencing is as I described. I’ll also link to Scorpion’s code, although it is a lot messier.
One last note: I love multi-jointed arms. They are a ton of fun to work with. But I’ve been starting to see more and more that they are rarely a good idea from a competitive standpoint. The complexities of which end effector positions are achievable, which aren’t, and which can only be reached through very precise motion cause a lot of headaches. This is a problem for software, but more than that it is a problem for designing and iterating the manipulator(s) at the end of the arm. Linear actuators (or simple arm designs) remove a lot of these constraints, enabling a better overall robot design.