Examples with DifferentialDriveVoltageConstraint edu.wpi.first.math.trajectory.constraint.DifferentialDriveVoltageConstraint used on opensource projects

Example 1 with DifferentialDriveVoltageConstraint

use of edu.wpi.first.math.trajectory.constraint.DifferentialDriveVoltageConstraint in project 2022 by Team5705.

the class RobotContainer method getAutonomousCommand.

// -----------------------------------------------------------------------------------------------------------------------------------------
public Command getAutonomousCommand() {
    // Create a voltage constraint to ensure we don't accelerate too fast
    var autoVoltageConstraint = new DifferentialDriveVoltageConstraint(new SimpleMotorFeedforward(pathWeaver.ksVolts, pathWeaver.kvVoltSecondsPerMeter, pathWeaver.kaVoltSecondsSquaredPerMeter), pathWeaver.kDriveKinematics, 10);
    // Create config for trajectory
    TrajectoryConfig config = new TrajectoryConfig(pathWeaver.kMaxSpeedMetersPerSecond, pathWeaver.kMaxAccelerationMetersPerSecondSquared).setKinematics(pathWeaver.kDriveKinematics).addConstraint(autoVoltageConstraint);
    // An example trajectory to follow.  All units in meters.
    Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory(// Start at the origin facing the +X direction
    new Pose2d(0, 0, new Rotation2d(0)), // Pass through these two interior waypoints, making an 's' curve path
    List.of(new Translation2d(1, 1), new Translation2d(2, -1)), // End 3 meters straight ahead of where we started, facing forward
    new Pose2d(3, 0, new Rotation2d(0)), // Pass config
    config);
    RamseteCommand ramseteCommand = new RamseteCommand(// Trayectoria cargada desde pathweaver
    trajectory, powertrain::getPose, new RamseteController(pathWeaver.kRamseteB, pathWeaver.kRamseteZeta), new SimpleMotorFeedforward(pathWeaver.ksVolts, pathWeaver.kvVoltSecondsPerMeter, pathWeaver.kaVoltSecondsSquaredPerMeter), pathWeaver.kDriveKinematics, powertrain::getWheelSpeeds, new PIDController(pathWeaver.kPDriveVel, 0, 0), new PIDController(pathWeaver.kPDriveVel, 0, 0), // RamseteCommand passes volts to the callback
    powertrain::setVolts, powertrain);
    // Reset odometry to the starting pose of the trajectory.
    powertrain.resetOdometry(exampleTrajectory.getInitialPose());
    // Run path following command, then stop at the end.
    return ramseteCommand.andThen(() -> powertrain.setVolts(0, 0));
}