diff --git a/src/main/java/frc/robot/auto/Autonomous.java b/src/main/java/frc/robot/auto/Autonomous.java index 0c425275..620bd420 100644 --- a/src/main/java/frc/robot/auto/Autonomous.java +++ b/src/main/java/frc/robot/auto/Autonomous.java @@ -75,13 +75,11 @@ private Command singleNeutralCycle(boolean rightSide) { robot .getSwerveDrive() .followPath("left_neutral.0", rightSide) - .deadlineFor( - robot.getIntakeExtension().extend(), robot.getIntakeRollers().intakeFast()), + .deadlineFor(robot.getIntakeExtension().extend(), robot.getIntakeRollers().intake()), robot .getSwerveDrive() .followPath("left_neutral.1", rightSide) - .deadlineFor( - robot.getIntakeExtension().extend(), robot.getIntakeRollers().intakeFast()), + .deadlineFor(robot.getIntakeExtension().extend(), robot.getIntakeRollers().intake()), robot.getSwerveDrive().followPath("left_neutral.2", rightSide), shootFuel.shoot()); } @@ -104,30 +102,25 @@ private Command doubleNeutralCycle(boolean rightSide) { robot .getSwerveDrive() .followPath("left_neutral.0", rightSide) - .deadlineFor( - robot.getIntakeExtension().extend(), robot.getIntakeRollers().intakeFast()), + .deadlineFor(robot.getIntakeExtension().extend(), robot.getIntakeRollers().intake()), robot .getSwerveDrive() .followPath("left_neutral.1", rightSide) - .deadlineFor( - robot.getIntakeExtension().extend(), robot.getIntakeRollers().intakeFast()), + .deadlineFor(robot.getIntakeExtension().extend(), robot.getIntakeRollers().intake()), robot.getSwerveDrive().followPath("left_neutral.2", rightSide), shootFuel.shootAllFuelStationary().withTimeout(20.0 - 13.2), robot .getSwerveDrive() .followPath("left_neutral.3", rightSide) - .deadlineFor( - robot.getIntakeExtension().extend(), robot.getIntakeRollers().intakeFast()), + .deadlineFor(robot.getIntakeExtension().extend(), robot.getIntakeRollers().intake()), robot .getSwerveDrive() .followPath("left_neutral.4", rightSide) - .deadlineFor( - robot.getIntakeExtension().extend(), robot.getIntakeRollers().intakeFast()), + .deadlineFor(robot.getIntakeExtension().extend(), robot.getIntakeRollers().intake()), robot .getSwerveDrive() .followPath("left_neutral.5", rightSide) - .deadlineFor( - robot.getIntakeExtension().extend(), robot.getIntakeRollers().intakeFast()), + .deadlineFor(robot.getIntakeExtension().extend(), robot.getIntakeRollers().intake()), shootFuel.shoot()); } @@ -150,24 +143,24 @@ public Command bump(boolean rightSide) { .getSwerveDrive() .followPath("left_neutral_bump.0", rightSide) .deadlineFor( - robot.getIntakeExtension().extend(), robot.getIntakeRollers().intakeFast()), + robot.getIntakeExtension().extend(), robot.getIntakeRollers().intake()), robot .getSwerveDrive() .followPath("left_neutral_bump.1", rightSide) .deadlineFor( - robot.getIntakeExtension().extend(), robot.getIntakeRollers().intakeFast()), + robot.getIntakeExtension().extend(), robot.getIntakeRollers().intake()), robot.getSwerveDrive().followPath("left_neutral_bump.2", rightSide), shootFuel.shootAllFuelStationary().withTimeout(5), robot .getSwerveDrive() .followPath("left_neutral_bump.3", rightSide) .deadlineFor( - robot.getIntakeExtension().extend(), robot.getIntakeRollers().intakeFast()), + robot.getIntakeExtension().extend(), robot.getIntakeRollers().intake()), robot .getSwerveDrive() .followPath("left_neutral_bump.4", rightSide) .deadlineFor( - robot.getIntakeExtension().extend(), robot.getIntakeRollers().intakeFast()), + robot.getIntakeExtension().extend(), robot.getIntakeRollers().intake()), robot.getSwerveDrive().followPath("left_neutral_bump.5", rightSide), shootFuel.shoot()) .withTimeout(20); diff --git a/src/main/java/frc/robot/subsystems/intakeextension/IntakeExtensionConstants.java b/src/main/java/frc/robot/subsystems/intakeextension/IntakeExtensionConstants.java index 1092cc12..addde9e8 100644 --- a/src/main/java/frc/robot/subsystems/intakeextension/IntakeExtensionConstants.java +++ b/src/main/java/frc/robot/subsystems/intakeextension/IntakeExtensionConstants.java @@ -27,16 +27,16 @@ public final class IntakeExtensionConstants { public static final int MOTOR_CAN_ID = 40; public static final DCMotor MOTOR_PHYSICS = DCMotor.getKrakenX44Foc(1); - public static final Mass MOVING_MASS = Pounds.of(12); + public static final Mass MOVING_MASS = Pounds.of(14.5); // Subject to Change public static final Distance MAX_POSITION = Inches.of(10.1); public static final Distance MIN_POSITION = Inches.of(0); public static final Distance RETRACT_POSITION = Inches.of(3); - public static final Angle ANGLE = Degrees.of(-18.5); - public static final Distance PINION_RADIUS = Inches.of(0.75); + public static final Angle ANGLE = Degrees.of(-18.6196233); + public static final Distance PINION_RADIUS = Inches.of(1.5); public static final int CANDI_DEVICE_ID = 20; public static final Distance POSITION_TOLERANCE = Inches.of(0.125); public static final Voltage FINE_CONTROL_VOLTAGE = Volts.of(1.0); - public static final double GEAR_RATIO = 7.2; + public static final double GEAR_RATIO = 7; public static final TalonFXConfiguration MOTOR_CONFIGURATION = new TalonFXConfiguration() diff --git a/src/main/java/frc/robot/subsystems/intakerollers/IntakeRollers.java b/src/main/java/frc/robot/subsystems/intakerollers/IntakeRollers.java index 0a43bf0a..52db192c 100644 --- a/src/main/java/frc/robot/subsystems/intakerollers/IntakeRollers.java +++ b/src/main/java/frc/robot/subsystems/intakerollers/IntakeRollers.java @@ -4,12 +4,14 @@ import static edu.wpi.first.units.Units.RotationsPerSecond; import com.ctre.phoenix6.BaseStatusSignal; -import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.controls.CoastOut; import com.ctre.phoenix6.controls.DutyCycleOut; +import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.MotorAlignmentValue; import com.team6962.lib.logging.CurrentDrawLogger; import com.team6962.lib.phoenix.StatusUtil; import dev.doglog.DogLog; @@ -23,27 +25,39 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; public class IntakeRollers extends SubsystemBase { + private TalonFX IntakeRollerMotor1; + private TalonFX IntakeRollerMotor2; private TalonFX intakeMotor; private StatusSignal velocitySignal; private StatusSignal statorCurrentSignal; private StatusSignal supplyCurrentSignal; private StatusSignal appliedVoltageSignal; private IntakeRollerSim simulation; - private double intakeVoltage = 4.0; + private double intakeVoltage = 12.0; private double intakeStallVoltage = 12.0; private Debouncer stallDebouncer = new Debouncer(0.25, DebounceType.kRising); private boolean stalling = false; /** Intializes motor and status signals Class for Intake Rollers */ public IntakeRollers() { - this.intakeMotor = - new TalonFX(IntakeRollersConstants.DEVICE_ID, new CANBus("subsystems")); // temporary - - intakeMotor.getConfigurator().apply(IntakeRollersConstants.MOTOR_CONFIGURATION); - this.velocitySignal = intakeMotor.getVelocity(); - this.statorCurrentSignal = intakeMotor.getStatorCurrent(); - this.supplyCurrentSignal = intakeMotor.getSupplyCurrent(); - this.appliedVoltageSignal = intakeMotor.getMotorVoltage(); + IntakeRollerMotor1 = + new TalonFX(IntakeRollersConstants.DEVICE_ID_1, IntakeRollersConstants.CANBUS); + + IntakeRollerMotor1.getConfigurator().apply(IntakeRollersConstants.MOTOR_CONFIGURATION); + + IntakeRollerMotor2 = + new TalonFX(IntakeRollersConstants.DEVICE_ID_2, IntakeRollersConstants.CANBUS); + + IntakeRollersConstants.MOTOR_CONFIGURATION.MotorOutput.Inverted = + IntakeRollersConstants.MOTOR_CONFIGURATION.MotorOutput.Inverted + == InvertedValue.Clockwise_Positive + ? InvertedValue.CounterClockwise_Positive + : InvertedValue.Clockwise_Positive; + + this.velocitySignal = IntakeRollerMotor1.getVelocity(); + this.statorCurrentSignal = IntakeRollerMotor1.getStatorCurrent(); + this.supplyCurrentSignal = IntakeRollerMotor2.getSupplyCurrent(); + this.appliedVoltageSignal = IntakeRollerMotor1.getMotorVoltage(); if (RobotBase.isSimulation()) { simulation = new IntakeRollerSim(intakeMotor); } @@ -63,6 +77,12 @@ public IntakeRollers() { }); CurrentDrawLogger.add("Intake Rollers", this::getSupplyCurrent); + + IntakeRollerMotor2.setControl( + new Follower(IntakeRollerMotor1.getDeviceID(), MotorAlignmentValue.Opposed)); + if (RobotBase.isSimulation()) { + simulation = new IntakeRollerSim(IntakeRollerMotor1); + } } /** Returns command to make the motor move and stop */ @@ -93,22 +113,6 @@ public Command intake() { }); } - /** - * Returns command where motor intakes fuel at full speed. This should only be used when shooting - * fuel out of the robot with the intake is not a concern. - * - * @return The command to intake fuel at full speed. - */ - public Command intakeFast() { - return startEnd( - () -> { - intakeMotor.setControl(new VoltageOut(7).withEnableFOC(false)); - }, - () -> { - intakeMotor.setControl(new CoastOut()); - }); - } - /** * Returns command where motor outtakes fuel * diff --git a/src/main/java/frc/robot/subsystems/intakerollers/IntakeRollersConstants.java b/src/main/java/frc/robot/subsystems/intakerollers/IntakeRollersConstants.java index 625f1155..c8b3fd01 100644 --- a/src/main/java/frc/robot/subsystems/intakerollers/IntakeRollersConstants.java +++ b/src/main/java/frc/robot/subsystems/intakerollers/IntakeRollersConstants.java @@ -2,6 +2,7 @@ import static edu.wpi.first.units.Units.Amps; +import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.FeedbackConfigs; import com.ctre.phoenix6.configs.MotorOutputConfigs; @@ -10,9 +11,11 @@ import com.ctre.phoenix6.signals.InvertedValue; public class IntakeRollersConstants { - public static final double GEAR_RATIO = 32 / 18; + public static final double GEAR_RATIO = 15 / 27; public static final double MOMENT_OF_INERTIA = 0.00074271944; - public static final int DEVICE_ID = 41; + public static final int DEVICE_ID_1 = 41; + public static final int DEVICE_ID_2 = 42; // Prob not Right + public static final CANBus CANBUS = new CANBus("subsystems"); public static final TalonFXConfiguration MOTOR_CONFIGURATION = new TalonFXConfiguration()