diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index a0d5e5c..da66be3 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -256,7 +256,7 @@ public class Robot extends TimedRobot { Robot.alliance = DriverStation.getAlliance(); - m_robotContainer.m_robotSwerveDrive.m_gyro.addYaw(-1 * m_robotContainer.m_robotSwerveDrive.m_gyro.getYaw()); + // m_robotContainer.m_robotSwerveDrive.m_gyro.addYaw(-1 * m_robotContainer.m_robotSwerveDrive.m_gyro.getYaw()); selectedOdo = odoChooser.getSelected(); if (selectedOdo == null) { diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 9f46fa2..866fafb 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -212,26 +212,26 @@ public class RobotContainer { // new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()), // m_robotTurret).withName("Turret runTurretWithInput defaultCommand")); - // m_robotTurret.setDefaultCommand( - // new RunCommand(() -> { - // if (this.currentControlMode.equals(ControlMode.SHOOTER)) { - // if (this.currentTurretMode.equals(TurretMode.MANUAL)) { m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()); } - // } - // if (this.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotTurret.runTurretWithInput(0); } - // }, m_robotTurret)); + m_robotTurret.setDefaultCommand( + new RunCommand(() -> { + if (this.currentControlMode.equals(ControlMode.SHOOTER)) { + if (this.currentTurretMode.equals(TurretMode.MANUAL)) { m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()); } + } + if (this.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotTurret.runTurretWithInput(0); } + }, m_robotTurret)); - // m_robotHood.setDefaultCommand( - // new RunCommand(() -> { - // if (this.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotHood.runHood(getOperatorController().getRightY()); } - // if (this.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotHood.runHood(0); } - // }, m_robotHood)); + m_robotHood.setDefaultCommand( + new RunCommand(() -> { + if (this.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotHood.runHood(getOperatorController().getRightY()); } + if (this.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotHood.runHood(0); } + }, m_robotHood)); - // m_robotClimber.setDefaultCommand( - // // new RunCommand(() -> { - // // if (this.currentMode.equals(Mode.SHOOTER)) { m_robotClimber.setMotors(0, 0); } - // /*if (this.currentMode.equals(Mode.CLIMBER)) { */new RunCommand(() -> m_robotClimber.setMotors(getOperatorController().getLeftX(), - // getOperatorController().getRightY()), m_robotClimber).withName("Climber controlWithInput defaultCommand")); //} - // }, m_robotClimber)); + m_robotClimber.setDefaultCommand( + new RunCommand(() -> { + if (this.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotClimber.setMotors(0, 0); } + if (this.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotClimber.setJointSpeeds(getOperatorController().getLeftX() * 10000, + getOperatorController().getRightY() * 10000); }} + , m_robotClimber)); // m_robotTurret.setDefaultCommand( // new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry)); @@ -269,6 +269,12 @@ public class RobotContainer { new JoystickButton(getDriverController(), XboxController.Button.kA.value) .whileHeld(() -> m_robotSwerveDrive.driveWithInput(0.0, 0.0, 1.0, 0.0, true)) .whenReleased(() -> m_robotSwerveDrive.stopModules()); + + new JoystickButton(getDriverController(), XboxController.Button.kB.value) + .whileHeld(new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry)) + .whenReleased(new InstantCommand(() -> m_robotTurret.runTurretWithInput(0.0), m_robotTurret)); + + /* Operator Buttons */ diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java b/src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java index 38a8888..df612be 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java @@ -35,29 +35,30 @@ public class AimToCenter extends CommandBase { // Called when the command is initially scheduled. @Override public void initialize() { - x = 0; - y = 0; + x = m_drive.getOdometry().getX(); + y = m_drive.getOdometry().getY(); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { m_targetAngle = angleToCenter(x, y, m_drive.getRegGyro().getDegrees()); + System.out.println("Target Angle" + m_targetAngle); m_turret.runShooterRotatePID(m_targetAngle); // Check if limelight is within range (comment out to disable vision odo) if (Math.abs(m_turret.getBoomBoomAngleDegrees() - m_targetAngle) < VisionConstants.RANGE){ - m_visionOdometry.updateOdometryWithVision(); - m_visionOdometry.setLEDs(true); + // m_visionOdometry.updateOdometryWithVision(); + // m_visionOdometry.setLEDs(true); } else{ - m_visionOdometry.setLEDs(false); + // m_visionOdometry.setLEDs(false); } } public static double angleToCenter(double x, double y, double gyro) { - double angle = ((Math.atan2(y, x) * (180./Math.PI) - gyro) + 180. + 360.) % 360.; // Finds the angle between the gyro of the robot and the target (positive x is gyro 0) - return angle; + double angle = ((Math.atan2(y, x) * (180./Math.PI) - gyro) + 180. + (360. * 4)) % 360.; // Finds the angle between the gyro of the robot and the target (positive x is gyro 0) + return (angle - 360); } /**