diff --git a/src/main/java/frc/robot/commands/states/Shooting.java b/src/main/java/frc/robot/commands/states/Shooting.java new file mode 100644 index 0000000..e9943dc --- /dev/null +++ b/src/main/java/frc/robot/commands/states/Shooting.java @@ -0,0 +1,54 @@ +// 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 frc.robot.commands.states; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.RobotContainer; +import frc.robot.constants.ConstMotion; +import frc.robot.constants.ConstRotors; + +public class Shooting extends Command { + /** Creates a new Shooting. */ + public Shooting() { + + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + RobotContainer.motionInstance.setHoodPivotAngle(ConstMotion.HOOD_PIVOT_ANGLE); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + + RobotContainer.rotorsInstance.setFlywheelSpeeds(ConstRotors.FLYWHEEL_SHOOTING_SPEED); + if (RobotContainer.rotorsInstance.isFlyWheelAtSpeed(ConstRotors.FLYWHEEL_SHOOTING_SPEED)) { + RobotContainer.rotorsInstance.setTransferRollersSpeeds(ConstRotors.INTAKE_TRANSFER_SPEED); + RobotContainer.rotorsInstance.setSerializerRollersSpeed(ConstRotors.SERIALIZER_SHOOTING_SPEED); + + } else { + RobotContainer.rotorsInstance.setTransferRollersSpeeds(ConstRotors.STOP_ALL); + } + + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + RobotContainer.rotorsInstance.setFlywheelSpeeds(ConstRotors.STOP_ALL); + RobotContainer.rotorsInstance.setTransferRollersSpeeds(ConstRotors.STOP_ALL); + RobotContainer.rotorsInstance.setSerializerRollersSpeed(ConstRotors.STOP_ALL); + + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/constants/ConstMotion.java b/src/main/java/frc/robot/constants/ConstMotion.java index c01f0b1..f16135c 100644 --- a/src/main/java/frc/robot/constants/ConstMotion.java +++ b/src/main/java/frc/robot/constants/ConstMotion.java @@ -4,11 +4,16 @@ package frc.robot.constants; +import static edu.wpi.first.units.Units.Degrees; + import com.ctre.phoenix6.configs.TalonFXConfiguration; +import edu.wpi.first.units.measure.Angle; + /** Add your docs here. */ public class ConstMotion { public static final double STOP = 0; public static final TalonFXConfiguration INTAKE_PIVOT_CONFIGURATION = new TalonFXConfiguration(); public static final TalonFXConfiguration HOOD_PIVOT_CONFIGURATION = new TalonFXConfiguration(); + public static final Angle HOOD_PIVOT_ANGLE = Degrees.of(14); } diff --git a/src/main/java/frc/robot/constants/ConstRotors.java b/src/main/java/frc/robot/constants/ConstRotors.java index 88117cc..32f527b 100644 --- a/src/main/java/frc/robot/constants/ConstRotors.java +++ b/src/main/java/frc/robot/constants/ConstRotors.java @@ -9,6 +9,9 @@ import com.ctre.phoenix6.signals.NeutralModeValue; import com.fasterxml.jackson.core.filter.TokenFilter.Inclusion; +import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.AngularVelocity; + /** Add your docs here. */ public class ConstRotors { @@ -19,6 +22,11 @@ public class ConstRotors { public static final TalonFXConfiguration TRANSFER_ROLLERS_WEST_CONFIGURATION = new TalonFXConfiguration(); public static final TalonFXConfiguration FLYWHEEL_EAST_CONFIGURATION = new TalonFXConfiguration(); public static final TalonFXConfiguration FLYWHEEL_WEST_CONFIGURATION = new TalonFXConfiguration(); + public static final AngularVelocity FLYWHEEL_SHOOTING_SPEED = Units.RPM.of(1000); + public static final AngularVelocity INTAKE_ROLLER_SPEED = Units.RPM.of(2000); + public static final AngularVelocity INTAKE_TRANSFER_SPEED = Units.RPM.of(3000); + public static final AngularVelocity SERIALIZER_SHOOTING_SPEED = Units.RPM.of(4000); + public static final AngularVelocity STOP_ALL = Units.RPM.of(0); static { diff --git a/src/main/java/frc/robot/subsystems/Rotors.java b/src/main/java/frc/robot/subsystems/Rotors.java index f62af36..8bc1e45 100644 --- a/src/main/java/frc/robot/subsystems/Rotors.java +++ b/src/main/java/frc/robot/subsystems/Rotors.java @@ -33,6 +33,9 @@ public class Rotors extends SubsystemBase { Follower flywheelWestFollower = new Follower(flywheelTopEast.getDeviceID(), true); Follower transferRollersEastFollower = new Follower(intakeRollersEast.getDeviceID(), true); Follower intakeRollerEastFollower = new Follower(intakeRollersEast.getDeviceID(), true); + final MotionMagicVelocityVoltage flyWheelVelocityRequest = new MotionMagicVelocityVoltage(0); + final MotionMagicVelocityVoltage transferRollersVelocityRequest = new MotionMagicVelocityVoltage(0); + final MotionMagicVelocityVoltage serializerVelocityRequest = new MotionMagicVelocityVoltage(0); private boolean flyWheelAtSpeed = false; // private boolean intakeRollersAtSpeed = false;/ @@ -50,7 +53,6 @@ public Rotors() { // final MotionMagicVelocityVoltage TransferVelocityRequest = new // MotionMagicVelocityVoltage(0);/ - final MotionMagicVelocityVoltage flyWheelVelocityRequest = new MotionMagicVelocityVoltage(0); public AngularVelocity getFlyWheelSpeeds() { if (Robot.isSimulation()) { @@ -71,8 +73,8 @@ public AngularVelocity getTransferRollersVelocity() { return transferRollersEast.getVelocity().getValue(); } - public void setSerializerRollersSpeed(double speed) { - serializerRollers.set(speed); + public void setSerializerRollersSpeed(AngularVelocity speed) { + serializerRollers.setControl(serializerVelocityRequest.withVelocity(speed)); } public void setIntakeRollersSpeeds(double speed) { @@ -80,8 +82,8 @@ public void setIntakeRollersSpeeds(double speed) { intakeRollersWest.setControl(intakeRollerEastFollower); } - public void setTransferRollersSpeeds(double speed) { - transferRollersEast.set(speed); + public void setTransferRollersSpeeds(AngularVelocity speed) { + transferRollersEast.setControl(transferRollersVelocityRequest.withVelocity(speed)); transferRollersWest.setControl(transferRollersEastFollower); }