gbuckholtz
commented
3 years ago It turns out that merging directly into main causes problems for others trying to put their changes into main. Instead, can you redo your changes and do a pull request?
All you have to do is
- [ ] create a branch
- [ ] paste this code into the RobotContainer.java file
- [ ] check it in
- [ ] push it up to github
- [ ] make a pull request
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package frc.robot;
import java.util.List;
import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.controller.PIDController;
import edu.wpi.first.wpilibj.controller.RamseteController;
import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.geometry.Translation2d;
import frc.robot.Constants.AutoConstants;
import frc.robot.Constants.DriveConstants;
import frc.robot.commands.ArcadeDrive;
import frc.robot.commands.AutonomousDistance;
import frc.robot.commands.AutonomousTime;
import frc.robot.subsystems.Drivetrain;
import frc.robot.subsystems.OnBoardIO;
import frc.robot.subsystems.OnBoardIO.ChannelMode;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.trajectory.Trajectory;
import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
import edu.wpi.first.wpilibj.trajectory.constraint.DifferentialDriveVoltageConstraint;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.PrintCommand;
import edu.wpi.first.wpilibj2.command.RamseteCommand;
import edu.wpi.first.wpilibj2.command.button.Button;
/**
* This class is where the bulk of the robot should be declared. Since Command-based is a
* "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
* periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
* subsystems, commands, and button mappings) should be declared here.
*/
public class RobotContainer {
// The robot's subsystems and commands are defined here...
private final Drivetrain m_drivetrain = new Drivetrain();
private final OnBoardIO m_onboardIO = new OnBoardIO(ChannelMode.INPUT, ChannelMode.INPUT);
// Assumes a gamepad plugged into channnel 0
private final Joystick m_controller = new Joystick(0);
// Create SmartDashboard chooser for autonomous routines
private final SendableChooser<Command> m_chooser = new SendableChooser<>();
// NOTE: The I/O pin functionality of the 5 exposed I/O pins depends on the hardware "overlay"
// that is specified when launching the wpilib-ws server on the Romi raspberry pi.
// By default, the following are available (listed in order from inside of the board to outside):
// - DIO 8 (mapped to Arduino pin 11, closest to the inside of the board)
// - Analog In 0 (mapped to Analog Channel 6 / Arduino Pin 4)
// - Analog In 1 (mapped to Analog Channel 2 / Arduino Pin 20)
// - PWM 2 (mapped to Arduino Pin 21)
// - PWM 3 (mapped to Arduino Pin 22)
//
// Your subsystem configuration should take the overlays into account
/** The container for the robot. Contains subsystems, OI devices, and commands. */
public RobotContainer() {
// Configure the button bindings
configureButtonBindings();
}
/**
* Generate a trajectory following Ramsete command
*
* This is very similar to the WPILib RamseteCommand example. It uses
* constants defined in the Constants.java file. These constants were
* found empirically by using the frc-characterization tool.
*
* @return A SequentialCommand that sets up and executes a trajectory following Ramsete command
*/
private Command generateRamseteCommand() {
var autoVoltageConstraint =
new DifferentialDriveVoltageConstraint(
new SimpleMotorFeedforward(DriveConstants.ksVolts,
DriveConstants.kvVoltSecondsPerMeter,
DriveConstants.kaVoltSecondsSquaredPerMeter),
DriveConstants.kDriveKinematics,
10);
TrajectoryConfig config =
new TrajectoryConfig(AutoConstants.kMaxSpeedMetersPerSecond,
AutoConstants.kMaxAccelerationMetersPerSecondSquared)
.setKinematics(DriveConstants.kDriveKinematics)
.addConstraint(autoVoltageConstraint);
// This trajectory can be modified to suit your purposes
// Note that all coordinates are in meters, and follow NWU conventions.
// If you would like to specify coordinates in inches (which might be easier
// to deal with for the Romi), you can use the Units.inchesToMeters() method
Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory(
// Start at the origin facing the +X direction
new Pose2d(0, 0, new Rotation2d(0)),
List.of(
new Translation2d(0.5, 0.25),
new Translation2d(1.0, -0.25),
new Translation2d(1.5, 0)
),
new Pose2d(0.0, 0, new Rotation2d(Math.PI)),
config);
RamseteCommand ramseteCommand = new RamseteCommand(
exampleTrajectory,
m_drivetrain::getPose,
new RamseteController(AutoConstants.kRamseteB, AutoConstants.kRamseteZeta),
new SimpleMotorFeedforward(DriveConstants.ksVolts, DriveConstants.kvVoltSecondsPerMeter, DriveConstants.kaVoltSecondsSquaredPerMeter),
DriveConstants.kDriveKinematics,
m_drivetrain::getWheelSpeeds,
new PIDController(DriveConstants.kPDriveVel, 0, 0),
new PIDController(DriveConstants.kPDriveVel, 0, 0),
m_drivetrain::tankDriveVolts,
m_drivetrain);
m_drivetrain.resetOdometry(exampleTrajectory.getInitialPose());
// Set up a sequence of commands
// First, we want to reset the drivetrain odometry
return new InstantCommand(() -> m_drivetrain.resetOdometry(exampleTrajectory.getInitialPose()), m_drivetrain)
// next, we run the actual ramsete command
.andThen(ramseteCommand)
// Finally, we make sure that the robot stops
.andThen(new InstantCommand(() -> m_drivetrain.tankDriveVolts(0, 0), m_drivetrain));
}
//
// ----------------------- start copy ------------------------------
//
// Glenn's custom trajectory
private Command generateZigZagCommand() {
var autoVoltageConstraint =
new DifferentialDriveVoltageConstraint(
new SimpleMotorFeedforward(DriveConstants.ksVolts,
DriveConstants.kvVoltSecondsPerMeter,
DriveConstants.kaVoltSecondsSquaredPerMeter),
DriveConstants.kDriveKinematics,
10);
TrajectoryConfig config =
new TrajectoryConfig(AutoConstants.kMaxSpeedMetersPerSecond,
AutoConstants.kMaxAccelerationMetersPerSecondSquared)
.setKinematics(DriveConstants.kDriveKinematics)
.addConstraint(autoVoltageConstraint);
// This trajectory can be modified to suit your purposes
// Note that all coordinates are in meters, and follow NWU conventions.
// If you would like to specify coordinates in inches (which might be easier
// to deal with for the Romi), you can use the Units.inchesToMeters() method
Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory(
// Start at the origin facing the +X direction
new Pose2d(0, 0, new Rotation2d(0)),
List.of(
new Translation2d(0.5, 0.25),
new Translation2d(1.0, -0.25),
new Translation2d(1.5, 0)
),
new Pose2d(0.0, 0, new Rotation2d(Math.PI)),
config);
RamseteCommand ramseteCommand = new RamseteCommand(
exampleTrajectory,
m_drivetrain::getPose,
new RamseteController(AutoConstants.kRamseteB, AutoConstants.kRamseteZeta),
new SimpleMotorFeedforward(DriveConstants.ksVolts, DriveConstants.kvVoltSecondsPerMeter, DriveConstants.kaVoltSecondsSquaredPerMeter),
DriveConstants.kDriveKinematics,
m_drivetrain::getWheelSpeeds,
new PIDController(DriveConstants.kPDriveVel, 0, 0),
new PIDController(DriveConstants.kPDriveVel, 0, 0),
m_drivetrain::tankDriveVolts,
m_drivetrain);
m_drivetrain.resetOdometry(exampleTrajectory.getInitialPose());
// Set up a sequence of commands
// First, we want to reset the drivetrain odometry
return new InstantCommand(() -> m_drivetrain.resetOdometry(exampleTrajectory.getInitialPose()), m_drivetrain)
// next, we run the actual ramsete command
.andThen(ramseteCommand)
// Finally, we make sure that the robot stops
.andThen(new InstantCommand(() -> m_drivetrain.tankDriveVolts(0, 0), m_drivetrain));
}
//
//---------------------------end copy ---------------------------
//
// change the name of the command from generateBackLeftCommand() to your own generate{nameHere}Command()
//
//-----------------paste here --------------
//
//
// ----------------------- start copy ------------------------------
//
// bambam7480 and fred7480's custom trajectory - forward 1 meter and right 25cm
private Command generateForwardRightCommand() {
var autoVoltageConstraint =
new DifferentialDriveVoltageConstraint(
new SimpleMotorFeedforward(DriveConstants.ksVolts,
DriveConstants.kvVoltSecondsPerMeter,
DriveConstants.kaVoltSecondsSquaredPerMeter),
DriveConstants.kDriveKinematics,
10);
TrajectoryConfig config =
new TrajectoryConfig(AutoConstants.kMaxSpeedMetersPerSecond,
AutoConstants.kMaxAccelerationMetersPerSecondSquared)
.setKinematics(DriveConstants.kDriveKinematics)
.addConstraint(autoVoltageConstraint);
// This trajectory can be modified to suit your purposes
// Note that all coordinates are in meters, and follow NWU conventions.
// If you would like to specify coordinates in inches (which might be easier
// to deal with for the Romi), you can use the Units.inchesToMeters() method
Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory(
// Start at the origin facing the +X direction
new Pose2d(0, 0, new Rotation2d(0)),
List.of(
new Translation2d(0.5, 0.25),
new Translation2d(1.0, -0.25),
new Translation2d(1.5, 0)
),
new Pose2d(0.0, 0, new Rotation2d(Math.PI)),
config);
RamseteCommand ramseteCommand = new RamseteCommand(
exampleTrajectory,
m_drivetrain::getPose,
new RamseteController(AutoConstants.kRamseteB, AutoConstants.kRamseteZeta),
new SimpleMotorFeedforward(DriveConstants.ksVolts, DriveConstants.kvVoltSecondsPerMeter, DriveConstants.kaVoltSecondsSquaredPerMeter),
DriveConstants.kDriveKinematics,
m_drivetrain::getWheelSpeeds,
new PIDController(DriveConstants.kPDriveVel, 0, 0),
new PIDController(DriveConstants.kPDriveVel, 0, 0),
m_drivetrain::tankDriveVolts,
m_drivetrain);
m_drivetrain.resetOdometry(exampleTrajectory.getInitialPose());
// Set up a sequence of commands
// First, we want to reset the drivetrain odometry
return new InstantCommand(() -> m_drivetrain.resetOdometry(exampleTrajectory.getInitialPose()), m_drivetrain)
// next, we run the actual ramsete command
.andThen(ramseteCommand)
// Finally, we make sure that the robot stops
.andThen(new InstantCommand(() -> m_drivetrain.tankDriveVolts(0, 0), m_drivetrain));
}
//
//---------------------------end copy ---------------------------
//
private Command generateBackLeftCommand() {
var autoVoltageConstraint =
new DifferentialDriveVoltageConstraint(
new SimpleMotorFeedforward(DriveConstants.ksVolts,
DriveConstants.kvVoltSecondsPerMeter,
DriveConstants.kaVoltSecondsSquaredPerMeter),
DriveConstants.kDriveKinematics,
10);
TrajectoryConfig config =
new TrajectoryConfig(AutoConstants.kMaxSpeedMetersPerSecond,
AutoConstants.kMaxAccelerationMetersPerSecondSquared)
.setKinematics(DriveConstants.kDriveKinematics)
.addConstraint(autoVoltageConstraint);
// This trajectory can be modified to suit your purposes
// Note that all coordinates are in meters, and follow NWU conventions.
// If you would like to specify coordinates in inches (which might be easier
// to deal with for the Romi), you can use the Units.inchesToMeters() method
Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory(
// Start at the origin facing the +X direction
new Pose2d(0, 0, new Rotation2d(0)),
List.of(
new Translation2d(-0.5, -0.5)
),
new Pose2d(-0.5, -0.5, new Rotation2d(0)),
config);
RamseteCommand ramseteCommand = new RamseteCommand(
exampleTrajectory,
m_drivetrain::getPose,
new RamseteController(AutoConstants.kRamseteB, AutoConstants.kRamseteZeta),
new SimpleMotorFeedforward(DriveConstants.ksVolts, DriveConstants.kvVoltSecondsPerMeter, DriveConstants.kaVoltSecondsSquaredPerMeter),
DriveConstants.kDriveKinematics,
m_drivetrain::getWheelSpeeds,
new PIDController(DriveConstants.kPDriveVel, 0, 0),
new PIDController(DriveConstants.kPDriveVel, 0, 0),
m_drivetrain::tankDriveVolts,
m_drivetrain);
m_drivetrain.resetOdometry(exampleTrajectory.getInitialPose());
// Set up a sequence of commands
// First, we want to reset the drivetrain odometry
return new InstantCommand(() -> m_drivetrain.resetOdometry(exampleTrajectory.getInitialPose()), m_drivetrain)
// next, we run the actual ramsete command
.andThen(ramseteCommand)
// Finally, we make sure that the robot stops
.andThen(new InstantCommand(() -> m_drivetrain.tankDriveVolts(0, 0), m_drivetrain));
}
//------------------------- BackDiagonal Right
private Command generateBackDiagonalRightCommand() {
var autoVoltageConstraint =
new DifferentialDriveVoltageConstraint(
new SimpleMotorFeedforward(DriveConstants.ksVolts,
DriveConstants.kvVoltSecondsPerMeter,
DriveConstants.kaVoltSecondsSquaredPerMeter),
DriveConstants.kDriveKinematics,
10);
TrajectoryConfig config =
new TrajectoryConfig(AutoConstants.kMaxSpeedMetersPerSecond,
AutoConstants.kMaxAccelerationMetersPerSecondSquared)
.setKinematics(DriveConstants.kDriveKinematics)
.addConstraint(autoVoltageConstraint);
// This trajectory can be modified to suit your purposes
// Note that all coordinates are in meters, and follow NWU conventions.
// If you would like to specify coordinates in inches (which might be easier
// to deal with for the Romi), you can use the Units.inchesToMeters() method
Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory(
// Start at the origin facing the +X direction
new Pose2d(0, 0, new Rotation2d(0)),
List.of(
new Translation2d(-0.5, -0.25),
new Translation2d(-1.0, -0.5),
new Translation2d(-1.5, -1.5)
),
new Pose2d(-1.5, -1.5, new Rotation2d(0)),
config);
RamseteCommand ramseteCommand = new RamseteCommand(
exampleTrajectory,
m_drivetrain::getPose,
new RamseteController(AutoConstants.kRamseteB, AutoConstants.kRamseteZeta),
new SimpleMotorFeedforward(DriveConstants.ksVolts, DriveConstants.kvVoltSecondsPerMeter, DriveConstants.kaVoltSecondsSquaredPerMeter),
DriveConstants.kDriveKinematics,
m_drivetrain::getWheelSpeeds,
new PIDController(DriveConstants.kPDriveVel, 0, 0),
new PIDController(DriveConstants.kPDriveVel, 0, 0),
m_drivetrain::tankDriveVolts,
m_drivetrain);
m_drivetrain.resetOdometry(exampleTrajectory.getInitialPose());
// Set up a sequence of commands
// First, we want to reset the drivetrain odometry
return new InstantCommand(() -> m_drivetrain.resetOdometry(exampleTrajectory.getInitialPose()), m_drivetrain)
// next, we run the actual ramsete command
.andThen(ramseteCommand)
// Finally, we make sure that the robot stops
.andThen(new InstantCommand(() -> m_drivetrain.tankDriveVolts(0, 0), m_drivetrain));
}
//--------------------------- end BackDiagonal right -----------------------------
/**
* 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() {
// Default command is arcade drive. This will run unless another command
// is scheduled over it.
m_drivetrain.setDefaultCommand(getArcadeDriveCommand());
// Example of how to use the onboard IO
Button onboardButtonA = new Button(m_onboardIO::getButtonAPressed);
onboardButtonA
.whenActive(new PrintCommand("Button A Pressed"))
.whenInactive(new PrintCommand("Button A Released"));
// Setup SmartDashboard options
m_chooser.setDefaultOption("Ramsete Trajectory", generateRamseteCommand());
m_chooser.addOption("ZigZag Trajectory", generateZigZagCommand());
//
//---- copy this command and paste it below the comment ----
//---- replace {yourName} with the name of the function you just created ---
//
// m_chooser.addOption("{yourName} Trajectory", generate{yourname}Command());
m_chooser.addOption("Forward Right Trajectory", generateForwardRightCommand());
m_chooser.addOption("Auto Routine Distance", new AutonomousDistance(m_drivetrain));
m_chooser.addOption("Auto Routine Time", new AutonomousTime(m_drivetrain));
m_chooser.addOption("BackLeft Trajectory", generateBackLeftCommand());
m_chooser.addOption("BackDiagonalRight Trajectory", generateBackDiagonalRightCommand());
SmartDashboard.putData(m_chooser);
}
/**
* Use this to pass the autonomous command to the main {@link Robot} class.
*
* @return the command to run in autonomous
*/
public Command getAutonomousCommand() {
return m_chooser.getSelected();
}
/**
* Use this to pass the teleop command to the main {@link Robot} class.
*
* @return the command to run in teleop
*/
public Command getArcadeDriveCommand() {
return new ArcadeDrive(
m_drivetrain, () -> -m_controller.getRawAxis(1), () -> m_controller.getRawAxis(2));
}
}
Just have to put in a "ForwardRight" in place of {yourName}