To tackle the FRC game Deep Space in 2019, we decided to emphasize flexibility. We wanted something that could traverse the field quickly to pick up and place game pieces, then run out again and return to deliver balls again without having to turn itself around. This robot design would need to have a lower center of gravity and many degrees of freedom, and the design we chose would be a low robot, with a jointed, extending arm capable of placing game pieces everywhere except for at the highest “rocket” level. This arm included 4 separate parts, all of which moved individually: Elbow, connecting the arm to the rest of the bot; Extension, which could slide out; and Wrist, which turned to put the Claw in position to pick up game pieces. This was a huge challenge control-wise: this arm would need to be able to move as freely as possible without crashing into the robot itself, extending itself dangerously, or otherwise taking “illegal motions”-that’s the problem I was tasked to help solve.
First step for developing an algorithm to move the arm: figuring out what the arm can and can’t do. I determined 30 possible configurations and actions-ArmStates-that the arm as a whole could be in: for example, grab a ball from the loading station/place a ball in the lowest-level target/etc. Each ArmState contained the aggregate of the angle of the elbow and wrist, intake state (because it could fold in and rotate out, affecting how the arm could move so as to steer clear of it), and extension length-this was put in code to track the arm’s positions. Then I tried something new: I made a map where each ArmState was a node, and drew edges connecting the possible ways to move between different ArmStates. These, I represented in code as ArmTransitions, and for each ArmState, I assigned to it an array of ArmTransitions that can be used to go to other ArmStates from it.
This may not seem impressive, but it’s what I used to approach one of our biggest challenges: systematically tuning the PID coefficients needed to control the motion of the arm. Without this, the robot wouldn’t have been able to use its massive arm. To prevent jerking, I implemented a trapezoidal motion profile. I also added an acceleration feedforward term, to correct error from accelerating/decelerating quickly. Then, I had to account for gravity by adjusting arbitrary feedforward, so the arm could hold its position. Each of the arm’s 4 components moved separately so for all this to function, they’d need careful individual tuning. Using clamps on a bar attached to the motors and controllers that would be on the robot to simulate the arm, I went through every variable that affected the “arm”’s motion to figure out a procedure for doing so. It was slow, but it taught me quite a bit about control theory, and I eventually managed to set up this process so it could be used for the robot.
To find the most efficient way for the arm and all its parts to move from any ArmState position to another, I could run a search on the ArmStates-ArmTransitions map. Originally, I tried A*, however, when I ran this code on the actual robot through every possible combination of ArmStates to test it, there were several cases where, as shown in the GIF, the arm would move to every single ArmState in succession, and then keep moving again until it reached the target again (for reference, I tried moving the arm from its neutral position to “place-low-goal-from-back” here, so it should have just extended and the elbow smoothly rotated back). In trying to debug my code, I realized I over-complicated the situation, and A* search maybe wasn’t the best tool for this task. We were running low on time, so I swapped A* for breadth first search-it did the job just fine, looking along the ArmTransitions extending out from the current ArmState and continuing from there to find optimal paths.
The Flex (Team 670 2019) @ Arizona 