diff --git a/src/main/java/frc/robot/DeviceIDs.java b/src/main/java/frc/robot/DeviceIDs.java index 2e05873..387d98b 100644 --- a/src/main/java/frc/robot/DeviceIDs.java +++ b/src/main/java/frc/robot/DeviceIDs.java @@ -38,7 +38,15 @@ public static class drivetrainIDs { // Rotor and Motion IDs: 10~29 public static class rotorIDs { - + public static final int SERIALIZER_ROLLERS_CAN = 10; + public static final int INTAKE_ROLLERS_WEST_CAN = 11; + public static final int INTAKE_ROLLERS_EAST_CAN = 18; + public static final int TRANSFER_ROLLERS_WEST_CAN = 13; + public static final int TRANSFER_ROLLERS_EAST_CAN = 12; + public static final int FLYWHEEL_TOP_WEST_CAN = 15; + public static final int FLYWHEEL_TOP_EAST_CAN = 14; + public static final int FLYWHEEL_BOTTOM_WEST_CAN = 17; + public static final int FLYWHEEL_BOTTOM_EAST_CAN = 16; } // Motion Profile IDs: 30~49 diff --git a/src/main/java/frc/robot/constants/ConstRotors.java b/src/main/java/frc/robot/constants/ConstRotors.java index 673cac9..88117cc 100644 --- a/src/main/java/frc/robot/constants/ConstRotors.java +++ b/src/main/java/frc/robot/constants/ConstRotors.java @@ -4,7 +4,64 @@ package frc.robot.constants; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.fasterxml.jackson.core.filter.TokenFilter.Inclusion; + /** Add your docs here. */ public class ConstRotors { + + public static final TalonFXConfiguration SERIALIZER_ROLLERS_CONFIGURATION = new TalonFXConfiguration(); + public static final TalonFXConfiguration INTAKE_ROLLERS_EAST_CONFIGURATION = new TalonFXConfiguration(); + public static final TalonFXConfiguration INTAKE_ROLLERS_WEST_CONFIGURATION = new TalonFXConfiguration(); + public static final TalonFXConfiguration TRANSFER_ROLLERS_EAST_CONFIGURATION = new TalonFXConfiguration(); + 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(); + + static { + + // SHOOTER_TRANSFER_EAST_CONFIGURATION.MotorOutput.NeutralMode = + // NeutralModeValue.Coast;/ + + FLYWHEEL_WEST_CONFIGURATION.MotorOutput.NeutralMode = NeutralModeValue.Coast; + FLYWHEEL_WEST_CONFIGURATION.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + FLYWHEEL_WEST_CONFIGURATION.CurrentLimits.SupplyCurrentLimitEnable = true; + FLYWHEEL_WEST_CONFIGURATION.CurrentLimits.SupplyCurrentLowerLimit = 35; + + FLYWHEEL_EAST_CONFIGURATION.MotorOutput.NeutralMode = NeutralModeValue.Coast; + FLYWHEEL_EAST_CONFIGURATION.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + FLYWHEEL_EAST_CONFIGURATION.CurrentLimits.SupplyCurrentLimitEnable = true; + FLYWHEEL_EAST_CONFIGURATION.CurrentLimits.SupplyCurrentLowerLimit = 35; + + SERIALIZER_ROLLERS_CONFIGURATION.MotorOutput.NeutralMode = NeutralModeValue.Coast; + SERIALIZER_ROLLERS_CONFIGURATION.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + SERIALIZER_ROLLERS_CONFIGURATION.CurrentLimits.SupplyCurrentLimitEnable = true; + SERIALIZER_ROLLERS_CONFIGURATION.CurrentLimits.SupplyCurrentLowerLimit = 35; + + INTAKE_ROLLERS_EAST_CONFIGURATION.MotorOutput.NeutralMode = NeutralModeValue.Coast; + INTAKE_ROLLERS_EAST_CONFIGURATION.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + INTAKE_ROLLERS_EAST_CONFIGURATION.CurrentLimits.SupplyCurrentLimitEnable = true; + INTAKE_ROLLERS_EAST_CONFIGURATION.CurrentLimits.SupplyCurrentLowerLimit = 35; + + INTAKE_ROLLERS_WEST_CONFIGURATION.MotorOutput.NeutralMode = NeutralModeValue.Coast; + INTAKE_ROLLERS_WEST_CONFIGURATION.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + INTAKE_ROLLERS_WEST_CONFIGURATION.CurrentLimits.SupplyCurrentLimitEnable = true; + INTAKE_ROLLERS_WEST_CONFIGURATION.CurrentLimits.SupplyCurrentLowerLimit = 35; + + TRANSFER_ROLLERS_EAST_CONFIGURATION.MotorOutput.NeutralMode = NeutralModeValue.Coast; + TRANSFER_ROLLERS_EAST_CONFIGURATION.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + TRANSFER_ROLLERS_EAST_CONFIGURATION.CurrentLimits.SupplyCurrentLimitEnable = true; + TRANSFER_ROLLERS_EAST_CONFIGURATION.CurrentLimits.SupplyCurrentLowerLimit = 35; + + TRANSFER_ROLLERS_WEST_CONFIGURATION.MotorOutput.NeutralMode = NeutralModeValue.Coast; + TRANSFER_ROLLERS_WEST_CONFIGURATION.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + TRANSFER_ROLLERS_WEST_CONFIGURATION.CurrentLimits.SupplyCurrentLimitEnable = true; + TRANSFER_ROLLERS_WEST_CONFIGURATION.CurrentLimits.SupplyCurrentLowerLimit = 35; + + } + public static final double STOP = 0; + } diff --git a/src/main/java/frc/robot/subsystems/Rotors.java b/src/main/java/frc/robot/subsystems/Rotors.java index fc2fd8f..f62af36 100644 --- a/src/main/java/frc/robot/subsystems/Rotors.java +++ b/src/main/java/frc/robot/subsystems/Rotors.java @@ -4,17 +4,108 @@ package frc.robot.subsystems; +import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.controls.MotionMagicVelocityVoltage; +import com.ctre.phoenix6.hardware.TalonFX; import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.DeviceIDs.rotorIDs; +import frc.robot.Robot; +import frc.robot.constants.ConstRotors; @Logged public class Rotors extends SubsystemBase { - /** Creates a new Rotors. */ + + final TalonFX serializerRollers = new TalonFX(rotorIDs.SERIALIZER_ROLLERS_CAN); + final TalonFX intakeRollersWest = new TalonFX(rotorIDs.INTAKE_ROLLERS_WEST_CAN); + final TalonFX intakeRollersEast = new TalonFX(rotorIDs.INTAKE_ROLLERS_EAST_CAN); + final TalonFX transferRollersWest = new TalonFX((rotorIDs.TRANSFER_ROLLERS_WEST_CAN)); + final TalonFX transferRollersEast = new TalonFX((rotorIDs.TRANSFER_ROLLERS_EAST_CAN)); + final TalonFX flywheelTopWest = new TalonFX((rotorIDs.FLYWHEEL_TOP_WEST_CAN)); + final TalonFX flywheelTopEast = new TalonFX((rotorIDs.FLYWHEEL_TOP_EAST_CAN)); + final TalonFX flywheelBottomWest = new TalonFX((rotorIDs.FLYWHEEL_BOTTOM_WEST_CAN)); + final TalonFX flywheelBottomEast = new TalonFX((rotorIDs.FLYWHEEL_BOTTOM_EAST_CAN)); + AngularVelocity lastDesiredFlyWheelSpeed = Units.RPM.of(0); + AngularVelocity lastDesiredTransferRollersSpeed = Units.RPM.of(0); + Follower flywheelEastFollower = new Follower(flywheelTopEast.getDeviceID(), false); + Follower flywheelWestFollower = new Follower(flywheelTopEast.getDeviceID(), true); + Follower transferRollersEastFollower = new Follower(intakeRollersEast.getDeviceID(), true); + Follower intakeRollerEastFollower = new Follower(intakeRollersEast.getDeviceID(), true); + private boolean flyWheelAtSpeed = false; + // private boolean intakeRollersAtSpeed = false;/ + public Rotors() { + serializerRollers.getConfigurator().apply(ConstRotors.SERIALIZER_ROLLERS_CONFIGURATION); + intakeRollersEast.getConfigurator().apply(ConstRotors.INTAKE_ROLLERS_EAST_CONFIGURATION); + intakeRollersWest.getConfigurator().apply(ConstRotors.INTAKE_ROLLERS_WEST_CONFIGURATION); + transferRollersEast.getConfigurator().apply(ConstRotors.TRANSFER_ROLLERS_EAST_CONFIGURATION); + transferRollersWest.getConfigurator().apply(ConstRotors.TRANSFER_ROLLERS_WEST_CONFIGURATION); + flywheelTopEast.getConfigurator().apply(ConstRotors.FLYWHEEL_EAST_CONFIGURATION); + flywheelTopWest.getConfigurator().apply(ConstRotors.FLYWHEEL_WEST_CONFIGURATION); + flywheelBottomEast.getConfigurator().apply(ConstRotors.FLYWHEEL_EAST_CONFIGURATION); + flywheelBottomWest.getConfigurator().apply(ConstRotors.FLYWHEEL_WEST_CONFIGURATION); + } + + // final MotionMagicVelocityVoltage TransferVelocityRequest = new + // MotionMagicVelocityVoltage(0);/ + final MotionMagicVelocityVoltage flyWheelVelocityRequest = new MotionMagicVelocityVoltage(0); + + public AngularVelocity getFlyWheelSpeeds() { + if (Robot.isSimulation()) { + return lastDesiredFlyWheelSpeed; + } + return flywheelTopEast.getVelocity().getValue(); + } + + public AngularVelocity getSerializerRollersVelocity() { + return serializerRollers.getVelocity().getValue(); + } + + public AngularVelocity getIntakeRollersVelocity() { + return intakeRollersEast.getVelocity().getValue(); + } + + public AngularVelocity getTransferRollersVelocity() { + return transferRollersEast.getVelocity().getValue(); + } + + public void setSerializerRollersSpeed(double speed) { + serializerRollers.set(speed); + } + + public void setIntakeRollersSpeeds(double speed) { + intakeRollersEast.set(speed); + intakeRollersWest.setControl(intakeRollerEastFollower); + } + + public void setTransferRollersSpeeds(double speed) { + transferRollersEast.set(speed); + transferRollersWest.setControl(transferRollersEastFollower); + } + + public void setFlywheelSpeeds(AngularVelocity speed) { + flywheelTopEast.setControl(flyWheelVelocityRequest.withVelocity(speed)); + flywheelTopWest.setControl(flywheelWestFollower); + flywheelBottomWest.setControl(flywheelWestFollower); + flywheelBottomEast.setControl(flywheelEastFollower); + lastDesiredFlyWheelSpeed = speed; + } + + public boolean isFlyWheelAtSpeed(AngularVelocity tolerance) { + AngularVelocity lowerLim = lastDesiredFlyWheelSpeed.minus(tolerance); + AngularVelocity upperLim = lastDesiredFlyWheelSpeed.plus(tolerance); + + AngularVelocity flyWheelSpeed = getFlyWheelSpeeds(); + + flyWheelAtSpeed = flyWheelSpeed.gte(lowerLim) + && flyWheelSpeed.lte(upperLim); + return flyWheelAtSpeed; + } - @Override public void periodic() { - // This method will be called once per scheduler run + } }