diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index ee6c645f..2e8d4b57 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -94,6 +94,7 @@ public void teleopInit() { } getAlliance(); m_robotContainer.updatePose(); + m_robotContainer.dropLauncher(); } /** This function is called periodically during operator control. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index fab5e63c..b669deb3 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -155,4 +155,8 @@ public void setMotorBrake(boolean brake) { public void updatePose() { m_swerve.updatePositionCommand(); } + + public void dropLauncher(){ + m_launch.releaseLauncher(); + } } diff --git a/src/main/java/frc/robot/subsystems/Launcher.java b/src/main/java/frc/robot/subsystems/Launcher.java index d3100a07..3d5a1eb2 100644 --- a/src/main/java/frc/robot/subsystems/Launcher.java +++ b/src/main/java/frc/robot/subsystems/Launcher.java @@ -33,7 +33,7 @@ public class Launcher extends SubsystemBase { private RelativeEncoder upperLauncherEncoder; private RelativeEncoder lowerLauncherEncoder; - private CANSparkMax angle = new CANSparkMax(Constants.Launch.angleID, MotorType.kBrushless); + private CANSparkFlex angle = new CANSparkFlex(Constants.Launch.angleID, MotorType.kBrushless); private SparkPIDController angleController; private SparkAbsoluteEncoder angleEncoder; @@ -46,7 +46,7 @@ public Launcher() { angle.setInverted(Constants.Launch.angleMotorInverted); upperLauncher.setInverted(Constants.Launch.upperMotorInverted); lowerLauncher.setInverted(Constants.Launch.lowerMotorInverted); - angle.setIdleMode(IdleMode.kCoast); + angle.setIdleMode(IdleMode.kBrake); angleController = angle.getPIDController(); upperLauncherController = upperLauncher.getPIDController(); @@ -207,6 +207,10 @@ public void periodic() { SmartDashboard.putNumber("Angle Position", angleEncoder.getPosition()); } + public void releaseLauncher(){ + angle.setIdleMode(IdleMode.kCoast); + } + public Command switchAngleCommand() { return this.runOnce(() -> switchAngle()); }