Trying to figure out Closed-Loop Control in C++

Technical Programming
1

First, let me start by saying that I am not one of my teams programmers, I do have some knowledge of C++ programming generally, and some experience using Labview to program FRC robots years ago, but I’m not familiar with FRC-specific syntax or libraries for C++. The past few years I’ve been working more on the electronics, controls, and engineering end of things, so I’m a bit out of the loop in regards to programming.

I’ve been trying to work with my teams programmers to implement closed-loop control for a number of functions on our robot (primarily elevator height control and autonomous pathfinding), but they’ve been having trouble getting anything implemented. I’ve pointed them in the direction of several resources (here on CD, FIRST programming documentation, CTRE Control system guides, etc.), but they’re still having some problems with it. I’ve heard of a number of teams who (claim to) have been able to get these kinds of features implemented in an afternoon, so I’m trying to figure out what we’re missing.

Here are the main things we’re trying to do:

Elevator position control
Our robot has an elevator that is driven by a Versaplanetary, the drive shaft has a 10-turn Potentiometer connected to it, which is wired into the Talon SRX that drives the elevator. Currently, we have the elevator programmed to go to a set position on the potentiometer and stop, the problem is that gravity quickly pulls it back down, and earlier attempts to get it to “hold” the setpoint resulted in it overshooting and thrashing up and down violently (as one would expect).

I would like to be able to have the arm decelerate as it approaches the setpoint, then once there, hold position by gradually increasing power to the motor as necessary to compensate for any error without overshooting (basically a position-based PID), but I’m not sure what the actual code or logic required to do this looks like

Drive Motion Profiling
Our drive system uses a combination of TalonSRXs and VictorSPXs all controlled via CAN. Each side of the drive system has an encoder wired into one of the Talons for each side.
Currently, we use a combination of encoder distance and Gyro angle (from a navX mounted to the roboRIO) for autonomous pathing. The system works decently, but is prone to errors at higher speeds or longer distances.

I’m aware of the various motion profile options of the TalonSRX speed controllers but with how busy I’ve been with other work on the robot, I haven’t had time to go through the 150-page book that is the TalonSRX manual (beyond some brief skimming) to get a good understanding of how they work or how to implement them. Ideally, I’d like to be able to implement some sort of motion profiling so we can have better consistency and control over our auton, and maybe even be able run it in high gear so we can do multiple cubes in auto.

Any guidance would be appreciated. I hate to make such a big request, especially for something we’re trying to get working for our first event this weekend, but I feel like we’ve been overlooking a simple solution that’s staring us in the face and we’re just not seeing it. ::rtm::

Thanks.

2

Elevator position control
Our robot has an elevator that is driven by a Versaplanetary, the drive shaft has a 10-turn Potentiometer connected to it, which is wired into the Talon SRX that drives the elevator. Currently, we have the elevator programmed to go to a set position on the potentiometer and stop, the problem is that gravity quickly pulls it back down, and earlier attempts to get it to “hold” the setpoint resulted in it overshooting and thrashing up and down violently (as one would expect).

I would like to be able to have the arm decelerate as it approaches the setpoint, then once there, hold position by gradually increasing power to the motor as necessary to compensate for any error without overshooting (basically a position-based PID), but I’m not sure what the actual code or logic required to do this looks like

It sounds like you have the implementation of PID control down, you just need to tune it. I’ve found that using straight PID (all feedback) on a system like this causes overshoot and oscillation (like you see) or a very sluggish response. You should think about implementing feed-forward control in addition to feedback. The most basic implementation of this is to figure out what output you need to hold the elevator against gravity (done through math or by testing different values e.g. in teleop) and add this to your output (e.g. using the talon config_kF). Unfortunately for the Talon PID it’s a little more involved, requiring some unit conversion and dividing out the position setpoint.

I can add some more in a bit once I get out of lab.

3

I don’t think they were using a PID, per say, more of a brute-force approach. I suspect the logic looked something like this (pardon my puedocode):

if Potentiometer < TargetRange
then MotorPower = 0.5

elseif Potentiometer > TargetRange
then MotorPower = -0.5

elseif Potentiometer = TargetRange
then MotorPower = 0

4

I can’t do much as I’m on mobile right now, but this is pretty much what is known as a bang-bang controller. If you start at 0 for example and your target is 1, there will be the first “bang”, raising quickly and probably overshooting. Then the second “bang” where it lowers again, hopefully close to the setpoint. If you set your speed too high, or your target range too small (deadband) it’ll probably be a bang bang bang bang bang bang bang bang controller

A simple P controller would be something like this (pseudocode)

 
error = setpoint - potentiometer
speed = kP * error
motor.set(speed)

That’ll get the slowing down effect you want. However it may not hebthatvgreat at holding a position. For that you want full PID, probably running on the talon SRX

5