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

Example 1 with SimpleMotorFeedforward

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

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

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

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

Example 5 with SimpleMotorFeedforward

use of edu.wpi.first.math.controller.SimpleMotorFeedforward in project RapidReactFRC by CyberLions-8521.

the class TrajectoryFollower method getRamseteCommand.

// classes
public static Command getRamseteCommand(Trajectory path, Drivebase m_drive) {
    RamseteCommand auto = new RamseteCommand(path, m_drive::getPose, new RamseteController(TrajectoryConstants.RAMSETE_B, TrajectoryConstants.RAMSETE_ZETA), // AutoConstants.autoMotorFeedforward,
    new SimpleMotorFeedforward(TrajectoryConstants.KS, TrajectoryConstants.KV, TrajectoryConstants.KA), TrajectoryConstants.DRIVE_KINEMATICS, m_drive::getWheelSpeeds, new PIDController(TrajectoryConstants.P_DRIVE, 0, 0), new PIDController(TrajectoryConstants.P_DRIVE, 0, 0), m_drive::tankDriveVolts, m_drive);
    m_drive.resetOdometry(path.getInitialPose());
    return auto.andThen(() -> m_drive.tankDriveVolts(0, 0));
}