Paso 2: Introducción de las constantes calculadas
Nota
En C++, es importante que las constantes feedforward se ingresen como el tipo de unidad correcto. Para obtener más información sobre las unidades C++, consulte Biblioteca de unidades de C++.
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.
Ganancias de Feedforward/Feedback
Firstly, we must enter the feedforward and feedback gains which we obtained from the identification tool.
Nota
Las ganancias de feedforward y feedback, en general, no se transfieren entre robots. No use las ganancias de este tutorial para su propio robot.
39 // These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT!
40 // These characterization values MUST be determined either experimentally or theoretically
41 // for *your* robot's drive.
42 // The Robot Characterization Toolsuite provides a convenient tool for obtaining these
43 // values for your robot.
44 public static final double ksVolts = 0.22;
45 public static final double kvVoltSecondsPerMeter = 1.98;
46 public static final double kaVoltSecondsSquaredPerMeter = 0.2;
47
48 // Example value only - as above, this must be tuned for your drive!
49 public static final double kPDriveVel = 8.5;
47// These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT!
48// These characterization values MUST be determined either experimentally or
49// theoretically for *your* robot's drive. The Robot Characterization
50// Toolsuite provides a convenient tool for obtaining these values for your
51// robot.
52inline constexpr auto ks = 0.22_V;
53inline constexpr auto kv = 1.98 * 1_V * 1_s / 1_m;
54inline constexpr auto ka = 0.2 * 1_V * 1_s * 1_s / 1_m;
55
56// Example value only - as above, this must be tuned for your drive!
57inline constexpr double kPDriveVel = 8.5;
DifferentialDriveKinematics
Además, debemos crear una instancia de la clase DifferentialDriveKinematics
, que nos permite usar el ancho de vía (es decir, la distancia horizontal entre las ruedas) del robot para convertir las velocidades del chasis a las velocidades de las ruedas. Como en otros lugares, mantenemos nuestras unidades en metros.
29 public static final double kTrackwidthMeters = 0.69;
30 public static final DifferentialDriveKinematics kDriveKinematics =
31 new DifferentialDriveKinematics(kTrackwidthMeters);
38inline constexpr auto kTrackwidth = 0.69_m;
39extern const frc::DifferentialDriveKinematics kDriveKinematics;
Velocidad máxima de la trayectoria/Aceleración
También debemos decidir sobre una aceleración máxima nominal y una velocidad máxima para el robot durante el seguimiento de la trayectoria. El valor de velocidad máxima debe establecerse algo por debajo de la velocidad libre nominal del robot. Debido al uso posterior del DifferentialDriveVoltageConstraint
, el valor máximo de aceleración no es extremadamente crucial.
Advertencia
Max velocity and acceleration, as defined here, are applied only during trajectory generation. They do not limit the RamseteCommand
itself, which may give values to the DriveSubsystem
that can cause the robot to greatly exceed these velocities and accelerations.
57 public static final double kMaxSpeedMetersPerSecond = 3;
58 public static final double kMaxAccelerationMetersPerSecondSquared = 1;
61inline constexpr auto kMaxSpeed = 3_mps;
62inline constexpr auto kMaxAcceleration = 1_mps_sq;
Parámetros de Ramsete
Finalmente, debemos incluir un par de parámetros para el controlador RAMSETE. Los valores que se muestran a continuación deberían funcionar bien para la mayoría de los robots, siempre que las distancias se hayan medido correctamente en metros; para obtener más información sobre cómo ajustar estos valores (si es necesario), consulte Construcción del objeto controlador Ramsete.
60 // Reasonable baseline values for a RAMSETE follower in units of meters and seconds
61 public static final double kRamseteB = 2;
62 public static final double kRamseteZeta = 0.7;
64// Reasonable baseline values for a RAMSETE follower in units of meters and
65// seconds
66inline constexpr auto kRamseteB = 2.0 * 1_rad * 1_rad / (1_m * 1_m);
67inline constexpr auto kRamseteZeta = 0.7 / 1_rad;