diff --git a/src/main/java/frc/robot/robot_manager/RobotManager.java b/src/main/java/frc/robot/robot_manager/RobotManager.java index 693dde69..b9ed085e 100644 --- a/src/main/java/frc/robot/robot_manager/RobotManager.java +++ b/src/main/java/frc/robot/robot_manager/RobotManager.java @@ -386,72 +386,72 @@ public void robotPeriodic() { case UNHOMED: shoulder.startPreMatchHoming(); intake.idleRequest(); - shooter.setMode(ShooterMode.IDLE); + shooter.setGoalMode(ShooterMode.IDLE); break; case HOMING: shoulder.startMidMatchHoming(); intake.idleRequest(); - shooter.setMode(ShooterMode.IDLE); + shooter.setGoalMode(ShooterMode.IDLE); break; case IDLE_UP_NO_GP: shoulder.setAngle(ShoulderPositions.STOWED_UP); intake.idleNoGPRequest(); - shooter.setMode(ShooterMode.IDLE); + shooter.setGoalMode(ShooterMode.IDLE); climber.setGoal(ClimberMode.IDLE); break; case IDLE_UP_WITH_GP: shoulder.setAngle(ShoulderPositions.STOWED_UP); intake.idleWithGPRequest(); - shooter.setMode(ShooterMode.IDLE); + shooter.setGoalMode(ShooterMode.IDLE); climber.setGoal(ClimberMode.IDLE); break; case IDLE_DOWN_NO_GP: shoulder.setAngle(ShoulderPositions.STOWED_DOWN); intake.idleNoGPRequest(); - shooter.setMode(ShooterMode.IDLE); + shooter.setGoalMode(ShooterMode.IDLE); climber.setGoal(ClimberMode.IDLE); break; case IDLE_DOWN_WITH_GP: shoulder.setAngle(ShoulderPositions.STOWED_DOWN); intake.idleWithGPRequest(); - shooter.setMode(ShooterMode.IDLE); + shooter.setGoalMode(ShooterMode.IDLE); climber.setGoal(ClimberMode.IDLE); break; case GROUND_INTAKING: shoulder.setAngle(ShoulderPositions.GROUND_INTAKING); intake.intakingRequest(); - shooter.setMode(ShooterMode.INTAKING); + shooter.setGoalMode(ShooterMode.INTAKING); climber.setGoal(ClimberMode.IDLE); break; case GROUND_INTAKING_SETTLING: shoulder.setAngle(ShoulderPositions.STOWED_DOWN); intake.intakingRequest(); - shooter.setMode(ShooterMode.INTAKING); + shooter.setGoalMode(ShooterMode.INTAKING); climber.setGoal(ClimberMode.IDLE); break; case SOURCE_INTAKING: shoulder.setAngle(ShoulderPositions.SOURCE_INTAKING); intake.intakingRequest(); - shooter.setMode(ShooterMode.INTAKING); + shooter.setGoalMode(ShooterMode.INTAKING); climber.setGoal(ClimberMode.IDLE); break; case SOURCE_INTAKING_SETTLING: shoulder.setAngle(ShoulderPositions.STOWED_UP); intake.intakingRequest(); - shooter.setMode(ShooterMode.INTAKING); + shooter.setGoalMode(ShooterMode.INTAKING); climber.setGoal(ClimberMode.IDLE); break; case OUTTAKING: shoulder.setAngle(ShoulderPositions.OUTTAKING); intake.outtakingRequest(); - shooter.setMode(ShooterMode.IDLE); + shooter.setGoalMode(ShooterMode.IDLE); climber.setGoal(ClimberMode.IDLE); break; case WAITING_FLOOR_SHOT: case PREPARE_FLOOR_SHOT: shoulder.setAngle(shoulderAngleForFloorSpot); intake.idleRequest(); - shooter.setMode(ShooterMode.FLOOR_SHOT); + shooter.setGoalMode(ShooterMode.FLOOR_SHOT); climber.setGoal(ClimberMode.IDLE); snaps.setAngle(robotAngleToFloorSpot); snaps.setEnabled(true); @@ -460,7 +460,7 @@ public void robotPeriodic() { case FLOOR_SHOOT: shoulder.setAngle(shoulderAngleForFloorSpot); intake.shootingRequest(); - shooter.setMode(ShooterMode.FLOOR_SHOT); + shooter.setGoalMode(ShooterMode.FLOOR_SHOT); climber.setGoal(ClimberMode.IDLE); snaps.setAngle(robotAngleToFloorSpot); snaps.setEnabled(true); @@ -470,20 +470,20 @@ public void robotPeriodic() { case PREPARE_SUBWOOFER_SHOT: shoulder.setAngle(ShoulderPositions.SUBWOOFER_SHOT); intake.idleRequest(); - shooter.setMode(ShooterMode.SUBWOOFER_SHOT); + shooter.setGoalMode(ShooterMode.SUBWOOFER_SHOT); climber.setGoal(ClimberMode.IDLE); break; case SUBWOOFER_SHOOT: shoulder.setAngle(ShoulderPositions.SUBWOOFER_SHOT); intake.shootingRequest(); - shooter.setMode(ShooterMode.SUBWOOFER_SHOT); + shooter.setGoalMode(ShooterMode.SUBWOOFER_SHOT); climber.setGoal(ClimberMode.IDLE); break; case WAITING_SPEAKER_SHOT: case PREPARE_SPEAKER_SHOT: shoulder.setAngle(shoulderAngleForSpeaker); intake.idleRequest(); - shooter.setMode(ShooterMode.SPEAKER_SHOT); + shooter.setGoalMode(ShooterMode.SPEAKER_SHOT); climber.setGoal(ClimberMode.IDLE); snaps.setAngle(robotAngleToSpeaker); snaps.setEnabled(true); @@ -492,7 +492,7 @@ public void robotPeriodic() { case SPEAKER_SHOOT: shoulder.setAngle(shoulderAngleForSpeaker); intake.shootingRequest(); - shooter.setMode(ShooterMode.SPEAKER_SHOT); + shooter.setGoalMode(ShooterMode.SPEAKER_SHOT); climber.setGoal(ClimberMode.IDLE); snaps.setAngle(robotAngleToSpeaker); snaps.setEnabled(true); @@ -502,44 +502,44 @@ public void robotPeriodic() { case PREPARE_AMP_SHOT: shoulder.setAngle(ShoulderPositions.AMP_SHOT); intake.idleRequest(); - shooter.setMode(ShooterMode.IDLE); + shooter.setGoalMode(ShooterMode.IDLE); climber.setGoal(ClimberMode.IDLE); break; case AMP_SHOOT: shoulder.setAngle(ShoulderPositions.AMP_SHOT); intake.shootingRequest(); - shooter.setMode(ShooterMode.IDLE); + shooter.setGoalMode(ShooterMode.IDLE); climber.setGoal(ClimberMode.IDLE); break; case WAITING_CLIMBER_RAISED: shoulder.setAngle(ShoulderPositions.WAITING_CLIMBER_RAISED); intake.idleRequest(); - shooter.setMode(ShooterMode.IDLE); + shooter.setGoalMode(ShooterMode.IDLE); climber.setGoal(ClimberMode.RAISED); break; case PREPARE_CLIMBER_RAISED: case CLIMBER_RAISED: shoulder.setAngle(ShoulderPositions.TRAP_SHOT); intake.idleRequest(); - shooter.setMode(ShooterMode.IDLE); + shooter.setGoalMode(ShooterMode.IDLE); climber.setGoal(ClimberMode.RAISED); break; case PREPARE_CLIMBER_HANGING: shoulder.setAngle(ShoulderPositions.TRAP_SHOT); intake.climbingRequest(); - shooter.setMode(ShooterMode.IDLE); + shooter.setGoalMode(ShooterMode.IDLE); climber.setGoal(ClimberMode.HANGING); break; case CLIMBER_HANGING: shoulder.setAngle(ShoulderPositions.TRAP_SHOT); intake.climbingRequest(); - shooter.setMode(ShooterMode.IDLE); + shooter.setGoalMode(ShooterMode.IDLE); climber.setGoal(ClimberMode.HANGING); break; case TRAP_SHOOT: shoulder.setAngle(ShoulderPositions.TRAP_SHOT); intake.trapOuttakeRequest(); - shooter.setMode(ShooterMode.IDLE); + shooter.setGoalMode(ShooterMode.IDLE); climber.setGoal(ClimberMode.HANGING); break; default: diff --git a/src/main/java/frc/robot/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/shooter/ShooterSubsystem.java index da424469..facc2199 100644 --- a/src/main/java/frc/robot/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/shooter/ShooterSubsystem.java @@ -41,8 +41,8 @@ public class ShooterSubsystem extends LifecycleSubsystem { public ShooterSubsystem(TalonFX leftMotor, TalonFX rightMotor) { super(SubsystemPriority.SHOOTER); - this.rightMotor = leftMotor; - this.leftMotor = rightMotor; + this.leftMotor = leftMotor; + this.rightMotor = rightMotor; RobotConfig.get().shooter().speakerShotRpms().accept(speakerDistanceToRPM); RobotConfig.get().shooter().floorShotRpms().accept(floorSpotDistanceToRPM); @@ -97,23 +97,23 @@ public void robotPeriodic() { Logger.recordOutput("Shooter/DistanceToFloorSpot", floorSpotDistance); Logger.recordOutput("Shooter/Mode", goalMode); Logger.recordOutput("Shooter/GoalRPM", goalRPM); - Logger.recordOutput("Shooter/TopMotor/Temperature", leftMotor.getDeviceTemp().getValue()); - Logger.recordOutput("Shooter/TopMotor/RPM", getRPM(leftMotor)); + Logger.recordOutput("Shooter/LeftMotor/Temperature", leftMotor.getDeviceTemp().getValue()); + Logger.recordOutput("Shooter/LeftMotor/RPM", getRPM(leftMotor)); Logger.recordOutput( - "Shooter/TopMotor/POutput", leftMotor.getClosedLoopProportionalOutput().getValue()); + "Shooter/LeftMotor/POutput", leftMotor.getClosedLoopProportionalOutput().getValue()); Logger.recordOutput( - "Shooter/TopMotor/SupplierCurrent", leftMotor.getSupplyCurrent().getValueAsDouble()); - Logger.recordOutput("Shooter/TopMotor/StatorCurrent", leftMotor.getStatorCurrent().getValue()); - Logger.recordOutput("Shooter/TopMotor/Voltage", leftMotor.getMotorVoltage().getValue()); + "Shooter/LeftMotor/SupplierCurrent", leftMotor.getSupplyCurrent().getValueAsDouble()); + Logger.recordOutput("Shooter/LeftMotor/StatorCurrent", leftMotor.getStatorCurrent().getValue()); + Logger.recordOutput("Shooter/LeftMotor/Voltage", leftMotor.getMotorVoltage().getValue()); Logger.recordOutput( - "Shooter/BottomMotor/StatorCurrent", rightMotor.getStatorCurrent().getValue()); - Logger.recordOutput("Shooter/BottomMotor/Voltage", rightMotor.getMotorVoltage().getValue()); - Logger.recordOutput("Shooter/BottomMotor/RPM", getRPM(rightMotor)); - Logger.recordOutput("Shooter/BottomMotor/Temperature", rightMotor.getDeviceTemp().getValue()); + "Shooter/RightMotor/StatorCurrent", rightMotor.getStatorCurrent().getValue()); + Logger.recordOutput("Shooter/RightMotor/Voltage", rightMotor.getMotorVoltage().getValue()); + Logger.recordOutput("Shooter/RightMotor/RPM", getRPM(rightMotor)); + Logger.recordOutput("Shooter/RightMotor/Temperature", rightMotor.getDeviceTemp().getValue()); Logger.recordOutput( - "Shooter/BottomMotor/POutput", rightMotor.getClosedLoopProportionalOutput().getValue()); + "Shooter/RightMotor/POutput", rightMotor.getClosedLoopProportionalOutput().getValue()); Logger.recordOutput( - "Shooter/BottomMotor/SupplierCurrent", rightMotor.getSupplyCurrent().getValueAsDouble()); + "Shooter/RightMotor/SupplierCurrent", rightMotor.getSupplyCurrent().getValueAsDouble()); Logger.recordOutput("Shooter/AtGoal", atGoal(goalMode)); } @@ -138,10 +138,14 @@ private double getRPM(TalonFX motor) { return motor.getVelocity().getValue() * 60.0; } - public void setMode(ShooterMode newMode) { + public void setGoalMode(ShooterMode newMode) { goalMode = newMode; } + public ShooterMode getGoalMode() { + return goalMode; + } + public void setSpeakerDistance(double distance) { speakerDistance = distance; }