Examples with RamseteCommand edu.wpi.first.wpilibj2.command.RamseteCommand used on opensource projects

Example 1 with RamseteCommand

use of edu.wpi.first.wpilibj2.command.RamseteCommand in project 2022RobotCode by AusTINCANsProgrammingTeam.

the class AutonModes method getRamseteCommands.

private Command[] getRamseteCommands(Trajectory... trajectories) {
    Command[] ramseteCommands = new Command[trajectories.length];
    for (int i = 0; i < trajectories.length; i++) {
        RamseteCommand r = new RamseteCommand(trajectories[i], driveBaseSubsystem::getPose, new RamseteController(Constants.ramseteB, // ramsete follower to follow trajectory
        Constants.ramseteZeta), Constants.driveKinematics, driveBaseSubsystem::acceptWheelSpeeds, driveBaseSubsystem);
        // first ramsete command needs to have driveBase reset odometry to match that of pathweaver
        if (i == 0) {
            Pose2d p = trajectories[i].getInitialPose();
            ramseteCommands[i] = r.beforeStarting(() -> driveBaseSubsystem.resetOdometry(p));
        } else {
            ramseteCommands[i] = r;
        }
    }
    return ramseteCommands;
}

Example 2 with RamseteCommand

use of edu.wpi.first.wpilibj2.command.RamseteCommand in project 2022-Rapid-React by frc3566.

the class getAutoTrajectory method getTrajectory.

public static RamseteCommand getTrajectory(DriveSubsystem driveSubsystem) {
    try {
        Path trajectoryPath = Filesystem.getDeployDirectory().toPath().resolve(trajectoryJSON);
        trajectory = TrajectoryUtil.fromPathweaverJson(trajectoryPath);
    } catch (IOException ex) {
        DriverStation.reportError("Unable to open trajectory: " + trajectoryJSON, ex.getStackTrace());
    }
    return new RamseteCommand(trajectory, driveSubsystem::getPose, new RamseteController(2.0, 0.7), new SimpleMotorFeedforward(Constants.Drive_ks, Constants.Drive_kv, Constants.Drive_ka), Constants.kDriveKinematics, driveSubsystem::getWheelSpeeds, new PIDController(Constants.DRIVETRAIN_VELOCITY_GAINS.kP, 0, 0), new PIDController(Constants.DRIVETRAIN_VELOCITY_GAINS.kP, 0, 0), // RamseteCommand passes volts to the callback
    driveSubsystem::setVoltage, driveSubsystem);
}

Example 3 with RamseteCommand

use of edu.wpi.first.wpilibj2.command.RamseteCommand in project SnobotSimV2 by snobotsim.

the class DriveTrajectoryCommand method createWithVoltage.

public static Command createWithVoltage(DrivetrainSubsystem drivetrain, Trajectory trajectory, boolean resetOnStart) {
    DrivetrainSubsystem.DrivetrainConstants drivetrainConstants = drivetrain.getConstants();
    RamseteCommand ramseteCommand = new RamseteCommand(trajectory, drivetrain::getPose, new RamseteController(TrajectoryFactory.AutoConstants.kRamseteB, TrajectoryFactory.AutoConstants.kRamseteZeta), new SimpleMotorFeedforward(drivetrainConstants.getKsVolts(), drivetrainConstants.getKvVoltSecondsPerMeter(), drivetrainConstants.getKaVoltSecondsSquaredPerMeter()), drivetrainConstants.getKinematics(), drivetrain::getWheelSpeeds, new PIDController(TrajectoryFactory.DriveConstants.kPDriveVel, 0, 0), new PIDController(TrajectoryFactory.DriveConstants.kPDriveVel, 0, 0), // RamseteCommand passes volts to the callback
    drivetrain::tankDriveVolts, drivetrain);
    Command runThenStop = ramseteCommand.andThen(() -> drivetrain.stop());
    if (resetOnStart) {
        Command resetPose = new InstantCommand(() -> drivetrain.resetOdometry(trajectory.getInitialPose()));
        return resetPose.andThen(runThenStop);
    }
    return runThenStop;
}

Example 4 with RamseteCommand

use of edu.wpi.first.wpilibj2.command.RamseteCommand in project SnobotSimV2 by snobotsim.

the class DriveTrajectoryCommand method createWithVelocity.

public static Command createWithVelocity(DrivetrainSubsystem drivetrain, Trajectory trajectory, boolean resetOnStart) {
    DrivetrainSubsystem.DrivetrainConstants drivetrainConstants = drivetrain.getConstants();
    RamseteCommand ramseteCommand = new RamseteCommand(trajectory, drivetrain::getPose, new RamseteController(TrajectoryFactory.AutoConstants.kRamseteB, TrajectoryFactory.AutoConstants.kRamseteZeta), drivetrainConstants.getKinematics(), drivetrain::smartVelocityControlMetersPerSec, drivetrain);
    Command runThenStop = ramseteCommand.andThen(() -> drivetrain.stop());
    if (resetOnStart) {
        Command resetPose = new InstantCommand(() -> drivetrain.resetOdometry(trajectory.getInitialPose()));
        return resetPose.andThen(runThenStop);
    }
    return runThenStop;
}

Example 5 with RamseteCommand

use of edu.wpi.first.wpilibj2.command.RamseteCommand in project 2022 by Team5705.

the class RobotContainer method configureButtonBindings.

/**
 * @param trajectoryPaths Número del path en la lista declarada
 * @return  Comando ramsete para su utilización en el seguimiento del path
 */
/*public Command ramseteC(String trajectoryPaths) {
    String path = trajectoryPaths;
    Trajectory trajectory;
    try {
      Path trajectoryPath = Filesystem.getDeployDirectory().toPath().resolve(path);
      trajectory = TrajectoryUtil.fromPathweaverJson(trajectoryPath);
      
      System.out.println("Path successfully read");
      new PrintCommand("Path successfully read");

    } catch (IOException ex) {
        DriverStation.reportError("Unable to open trajectory: " + trajectoryPaths, ex.getStackTrace());
        System.out.println("Unable to open trajectory: " + path);
        new PrintCommand("Unable to open trajectory: " + path);
        
      } 
    
      return new RamseteCommand(trajectory, powertrain::getPosition,
                new RamseteController(2.0, .7), powertrain.getFeedFoward(), powertrain.getDifferentialDriveKinematics(),
                powertrain::getWheelSpeeds, powertrain.getLeftPIDController(), powertrain.getRightPIDController(),
                powertrain::setVolts, powertrain);
                powertrain.zeroHeading();

  }*/
/**
 * Use this method to define your button->command mappings. Buttons can be
 * created by instantiating a {@link GenericHID} or one of its subclasses
 * ({@link edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then
 * passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
 */
private void configureButtonBindings() {
    // Buttons
    // new JoystickButton(driverController, 2).whenPressed(new TurnPIDB(powertrain, 50));
    // new JoystickButton(driverController, 3).whenPressed(new TurnPIDB(powertrain, 110));
    new JoystickButton(driverController, 2).whenPressed(new InstantCommand(intake::toExtendIntake, intake));
    new JoystickButton(driverController, 3).whenPressed(new InstantCommand(intake::saveIntake, intake));
    // new JoystickButton(driverController, 4).whenPressed(new Shoot(shooter, intake, powertrain, vision));
    // new JoystickButton(driverController, 4).whileHeld(new Tracking(powertrain, vision));
    // new JoystickButton(driverController, 5).whileHeld(new TakeAll(intake));
    // new JoystickButton(driverController, 6).toggleWhenPressed(new TakeWithSensor(intake));
    // new JoystickButton(driverController, 6).toggleWhenPressed(new TakeWithSensor(intake));
    // new JoystickButton(driverController, 8).whenPressed(new InstantCommand(powertrain::resetGyro)); // Boton Select
    // POV
    new POVButton(driverController, 270).whileHeld(new EjectBalls(intake));
}