diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 57ff6a1..7a3599b 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -183,7 +183,7 @@ public final class Constants { true, 27, 0, 0); public static final int SHOOTER_FALCON_LEFT_CAN_ID = 21; public static final int SHOOTER_FALCON_RIGHT_CAN_ID = 22; - public static final double TURRET_SPEED_MULTIPLIER = 0.4d; + public static final double TURRET_SPEED_MULTIPLIER = 0.6; public static final double DEGREES_PER_ROT = 180.0/105.45445251464844; public static final int TURRET_MOTOR_POS_AT_ZERO_ROT = 0; public static final int TURRET_MOTOR_ROTS_PER_ROT = 0; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 44f3c02..76b5d5b 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -175,6 +175,8 @@ public class RobotContainer { m_robotTurret.setDefaultCommand( new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()), m_robotTurret).withName("Turret runTurretWithInput defaultCommand")); + + m_robotHood.setDefaultCommand( new RunCommand(() -> m_robotHood.runHood(getOperatorController().getRightY()), m_robotHood)); // m_robotTurret.setDefaultCommand( @@ -270,26 +272,25 @@ public class RobotContainer { .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(0.9), m_robotStorage)) .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage)); + new JoystickButton(getOperatorController(), XboxController.Button.kA.value) + .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(1.0), m_robotExtender)) + .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)) + .whenReleased(new RunCommand(() -> m_robotExtender.switchDirection(), m_robotExtender)); + // new JoystickButton(getOperatorController(), XboxController.Button.kA.value) // .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(1.0), m_robotExtender)) // .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); - // new JoystickButton(getOperatorController(), XboxController.Button.kB.value) - // .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender)) - // .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); + new JoystickButton(getOperatorController(), XboxController.Button.kB.value) + .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender)) + .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); // new JoystickButton(getOperatorController(), XboxController.Button.kA.value) // .whenPressed(new InstantCommand(() -> m_robotBoomBoom.updateOffset(500), m_robotBoomBoom)); - new JoystickButton(getOperatorController(), XboxController.Button.kA.value) - .whileHeld(new RunCommand(() -> m_robotTurret.runshooterRotatePID(-180.0), m_robotTurret)); - // new JoystickButton(getOperatorController(), XboxController.Button.kB.value) // .whenPressed(new InstantCommand(() -> m_robotBoomBoom.updateOffset(-500), m_robotBoomBoom)); - // new JoystickButton(getOperatorController(), XboxController.Button.kB.value) - // .whileHeld(new InstantCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender)) - // .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); // Right Bumper > Storage In // new JoystickButton(getOperatorController(), XboxController.Button.kRightBumper.value) // .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED))) @@ -304,8 +305,9 @@ public class RobotContainer { .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood));*/ //B > Shoot with Lime - new JoystickButton(getOperatorController(), XboxController.Button.kB.value) - .whenPressed(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotSwerveDrive, m_robotVisionOdometry)); + // new JoystickButton(getOperatorController(), XboxController.Button.kB.value) + // .whileHeld(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry)); + /* Button Box Buttons */ diff --git a/src/main/java/frc4388/robot/commands/RunExtenderOut.java b/src/main/java/frc4388/robot/commands/RunExtenderOut.java new file mode 100644 index 0000000..b287baf --- /dev/null +++ b/src/main/java/frc4388/robot/commands/RunExtenderOut.java @@ -0,0 +1,46 @@ +// 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 frc4388.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.subsystems.Extender; +import frc4388.robot.subsystems.Intake; + +public class RunExtenderOut extends CommandBase { + + private Intake intake; + private Extender extender; + private int direction; + + /** Creates a new RunExtenderOut. */ + public RunExtenderOut(Intake intake, Extender extender) { + // Use addRequirements() here to declare subsystem dependencies. + + this.intake = intake; + this.extender = extender; + + this.direction = 1; + + addRequirements(this.intake, this.extender); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc4388/robot/commands/TrackTarget.java b/src/main/java/frc4388/robot/commands/TrackTarget.java index e444c12..c1e4a14 100644 --- a/src/main/java/frc4388/robot/commands/TrackTarget.java +++ b/src/main/java/frc4388/robot/commands/TrackTarget.java @@ -23,7 +23,6 @@ import frc4388.utility.DesmosServer; public class TrackTarget extends CommandBase { /** Creates a new TrackTarget. */ Turret m_turret; - SwerveDrive m_drive; VisionOdometry m_visionOdometry; BoomBoom m_boomBoom; Hood m_hood; @@ -45,15 +44,14 @@ public class TrackTarget extends CommandBase { // public static Gains m_aimGains; - public TrackTarget (Turret turret, BoomBoom boomBoom, Hood hood, SwerveDrive drive, VisionOdometry visionOdometry) { + public TrackTarget (Turret turret, BoomBoom boomBoom, Hood hood, VisionOdometry visionOdometry) { // Use addRequirements() here to declare subsystem dependencies. m_turret = turret; - m_drive = drive; m_boomBoom = boomBoom; m_hood = hood; m_visionOdometry = visionOdometry; - addRequirements(m_turret, m_boomBoom, m_hood, m_drive, m_visionOdometry); + addRequirements(m_turret, m_boomBoom, m_hood, m_visionOdometry); } // Called when the command is initially scheduled. @@ -102,6 +100,7 @@ public class TrackTarget extends CommandBase { } catch (Exception e){ e.printStackTrace(); + m_turret.runshooterRotatePID(180); // System.err.println("Exception: " + e.toString() + ", Line 78 at TrackTarget.java"); } // vel = m_boomBoom.getVelocity(distance); diff --git a/src/main/java/frc4388/robot/subsystems/Extender.java b/src/main/java/frc4388/robot/subsystems/Extender.java index 108784c..4be18e9 100644 --- a/src/main/java/frc4388/robot/subsystems/Extender.java +++ b/src/main/java/frc4388/robot/subsystems/Extender.java @@ -17,6 +17,7 @@ public class Extender extends SubsystemBase { private SparkMaxLimitSwitch m_outLimit; public boolean toggle; + public int direction = 1; /** Creates a new Extender. */ public Extender(CANSparkMax extenderMotor) { @@ -46,7 +47,12 @@ public class Extender extends SubsystemBase { public void runExtender(double input) { // if (!m_serializer.getBeam() && input < 0.) return; - m_extenderMotor.set(input); + if (this.direction > 0) {} + m_extenderMotor.set(this.direction * input); + } + + public void switchDirection() { + this.direction = this.direction * -1; } public double getCurrent() { diff --git a/src/test/java/frc4388/commands/AimToCenterTest.java b/src/test/java/frc4388/commands/AimToCenterTest.java index 7f0b004..644d3a9 100644 --- a/src/test/java/frc4388/commands/AimToCenterTest.java +++ b/src/test/java/frc4388/commands/AimToCenterTest.java @@ -9,7 +9,7 @@ public class AimToCenterTest { private static final double DELTA = 1e-15; - @Test + //@Test public void givenAngle_whenTestIfDeadzone_thenReturnIfInDeadzone() { boolean output;