diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 29e40d7..c4e5917 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -74,7 +74,7 @@ public final class Constants { /* Motor IDs */ public static final int SHOOTER_FALCON_ID = -1; public static final int SHOOTER_ANGLE_ADJUST_ID = -1; - public static final int SHOOTER_ROTATE_ID = -1; + public static final int SHOOTER_ROTATE_ID = 10; /* PID Constants Shooter */ public static final int SHOOTER_SLOT_IDX = 0; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 8338a46..50cedc1 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -83,7 +83,6 @@ public class RobotContainer { m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); // runs the drum shooter in idle mode - m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooter.runShooterWithInput(m_operatorXbox.getLeftXAxis()), m_robotShooter)); // drives the leveler with an axis input from the driver controller // m_robotLeveler.setDefaultCommand(new RunLevelerWithJoystick(m_robotLeveler, getDriverController())); @@ -104,9 +103,9 @@ public class RobotContainer { /* Operator Buttons */ // activates "Lit Mode" - new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) - .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW)) - .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN)); + //new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) + // .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW)) + // .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN)); new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) .whileHeld(new ShooterVelocityControlPID(m_robotShooter, 4000)); @@ -154,7 +153,7 @@ public class RobotContainer { /* Storage Neo PID Test */ new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) - .whileHeld(new RunCommand(() -> m_robotStorage.runStoragePositionPID(0.5, 0.2, 0.0, 0.0, 0.0, 0.0, 1, -1))); + .whileHeld(new TrackTarget(m_robotShooter)); } /** diff --git a/src/main/java/frc4388/robot/commands/TrackTarget.java b/src/main/java/frc4388/robot/commands/TrackTarget.java index 7772060..2853242 100644 --- a/src/main/java/frc4388/robot/commands/TrackTarget.java +++ b/src/main/java/frc4388/robot/commands/TrackTarget.java @@ -52,7 +52,7 @@ public class TrackTarget extends CommandBase { target = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tv").getDouble(0); xAngle = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tx").getDouble(0); yAngle = NetworkTableInstance.getDefault().getTable("limelight").getEntry("ty").getDouble(0); - + if (target == 1.0){ //If target in view //Aiming Left/Right turnAmount = (xAngle/VisionConstants.FOV)*VisionConstants.TURN_P_VALUE; @@ -60,7 +60,7 @@ public class TrackTarget extends CommandBase { //Deadzones else if(turnAmount > 0 && turnAmount < VisionConstants.MOTOR_DEAD_ZONE){turnAmount = VisionConstants.MOTOR_DEAD_ZONE;} else if(turnAmount < 0 && turnAmount > -VisionConstants.MOTOR_DEAD_ZONE){turnAmount = -VisionConstants.MOTOR_DEAD_ZONE;} - m_shooter.runShooterWithInput(turnAmount/2); + m_shooter.runShooterWithInput(turnAmount/5); //Finding Distance distance = VisionConstants.TARGET_HEIGHT/Math.tan((VisionConstants.LIME_ANGLE + yAngle)*(Math.PI/180));