diff --git a/src/main/java/frc/robot/constants/ConstRotors.java b/src/main/java/frc/robot/constants/ConstRotors.java index 64c3624..b0997ef 100644 --- a/src/main/java/frc/robot/constants/ConstRotors.java +++ b/src/main/java/frc/robot/constants/ConstRotors.java @@ -96,7 +96,13 @@ public class ConstRotors { TRANSFER_ROLLERS_WEST_CONFIGURATION.MotorOutput.NeutralMode = NeutralModeValue.Coast; TRANSFER_ROLLERS_WEST_CONFIGURATION.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; - + TRANSFER_ROLLERS_WEST_CONFIGURATION.Slot0.kP = 0.7; + TRANSFER_ROLLERS_WEST_CONFIGURATION.Slot0.kS = 0.15; + TRANSFER_ROLLERS_WEST_CONFIGURATION.Slot0.kV = 0.12; + TRANSFER_ROLLERS_WEST_CONFIGURATION.Slot0.kA = 0; + TRANSFER_ROLLERS_WEST_CONFIGURATION.MotionMagic.MotionMagicCruiseVelocity = 0; + TRANSFER_ROLLERS_WEST_CONFIGURATION.MotionMagic.MotionMagicAcceleration = 9999; + TRANSFER_ROLLERS_WEST_CONFIGURATION.MotionMagic.MotionMagicJerk = 0; } } diff --git a/src/main/java/frc/robot/subsystems/Rotors.java b/src/main/java/frc/robot/subsystems/Rotors.java index 0ee0be0..55592e7 100644 --- a/src/main/java/frc/robot/subsystems/Rotors.java +++ b/src/main/java/frc/robot/subsystems/Rotors.java @@ -20,21 +20,33 @@ @Logged public class Rotors extends SubsystemBase { - 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 serializerRollersLeader = new TalonFX(rotorIDs.SERIALIZER_ROLLERS_CAN); + + final TalonFX intakeRollersWestFollower = new TalonFX(rotorIDs.INTAKE_ROLLERS_WEST_CAN); + final TalonFX intakeRollersEastLeader = new TalonFX(rotorIDs.INTAKE_ROLLERS_EAST_CAN); + + final TalonFX transferRollersWestLeader = new TalonFX((rotorIDs.TRANSFER_ROLLERS_WEST_CAN)); + final TalonFX transferRollersEastFollower = 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 flywheelTopEastLeader = 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(), MotorAlignmentValue.Aligned); - Follower flywheelWestFollower = new Follower(flywheelTopEast.getDeviceID(), MotorAlignmentValue.Opposed); - Follower transferRollersEastFollower = new Follower(transferRollersWest.getDeviceID(), MotorAlignmentValue.Opposed); - Follower intakeRollerEastFollower = new Follower(intakeRollersEast.getDeviceID(), MotorAlignmentValue.Opposed); + + Follower flywheelFollowerAlignedRequest = new Follower(flywheelTopEastLeader.getDeviceID(), + MotorAlignmentValue.Aligned); + Follower flywheelFollowerOpposedRequest = new Follower(flywheelTopEastLeader.getDeviceID(), + MotorAlignmentValue.Opposed); + + Follower transferRollersFollowerOpposedRequest = new Follower(transferRollersWestLeader.getDeviceID(), + MotorAlignmentValue.Opposed); + + Follower intakeRollerWestFollowerOpposedRequest = new Follower(intakeRollersEastLeader.getDeviceID(), + MotorAlignmentValue.Opposed); + final MotionMagicVelocityVoltage flyWheelVelocityRequest = new MotionMagicVelocityVoltage(0); final MotionMagicVelocityVoltage transferRollersVelocityRequest = new MotionMagicVelocityVoltage(0); final MotionMagicVelocityVoltage serializerVelocityRequest = new MotionMagicVelocityVoltage(0); @@ -44,12 +56,12 @@ public class Rotors extends SubsystemBase { // 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); + serializerRollersLeader.getConfigurator().apply(ConstRotors.SERIALIZER_ROLLERS_CONFIGURATION); + intakeRollersEastLeader.getConfigurator().apply(ConstRotors.INTAKE_ROLLERS_EAST_CONFIGURATION); + intakeRollersWestFollower.getConfigurator().apply(ConstRotors.INTAKE_ROLLERS_WEST_CONFIGURATION); + transferRollersEastFollower.getConfigurator().apply(ConstRotors.TRANSFER_ROLLERS_EAST_CONFIGURATION); + transferRollersWestLeader.getConfigurator().apply(ConstRotors.TRANSFER_ROLLERS_WEST_CONFIGURATION); + flywheelTopEastLeader.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); @@ -62,53 +74,54 @@ public AngularVelocity getFlyWheelSpeeds() { if (Robot.isSimulation()) { return lastDesiredFlyWheelSpeed; } - return flywheelTopEast.getVelocity().getValue(); + return flywheelTopEastLeader.getVelocity().getValue(); } public AngularVelocity getSerializerRollersVelocity() { - return serializerRollers.getVelocity().getValue(); + return serializerRollersLeader.getVelocity().getValue(); } public AngularVelocity getIntakeRollersVelocity() { - return intakeRollersEast.getVelocity().getValue(); + return intakeRollersEastLeader.getVelocity().getValue(); } public AngularVelocity getTransferRollersVelocity() { - return transferRollersEast.getVelocity().getValue(); + return transferRollersWestLeader.getVelocity().getValue(); } public void setSerializerRollersPercentOutput(Double speed) { - serializerRollers.set(speed); + serializerRollersLeader.set(speed); } public void setIntakeRollersPercentOutput(Double speed) { - intakeRollersEast.set(speed); - intakeRollersWest.setControl(intakeRollerEastFollower); + intakeRollersEastLeader.set(speed); + intakeRollersWestFollower.setControl(intakeRollerWestFollowerOpposedRequest); } public void setTransferRollersSpeeds(AngularVelocity speed) { - transferRollersEast.setControl(transferRollersVelocityRequest.withVelocity(speed)); - transferRollersWest.setControl(transferRollersEastFollower); + // THIS WAS THE BUG, LEADER AND FOLLOWER WERE SWITCHED + transferRollersWestLeader.setControl(transferRollersVelocityRequest.withVelocity(speed)); + transferRollersEastFollower.setControl(transferRollersFollowerOpposedRequest); } public void setTransferRollersPercentOutput(Double speed) { - transferRollersEast.set(speed); - transferRollersWest.setControl(transferRollersEastFollower); + transferRollersWestLeader.set(speed); + transferRollersEastFollower.setControl(transferRollersFollowerOpposedRequest); } public void setFlyWheelSpeeds(AngularVelocity speed) { - flywheelTopEast.setControl(flyWheelVelocityRequest.withVelocity(speed)); - flywheelTopWest.setControl(flywheelWestFollower); - flywheelBottomWest.setControl(flywheelWestFollower); - flywheelBottomEast.setControl(flywheelEastFollower); + flywheelTopEastLeader.setControl(flyWheelVelocityRequest.withVelocity(speed)); + flywheelTopWest.setControl(flywheelFollowerOpposedRequest); + flywheelBottomWest.setControl(flywheelFollowerOpposedRequest); + flywheelBottomEast.setControl(flywheelFollowerAlignedRequest); lastDesiredFlyWheelSpeed = speed; } public void setFlywheelPercentOutput(double speed) { - flywheelTopEast.set(speed); - flywheelTopWest.setControl(flywheelWestFollower); - flywheelBottomWest.setControl(flywheelWestFollower); - flywheelBottomEast.setControl(flywheelEastFollower); + flywheelTopEastLeader.set(speed); + flywheelTopWest.setControl(flywheelFollowerOpposedRequest); + flywheelBottomWest.setControl(flywheelFollowerOpposedRequest); + flywheelBottomEast.setControl(flywheelFollowerAlignedRequest); } public boolean isFlyWheelAtSpeed(AngularVelocity tolerance) {