shooter and intake command
This commit is contained in:
parent
9e8377b574
commit
9c83be677b
@ -25,157 +25,181 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
|
||||
import edu.wpi.first.wpilibj2.command.button.Trigger;
|
||||
import frc.robot.constants.OIConstants;
|
||||
import frc.robot.constants.PhotonConstants;
|
||||
import frc.robot.constants.ShooterConstants;
|
||||
import frc.robot.subsystems.Climber;
|
||||
import frc.robot.subsystems.Drivetrain;
|
||||
import frc.robot.utilities.PhotonVision;
|
||||
import frc.robot.subsystems.Intake;
|
||||
import frc.robot.subsystems.Shooter;
|
||||
|
||||
public class RobotContainer {
|
||||
private static final String kAutoTabName = "Autonomous";
|
||||
private static final String kTeleopTabName = "Teloperated";
|
||||
private static final String kAutoTabName = "Autonomous";
|
||||
private static final String kTeleopTabName = "Teloperated";
|
||||
|
||||
private PhotonVision photonvision;
|
||||
private PhotonVision photonvision;
|
||||
|
||||
private Drivetrain drivetrain;
|
||||
private Drivetrain drivetrain;
|
||||
private Intake intake;
|
||||
private Shooter shooter;
|
||||
private Climber climber;
|
||||
|
||||
private CommandXboxController driver;
|
||||
private CommandXboxController operator;
|
||||
private CommandXboxController driver;
|
||||
private CommandXboxController operator;
|
||||
|
||||
private Trigger isTeleopTrigger;
|
||||
private Trigger isTeleopTrigger;
|
||||
|
||||
private SendableChooser<Command> autoChooser;
|
||||
private SendableChooser<Command> autoChooser;
|
||||
|
||||
private int ampTagID;
|
||||
private int ampTagID;
|
||||
|
||||
// TODO There's more than one source tag, how do we want to handle this?
|
||||
private int sourceTagID;
|
||||
// TODO There's more than one source tag, how do we want to handle this?
|
||||
private int sourceTagID;
|
||||
|
||||
// TODO There's more than one speaker tag, how do we want to handle this?
|
||||
private int speakerTag;
|
||||
// TODO There's more than one speaker tag, how do we want to handle this?
|
||||
private int speakerTag;
|
||||
|
||||
public RobotContainer() {
|
||||
try {
|
||||
photonvision = new PhotonVision(
|
||||
PhotonConstants.kCameraName,
|
||||
PhotonConstants.kCameraTransform,
|
||||
PhotonConstants.kCameraHeightMeters,
|
||||
PhotonConstants.kCameraPitchRadians
|
||||
);
|
||||
} catch (IOException e) {
|
||||
photonvision = null;
|
||||
public RobotContainer() {
|
||||
try {
|
||||
photonvision = new PhotonVision(
|
||||
PhotonConstants.kCameraName,
|
||||
PhotonConstants.kCameraTransform,
|
||||
PhotonConstants.kCameraHeightMeters,
|
||||
PhotonConstants.kCameraPitchRadians
|
||||
);
|
||||
} catch (IOException e) {
|
||||
photonvision = null;
|
||||
}
|
||||
|
||||
// TODO Need to provide a real initial pose
|
||||
// TODO Need to provide a real IAprilTagProvider, null means we're not using one at all
|
||||
// TODO Need to provide a real VisualPoseProvider, null means we're not using one at all
|
||||
drivetrain = new Drivetrain(new Pose2d(), photonvision, null);
|
||||
|
||||
intake = new Intake();
|
||||
|
||||
shooter = new Shooter();
|
||||
|
||||
climber = new Climber(shooter.getShooterAngle());
|
||||
|
||||
// An example Named Command, doesn't need to remain once we start actually adding real things
|
||||
// ALL Named Commands need to be defined AFTER subsystem initialization and BEFORE auto/controller configuration
|
||||
NamedCommands.registerCommand("Set Drivetrain X", drivetrain.setXCommand());
|
||||
|
||||
// TODO Specify a default auto string once we have one
|
||||
autoChooser = AutoBuilder.buildAutoChooser();
|
||||
|
||||
driver = new CommandXboxController(OIConstants.kPrimaryXboxUSB);
|
||||
operator = new CommandXboxController(OIConstants.kSecondaryXboxUSB);
|
||||
|
||||
isTeleopTrigger = new Trigger(DriverStation::isTeleopEnabled);
|
||||
|
||||
Optional<DriverStation.Alliance> alliance = DriverStation.getAlliance();
|
||||
|
||||
if (alliance.isEmpty() || alliance.get() == DriverStation.Alliance.Red) {
|
||||
ampTagID = 5;
|
||||
sourceTagID = 9;
|
||||
speakerTag = 4;
|
||||
} else {
|
||||
ampTagID = 6;
|
||||
sourceTagID = 1;
|
||||
speakerTag = 8;
|
||||
}
|
||||
|
||||
configureBindings();
|
||||
shuffleboardSetup();
|
||||
}
|
||||
|
||||
// TODO Need to provide a real initial pose
|
||||
// TODO Need to provide a real IAprilTagProvider, null means we're not using one at all
|
||||
// TODO Need to provide a real VisualPoseProvider, null means we're not using one at all
|
||||
drivetrain = new Drivetrain(new Pose2d(), photonvision, null);
|
||||
// The objective should be to have the subsystems expose methods that return commands
|
||||
// that can be bound to the triggers provided by the CommandXboxController class.
|
||||
// This mindset should help keep RobotContainer a little cleaner this year.
|
||||
// This is not to say you can't trigger or command chain here (see driveCardnial drivetrain example),
|
||||
// but generally reduce/avoid any situation where the keyword "new" is involved if you're working
|
||||
// with a subsystem
|
||||
private void configureBindings() {
|
||||
isTeleopTrigger.onTrue(new InstantCommand(() -> Shuffleboard.selectTab(kTeleopTabName)));
|
||||
|
||||
// An example Named Command, doesn't need to remain once we start actually adding real things
|
||||
// ALL Named Commands need to be defined AFTER subsystem initialization and BEFORE auto/controller configuration
|
||||
NamedCommands.registerCommand("Set Drivetrain X", drivetrain.setXCommand());
|
||||
|
||||
// TODO Specify a default auto string once we have one
|
||||
autoChooser = AutoBuilder.buildAutoChooser();
|
||||
|
||||
driver = new CommandXboxController(OIConstants.kPrimaryXboxUSB);
|
||||
//secondary = new CommandXboxController(OIConstants.kSecondaryXboxUSB);
|
||||
|
||||
isTeleopTrigger = new Trigger(DriverStation::isTeleopEnabled);
|
||||
|
||||
Optional<DriverStation.Alliance> alliance = DriverStation.getAlliance();
|
||||
|
||||
if (alliance.isEmpty() || alliance.get() == DriverStation.Alliance.Red) {
|
||||
ampTagID = 5;
|
||||
sourceTagID = 9;
|
||||
speakerTag = 4;
|
||||
} else {
|
||||
ampTagID = 6;
|
||||
sourceTagID = 1;
|
||||
speakerTag = 8;
|
||||
}
|
||||
|
||||
configureBindings();
|
||||
shuffleboardSetup();
|
||||
}
|
||||
|
||||
// The objective should be to have the subsystems expose methods that return commands
|
||||
// that can be bound to the triggers provided by the CommandXboxController class.
|
||||
// This mindset should help keep RobotContainer a little cleaner this year.
|
||||
// This is not to say you can't trigger or command chain here (see driveCardnial drivetrain example),
|
||||
// but generally reduce/avoid any situation where the keyword "new" is involved if you're working
|
||||
// with a subsystem
|
||||
private void configureBindings() {
|
||||
isTeleopTrigger.onTrue(new InstantCommand(() -> Shuffleboard.selectTab(kTeleopTabName)));
|
||||
|
||||
drivetrain.setDefaultCommand(
|
||||
drivetrain.teleopCommand(
|
||||
driver::getLeftY,
|
||||
driver::getLeftX,
|
||||
driver::getRightX,
|
||||
OIConstants.kTeleopDriveDeadband
|
||||
)
|
||||
);
|
||||
|
||||
driver.povCenter().onFalse(
|
||||
drivetrain.driveCardinal(
|
||||
driver::getLeftY,
|
||||
driver::getLeftX,
|
||||
driver.getHID()::getPOV,
|
||||
OIConstants.kTeleopDriveDeadband).until(
|
||||
() -> MathUtil.applyDeadband(driver.getRightX(), OIConstants.kTeleopDriveDeadband) != 0
|
||||
drivetrain.setDefaultCommand(
|
||||
drivetrain.teleopCommand(
|
||||
driver::getLeftY,
|
||||
driver::getLeftX,
|
||||
driver::getRightX,
|
||||
OIConstants.kTeleopDriveDeadband
|
||||
)
|
||||
);
|
||||
);
|
||||
|
||||
// Lock on to the appropriate Amp AprilTag while still being able to strafe and drive forward and back
|
||||
driver.a().onTrue(
|
||||
drivetrain.driveAprilTagLock(
|
||||
driver::getLeftY,
|
||||
driver::getLeftX,
|
||||
OIConstants.kTeleopDriveDeadband,
|
||||
ampTagID
|
||||
).until(
|
||||
() -> MathUtil.applyDeadband(driver.getRightX(), OIConstants.kTeleopDriveDeadband) != 0
|
||||
)
|
||||
);
|
||||
intake.setDefaultCommand(intake.intakeUpCommand());
|
||||
|
||||
// Lock on to the appropriate Source AprilTag while still being able to strafe and drive forward and back
|
||||
driver.b().onTrue(
|
||||
drivetrain.driveAprilTagLock(
|
||||
driver::getLeftY,
|
||||
driver::getLeftX,
|
||||
OIConstants.kTeleopDriveDeadband,
|
||||
sourceTagID
|
||||
).until(
|
||||
() -> MathUtil.applyDeadband(driver.getRightX(), OIConstants.kTeleopDriveDeadband) != 0
|
||||
)
|
||||
);
|
||||
shooter.setDefaultCommand(shooter.angleSpeedsSetpoints(ShooterConstants.kShooterLoadAngle, 0.0, 0.0));
|
||||
|
||||
// Lock on to the appropriate Speaker AprilTag while still being able to strafe and drive forward and back
|
||||
driver.x().onTrue(
|
||||
drivetrain.driveAprilTagLock(
|
||||
driver::getLeftY,
|
||||
driver::getLeftX,
|
||||
OIConstants.kTeleopDriveDeadband,
|
||||
speakerTag
|
||||
).until(
|
||||
() -> MathUtil.applyDeadband(driver.getRightX(), OIConstants.kTeleopDriveDeadband) != 0
|
||||
)
|
||||
);
|
||||
climber.setDefaultCommand(climber.stop());
|
||||
|
||||
// This was originally a run while held, not sure that's really necessary, change it if need be
|
||||
driver.y().onTrue(drivetrain.zeroHeadingCommand());
|
||||
driver.povCenter().onFalse(
|
||||
drivetrain.driveCardinal(
|
||||
driver::getLeftY,
|
||||
driver::getLeftX,
|
||||
driver.getHID()::getPOV,
|
||||
OIConstants.kTeleopDriveDeadband).until(
|
||||
() -> MathUtil.applyDeadband(driver.getRightX(), OIConstants.kTeleopDriveDeadband) != 0
|
||||
)
|
||||
);
|
||||
|
||||
// This was originally a run while held, not sure that's really necessary, change it if need be
|
||||
driver.rightBumper().onTrue(drivetrain.setXCommand());
|
||||
// Lock on to the appropriate Amp AprilTag while still being able to strafe and drive forward and back
|
||||
driver.a().onTrue(
|
||||
drivetrain.driveAprilTagLock(
|
||||
driver::getLeftY,
|
||||
driver::getLeftX,
|
||||
OIConstants.kTeleopDriveDeadband,
|
||||
ampTagID
|
||||
).until(
|
||||
() -> MathUtil.applyDeadband(driver.getRightX(), OIConstants.kTeleopDriveDeadband) != 0
|
||||
)
|
||||
);
|
||||
|
||||
/*
|
||||
* This has been added because interest has been expressed in trying field relative vs
|
||||
* robot relative control. This should <i>default</i> to field relative, but give the option
|
||||
* for the person practicing driving to push start on the driver's controller to quickly switch to
|
||||
* the other style.
|
||||
*
|
||||
* If it becomes something that we need to switch <i>prior</i> to the start of the match, a different
|
||||
* mechanism will need to be devised, this will work for now.
|
||||
*/
|
||||
driver.start().onTrue(drivetrain.toggleFieldRelativeControl());
|
||||
// Lock on to the appropriate Source AprilTag while still being able to strafe and drive forward and back
|
||||
driver.b().onTrue(
|
||||
drivetrain.driveAprilTagLock(
|
||||
driver::getLeftY,
|
||||
driver::getLeftX,
|
||||
OIConstants.kTeleopDriveDeadband,
|
||||
sourceTagID
|
||||
).until(
|
||||
() -> MathUtil.applyDeadband(driver.getRightX(), OIConstants.kTeleopDriveDeadband) != 0
|
||||
)
|
||||
);
|
||||
|
||||
// Lock on to the appropriate Speaker AprilTag while still being able to strafe and drive forward and back
|
||||
driver.x().onTrue(
|
||||
drivetrain.driveAprilTagLock(
|
||||
driver::getLeftY,
|
||||
driver::getLeftX,
|
||||
OIConstants.kTeleopDriveDeadband,
|
||||
speakerTag
|
||||
).until(
|
||||
() -> MathUtil.applyDeadband(driver.getRightX(), OIConstants.kTeleopDriveDeadband) != 0
|
||||
)
|
||||
);
|
||||
|
||||
// This was originally a run while held, not sure that's really necessary, change it if need be
|
||||
driver.y().onTrue(drivetrain.zeroHeadingCommand());
|
||||
|
||||
// This was originally a run while held, not sure that's really necessary, change it if need be
|
||||
driver.rightBumper().onTrue(drivetrain.setXCommand());
|
||||
|
||||
/*
|
||||
* This has been added because interest has been expressed in trying field relative vs
|
||||
* robot relative control. This should <i>default</i> to field relative, but give the option
|
||||
* for the person practicing driving to push start on the driver's controller to quickly switch to
|
||||
* the other style.
|
||||
*
|
||||
* If it becomes something that we need to switch <i>prior</i> to the start of the match, a different
|
||||
* mechanism will need to be devised, this will work for now.
|
||||
*/
|
||||
driver.start().onTrue(drivetrain.toggleFieldRelativeControl());
|
||||
|
||||
driver.leftBumper().toggleOnTrue(intake.intakeDownCommand());
|
||||
|
||||
operator.y().onTrue(climber.setSpeed(operator.getRightTriggerAxis()));
|
||||
|
||||
}
|
||||
|
||||
private void shuffleboardSetup() {
|
||||
@ -190,12 +214,12 @@ public class RobotContainer {
|
||||
|
||||
ShuffleboardTab teleopTab = Shuffleboard.getTab(kTeleopTabName);
|
||||
teleopTab.addBoolean("Field Relative Controls Enabled?", drivetrain::isFieldRelativeControl)
|
||||
.withPosition(0, 0)
|
||||
.withSize(2, 1)
|
||||
.withWidget(BuiltInWidgets.kBooleanBox);
|
||||
.withPosition(0, 0)
|
||||
.withSize(2, 1)
|
||||
.withWidget(BuiltInWidgets.kBooleanBox);
|
||||
}
|
||||
|
||||
public Command getAutonomousCommand() {
|
||||
return autoChooser.getSelected();
|
||||
return autoChooser.getSelected();
|
||||
}
|
||||
}
|
||||
|
||||
@ -16,8 +16,7 @@ public class IntakeConstants {
|
||||
public static final double kGFeedForward = 0;
|
||||
public static final double kVFeedForward = 0;
|
||||
|
||||
|
||||
public static final double kStartingAngle = Math.toRadians(110.0);
|
||||
public static final double kUpAngle = Math.toRadians(100.0);
|
||||
public static final double kDownAngle = Math.toRadians(-40.0);
|
||||
public static final double kStartingAngle = Math.toRadians(105.0);
|
||||
public static final double kUpAngle = Math.toRadians(90.0);
|
||||
public static final double kDownAngle = Math.toRadians(-34.0);
|
||||
}
|
||||
|
||||
@ -17,6 +17,9 @@ public class ShooterConstants {
|
||||
public static final double kShooterPivotI = 0.0;
|
||||
public static final double kShooterPivotD = 0.0;
|
||||
|
||||
public static final double kShooterLoadAngle = Math.toRadians(-20.0);
|
||||
public static final double kShooterAmpAngle = Math.toRadians(105.0);
|
||||
|
||||
public static final double kPivotConversion = 1/(40.0*(28.0/15.0));
|
||||
|
||||
public static final double kSShooterPivotFF = 0.0;
|
||||
|
||||
@ -14,7 +14,7 @@ public class Climber extends SubsystemBase {
|
||||
private VictorSPX motor2;
|
||||
|
||||
//TODO What tells the climber to stop climbing when it achieves the target height?
|
||||
public Climber() {
|
||||
public Climber(DoubleSupplier shooterAngle) {
|
||||
motor1 = new VictorSPX(ClimberConstants.kClimberMotor1CANID);
|
||||
motor2 = new VictorSPX(ClimberConstants.kClimberMotor2CANID);
|
||||
|
||||
|
||||
@ -1,5 +1,7 @@
|
||||
package frc.robot.subsystems;
|
||||
|
||||
import java.util.function.DoubleSupplier;
|
||||
|
||||
import com.revrobotics.CANSparkMax;
|
||||
import com.revrobotics.RelativeEncoder;
|
||||
import com.revrobotics.CANSparkLowLevel.MotorType;
|
||||
@ -79,8 +81,12 @@ public class Shooter extends SubsystemBase{
|
||||
shooterPivotFF.calculate(setpointAngle, 0.0));
|
||||
|
||||
topShooter.setVoltage(topShooterPID.calculate(topEncoder.getVelocity(), topRPM)+topShooterFF.calculate(topRPM));
|
||||
|
||||
|
||||
bottomShooter.setVoltage(bottomShooterPID.calculate(bottomEncoder.getVelocity(), bottomRPM)+bottomShooterFF.calculate(bottomRPM));
|
||||
});
|
||||
}
|
||||
|
||||
public DoubleSupplier getShooterAngle(){
|
||||
return pivotEncoder.getDistance();
|
||||
}
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user