Ensuring a level climb


I’m looking for a bit of advice to ensure our climbing mechanism lifts the robot level. Right now we have 3 motors pushing down “stilts” using a custom rack and pinion gear (one motor for front left, one for front right, and one for the two rear ones which are connected together). Right now we are using Talon SRXs with potentiometers for feedback, and our PID loops drive them to a consistent and correct height, which is level. The problem is with keeping the robot level as it climbs, as the weight distribution isn’t exactly the same. Currently, we are using output limiting on the rear Talon to try and match the speed of the front motors, but this “precision guesswork” just does not cut it when batteries are depleted to different levels.

We would like to add in even more feedback to help the robot stay level. Would the built in RIO gyro/accelerometer be sufficient for this task? Do we need to purchase an IMU?

I have no idea where to start to combine PID loops like this. Right now, we are just feeding the position to the set output block. How would we add in a function to keep it level by changing the output power of the rear motor?

Any help/thoughts would be greatly appreciated!


Probably not, most people have issue using the included gyro. I would recommend the Navx. It plugs right in to the RIO.


Since you’re using Talon SRX’s, could you implement motion magic to control the acceleration and velocity of how fast each rack extends?


Our team is using a stilts and a dual elevator system this year. In order to have the elevator and the stilts go up level, we are using encoders rather than potentiometers for feedback. What the students have done is set one of the legs/elevators as the leader. Whatever height we want for the leg/stilt is set for the setpoint on the leader. What we are using as the the setpoint position for the follower leg/stilt is the outputted position of the encoder of the leader. This has worked very well for keeping our climber legs and dual elevators level. We also found that setting the P value on the follower slightly higher (.1 I think) helped keep them even closer to level.’

Humble brag-- Our team won an award for Innovation in Controls based on this arrangement last weekend.

I can try and send a code snippet later today if you would like.


I haven’t ever looked at motion magic. I can start reading up on that and see if that helps.

This actually sounds promising - I never thought to just set them as follower mode. All of the stilts are the same length, so this may be the way to go.


We are not actually setting the motor controllers on follower mode. We programmed as if it was as following based on the encoder value. We were concerned that would be enough differences in current draw that the built in follower mode wouldn’t work.

We ran a branch off the wire from the encoder value be used as the process variable on our PID loop to the “leader leg” and used that branch to be the setpoint on our PID loop for the “follower leg”


With potentiometers on each stilt, you probably have enough data to effectively control the climb, but the actual software component of keeping separate mechanical systems synchronized tends to get a little complicated.

I would try using some cascading PID loops. Let the back stilt be your “master”, so that stilt is controlled by a single PID loop, feeding the final position as the setpoint.

Your other two stilts would be controlled by two PID loops with their outputs added together. The first PID loop is the same as the back stilt (probably with different gains, as they’re mechanically different). The second PID loop takes in the position of the “master” as the setpoint. This second loop is essentially a correction factor; if a stilt is slightly ahead or behind the position of the master stilt, this PID loop will slightly modify the power output of the first.

Note that this is isn’t what I would call the “correct” way to solve this problem, but should work better than power limiting. Just setting the setpoint of the PID loop to the final position is essentially telling a mechanism “I want you to go to this position immediately”, and the PID loop will do it’s best to accomplish that. It doesn’t give you fine control over HOW the mechanism gets there, only that it eventually does.

Another way would be to look into motion profiles, which is basically a line of position vs time. By feeding that into your PID loop instead of just the final setpoint, you might be able to get slightly more consistent results.

EDIT: I would definitely try what BCR-Jim is suggesting first, it’s a bit simpler than what I’m suggesting.


We use a PID loop to control the front height as the rear extends at a certain rate. The error term input to the PID being the pitch angle of an IMU. (CTRE Pigeon in our case) Works like a charm and took <5 minutes to tune, only needing the P coef.


Sounds like a great solution. We planned on trying to use the gyro if our leader/follower solution didn’t work, but we were satisfied without having to add in a IMU in the right orientation.


One of the greatest difficulties (for me at least, as I am not the best at math myself) is tuning the PID “correctly”. We get the setpoints close enough for what we need - we don’t necessarily tune to be dead on. Lots of the examples in the Talon manual seem to be for encoders and not potentiometers. To add a layer of complexity to the whole situation, one of the motors are flipped relative to the others, meaning while two of the motors drive the potentiometers “down”, one of them drives it “up”. We went with potentiometers to keep them held up at a known 0 position throughout the match, as they drift down when no power is applied, and we didn’t want to deal with encoders having to run to a “home” position to zero themselves out.

What is also frustrating is not having a programming team at the competition - all of them could not travel for one reason or another. We are trying our best to provide remote support, but it gets difficult when other students have not used LabVIEW before, let alone actually programmed in ANY language. Our first competition may be rough, but hopefully we can get something worked out in time for the second one. I appreciate all the answers and ideas on how to help! Thank you all!


I think your sensor setup for this is great; exactly what I would have done.

If it simplifies things, you shouldn’t have to worry about I or D with these loops. Stilts pushing down to lift your robot up is a fairly straightforward mechanical system, and doesn’t have any weird friction characteristics like a drivetrain turning on carpet. I would leave I and D at 0 and only worry about tuning P.

For the motor that is flipped, I would handle that inversion at the very end. Right before you set the output power of that motor, multiply it by -1. The rest of the control logic should then be the same for all the motors.


That is exactly what we have now - we are within an error margin of only 10, so we are happy with the results. It just isn’t moving at the same speed, which is to be expected, honestly. Unfortunately, the solutions posted either require hardware we do not currently have (a navX) or a somewhat experienced programmer on site, so we may have to rely on the output limiting for this competition and work on a better solution for the next one.


Can you post a copy of your code? Might be able to help you work on things remotely.


Like I said before, I would strongly encourage you to implement motion magic - or at least try it. Especially since it sounds like you’re already using a position based PID loop. It’s easy to implement (basically the same effort as position PID except for added acceleration and velocity terms), and will solve your problem due to the velocity being controlled throughout the motion. Using it will also give you some experience so that you can use it on other mechanisms as well. It just makes everything smoother and more controllable.


If you wanted to fix it mechanically or else come close you could use surgical tubing on the stilts to bridge the force gap.


We at 2702 are using a PID with an IMU to detect the tilt angle of the robot. This feeds back to our climbing racks at the back of the robot, allowing it to keep up with the cargo intake that pushes up the front.

You might be able to apply the same concept to stilts if you have the resources.


So I am attempting to do the math for the feed forward gain that is required for the Motion Magic, and I am having a hard time wrapping my head around calculating the native units.

From the manual:

Velocity is measured in change in native units per TvelMeas= 100ms.

(600 Rotations / min) X (1 min / 60 sec) X (1 sec / 10 TvelMeas) X (4096 native units / rotation)
= 4123 native units per 100ms

I have tried doing the calculation myself with the example posted, but I am doing something horribly wrong here apparently. If I simplify to 600 x .016 x 1 x 4096, I get 393,216. Where am I going wrong? I do not fully understand the TvelMeas - I see that it is .1 second (10 times .1 is 1, divided by 1 still equals 1).

I have calculated the RPM of our stilts to be 146 (BAG motor through a 90:1 gearbox), and we have 3 turn potentiometers on them, so that would be 1024/3, or 341 units. I just cannot figure out how to calculate the native units per 100ms required for the feed forward gain.


Before you start calculating FF you need to know some things about your system. What speed do you want your system running at and how many units per rotation is your potentiometer? You technically don’t need FF for motion magic, but it helps with quite a bit of the heavy lifting. We actually didn’t use any FF on our elevator and wrist - just a PD controller with motion magic.


Right now we are using around 50% power for our lift, which would be 73RPM. With 3 turn potentiometers, we should have 341 units per revolution.


Using this, you’re velocity for half speed would be 73*(1/600)*341=41.5 sensor units/100ms. This would be your calculated velocity for 50% speed. (1/600 is just the conversion from 1 minute to 100ms). Initially, set your acceleration to 4 so that it takes around a second to reach max speed. You can tune the acceleration higher when you’re comfortable. Now for FF. According to CTRE’s docs, we can calculate our FF value by taking the max input of our motor controller (1023) and dividing it by our disired velocity (41.5). 1023/41.5=24.65.

Like I noted before, you don’t actually need the FF term for motion magic to work, so if you are having problems, just go with a basic pid loop while still running motion magic. All motion magic is doing is making a motion profile on the fly (position, velocity, and acceleration points for each period of time). A PID loop is usually good enough. FF just makes things smoother and puts less strain on the PID loop. Good luck!