Examples with RamseteController edu.wpi.first.math.controller.RamseteController used on opensource projects

Example 1 with RamseteController

use of edu.wpi.first.math.controller.RamseteController 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 RamseteController

use of edu.wpi.first.math.controller.RamseteController in project 3128-robot-2022 by Team3128.

the class RobotContainer method initAutos.

private void initAutos() {
    try {
        for (int i = 0; i < trajJson.length; i++) {
            // Get a path from the string specified in trajJson, and load it into trajectory[i]
            Path path = Filesystem.getDeployDirectory().toPath().resolve("paths").resolve(trajJson[i]);
            trajectory[i] = TrajectoryUtil.fromPathweaverJson(path);
            Log.info("InitAutos", "Trajectory" + i + " = path" + path.toString());
        }
    } catch (IOException ex) {
        DriverStation.reportError("IOException opening trajectory:", ex.getStackTrace());
    }
    initialPoses = new HashMap<Command, Pose2d>();
    climbCommand = new CmdClimb(m_climber);
    climbTraversalCommand = new CmdClimbTraversal(m_climber);
    extendIntakeAndReverse = new SequentialCommandGroup(new CmdExtendIntake(m_intake).withTimeout(0.1), new CmdReverseIntake(m_intake, m_hopper));
    // this shoot command is the ideal one with all capabilities
    shootCommand = new SequentialCommandGroup(new InstantCommand(m_shooterLimelight::turnLEDOn), new CmdRetractHopper(m_hopper), new InstantCommand(() -> m_shooter.setState(ShooterState.UPPERHUB)), // new CmdExtendIntake(m_intake),
    new ParallelCommandGroup(// new RunCommand(m_intake::runIntake, m_intake),
    new CmdAlign(m_drive, m_shooterLimelight), new CmdHopperShooting(m_hopper, m_shooter::isReady), new CmdShootDist(m_shooter, m_shooterLimelight)));
    // use this shoot command for testing
    manualShoot = new SequentialCommandGroup(new InstantCommand(m_shooterLimelight::turnLEDOn), new CmdRetractHopper(m_hopper), new InstantCommand(() -> m_shooter.setState(ShooterState.UPPERHUB)), new ParallelCommandGroup(new CmdHopperShooting(m_hopper, m_shooter::isReady), new CmdShootDist(m_shooter, m_shooterLimelight)));
    lowerHubShoot = new SequentialCommandGroup(new CmdRetractHopper(m_hopper), new InstantCommand(() -> m_shooter.setState(ShooterState.LOWERHUB)), // new CmdExtendIntake(m_intake),
    new ParallelCommandGroup(// new RunCommand(m_intake::runIntake, m_intake),
    new RunCommand(m_drive::stop, m_drive), new CmdHopperShooting(m_hopper, m_shooter::isReady), new CmdShootRPM(m_shooter, 1100)));
    // AUTONOMOUS ROUTINES
    auto_1Ball = new SequentialCommandGroup(retractHopperAndShootCmdLL(3750), new RamseteCommand(Trajectories.driveBack30In, m_drive::getPose, new RamseteController(Constants.DriveConstants.RAMSETE_B, Constants.DriveConstants.RAMSETE_ZETA), new SimpleMotorFeedforward(Constants.DriveConstants.kS, Constants.DriveConstants.kV, Constants.DriveConstants.kA), Constants.DriveConstants.DRIVE_KINEMATICS, m_drive::getWheelSpeeds, new PIDController(Constants.DriveConstants.RAMSETE_KP, 0, 0), new PIDController(Constants.DriveConstants.RAMSETE_KP, 0, 0), m_drive::tankDriveVolts, m_drive));
    auto_2BallBot = new SequentialCommandGroup(// pick up 1 ball
    new ParallelDeadlineGroup(trajectoryCmd(0).andThen(m_drive::stop, m_drive), new CmdExtendIntakeAndRun(m_intake, m_hopper)), // shoot preloaded + first
    retractHopperAndShootCmd(3250));
    auto_2BallMid = new SequentialCommandGroup(// pick up 1 ball
    new ParallelDeadlineGroup(trajectoryCmd(1).andThen(m_drive::stop, m_drive), new CmdExtendIntakeAndRun(m_intake, m_hopper)), // shoot first + preloaded
    retractHopperAndShootCmd(3250));
    auto_2BallTop = new SequentialCommandGroup(new CmdExtendIntake(m_intake), new ParallelDeadlineGroup(trajectoryCmd(2).andThen(m_drive::stop, m_drive), new CmdIntakeCargo(m_intake, m_hopper)), new InstantCommand(() -> m_intake.retractIntake()), // shoot first + preloaded
    retractHopperAndShootCmdLL(3750));
    auto_3BallHook = new SequentialCommandGroup(// shoot preloaded ball
    retractHopperAndShootCmd(3350), // pick up two balls
    new ParallelDeadlineGroup(new SequentialCommandGroup(trajectoryCmd(3), trajectoryCmd(4), new InstantCommand(m_drive::stop, m_drive)), new CmdExtendIntakeAndRun(m_intake, m_hopper)), // shoot two balls
    retractHopperAndShootCmd(3250));
    auto_3BallHersheyKiss = new SequentialCommandGroup(// shoot preload
    retractHopperAndShootCmd(3500), // pick up two balls
    new ParallelDeadlineGroup(new SequentialCommandGroup(trajectoryCmd(5), trajectoryCmd(6), new InstantCommand(m_drive::stop, m_drive)), new CmdExtendIntakeAndRun(m_intake, m_hopper)), // shoot two balls
    retractHopperAndShootCmdLL(3500));
    auto_3BallTerminal = new SequentialCommandGroup(// shoot preloaded ball
    retractHopperAndShootCmd(3000), trajectoryCmd(7), new ParallelDeadlineGroup(new SequentialCommandGroup(trajectoryCmd(8), new InstantCommand(m_drive::stop, m_drive), new WaitCommand(1)), new CmdExtendIntakeAndRun(m_intake, m_hopper)), trajectoryCmd(19), trajectoryCmd(20), new InstantCommand(m_drive::stop, m_drive), // shoot two balls
    retractHopperAndShootCmd(3000));
    auto_4BallE = new SequentialCommandGroup(// pick up first ball
    new ParallelDeadlineGroup(new SequentialCommandGroup(trajectoryCmd(11), new InstantCommand(m_drive::stop, m_drive)), new CmdExtendIntakeAndRun(m_intake, m_hopper)), // shoot first + preloaded
    retractHopperAndShootCmd(3000), // pick up two more balls
    new CmdExtendIntake(m_intake).withTimeout(0.1), new ParallelDeadlineGroup(new SequentialCommandGroup(trajectoryCmd(12), new InstantCommand(m_drive::stop, m_drive)), new CmdExtendIntakeAndRun(m_intake, m_hopper)), // shoot two more balls
    retractHopperAndShootCmd(3250));
    auto_4BallTerm = new SequentialCommandGroup(new CmdExtendIntake(m_intake), new ParallelDeadlineGroup(new SequentialCommandGroup(trajectoryCmd(13), new InstantCommand(m_drive::stop, m_drive)), new CmdIntakeCargo(m_intake, m_hopper)), new CmdRetractHopper(m_hopper), retractHopperAndShootCmd(3750), new CmdExtendIntake(m_intake), new ParallelDeadlineGroup(new SequentialCommandGroup(trajectoryCmd(14), new InstantCommand(m_drive::stop, m_drive), new WaitCommand(0.5)), new CmdIntakeCargo(m_intake, m_hopper)), new CmdRetractHopper(m_hopper), trajectoryCmd(15), trajectoryCmd(16), new InstantCommand(m_drive::stop, m_drive), // shoot two balls
    retractHopperAndShootCmd(3750));
    auto_5Ball = new SequentialCommandGroup(new SequentialCommandGroup(new CmdRetractHopper(m_hopper).withTimeout(0.5), new ParallelCommandGroup(new CmdHopperShooting(m_hopper, m_shooter::isReady), new CmdShootRPM(m_shooter, 3250)).withTimeout(1)), // pick up first ball
    new ParallelDeadlineGroup(new SequentialCommandGroup(trajectoryCmd(15), trajectoryCmd(16), new InstantCommand(m_drive::stop, m_drive)), new CmdExtendIntakeAndRun(m_intake, m_hopper)), retractHopperAndShootCmd(3750), trajectoryCmd(17), new ParallelDeadlineGroup(new SequentialCommandGroup(trajectoryCmd(18), new InstantCommand(m_drive::stop, m_drive), new WaitCommand(0.5)), new CmdExtendIntakeAndRun(m_intake, m_hopper)), trajectoryCmd(19), trajectoryCmd(20), new InstantCommand(m_drive::stop, m_drive), // shoot two balls
    retractHopperAndShootCmd(3750));
    // Setup auto-selector
    NarwhalDashboard.addAuto("1 Ball", auto_1Ball);
    NarwhalDashboard.addAuto("2 Ball Bottom", auto_2BallBot);
    NarwhalDashboard.addAuto("2 Ball Mid", auto_2BallMid);
    NarwhalDashboard.addAuto("2 Ball Top", auto_2BallTop);
    NarwhalDashboard.addAuto("3 Ball Hook", auto_3BallHook);
    NarwhalDashboard.addAuto("3 Ball Terminal", auto_3BallTerminal);
    NarwhalDashboard.addAuto("3 Ball Hershey Kiss", auto_3BallHersheyKiss);
    NarwhalDashboard.addAuto("4 Ball E", auto_4BallE);
    NarwhalDashboard.addAuto("4 Ball Terminal", auto_4BallTerm);
    NarwhalDashboard.addAuto("5 Ball", auto_5Ball);
}

Example 3 with RamseteController

use of edu.wpi.first.math.controller.RamseteController 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 4 with RamseteController

use of edu.wpi.first.math.controller.RamseteController 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 5 with RamseteController

use of edu.wpi.first.math.controller.RamseteController 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;
}