From 1cb4870a9dd8f41f4b2e004b0231698c816b2af0 Mon Sep 17 00:00:00 2001 From: KPatel008 <122328545+KPatel008@users.noreply.github.com> Date: Wed, 27 Mar 2024 14:02:41 -0400 Subject: [PATCH] make intake changes --- src/main/java/frc/robot/arm/ArmIOTalonFX.java | 2 +- src/main/java/frc/robot/intake/Intake.java | 16 ++++---- src/main/java/frc/robot/intake/IntakeIO.java | 4 +- .../java/frc/robot/intake/IntakeIOSim.java | 4 +- .../frc/robot/intake/IntakeIOSparkMAX.java | 4 +- .../frc/robot/intake/IntakeIOTalonFX.java | 40 +++++++++---------- src/main/java/frc/robot/lights/Lights.java | 2 +- 7 files changed, 36 insertions(+), 36 deletions(-) diff --git a/src/main/java/frc/robot/arm/ArmIOTalonFX.java b/src/main/java/frc/robot/arm/ArmIOTalonFX.java index aa26dc9..9c3432c 100644 --- a/src/main/java/frc/robot/arm/ArmIOTalonFX.java +++ b/src/main/java/frc/robot/arm/ArmIOTalonFX.java @@ -78,7 +78,7 @@ public ArmIOTalonFX() { wristLeftMotorConfig.ClosedLoopGeneral.ContinuousWrap = true; wristLeftMotorConfig.Feedback.SensorToMechanismRatio = ArmConstants.kWristReduction; wristLeftMotorConfig.Slot0.GravityType = GravityTypeValue.Arm_Cosine; - wristLeftMotorConfig.Slot0.kP = 30.612; + wristLeftMotorConfig.Slot0.kP = 22.742; wristLeftMotorConfig.Slot0.kD = 5.5071; wristLeftMotorConfig.Slot0.kS = 0.57211; wristLeftMotorConfig.Slot0.kV = 2.5239; diff --git a/src/main/java/frc/robot/intake/Intake.java b/src/main/java/frc/robot/intake/Intake.java index 2153942..4b7635e 100644 --- a/src/main/java/frc/robot/intake/Intake.java +++ b/src/main/java/frc/robot/intake/Intake.java @@ -42,8 +42,8 @@ public void periodic() { public Command idle() { return this.run( () -> { - m_io.setTopRollerVoltage(0.0); - m_io.setBottomRollerVoltage(0.0); + m_io.setFrontRollersVoltage(0.0); + m_io.setBackRollersVoltage(0.0); m_io.setCenteringMotorsVoltage(0.0); }) .withName("Idle (Intake)"); @@ -56,8 +56,8 @@ public Command stop() { public Command spit() { return this.run( () -> { - m_io.setTopRollerVoltage(-9.6); - m_io.setBottomRollerVoltage(-9.6); + m_io.setFrontRollersVoltage(-9.6); + m_io.setBackRollersVoltage(-9.6); m_io.setCenteringMotorsVoltage(-10.2); }) .withName("Spit (Intake)"); @@ -66,8 +66,8 @@ public Command spit() { public Command intake() { return this.run( () -> { - m_io.setTopRollerVoltage(9.6); - m_io.setBottomRollerVoltage(9.6); + m_io.setFrontRollersVoltage(9.6); + m_io.setBackRollersVoltage(9.6); m_io.setCenteringMotorsVoltage(10.2); }) .alongWith( @@ -112,8 +112,8 @@ public void resetSimIntookPieces() { public Command feed() { return this.run( () -> { - m_io.setTopRollerVoltage(4.8); - m_io.setBottomRollerVoltage(4.8); + m_io.setFrontRollersVoltage(4.8); + m_io.setBackRollersVoltage(4.8); m_io.setCenteringMotorsVoltage(6.0); }); } diff --git a/src/main/java/frc/robot/intake/IntakeIO.java b/src/main/java/frc/robot/intake/IntakeIO.java index 4dbdb26..78089e2 100644 --- a/src/main/java/frc/robot/intake/IntakeIO.java +++ b/src/main/java/frc/robot/intake/IntakeIO.java @@ -21,9 +21,9 @@ public class IntakeIOInputs { public default void updateInputs(IntakeIOInputs inputs) {} - public default void setTopRollerVoltage(double volts) {} + public default void setFrontRollersVoltage(double volts) {} - public default void setBottomRollerVoltage(double volts) {} + public default void setBackRollersVoltage(double volts) {} public default void setCenteringMotorsVoltage(double volts) {} diff --git a/src/main/java/frc/robot/intake/IntakeIOSim.java b/src/main/java/frc/robot/intake/IntakeIOSim.java index e1b07b1..a5dc869 100644 --- a/src/main/java/frc/robot/intake/IntakeIOSim.java +++ b/src/main/java/frc/robot/intake/IntakeIOSim.java @@ -39,12 +39,12 @@ public void updateInputs(IntakeIOInputs inputs) { } @Override - public void setTopRollerVoltage(double voltage) { + public void setFrontRollersVoltage(double voltage) { m_topRollerAppliedVolts = MathUtil.clamp(voltage, -12.0, 12.0); } @Override - public void setBottomRollerVoltage(double voltage) { + public void setBackRollersVoltage(double voltage) { m_bottomRollerAppliedVolts = MathUtil.clamp(voltage, -12.0, 12.0); } diff --git a/src/main/java/frc/robot/intake/IntakeIOSparkMAX.java b/src/main/java/frc/robot/intake/IntakeIOSparkMAX.java index 48d7b3f..c730d4a 100644 --- a/src/main/java/frc/robot/intake/IntakeIOSparkMAX.java +++ b/src/main/java/frc/robot/intake/IntakeIOSparkMAX.java @@ -45,12 +45,12 @@ public void updateInputs(IntakeIOInputs inputs) { } @Override - public void setTopRollerVoltage(double volts) { + public void setFrontRollersVoltage(double volts) { topRollerMotor.setVoltage(volts); } @Override - public void setBottomRollerVoltage(double volts) { + public void setBackRollersVoltage(double volts) { bottomRollerMotor.setVoltage(volts); } diff --git a/src/main/java/frc/robot/intake/IntakeIOTalonFX.java b/src/main/java/frc/robot/intake/IntakeIOTalonFX.java index d0553ad..8fca8c0 100644 --- a/src/main/java/frc/robot/intake/IntakeIOTalonFX.java +++ b/src/main/java/frc/robot/intake/IntakeIOTalonFX.java @@ -13,7 +13,7 @@ public class IntakeIOTalonFX implements IntakeIO { - private final TalonFX m_topRollerMotor, m_bottomRollerMotor; + private final TalonFX m_frontRollersMotor, m_backRollersMotor; private final BaseTalon m_centeringMotors; private final StatusSignal m_topRollerVelocitySignal, @@ -24,28 +24,28 @@ public class IntakeIOTalonFX implements IntakeIO { m_bottomRollerCurrentSignal; public IntakeIOTalonFX() { - m_topRollerMotor = new TalonFX(22, Constants.kSuperstructureCanBus); - m_bottomRollerMotor = new TalonFX(23, Constants.kSuperstructureCanBus); + m_frontRollersMotor = new TalonFX(22, Constants.kSuperstructureCanBus); + m_backRollersMotor = new TalonFX(23, Constants.kSuperstructureCanBus); m_centeringMotors = new BaseTalon(24, "Talon SRX", "rio"); - m_topRollerMotor.getConfigurator().apply(new TalonFXConfiguration()); - m_bottomRollerMotor.getConfigurator().apply(new TalonFXConfiguration()); + m_frontRollersMotor.getConfigurator().apply(new TalonFXConfiguration()); + m_backRollersMotor.getConfigurator().apply(new TalonFXConfiguration()); final CurrentLimitsConfigs currentLimits = new CurrentLimitsConfigs().withSupplyCurrentLimit(40.0).withSupplyCurrentLimitEnable(true); - m_topRollerMotor.getConfigurator().apply(currentLimits); - m_bottomRollerMotor.getConfigurator().apply(currentLimits); + m_frontRollersMotor.getConfigurator().apply(currentLimits); + m_backRollersMotor.getConfigurator().apply(currentLimits); m_centeringMotors.configSupplyCurrentLimit( new SupplyCurrentLimitConfiguration(true, 40.0, 0.0, 1.0), 1000); m_centeringMotors.configVoltageCompSaturation(12.0); m_centeringMotors.enableVoltageCompensation(true); - m_topRollerVelocitySignal = m_topRollerMotor.getVelocity(); - m_topRollerAppliedVoltageSignal = m_topRollerMotor.getMotorVoltage(); - m_topRollerCurrentSignal = m_topRollerMotor.getSupplyCurrent(); + m_topRollerVelocitySignal = m_frontRollersMotor.getVelocity(); + m_topRollerAppliedVoltageSignal = m_frontRollersMotor.getMotorVoltage(); + m_topRollerCurrentSignal = m_frontRollersMotor.getSupplyCurrent(); - m_bottomRollerVelocitySignal = m_bottomRollerMotor.getVelocity(); - m_bottomRollerAppliedVoltageSignal = m_bottomRollerMotor.getMotorVoltage(); - m_bottomRollerCurrentSignal = m_bottomRollerMotor.getSupplyCurrent(); + m_bottomRollerVelocitySignal = m_backRollersMotor.getVelocity(); + m_bottomRollerAppliedVoltageSignal = m_backRollersMotor.getMotorVoltage(); + m_bottomRollerCurrentSignal = m_backRollersMotor.getSupplyCurrent(); BaseStatusSignal.setUpdateFrequencyForAll( 50.0, @@ -55,7 +55,7 @@ public IntakeIOTalonFX() { m_bottomRollerVelocitySignal, m_bottomRollerAppliedVoltageSignal, m_bottomRollerCurrentSignal); - ParentDevice.optimizeBusUtilizationForAll(m_topRollerMotor, m_bottomRollerMotor); + ParentDevice.optimizeBusUtilizationForAll(m_frontRollersMotor, m_backRollersMotor); } @Override @@ -83,13 +83,13 @@ public void updateInputs(IntakeIOInputs inputs) { } @Override - public void setTopRollerVoltage(double volts) { - m_topRollerMotor.setVoltage(volts); + public void setFrontRollersVoltage(double volts) { + m_frontRollersMotor.setVoltage(volts); } @Override - public void setBottomRollerVoltage(double volts) { - m_bottomRollerMotor.setVoltage(volts); + public void setBackRollersVoltage(double volts) { + m_backRollersMotor.setVoltage(volts); } @Override @@ -99,8 +99,8 @@ public void setCenteringMotorsVoltage(double volts) { @Override public void stopRollers() { - m_topRollerMotor.stopMotor(); - m_bottomRollerMotor.stopMotor(); + m_frontRollersMotor.stopMotor(); + m_backRollersMotor.stopMotor(); m_centeringMotors.set(ControlMode.PercentOutput, 0.0); } } diff --git a/src/main/java/frc/robot/lights/Lights.java b/src/main/java/frc/robot/lights/Lights.java index 5156327..4c32ffc 100644 --- a/src/main/java/frc/robot/lights/Lights.java +++ b/src/main/java/frc/robot/lights/Lights.java @@ -17,7 +17,7 @@ public class Lights extends SubsystemBase { - private final int kLedCount = 8; + private final int kLedCount = Constants.kIsViper ? 8 : 32; private final CANdle m_ledController; public Lights() {