https://youtu.be/yqQE-iRvkdo This may be 6377’s best robot yet! Howdy Bots features an excellent robot that is able to cycle quickly and has smooth movements in their mechanisms. Hear about their match strategy and programming of this Charged Up Robot on Behind the Bumpers.

I want to elaborate a bit on what I said here, because its hard to communicate well when you’re on video and trying to be brief.

The rudimentary way to control this arm is treat both linear mechanisms as independent. This is problematic for a couple of reasons.

- our extension will happen faster than our rotation, this means that their is a period of time where our elevator is both extended, and at an elevation where another robot could potentially hit it. This would turn the arm into a big lever that would introduce all kinds of nasty sheer forces that could damage the robot.
- our extension will happen faster than our rotation, this means that we will be extended while at an elevation low enough that we could collide with the hybrid/mid node. This is a less scary hit, but still something you want to avoid
- our extension will happen faster than our rotation meaning that, because our moment of inertia is proportional to our radius, our shoulder is fighting a lot harder than it should, and when it gets to its setpoint, its going to have a whole lot of inertia (the robot would literally tip backwards off its wheels a few degrees)

The solution:

First, lets make our extension a function of our rotation, so that both our actuators reach their setpoints at the same time. Because we are using WPILib’s ProfiledPIDController which computes “setpoints” which are points inbetween your starting position, and your “goal”, we can set our elevator to go to a point which is a function of our shoulder’s current setpoint. This is pretty simple to implement, simply compute how far through its motion the shoulder is, and than multiply the final elevator target by this ratio.

```
double r = ((thetaCurrent - thetaInitial)/(thetaFinal - thetaInitial))*rFinal;
```

this already works significantly better, but we’re not done yet. To optimize for minimum inertia, we want to spend most of our time with the minimum extension we can get away with. To do this we can switch out our linear relationship for one that does most of our extension towards the end of the motion like a power function. This works until we have a case were our initial radius is greater than our final radius. In this case we actually want to do our extension **before** our rotation so that our shoulder does the minimum amount of work. Luckily this a pretty easy fix because inverse power functions do what we want.

```
pow = (rInitial > rFinal) ? 1 / pow : pow;
```

There are obviously advantages to having a super complicated state-space model of your arm hooked up to a spline generator, but I really like this implementation because it gives us very competitive performance from math that our freshman can understand.

here’s a link to this function is desmos if anyone is curious.