Programming the Joystick throttle to control our arm

This year our team decided we were going to go big or go home witht the robot, we plan to use absolute encoders and ultrasonic sensors to control a basic robot “arm” mechanism. Our problem is

  1. We dont know how to set up a PIDController to smooth movement (it jerks quite a but from the high torque)
  2. We want ti use the theottle on the provided joystick to give an output related to the output of our encoder (180° motion on a MA3 absolute encoder so -1=0, 0=256, and 1=512)
    We would then read that output from the throttle and have the motors match it, acting as heavy duty servos basically?

And 3. We have almost no idea how to do all this, we know its possible but can seem to find example anywhere,

we’re using Java in visual studio.

Make your arm balanced. It will help a lot with smooth movements. Also what motor controller are you using to drive the arm?


We will be using a Vector SPX on each motor lifting the arm, thanks for the above advice!

I discuss the control theory this sort of mechanism here.

For your specific setup:

  1. Use the absolute encoder to determine your “actual” angle.
  2. Use the joystick throttle to determine your “desired” angle.
  3. Balance the mechanism so gravity doesn’t matter, or use an arbitrary feed forward as described in the post above.

Depending on your robot, “getting to the angle” as fast as possible may not be desirable. For example, in 2019, we needed to minimize jerk on the arm, so as not to topple the robot when moving quickly. We ended up using trapezoidal motion profiling to minimize excess forces, though that’s far from the only solution. Here’s the code, though it won’t directly apply to your case, since we used a Spark Max + NEO.

1 Like

Once you have read the relevant background info already linked here, and have mechanically had your team ensure there is adequate counter balancing of the arm as to help you control upward and downward movement:

Use MotionMagic in your arm for smooth acceleration and deceleration.

Here is a link to our Arm subsystem from last year. We controlled it with MotionMagic and the Arbitrary FeedForward term, and it worked great.

1 Like

while all of this is nice and im sure i can find a good use for it, im not quite sure if its what my team needs, so let me put some light on the table here,

  1. our arm has two joints, i’ll link the cad model HERE:
    the arm runs off of 2 motors pulling the arm up like half of a scissor lift, the bottom arm moves 70 degrees and the top arm moves 175 degrees at the same time

  2. our team understands the basics of a PID, the problem is that the WPILIB PIDcontroller has been depreciated for removal and the new one doesnt indicate any way of using it to control a motors position

we just need to use the motors as servos and we were planning to have set heights for the arm to reach, we would use the absolute encoder for input.

if anyone has better solutions that doesnt include spending hundreds of dollars on some talon SRX’s, that’d be nice.

Here is CTRE’s documentation on MotionMagic and other closed-loop control modes.

From that link:

In your earlier post you said you were using VictorSPX controllers. So if you connect an encoder to it, you can use the built-in MotionMagic controls.

That will let you both set your motor to a given position, and provide you smooth acceleration and deceleration (trapezoidal profile) as you approach the set point…which is awesome for mechanisms like arms that are difficult to control precisely since they often move and oscillate with outside forces like inertia and gravity.

1 Like

so when i went go check this out, turns out their motion magic controller doesnt work with the victor SPX

“Current closed loop is not available on Victor SPX, it is only available on Talon SRX.”

Arms can be a real challenge. We did this last year, and it was a huge learning exercise, but we got it working pretty smoothly eventually.

The first thing you should consider is trying to “linearize” the system, and model it to counteract the forces of gravity. Most notably, when the arm is straight out, it will respond VASTLY differently than it will if it’s straight up. This makes it nearly impossible to set good gains, because the gains change significantly based on the position of the arm. PIDs are linear controllers, and the math just doesn’t handle non-linear changes well.

What we did for this was figuring out the max force applied when directed straight outward (highest static force), and save it as a value kArmFF. Then just add this to your PID’s output, like so.

armPower = /get PID output here/;
armPower += kArmFF*Math.cos(Math.toRadians(currentArmPos)

This is known as an “arbitrary feed forward”, and removes gravity from the equation. Note though, that your system may not agree perfectly with the math, so don’t worry if you have to fudge numbers a bit. Lowering the FF, or applying less FF when going down may be what your system needs to operate properly. We had to do this due to our system having a large FF, and the stickyness of our system meant gravity and small outputs for common PID changes often couldn’t force it back down otherwise.

If you want to see our code that has this, you can find it here:

Because of your system design, the arbitrary feed forward can be significantly more complex, and may need to account for the second arm tilted up (as well as roughly level), and possibly account for the the variable weight of the balls. It may be a bit tedious to calculate, but if you just add things piecemeal, figure out the min/max forces, and scale appropriately based on your angles, you should be able to get FeedForward motor outputs that hold the system stationary. This lets the PID’s error correction work properly, which will greatly the PID tuning process and minimize points where the PID behaves poorly.

Just having your PID and feed forward will probably be enough to let you hook it to the throttle. To do this, you just need to use some linear interpolation or math to convert -1-1 to 256-512. Pass this to a manual control setup, and you’re good.
I can warn you of one “gotchya” we’ve seen. An unplugged controller generates an input of 0 on that axis. This will send the arm directly to a middle position, which tends to be extremely unsafe. I would recommend the arm command ignore input until the joystick input roughly matches the bot’s current position, after which you can have the arm follow the throttle.

For smooth motion in auto commands, a proper motion profiled PID loop is best. However, if this is proving out of reach, this you can simply rate-limit your setpoint using the SlewRateLimiter class. This will let you force the full setpoint change to occur over a specified number of seconds. This isn’t a great fix in general, and should be considered a stopgap solution: This demands sub-optimal performance, and it’s a headache maintaining the various slew rates and time profiles across multiple commands or sequences. It’s a good quick fix for one or two autos as week 1 competitions approach though.

If you’re still struggling to get a PID base class, I can point you to this one our team put together and make a lot of use of. It’s not perfect, but designed for quick PID setup, and has couple math tweaks to make sure it stays fairly stable when used without in motion profiles (notably in setpoint constraints and I-term wind-up prevention, and output ramping). Currently there’s no internal support for arbitrary feed forward, but you can just add it to the output and get the same results in almost all cases.

You want MotionMagic (NOT current closed loop), with probably an added Arbitrary FeedForward for your Arm.

I don’t have any VictorSPX’s to test, but I already quoted their documentation above where it says VictorSPX supports MotionMagic.

Note that there’s a FRC characterization tool and a matching WPILib feedforward object that can do the “hard” parts of this for you. You should still understand how they work before using them, however.


This topic was automatically closed 365 days after the last reply. New replies are no longer allowed.