Step 2: Entering the Calculated Constants
Note
In C++, it is important that the feedforward constants be entered as the correct unit type. For more information on C++ units, see The C++ Units Library.
Now that we have our system constants, it is time to place them in our code. The recommended place for this is the Constants
file of the standard command-based project structure.
The relevant parts of the constants file from the RamseteCommand Example Project (Java, C++) can be seen below.
Feedforward/Feedback Gains
Firstly, we must enter the feedforward and feedback gains which we obtained from the identification tool.
Note
Feedforward and feedback gains do not, in general, transfer across robots. Do not use the gains from this tutorial for your own robot.
44 // These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT!
45 // These characterization values MUST be determined either experimentally or theoretically
46 // for *your* robot's drive.
47 // The Robot Characterization Toolsuite provides a convenient tool for obtaining these
48 // values for your robot.
49 public static final double ksVolts = 0.22;
50 public static final double kvVoltSecondsPerMeter = 1.98;
51 public static final double kaVoltSecondsSquaredPerMeter = 0.2;
52
53 // Example value only - as above, this must be tuned for your drive!
54 public static final double kPDriveVel = 8.5;
46// These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT!
47// These characterization values MUST be determined either experimentally or
48// theoretically for *your* robot's drive. The Robot Characterization
49// Toolsuite provides a convenient tool for obtaining these values for your
50// robot.
51constexpr auto ks = 0.22_V;
52constexpr auto kv = 1.98 * 1_V * 1_s / 1_m;
53constexpr auto ka = 0.2 * 1_V * 1_s * 1_s / 1_m;
54
55// Example value only - as above, this must be tuned for your drive!
56constexpr double kPDriveVel = 8.5;
DifferentialDriveKinematics
Additionally, we must create an instance of the DifferentialDriveKinematics
class, which allows us to use the trackwidth (i.e. horizontal distance between the wheels) of the robot to convert from chassis speeds to wheel speeds. As elsewhere, we keep our units in meters.
32 public static final double kTrackwidthMeters = 0.69;
33 public static final DifferentialDriveKinematics kDriveKinematics =
34 new DifferentialDriveKinematics(kTrackwidthMeters);
35constexpr auto kTrackwidth = 0.69_m;
36extern const frc::DifferentialDriveKinematics kDriveKinematics;
Max Trajectory Velocity/Acceleration
We must also decide on a nominal max acceleration and max velocity for the robot during path-following. The maximum velocity value should be set somewhat below the nominal free-speed of the robot. Due to the later use of the DifferentialDriveVoltageConstraint
, the maximum acceleration value is not extremely crucial.
62 public static final double kMaxSpeedMetersPerSecond = 3;
63 public static final double kMaxAccelerationMetersPerSecondSquared = 3;
60constexpr auto kMaxSpeed = 3_mps;
61constexpr auto kMaxAcceleration = 3_mps_sq;
Ramsete Parameters
Finally, we must include a pair of parameters for the RAMSETE controller. The values shown below should work well for most robots, provided distances have been correctly measured in meters - for more information on tuning these values (if it is required), see Constructing the Ramsete Controller Object.
65 // Reasonable baseline values for a RAMSETE follower in units of meters and seconds
66 public static final double kRamseteB = 2;
67 public static final double kRamseteZeta = 0.7;
63// Reasonable baseline values for a RAMSETE follower in units of meters and
64// seconds
65constexpr double kRamseteB = 2;
66constexpr double kRamseteZeta = 0.7;