Simple Turret Control - uw-advanced-robotics/taproot GitHub Wiki
Good turret control is imperative to one's success in RoboMaster. The software is quite simple compared to implementing an auto-aim system that detects and predicts target pose, but is one of the many “simple” things that needs to be implemented well in order to have a good auto-aim system.
I'll go over the turret control system from receiving a target pose in 3D to commanding motor output. I'll also briefly discuss the user turret control pipeline. There's a bit of taproot-specific stuff in here, but much of it is pretty general to any platform you are running your embedded system on.
World Frame Controller
World relative turret controllers have many benefits. In this configuration, you control the turret to some world relative setpoint. This is in contrast to chassis relative turret control, which controls the turret to some chassis relative setpoint. It is intuitive to control the turret in this way--the user or an auto aim algorithm typically wants to point the turret at some location in space independent of which way the chassis is pointed.
For ARUW, the world relative frame is measured by an IMU mounted on the turret, which provides the turret's 3D orientation (pitch/roll/yaw). Previously, before ARUW had a turret-mounted IMU, we had one mounted on the chassis. This theoretically allows for world-frame turret control by combining chassis IMU and turret encoder values; however, in practice the resultant controller cannot be tuned as well and has high frequency oscillations compared to a controller that uses purely IMU data as controller measurements. We used a chassis mounted IMU for a short time before switching to a turret IMU. We (and all established RoboMaster teams that I know of) have found it is imperative to have turret mounted IMUs for turret control.
The world frame controller's measured pitch and yaw values are thus provided directly from the turret IMU. A cascade (position fed into velocity) PID controller is used to control the turret's pitch and yaw axis.
Pitch/yaw controller:
// worldFrameAngleSetpoint is provided by the user or auto aim system.
// worldFrameAngleMeasurement is the turret IMU's yaw value.
// getValidMinError is equivalent to (setpoint - measured).
const float positionControllerError = turretMotor.getValidMinError(worldFrameAngleSetpoint, worldFrameAngleMeasurement);
// worldFrameVelocityMeasured is the turret IMU's measured velocity, directly from the gyroscope.
const float positionPidOutput = positionPid.runController(positionControllerError, worldFrameVelocityMeasured, dt);
const float velocityControllerError = positionPidOutput - worldFrameVelocityMeasured;
const float velocityPidOutput = velocityPid.runControllerDerivateError(velocityControllerError, dt);
return velocityPidOutput; // Set motor output to this value.
Some additional limiting/wrapping is also required to ensure the setpoint is within valid turret bounds and that the turret does not attempt to travel through an invalid angle range to reach a valid turret setpoint. This is pretty simple to do and just requires some careful thought to implement properly. Here, unit tests are particularly useful to ensure the edge cases are handled properly.
When running the turret control, ensure the latency between receiving measured data and running the turret control is minimized. ARUW runs their turret controller at 500 Hz and the IMU measurements are guaranteed to be captured less than 1 millisecond before the turret controller runs.
Tuning Turret Cascade PID Controller
This section is specific to tuning two cascade PID controllers, where the PID controllers are of
type tap::algorithms::SmoothPid found in taproot, though there are concepts that apply to any PID
implementation. This PID implementation has a few (somewhat unnecessary) additions that I added for
various weird cases that should probably be ignored when tuning turret PID. In particular,
errDeadzone and errorDerivativeFloor can be safely ignored (leave at 0). Furthermore, the Kalman
filter terms should be left at their default values if you are using an approach that purely uses
IMU data. Leaving them at default means no Kalman filtering will be applied. If encoders are being
used as the turret's measured angles, these will likely need to be tuned to dampen high frequency
resonance caused by using the encoders.
Note that this is my strategy for tuning cascade PID--there are certainly better ways of doing this.
Cascade PID Tuning Steps
These steps are simply a suggestion for what to do if you don't have experience with PID tuning. There will be position and velocity PID configuration structs whose values will require tuning. If you have access to a J-Link, values in these structs can be tuned live (without having to compile and flash code each time you change a variable). I recommend investing in a J-Link if you don't have one. Otherwise, if you are using an ST-Link through Keil you should also be able to watch and edit variables live. Here is a guide for debugging with the J-Link that is useful (whether or not you use taproot): https://gitlab.com/aruw/controls/taproot/-/wikis/Debugging-With-JLink. Below, I'll assume you are using a J-Link and know how to watch and edit (by editing them in the “Watched Data” panel) variables live.
Here is the PID configuration struct. We will be tuning PID + the max I and max output terms.
struct SmoothPidConfig
{
float kp = 0.0f;
float ki = 0.0f;
float kd = 0.0f;
float maxICumulative = 0.0f;
float maxOutput = 0.0f;
float tQDerivativeKalman = 1.0f;
float tRDerivativeKalman = 0.0f;
float tQProportionalKalman = 1.0f;
float tRProportionalKalman = 0.0f;
float errDeadzone = 0.0f;
float errorDerivativeFloor = 0.0f;
};
Let's also assume you have access to watch the PID controller's curP term, that being the PID
controller's output after computing the P term. Also assume you have access to watch the PID
controller's and output term.
If you are starting from scratch, set all relevant values to their defaults. Watch the position PID
controller's output. Set this controller'smaxOutput to something reasonable. The output from the
position PID controller will feed into the velocity PID controller, so the max output should be a
value of rotational speed. Therefore, the max rotational speed of the turret is controlled by the
position PID controller's max output. This is going to be different for every turret. Set the
position PID controller's kp term. This term effectively controls the turret's acceleration. A
good starting value would be to dividemaxOutput by the maximum position error possible (if, for
example,maxOutput is 3600 deg/second, setmaxOutput to 3600/180 = 20). This is by no means a
scientific way of computing this value, but it helps give a ballpark estimate of what the value
should be if you have no idea. Then tune it up or down based on the desired acceleration rate (first
try tuning it up if possible). Initially, I would leave the position PID controller's ki term at
0, and only add a small amount of D later on during the fine tuning process. Move on to tuning the
velocity PID controller. Start by increasing kp. At this point, themaxOutput term should still be
0. Watch the velocity PID controller's curP term. Move the turret around by hand and ensure the
curP term is reasonable (the correct sign, not oscillating). Slowly increase the velocity PID
controller'smaxOutput term. Increase/decrease kp as required to get the turret to respond quickly
and not jitter. When watching curP, this value should not oscillate. You should attempt to
increase kp as much as possible without causing the turret to jitter. If there are a small amount
of oscillations caused by velocity overshoot, a small amount of D can be added by slowly increasing
kd. Note that you should be able to tune your cascade PID controller pretty well with just the above
steps assuming your turret doesn't have mechanical deficiencies, but for more fine control you can
tune the ki and kd terms. If you see that the turret error is consistently some nonzero value,
you can experiment with adding a bit of I to the position PID controller by increasing ki and
making maxICumulative nonzero. This is particularly useful for pitch control, where n unbalanced
turret might consistently have some steady state position error. While small amounts of steady state
error don't matter for user control (instead responsiveness and smoothness are important) and adding
an I gain is bad for user control, it is important for CV accuracy--ARUW has separate PID
configurations for turret and CV control. The CV control PID configuration is tuned to minimize
error at the cost of some user-perceived smoothness. Make sure to copy over configuration parameters
tuned in Ozone to your source code.
Turret Control Pipeline
The turret controller lies at the last stage of the turret control pipeline. On the other end of the pipeline, the system receives commands from the user or auto aim system.
User Control
Briefly, I will describe the user control pipeline, which is quite simple.
- Receive remote control input (iirc this is done at 70 Hz, the frequency that the dr16 receives data from the remote).
- Remote joystick and mouse movement are used as turret velocity setpoints. User pitch and yaw
setpoints are maintained and incremented based on remote input (e.g.
yawSetpoint += mouse.x * raw_mouse_to_velocity_unit_scalar * dt). Setpoints are also limited when required (done at main control loop frequency, 500 Hz). - Run the world frame cascade PID control (done at main control loop frequency).
Auto Aim
The auto aim control pipeline is slightly more complex since ballistics is performed in the controls codebase, but is still very simple.
- Receive auto aim data from the vision coprocessor (idk what frequency this is being received at, presumably the frame rate of the auto aim system…it is going to be slower than the main control loop frequency).
- When a target exists, compute a ballistics solution for the target.
- The target received from the vision coprocessor is a position, velocity, and acceleration in the world frame (measured by the turret IMU and chassis odometry).
- Using known odometry data (which the main control board computes), find the position, velocity, and acceleration of the target relative to the turret.
- Project this kinematic state into the present (since the given aim data is using some captured camera frame, which is in the past).
- Compute the ballistic trajectory required to hit the target (pitch and yaw angles in the world, turret IMU frame). (done at main control loop frequency, 500 Hz)
- Assuming ballistics solution is found, feed the solution into the turret controller as pitch/yaw setpoints (done at main control loop frequency).
Mechanical Limiters
Good turret control is limited by a number of mechanical limiters. You might be able to overcome many of these mechanical deficiencies; however, it makes controls go from applying relatively simple concepts to using advanced signal filtering and control theory.
- Any sort of mechanical slop between the turret and the motor controlling it is bad. This includes slop introduced by poorly tensioned belts.
- Significant friction in the turret's rotational axis makes it hard to properly tune PID controllers. This is especially true if the friction is not uniform across the entirety of the turret's rotation.
- Vibrations due to a poorly mounted IMU will reduce IMU stability and likely increase IMU yaw drift. If you are using a high quality IMU, this might not be an issue, but if you are using the mpu6500 or bmi088, it will be. In 2021 ARUW initially mounted an IMU on a thin piece of PLA that was connected to the VTM mount, which caused major IMU instability (especially when the flywheels were running). To remedy this, ARUW mounted the IMU on standoffs directly connected to the main turret plate; the IMU henceforth had far fewer issues with resonance.
- Ideally, the turret's pitch axis is well balanced to reduce the load on the motor. This isn't as strict a requirement as items higher up on this list; however, it reduces the chance that you burn out a motor. Furthermore, if you are close to the motor's stall torque while holding the pitch axis steady, you won't be able to quickly accelerate the pitch axis against gravity.
- ARUW found the bmi088 of the RoboMaster Type C Board to be a better quality IMU than the mpu6500, which is on the RoboMaster Type A Board. The Type C Board also has mounting designed to reduce IMU vibration, which likely also helps make the Type C Board's IMU better than the Type A.