diff --git a/shuffleboard.json b/shuffleboard.json index 4b18133..8709f7e 100644 --- a/shuffleboard.json +++ b/shuffleboard.json @@ -24,7 +24,7 @@ "_showGlyph": true } }, - "8,3": { + "8,2": { "size": [ 2, 1 @@ -37,9 +37,9 @@ "_showGlyph": true } }, - "8,2": { + "0,3": { "size": [ - 2, + 4, 1 ], "content": { @@ -80,7 +80,7 @@ "_showGlyph": true } }, - "5,3": { + "6,2": { "size": [ 1, 1 @@ -306,7 +306,7 @@ "Colors/Color when false": "#8B0000FF" } }, - "5,2": { + "5,3": { "size": [ 1, 1 @@ -319,7 +319,7 @@ "_showGlyph": false } }, - "6,2": { + "6,3": { "size": [ 1, 1 @@ -331,6 +331,45 @@ "_glyph": 148, "_showGlyph": false } + }, + "9,3": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Command", + "_source0": "network_table:///SmartDashboard/ZeroYaw", + "_title": "ZeroYaw", + "_glyph": 139, + "_showGlyph": true + } + }, + "8,3": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Command", + "_source0": "network_table:///SmartDashboard/ArmUp", + "_title": "ArmUp", + "_glyph": 484, + "_showGlyph": true + } + }, + "5,2": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/FlywheelTime", + "_title": "FlywheelTime", + "_glyph": 148, + "_showGlyph": false + } } } } diff --git a/simgui-ds.json b/simgui-ds.json index 6c77d07..db1c7b8 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -101,6 +101,7 @@ } ], "robotJoysticks": [ + {}, { "guid": "Keyboard0" } diff --git a/simgui.json b/simgui.json index 4d48afe..5110bde 100644 --- a/simgui.json +++ b/simgui.json @@ -2,8 +2,10 @@ "NTProvider": { "types": { "/FMSInfo": "FMSInfo", + "/SmartDashboard/ArmUp": "Command", "/SmartDashboard/Auto Chooser": "String Chooser", - "/SmartDashboard/SendableChooser[0]": "String Chooser" + "/SmartDashboard/SendableChooser[0]": "String Chooser", + "/SmartDashboard/ZeroYaw": "Command" } }, "NetworkTables": { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 25d08b4..c2d6449 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -30,9 +30,13 @@ import frc.robot.subsystems.intake.IntakeShooter; import frc.robot.subsystems.intake.RealShooter; +import java.util.Optional; + import com.kauailabs.navx.frc.AHRS; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.GenericHID.HIDType; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -40,10 +44,10 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.net.PortForwarder; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior; import edu.wpi.first.wpilibj2.command.button.CommandJoystick; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; @@ -135,20 +139,19 @@ public class RobotContainer { * The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { - autoChooser.setDefaultOption("Forward", Autos.startingAuto(drivetrain, arm, leftHang, rightHang)); - autoChooser.addOption("1+Forward", - Autos.shootStartingAuto(drivetrain, arm, intakeShooter, leftHang, rightHang)); - autoChooser.addOption("2+Forward", + autoChooser.addOption("Forward", Autos.startingAuto(drivetrain, arm, leftHang, rightHang)); + autoChooser.setDefaultOption("2+Forward", Autos.shoot2StartingAuto(drivetrain, arm, intakeShooter, leftHang, rightHang)); + autoChooser.addOption("2+AmpSide", + Autos.shootFromAmpSideAuto(drivetrain, arm, intakeShooter, leftHang, rightHang)); + autoChooser.addOption("2+SourceSide", + Autos.shootFromFarSideAuto(drivetrain, arm, intakeShooter, leftHang, rightHang)); + SmartDashboard.putData("Auto Chooser", autoChooser); SmartDashboard.putData("ArmUp", new ArmRotateTo(arm, ArmConstants.ARM_START_DEGREES)); SmartDashboard.putData("ZeroYaw", new InstantCommand(drivetrain::zeroYaw)); - SmartDashboard.putData(drivetrain); - SmartDashboard.putData(arm); - SmartDashboard.putData(intakeShooter); - SmartDashboard.putString("Bot Name", Constants.currentBot.toString() + " - " + Constants.serialNumber); configureBindings(); @@ -184,6 +187,11 @@ public void setUpDriveController() { new InstantCommand(drivetrain::toDefaultStates, drivetrain)); new InstantCommand(lightStrip::toDefaultPattern, lightStrip); + double flip = 1; + Optional ally = DriverStation.getAlliance(); + if (ally.isPresent() && ally.get() == Alliance.Red) + flip = -1; + if (genericHIDType == null) { SmartDashboard.putString("Drive Ctrl", onPortMsg + "None"); } else if (genericHIDType.equals(GenericHID.HIDType.kHIDJoystick)) { @@ -230,10 +238,12 @@ public void setUpDriveController() { xbox.y().onTrue(Commands.runOnce(inputs::toggleFieldRelative)); - // xbox.rightBumper().onTrue(new AutoRotateTo(drivetrain, Rotation2d.fromDegrees(0), true)); - // xbox.leftBumper().onTrue(new AutoRotateTo(drivetrain, Rotation2d.fromDegrees(-90), true)); + // xbox.rightBumper().onTrue(new AutoRotateTo(drivetrain, + // Rotation2d.fromDegrees(0), true)); + // xbox.leftBumper().onTrue(new AutoRotateTo(drivetrain, + // Rotation2d.fromDegrees(-90), true)); xbox.rightBumper().whileFalse(new AimAtAngle(drivetrain, inputs, Rotation2d.fromDegrees(0))); - xbox.leftBumper().whileFalse(new AimAtAngle(drivetrain, inputs, Rotation2d.fromDegrees(-90))); + xbox.leftBumper().whileFalse(new AimAtAngle(drivetrain, inputs, Rotation2d.fromDegrees(-90 * flip))); if (vision != null) xbox.x().onTrue(Commands.runOnce(vision::toggleUsing, vision)); @@ -277,12 +287,19 @@ public void setUpOperatorController() { joystick.button(1).onTrue(Autos.intakeFromFloorStart(arm, intakeShooter)); joystick.button(1).onFalse(Autos.intakeFromFloorEnd(arm, intakeShooter)); - joystick.button(2).onTrue(Autos.shootInSpeaker(drivetrain, arm, intakeShooter, vision, inputs)); + joystick.button(2).onTrue(Autos.dropInAmp(drivetrain, arm, intakeShooter, vision, inputs).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); + joystick.button(3).onTrue(Autos.shootInSpeaker(drivetrain, arm, intakeShooter, vision, inputs).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); + + joystick.button(4).onTrue(new SpinFlywheelShooter(intakeShooter, IntakeShooterConstants.FLYWHEEL_SHOOTER_SPEED_SPEAKER)); + joystick.button(4).onFalse(new SpinFlywheelShooter(intakeShooter, 0)); + + joystick.button(5).onTrue(stowArm); + joystick.button(6).onTrue(stowArm2); - joystick.button(3).onTrue(Autos.dropInAmp(drivetrain, arm, intakeShooter, vision, inputs)); + joystick.button(7).whileTrue(Commands.startEnd(intakeShooter::eject, intakeShooter::stop, intakeShooter)); - joystick.button(4).onTrue(stowArm); - joystick.button(6).onTrue(stowArm); + leftHang.setDefaultCommand(new HangControl(leftHang, joystick::getX)); + rightHang.setDefaultCommand(new HangControl(rightHang, joystick::getY)); joystick.button(10).onTrue(cancelCommand); @@ -299,7 +316,8 @@ public void setUpOperatorController() { xbox.rightBumper().onTrue( new SpinFlywheelShooter(intakeShooter, IntakeShooterConstants.FLYWHEEL_SHOOTER_SPEED_SPEAKER)); - xbox.rightBumper().onFalse(new SpinFlywheelShooter(intakeShooter, 0)); + xbox.rightBumper().onFalse( + new SpinFlywheelShooter(intakeShooter, 0).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); xbox.y().onTrue(stowArm); xbox.a().onTrue(stowArm2); diff --git a/src/main/java/frc/robot/commands/Autos.java b/src/main/java/frc/robot/commands/Autos.java index 4f5924b..7c36293 100644 --- a/src/main/java/frc/robot/commands/Autos.java +++ b/src/main/java/frc/robot/commands/Autos.java @@ -92,6 +92,55 @@ public static Command shoot2StartingAuto(SwerveDrivetrain drivetrain, Arm arm, I new PullHangerDown(leftHang, HangConstants.SPEED)); } + public static Command shootFromSideAuto(SwerveDrivetrain drivetrain, Arm arm, IntakeShooter shooter, Hang leftHang, + Hang rightHang, boolean flipped) { + + Optional ally = DriverStation.getAlliance(); + if (ally.isPresent() && ally.get() == Alliance.Red) { + flipped = !flipped; + } + + drivetrain.setFrontOffset(Rotation2d.fromDegrees(flipped ? -60 : 60)); + + return Commands.parallel( + Commands.sequence( + // First note + shootInSpeaker(drivetrain, arm, shooter, null, null), + + new ArmRotateTo(arm, ArmConstants.ARM_STOW_2_DEGREES), + // Line up X to second note + new AutoDriveTo(drivetrain, new Translation2d(0.25, 0)), + new AutoRotateTo(drivetrain, new Rotation2d(0), true), + + // Pick up First note + intakeFromFloorStart(arm, shooter), + new AutoDriveTo(drivetrain, new Translation2d(2.15, 0)), + intakeFromFloorEnd(arm, shooter), + + new ArmRotateTo(arm, ArmConstants.ARM_STOW_2_DEGREES), + // Get Back + new AutoDriveTo(drivetrain, new Translation2d(-2.15, 0)), + new AutoRotateTo(drivetrain, new Rotation2d(flipped ? 60 : -60), true), + new AutoDriveTo(drivetrain, new Translation2d(0.25, 0)), + + // Shoot Second + shootInSpeaker(drivetrain, arm, shooter, null, null) + ), + + new PullHangerDown(rightHang, HangConstants.SPEED), + new PullHangerDown(leftHang, HangConstants.SPEED)); + } + + public static Command shootFromAmpSideAuto(SwerveDrivetrain drivetrain, Arm arm, IntakeShooter shooter, + Hang leftHang, Hang rightHang) { + return shootFromSideAuto(drivetrain, arm, shooter, leftHang, rightHang, false); + } + + public static Command shootFromFarSideAuto(SwerveDrivetrain drivetrain, Arm arm, IntakeShooter shooter, + Hang leftHang, Hang rightHang) { + return shootFromSideAuto(drivetrain, arm, shooter, leftHang, rightHang, true); + } + public static Command dropInAmp(SwerveDrivetrain drivetrain, Arm arm, IntakeShooter shooter, Vision vision, ChassisDriveInputs inputs) { @@ -114,9 +163,10 @@ public static Command dropInAmp(SwerveDrivetrain drivetrain, Arm arm, IntakeShoo } return Commands.sequence( - new ArmRotateTo(arm, ArmConstants.ARM_AMP_SHOOTING_DEGREES).alongWith( - new SpinFlywheelShooter(shooter, IntakeShooterConstants.FLYWHEEL_SHOOTER_SPEED_AMP), - new WaitCommand(0.5)), + Commands.parallel( + new SpinFlywheelShooterForTime(shooter, IntakeShooterConstants.FLYWHEEL_SHOOTER_SPEED_AMP, + 0.5), + new ArmRotateTo(arm, ArmConstants.ARM_AMP_SHOOTING_DEGREES)), new SpinIntakeGrabbers(shooter, IntakeShooterConstants.INTAKE_GRABBER_SPEED_AMP), new WaitCommand(0.2), new SpinFlywheelShooter(shooter, 0), diff --git a/src/main/java/frc/robot/subsystems/intake/RealShooter.java b/src/main/java/frc/robot/subsystems/intake/RealShooter.java index 5fc4e43..9dbd246 100644 --- a/src/main/java/frc/robot/subsystems/intake/RealShooter.java +++ b/src/main/java/frc/robot/subsystems/intake/RealShooter.java @@ -3,6 +3,7 @@ import com.revrobotics.CANSparkLowLevel; import com.revrobotics.CANSparkMax; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.Constants.IntakeShooterConstants; import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX; @@ -48,8 +49,11 @@ public RealShooter(int flywheel1Id, int flywheel2Id, int intakeID, int intakeLim @Override public void setFlyWheelShooterSpeed(double speed) { SmartDashboard.putNumber("FlywheelShooter", speed); - flywheelSpeed = speed; - timeFlywheelSet = System.currentTimeMillis(); + + if (flywheelSpeed != speed) { + flywheelSpeed = speed; + timeFlywheelSet = System.currentTimeMillis(); + } flywheel1.set(-speed); flywheel2.set(-speed); @@ -76,9 +80,8 @@ public void eject() { @Override public void stop() { - flywheel1.set(0); - flywheel2.set(0); - intake.set(0); + setFlyWheelShooterSpeed(0); + setIntakeGrabberSpeed(0); flywheel1.stopMotor(); flywheel2.stopMotor(); intake.stopMotor(); @@ -90,4 +93,9 @@ public double howLongAtSpeedMillis(double speed) { return -1; return System.currentTimeMillis() - timeFlywheelSet; } + + @Override + public void periodic() { + SmartDashboard.putString("FlywheelTime", flywheelSpeed + " for " + Units.millisecondsToSeconds(System.currentTimeMillis() - timeFlywheelSet)); + } }