So I wrote some trajectory following code (https://github.com/team1771/2022-code) for our swerve drive system and am a little confused on how to tune the HolonomicDriveController. I tried printing the desired state, current odometry state, and error and tuning by guess-and-check, but even after a few hours I only managed to get the x & y to within a foot of error after following a simple square path (through pathplanner). This is mainly due to slow spin down of the wheels (robot drifts too much; overshoots). Tuning the rotational PID controller was also pretty simple, but when I started combining it with the x & y wacky stuff happened. I was wondering if there is a better way to tune. I have seen the SysID can tune swerve (drivers only) by blocking the turning of the modules, but this doesn’t help as much for rotational. I was also wondering if a way to graph info from NetworkTables to making tuning easier. Thanks for the help!
I believe the sys ID is still helpful in this case. The kPs, ka, kv, ks values can help inform a starting point for your rotational pid. It’s just a matter of units.
So, I would use the same k values for the rotational as the x & y and just fine-tune from there?
Not the same they need to be scaled appropriately. If you have the max speed of your robot you should have something similar to the max radiance per second for the rotation of your robot. That ratio from max speed to max radians per second should proportionally effector KP I think. Many smarter people than me should be able to help with this if you can’t figure it out on your own
I think I understand. I will try to test this and let you know. Thanks!
SysID will give you feedforward values and estimate feedback gains for your swerve modules (voltage <-> wheel speed/acceleration) but your HolonomicDriveController operates on ChassisSpeeds. You want your swerve modules to accurately reach their target state(each determined by target ChassisSpeeds)-- doing this with the trajectory’s supplied ChassisSpeeds perfectly would mean you perfectly track every sampled pose along the trajectory.
We can’t get perfect feedforward though, so we use feedback to account for disturbances (our imperfect ChassisSpeeds control error is treated as a disturbance). PIDControllers on pose modify our target ChassisSpeeds to bring us closer to the target pose.
You can output each setpoint and reference position from the pose PID controllers to see where you are getting error-- more importantly, you also want to output your target/reference ChassisSpeeds values to see if you are commanding speeds correctly(if this is wrong, your positional control will be wrong).
So I did end up testing & tuning swerve modules and the holonomic drive controller and got to something I was fairly happy with. The holonomic rotation is still jerky though, as it will sometimes overcorrect but sometimes not correct at all. I noticed the graph was off by about a half radian/sec (real rot speed compared to expected from holonomic drive rotation). My mentor recommended sysID, so we locked the modules and ran that (results are attached, but I am not entirely sure if they are accurate, since the modules were not perfectly locked)
. However, I am not sure how to use these values. Would they be for my talonfxs in my swervemodule code, or could I use it for my holonomicdrivecontroller? (code for reference is at GitHub - TEAM1771/Black-Widdow-2022-Update: This is a codebase containing some updates (mainly trajectory following) for our 2020/21 Infinite Recharge "Black Widdow" robot.). If for the talonfxs, how would I convert the feedforward Ks, Kv, and Kv, into a Kf value? If for the holonomicdrivecontroller, would I follow the previous suggestion of scaling these values for rotational speed?
After reading some other threads like this, I’m thinking maybe I should just not the rot_speed from holonomicdrivecontroller. I do have a similar thing implemented in my code that already works:
void Drivetrain::faceDirection(units::meters_per_second_t const &dx,
units::meters_per_second_t const &dy,
units::degree_t const &theta,
bool const &field_relative,
double const &rot_p = ROTATE_P,
units::radians_per_second_t const &max_rot_speed = TELEOP_MAX_ANGULAR_SPEED)
{
int error_theta = (theta - getAngle()).to<int>() % 360; // Get difference between old and new angle;
// gets the equivalent value between -360 and 360
if (error_theta < -180)
error_theta += 360; // Ensure angle is between -180 and 360
if (error_theta > 180)
error_theta -= 360; // Optimizes angle if over 180
if (std::abs(error_theta) < 10)
error_theta = 0; // Dead-zone to prevent oscillation
double p_rotation = error_theta * rot_p; // Modifies error_theta in order to get a faster turning speed
if (std::abs(p_rotation) > max_rot_speed.value())
p_rotation = max_rot_speed.value() * ((p_rotation > 0) ? 1 : -1); // Constrains turn speed
// p_rotation is negated since the robot actually turns ccw, not cw
drive(dx, dy, units::degrees_per_second_t{-p_rotation}, field_relative);
}
void Drivetrain::faceClosest(units::meters_per_second_t const &dx,
units::meters_per_second_t const &dy,
bool const &field_relative,
double const &rot_p = ROTATE_P,
units::radians_per_second_t const &max_rot_speed = TELEOP_MAX_ANGULAR_SPEED)
{
int current_rotation = getAngle().to<int>() % 360; // Ensure angle is between -360 and 360
if (current_rotation < 0)
current_rotation += 360; // Ensure angle is between 0 and 360
if (current_rotation <= 90 || current_rotation >= 270)
faceDirection(dx, dy, 0_deg, field_relative, rot_p, max_rot_speed);
else
faceDirection(dx, dy, 180_deg, field_relative, rot_p, max_rot_speed);
}Worked splendidly! https://cdn.discordapp.com/attachments/447569459901759488/942963575226527754/trajectory.MOV
This topic was automatically closed 365 days after the last reply. New replies are no longer allowed.