From 916f35f676fb78548aee96294832a0970c56965c Mon Sep 17 00:00:00 2001 From: OscarCDavis Date: Sat, 1 Feb 2025 15:23:36 -0700 Subject: [PATCH 1/9] added sys id --- src/main/java/frc/robot/RobotContainer.java | 11 ++++ .../frc/robot/commands/Climb/ClimbExtend.java | 3 +- .../robot/commands/Climb/ClimbRetract.java | 3 +- .../frc/robot/subsystems/Climb/Climb.java | 17 +++--- .../subsystems/Climb/ClimbIOTalonFX.java | 54 +++++++++---------- .../subsystems/Intake/IntakeIOKraken.java | 4 +- 6 files changed, 52 insertions(+), 40 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d893a83..86470ed 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -401,6 +401,17 @@ private void definesysIdRoutines() { // autoChooserPathPlanner.addOption( // "Flywheel SysId (Dynamic Reverse)", // m_flywheel.sysIdDynamic(SysIdRoutine.Direction.kReverse)); + + autoChooserPathPlanner.addOption( + "Intake SysId (Quasistatic Forward)", + m_intake.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); + autoChooserPathPlanner.addOption( + "Intake SysId (Quasistatic Reverse)", + m_intake.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); + autoChooserPathPlanner.addOption( + "Intake SysId (Dynamic Forward)", m_intake.sysIdDynamic(SysIdRoutine.Direction.kForward)); + autoChooserPathPlanner.addOption( + "Intake SysId (Dynamic Reverse)", m_intake.sysIdDynamic(SysIdRoutine.Direction.kReverse)); } } diff --git a/src/main/java/frc/robot/commands/Climb/ClimbExtend.java b/src/main/java/frc/robot/commands/Climb/ClimbExtend.java index 49ad085..308bc2a 100644 --- a/src/main/java/frc/robot/commands/Climb/ClimbExtend.java +++ b/src/main/java/frc/robot/commands/Climb/ClimbExtend.java @@ -5,6 +5,7 @@ public class ClimbExtend extends Command { private final Climb climb; + public ClimbExtend(Climb climb) { this.climb = climb; } @@ -21,4 +22,4 @@ public void execute() { @Override public void end(boolean interrupted) {} -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/commands/Climb/ClimbRetract.java b/src/main/java/frc/robot/commands/Climb/ClimbRetract.java index 4a18310..e486920 100644 --- a/src/main/java/frc/robot/commands/Climb/ClimbRetract.java +++ b/src/main/java/frc/robot/commands/Climb/ClimbRetract.java @@ -5,6 +5,7 @@ public class ClimbRetract extends Command { private final Climb climb; + public ClimbRetract(Climb climb) { this.climb = climb; } @@ -21,4 +22,4 @@ public void execute() { @Override public void end(boolean interrupted) {} -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/subsystems/Climb/Climb.java b/src/main/java/frc/robot/subsystems/Climb/Climb.java index 7db8764..e85a370 100644 --- a/src/main/java/frc/robot/subsystems/Climb/Climb.java +++ b/src/main/java/frc/robot/subsystems/Climb/Climb.java @@ -3,27 +3,26 @@ public class Climb { private ClimbIO io; private boolean rachetToggle; - - public Climb(ClimbIO io){ + + public Climb(ClimbIO io) { this.io = io; io.configPID(1, 0, 0); rachetToggle = false; } - public void twistToPosition(double position){ + public void twistToPosition(double position) { io.twistMotorToPosition(position); } - public void manualControl(double controlVolts){ - io.twistMotorVolts(controlVolts*3); + public void manualControl(double controlVolts) { + io.twistMotorVolts(controlVolts * 3); } - public void rachetToggle (boolean toggleSwitch) { - if(rachetToggle){ + public void rachetToggle(boolean toggleSwitch) { + if (rachetToggle) { io.turnClimbServo(0); - }else{ + } else { io.turnClimbServo(1); } } - } diff --git a/src/main/java/frc/robot/subsystems/Climb/ClimbIOTalonFX.java b/src/main/java/frc/robot/subsystems/Climb/ClimbIOTalonFX.java index 0d3f313..b8bae5f 100644 --- a/src/main/java/frc/robot/subsystems/Climb/ClimbIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/Climb/ClimbIOTalonFX.java @@ -5,38 +5,38 @@ import com.ctre.phoenix6.controls.PositionDutyCycle; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.NeutralModeValue; - import edu.wpi.first.wpilibj.Servo; -public class ClimbIOTalonFX implements ClimbIO{ +public class ClimbIOTalonFX implements ClimbIO { private final Servo climbExtender = new Servo(1); private final TalonFX climber = new TalonFX(31); - public ClimbIOTalonFX() { - var climbConfig = new TalonFXConfiguration(); - climbConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; - } - @Override - public void turnClimbServo(double position) { - climbExtender.set(position); - } - @Override - public void twistMotorToPosition(double position) { - climber.setControl(new PositionDutyCycle(position)); - } + public ClimbIOTalonFX() { + var climbConfig = new TalonFXConfiguration(); + climbConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + } + + @Override + public void turnClimbServo(double position) { + climbExtender.set(position); + } - @Override - public void twistMotorVolts(double volts) { - climber.setVoltage(volts); - } + @Override + public void twistMotorToPosition(double position) { + climber.setControl(new PositionDutyCycle(position)); + } - @Override - public void configPID(double kP, double kI, double kD) { - Slot0Configs pid = new Slot0Configs(); - pid.withKP(kP); - pid.withKI(kI); - pid.withKD(kD); - climber.getConfigurator().apply(pid); - } + @Override + public void twistMotorVolts(double volts) { + climber.setVoltage(volts); + } - } + @Override + public void configPID(double kP, double kI, double kD) { + Slot0Configs pid = new Slot0Configs(); + pid.withKP(kP); + pid.withKI(kI); + pid.withKD(kD); + climber.getConfigurator().apply(pid); + } +} diff --git a/src/main/java/frc/robot/subsystems/Intake/IntakeIOKraken.java b/src/main/java/frc/robot/subsystems/Intake/IntakeIOKraken.java index 2d38d75..369800b 100644 --- a/src/main/java/frc/robot/subsystems/Intake/IntakeIOKraken.java +++ b/src/main/java/frc/robot/subsystems/Intake/IntakeIOKraken.java @@ -33,9 +33,9 @@ public IntakeIOKraken() {} public void updateInputs(IntakeIOInputs inputs) { BaseStatusSignal.refreshAll(pivotPosition, pivotVelocity, pivotAppliedVolts, pivotCurrent); inputs.positionRad = - Units.rotationsToRadians(pivotPosition.getValueAsDouble()) / 21.42857; // gear ratio + Units.rotationsToRadians(pivotPosition.getValueAsDouble()); // 21.42857; // gear ratio inputs.velocityRadPerSec = - Units.rotationsToRadians(pivotVelocity.getValueAsDouble()) / 21.42857; // gear ratio + Units.rotationsToRadians(pivotVelocity.getValueAsDouble()); // 21.42857; // gear ratio inputs.appliedVolts = pivotAppliedVolts.getValueAsDouble(); inputs.currentAmps = new double[] {pivotCurrent.getValueAsDouble()}; } From 2ecd3587428bf5396e61c38fd192d8c207bf0694 Mon Sep 17 00:00:00 2001 From: OscarCDavis Date: Sat, 1 Feb 2025 17:08:16 -0700 Subject: [PATCH 2/9] Changed code from motion magic to encoder system based off of george --- src/main/java/frc/robot/RobotContainer.java | 23 +-------- .../frc/robot/commands/IntakeCommand.java | 12 +++-- .../frc/robot/subsystems/Intake/Intake.java | 28 +++-------- .../frc/robot/subsystems/Intake/IntakeIO.java | 16 ++---- .../subsystems/Intake/IntakeIOKraken.java | 50 +++++++------------ 5 files changed, 39 insertions(+), 90 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ea1e3b7..e7602bc 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -86,7 +86,7 @@ public class RobotContainer { private final PowerMonitoring m_power; private final Intake m_intake = new Intake(new IntakeIOKraken()); private final LED led = new LED(new LEDIOCandle()); - private final DigitalInput lightStop = new DigitalInput(1); + private final DigitalInput lightStop = new DigitalInput(5); /** Dashboard inputs ***************************************************** */ // AutoChoosers for both supported path planning types @@ -296,16 +296,7 @@ private void configureBindings() { // driverController.a().whileTrue(new IntakeCommand(m_intake, 0)); - m_intake.setDefaultCommand( - Commands.run( - () -> - m_intake.runPivotVolts( - driverController.getRightTriggerAxis() - - driverController.getLeftTriggerAxis() * 5), - m_intake)); - - driverController.rightBumper().whileTrue(new IntakeCommand(m_intake, 10)); - driverController.leftBumper().whileTrue(new IntakeCommand(m_intake, -1)); + driverController.rightBumper().whileTrue(new IntakeCommand(m_intake, 0.4)); // Press Y button --> Manually Re-Zero the Gyro driverController @@ -414,16 +405,6 @@ private void definesysIdRoutines() { // "Flywheel SysId (Dynamic Reverse)", // m_flywheel.sysIdDynamic(SysIdRoutine.Direction.kReverse)); - autoChooserPathPlanner.addOption( - "Intake SysId (Quasistatic Forward)", - m_intake.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); - autoChooserPathPlanner.addOption( - "Intake SysId (Quasistatic Reverse)", - m_intake.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); - autoChooserPathPlanner.addOption( - "Intake SysId (Dynamic Forward)", m_intake.sysIdDynamic(SysIdRoutine.Direction.kForward)); - autoChooserPathPlanner.addOption( - "Intake SysId (Dynamic Reverse)", m_intake.sysIdDynamic(SysIdRoutine.Direction.kReverse)); } } diff --git a/src/main/java/frc/robot/commands/IntakeCommand.java b/src/main/java/frc/robot/commands/IntakeCommand.java index 4b457f2..302f50b 100644 --- a/src/main/java/frc/robot/commands/IntakeCommand.java +++ b/src/main/java/frc/robot/commands/IntakeCommand.java @@ -6,19 +6,21 @@ public class IntakeCommand extends Command { private final Intake intake; - private final double direction; + private final double wantedPosistion; - public IntakeCommand(Intake intake, double direction) { + public IntakeCommand(Intake intake, double wantedPosistion) { this.intake = intake; - this.direction = direction; + this.wantedPosistion = wantedPosistion; } @Override - public void initialize() {} + public void initialize() { + intake.configure(1, 0, 0); + } @Override public void execute() { - intake.setRollerVolts(-direction); + intake.setPivotPosition(wantedPosistion); } @Override diff --git a/src/main/java/frc/robot/subsystems/Intake/Intake.java b/src/main/java/frc/robot/subsystems/Intake/Intake.java index 83cc69e..17da2c9 100644 --- a/src/main/java/frc/robot/subsystems/Intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake/Intake.java @@ -4,7 +4,6 @@ import static edu.wpi.first.units.Units.Volts; import edu.wpi.first.units.Units; -import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.util.RBSISubsystem; import org.littletonrobotics.junction.Logger; @@ -50,30 +49,17 @@ public void stop() { io.stop(); } - public void configure( - double kG, - double kS, - double kV, - double kA, - double kP, - double kI, - double kD, - double velocity, - double acceleration, - double jerk) { - io.configure(kG, kS, kV, kA, kP, kI, kD, velocity, acceleration, jerk); + public void configure(double kP, double kI, double kD) { + io.configure(kP, kI, kD); + } + ; + + public double getEncoder() { + return io.getEncoder(); } @Override public int[] getPowerPorts() { return io.powerPorts; } - - public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { - return sysId.quasistatic(direction); - } - - public Command sysIdDynamic(SysIdRoutine.Direction direction) { - return sysId.dynamic(direction); - } } diff --git a/src/main/java/frc/robot/subsystems/Intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/Intake/IntakeIO.java index 5afe07f..ffbf45e 100644 --- a/src/main/java/frc/robot/subsystems/Intake/IntakeIO.java +++ b/src/main/java/frc/robot/subsystems/Intake/IntakeIO.java @@ -22,17 +22,11 @@ public default void stop() {} public default void setPivotVolts(double volts) {} + public default void configure(double kP, double kI, double kD) {} + public default void setRollerVolts(double volts) {} - public default void configure( - double kG, - double kS, - double kV, - double kA, - double kP, - double kI, - double kD, - double velocity, - double acceleration, - double jerk) {} + public default double getEncoder() { + return 0.0; + } } diff --git a/src/main/java/frc/robot/subsystems/Intake/IntakeIOKraken.java b/src/main/java/frc/robot/subsystems/Intake/IntakeIOKraken.java index 369800b..8e64075 100644 --- a/src/main/java/frc/robot/subsystems/Intake/IntakeIOKraken.java +++ b/src/main/java/frc/robot/subsystems/Intake/IntakeIOKraken.java @@ -2,14 +2,14 @@ import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.MotionMagicVoltage; import com.ctre.phoenix6.hardware.TalonFX; +import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.wpilibj.DutyCycleEncoder; import frc.robot.Constants.CANandPowerPorts; public class IntakeIOKraken implements IntakeIO { @@ -18,6 +18,13 @@ public class IntakeIOKraken implements IntakeIO { private final TalonFX intakePivot = new TalonFX(CANandPowerPorts.INTAKE_PIVOT.getDeviceNumber()); + private static final DutyCycleEncoder encoderActual = + new DutyCycleEncoder( + 1); // Encoder actual is the actual encoder object where as encoder is a double with the + // value of encoder actaul + + PIDController pivotPID = new PIDController(0, 0, 0); + public final int[] powerPorts = { CANandPowerPorts.INTAKE_PIVOT.getPowerPort(), CANandPowerPorts.INTAKE_ROLLER.getPowerPort() }; @@ -58,39 +65,18 @@ public void stop() { @Override public void setPivotPosition(double position) { - final MotionMagicVoltage m_request = new MotionMagicVoltage(0); - - intakePivot.setControl(m_request.withPosition(position)); + intakePivot.set(-pivotPID.calculate(encoderActual.get(), position)); } @Override - public void configure( - double kG, - double kS, - double kV, - double kA, - double kP, - double kI, - double kD, - double velocity, - double acceleration, - double jerk) { - - var talonFXConfigs = new TalonFXConfiguration(); - var slot0Configs = talonFXConfigs.Slot0; - - slot0Configs.kG = kG; - slot0Configs.kS = kS; - slot0Configs.kV = kV; - slot0Configs.kA = kA; - slot0Configs.kP = kP; - slot0Configs.kI = kI; - slot0Configs.kD = kD; - - var motionMagicConfigs = talonFXConfigs.MotionMagic; + public void configure(double kP, double kI, double kD) { + pivotPID.setP(kP); + pivotPID.setI(kI); + pivotPID.setD(kD); + } - motionMagicConfigs.MotionMagicCruiseVelocity = velocity; - motionMagicConfigs.MotionMagicAcceleration = acceleration; - motionMagicConfigs.MotionMagicJerk = jerk; + @Override + public double getEncoder() { + return encoderActual.get(); } } From a4a1f75b799d12500d38c653b8a2a018ad73480f Mon Sep 17 00:00:00 2001 From: OscarCDavis Date: Sat, 1 Feb 2025 18:25:48 -0700 Subject: [PATCH 3/9] it kinda works but it doesnt go up yet --- src/main/java/frc/robot/RobotContainer.java | 14 +++++++++++++- .../java/frc/robot/commands/IntakeCommand.java | 15 ++++++++++++--- .../java/frc/robot/subsystems/Intake/Intake.java | 4 ++++ .../frc/robot/subsystems/Intake/IntakeIO.java | 2 ++ .../robot/subsystems/Intake/IntakeIOKraken.java | 12 +++++++----- 5 files changed, 38 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e7602bc..3ff9072 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -296,7 +296,19 @@ private void configureBindings() { // driverController.a().whileTrue(new IntakeCommand(m_intake, 0)); - driverController.rightBumper().whileTrue(new IntakeCommand(m_intake, 0.4)); + // driverController + // .rightBumper() + // .whileTrue( + // Commands.runEnd( + // () -> m_intake.setPivotPosition(0.9), + // () -> m_intake.setPivotPosition(0.3), + // m_intake)); + + m_intake.setDefaultCommand(); + + driverController.rightBumper().whileTrue(new IntakeCommand(m_intake, 0.9, 0.1, 0)); + + driverController.leftBumper().whileTrue(new IntakeCommand(m_intake, 0.4, 0, 0)); // Press Y button --> Manually Re-Zero the Gyro driverController diff --git a/src/main/java/frc/robot/commands/IntakeCommand.java b/src/main/java/frc/robot/commands/IntakeCommand.java index 302f50b..79b2127 100644 --- a/src/main/java/frc/robot/commands/IntakeCommand.java +++ b/src/main/java/frc/robot/commands/IntakeCommand.java @@ -7,10 +7,14 @@ public class IntakeCommand extends Command { private final Intake intake; private final double wantedPosistion; + private final double rollerSpeed; + private final double test; // get rid of this variable in future - public IntakeCommand(Intake intake, double wantedPosistion) { + public IntakeCommand(Intake intake, double wantedPosistion, double rollerSpeed, double test) { this.intake = intake; this.wantedPosistion = wantedPosistion; + this.rollerSpeed = rollerSpeed; + this.test = test; } @Override @@ -20,11 +24,16 @@ public void initialize() { @Override public void execute() { - intake.setPivotPosition(wantedPosistion); + if (test == 0) { + intake.setPivotPosition(wantedPosistion); + intake.rollerSpeed(rollerSpeed); + } else if (test == 1) { + System.out.println(intake.getEncoder()); + } } @Override public void end(boolean interrupted) { - intake.stop(); + intake.stop(); } } diff --git a/src/main/java/frc/robot/subsystems/Intake/Intake.java b/src/main/java/frc/robot/subsystems/Intake/Intake.java index 17da2c9..0fbf3bb 100644 --- a/src/main/java/frc/robot/subsystems/Intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake/Intake.java @@ -37,6 +37,10 @@ public void setPivotPosition(double position) { io.setPivotPosition(position); } + public void rollerSpeed(double speed) { + io.rollerSpeed(speed); + } + public void runPivotVolts(double volts) { io.setPivotVolts(volts); } diff --git a/src/main/java/frc/robot/subsystems/Intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/Intake/IntakeIO.java index ffbf45e..fe86e87 100644 --- a/src/main/java/frc/robot/subsystems/Intake/IntakeIO.java +++ b/src/main/java/frc/robot/subsystems/Intake/IntakeIO.java @@ -18,6 +18,8 @@ public default void updateInputs(IntakeIOInputs inputs) {} public default void setPivotPosition(double position) {} + public default void rollerSpeed(double speed) {} + public default void stop() {} public default void setPivotVolts(double volts) {} diff --git a/src/main/java/frc/robot/subsystems/Intake/IntakeIOKraken.java b/src/main/java/frc/robot/subsystems/Intake/IntakeIOKraken.java index 8e64075..9335536 100644 --- a/src/main/java/frc/robot/subsystems/Intake/IntakeIOKraken.java +++ b/src/main/java/frc/robot/subsystems/Intake/IntakeIOKraken.java @@ -13,15 +13,12 @@ import frc.robot.Constants.CANandPowerPorts; public class IntakeIOKraken implements IntakeIO { + private final TalonFX intakeRoller = new TalonFX(CANandPowerPorts.INTAKE_ROLLER.getDeviceNumber()); - private final TalonFX intakePivot = new TalonFX(CANandPowerPorts.INTAKE_PIVOT.getDeviceNumber()); - private static final DutyCycleEncoder encoderActual = - new DutyCycleEncoder( - 1); // Encoder actual is the actual encoder object where as encoder is a double with the - // value of encoder actaul + private static final DutyCycleEncoder encoderActual = new DutyCycleEncoder(1); PIDController pivotPID = new PIDController(0, 0, 0); @@ -68,6 +65,11 @@ public void setPivotPosition(double position) { intakePivot.set(-pivotPID.calculate(encoderActual.get(), position)); } + @Override + public void rollerSpeed(double speed) { + intakeRoller.set(speed); + } + @Override public void configure(double kP, double kI, double kD) { pivotPID.setP(kP); From 94220e601b0fb3416a2f2a0e0096365f7a3ede3a Mon Sep 17 00:00:00 2001 From: OscarCDavis Date: Sat, 1 Feb 2025 20:37:23 -0700 Subject: [PATCH 4/9] IT WORKS (on the practice bot) --- src/main/java/frc/robot/RobotContainer.java | 22 ++++++++++++++++--- .../frc/robot/commands/IntakeCommand.java | 2 +- 2 files changed, 20 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 3ff9072..5600fd1 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -36,6 +36,7 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; +import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Constants.AprilTagConstants.AprilTagLayoutType; import frc.robot.Constants.OperatorConstants; @@ -72,6 +73,9 @@ public class RobotContainer { // Replace with ``CommandPS4Controller`` or ``CommandJoystick`` if needed final CommandXboxController driverController = new CommandXboxController(0); // Main Driver + private Trigger leftBumper = driverController.leftBumper(); + private Trigger rightBumper = driverController.rightBumper(); + final CommandXboxController operatorController = new CommandXboxController(1); // Second Operator final OverrideSwitches overrides = new OverrideSwitches(2); // Console toggle switches @@ -304,11 +308,23 @@ private void configureBindings() { // () -> m_intake.setPivotPosition(0.3), // m_intake)); - m_intake.setDefaultCommand(); + // m_elevator.setDefaultCommand( + // Commands.run( + // () -> m_elevator.runVolts(driverController.getRightTriggerAxis()), m_elevator)); + + driverController + .rightBumper() + .whileTrue(new IntakeCommand(m_intake, 0.9, 0.1, 0)) + .whileFalse(new IntakeCommand(m_intake, 0.4, 0, 0).until(leftBumper)); + + driverController + .leftBumper() + .whileTrue(new IntakeCommand(m_intake, 0.6, -0.5, 0)) + .whileFalse(new IntakeCommand(m_intake, 0.4, 0, 0).until(rightBumper)); - driverController.rightBumper().whileTrue(new IntakeCommand(m_intake, 0.9, 0.1, 0)); + - driverController.leftBumper().whileTrue(new IntakeCommand(m_intake, 0.4, 0, 0)); + driverController.a().whileTrue(new IntakeCommand(m_intake, 0.9, 0, 1)); // Press Y button --> Manually Re-Zero the Gyro driverController diff --git a/src/main/java/frc/robot/commands/IntakeCommand.java b/src/main/java/frc/robot/commands/IntakeCommand.java index 79b2127..844648f 100644 --- a/src/main/java/frc/robot/commands/IntakeCommand.java +++ b/src/main/java/frc/robot/commands/IntakeCommand.java @@ -34,6 +34,6 @@ public void execute() { @Override public void end(boolean interrupted) { - intake.stop(); + intake.stop(); } } From a0408f96c30e268d8688bc38311d6840a52fc96f Mon Sep 17 00:00:00 2001 From: OscarCDavis Date: Sat, 1 Feb 2025 21:17:30 -0700 Subject: [PATCH 5/9] added comment about waht code does --- src/main/java/frc/robot/RobotContainer.java | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5600fd1..f5cd906 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -312,6 +312,9 @@ private void configureBindings() { // Commands.run( // () -> m_elevator.runVolts(driverController.getRightTriggerAxis()), m_elevator)); + + //the two driver controller bumpers below make it so when you let go of either button the intake pivot will go to a resting posistion + driverController .rightBumper() .whileTrue(new IntakeCommand(m_intake, 0.9, 0.1, 0)) From 1efdfc2ff2e23910a9098a80add1ef58c8bb2286 Mon Sep 17 00:00:00 2001 From: OscarCDavis Date: Mon, 3 Feb 2025 18:02:55 -0700 Subject: [PATCH 6/9] changed encoder to cancoder --- src/main/java/frc/robot/RobotContainer.java | 6 ++---- .../frc/robot/subsystems/Intake/IntakeIOKraken.java | 10 +++++----- 2 files changed, 7 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f5cd906..4631a3a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -312,9 +312,9 @@ private void configureBindings() { // Commands.run( // () -> m_elevator.runVolts(driverController.getRightTriggerAxis()), m_elevator)); + // the two driver controller bumpers below make it so when you let go of either button the + // intake pivot will go to a resting posistion - //the two driver controller bumpers below make it so when you let go of either button the intake pivot will go to a resting posistion - driverController .rightBumper() .whileTrue(new IntakeCommand(m_intake, 0.9, 0.1, 0)) @@ -325,8 +325,6 @@ private void configureBindings() { .whileTrue(new IntakeCommand(m_intake, 0.6, -0.5, 0)) .whileFalse(new IntakeCommand(m_intake, 0.4, 0, 0).until(rightBumper)); - - driverController.a().whileTrue(new IntakeCommand(m_intake, 0.9, 0, 1)); // Press Y button --> Manually Re-Zero the Gyro diff --git a/src/main/java/frc/robot/subsystems/Intake/IntakeIOKraken.java b/src/main/java/frc/robot/subsystems/Intake/IntakeIOKraken.java index 9335536..52dc6de 100644 --- a/src/main/java/frc/robot/subsystems/Intake/IntakeIOKraken.java +++ b/src/main/java/frc/robot/subsystems/Intake/IntakeIOKraken.java @@ -2,6 +2,7 @@ import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.util.Units; @@ -9,7 +10,6 @@ import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Voltage; -import edu.wpi.first.wpilibj.DutyCycleEncoder; import frc.robot.Constants.CANandPowerPorts; public class IntakeIOKraken implements IntakeIO { @@ -18,7 +18,7 @@ public class IntakeIOKraken implements IntakeIO { new TalonFX(CANandPowerPorts.INTAKE_ROLLER.getDeviceNumber()); private final TalonFX intakePivot = new TalonFX(CANandPowerPorts.INTAKE_PIVOT.getDeviceNumber()); - private static final DutyCycleEncoder encoderActual = new DutyCycleEncoder(1); + private final CANcoder encoderActual = new CANcoder(1); PIDController pivotPID = new PIDController(0, 0, 0); @@ -26,7 +26,7 @@ public class IntakeIOKraken implements IntakeIO { CANandPowerPorts.INTAKE_PIVOT.getPowerPort(), CANandPowerPorts.INTAKE_ROLLER.getPowerPort() }; - private final StatusSignal pivotPosition = intakePivot.getPosition(); + private final StatusSignal pivotPosition = encoderActual.getAbsolutePosition(); private final StatusSignal pivotVelocity = intakePivot.getVelocity(); private final StatusSignal pivotAppliedVolts = intakePivot.getMotorVoltage(); private final StatusSignal pivotCurrent = intakePivot.getSupplyCurrent(); @@ -62,7 +62,7 @@ public void stop() { @Override public void setPivotPosition(double position) { - intakePivot.set(-pivotPID.calculate(encoderActual.get(), position)); + intakePivot.set(-pivotPID.calculate(pivotPosition.getValueAsDouble(), position)); } @Override @@ -79,6 +79,6 @@ public void configure(double kP, double kI, double kD) { @Override public double getEncoder() { - return encoderActual.get(); + return pivotPosition.getValueAsDouble(); } } From d5df6783d08a6b49e1085a6996f65be0c6b2c8d1 Mon Sep 17 00:00:00 2001 From: OscarCDavis Date: Mon, 3 Feb 2025 18:09:54 -0700 Subject: [PATCH 7/9] added manual set voltage inputs --- src/main/java/frc/robot/RobotContainer.java | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4631a3a..f6a122e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -300,14 +300,6 @@ private void configureBindings() { // driverController.a().whileTrue(new IntakeCommand(m_intake, 0)); - // driverController - // .rightBumper() - // .whileTrue( - // Commands.runEnd( - // () -> m_intake.setPivotPosition(0.9), - // () -> m_intake.setPivotPosition(0.3), - // m_intake)); - // m_elevator.setDefaultCommand( // Commands.run( // () -> m_elevator.runVolts(driverController.getRightTriggerAxis()), m_elevator)); @@ -315,6 +307,13 @@ private void configureBindings() { // the two driver controller bumpers below make it so when you let go of either button the // intake pivot will go to a resting posistion + m_intake.setDefaultCommand( + Commands.run( + () -> + m_intake.runPivotVolts( + driverController.getLeftTriggerAxis() + - driverController.getRightTriggerAxis()))); + driverController .rightBumper() .whileTrue(new IntakeCommand(m_intake, 0.9, 0.1, 0)) From 1e803ed807367012df146acfb1b1fc3531242662 Mon Sep 17 00:00:00 2001 From: OscarCDavis Date: Mon, 3 Feb 2025 18:14:55 -0700 Subject: [PATCH 8/9] fixed manual voltage controls --- src/main/java/frc/robot/RobotContainer.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f6a122e..eacbdc2 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -311,8 +311,8 @@ private void configureBindings() { Commands.run( () -> m_intake.runPivotVolts( - driverController.getLeftTriggerAxis() - - driverController.getRightTriggerAxis()))); + driverController.getRightTriggerAxis() + - driverController.getLeftTriggerAxis()))); driverController .rightBumper() From 322d11ff28fcd630389cf8002c0d1932b6e95b39 Mon Sep 17 00:00:00 2001 From: OscarCDavis Date: Wed, 5 Feb 2025 16:35:42 -0700 Subject: [PATCH 9/9] IT WORKS --- src/main/java/frc/robot/RobotContainer.java | 16 ++++++++++------ .../robot/subsystems/Intake/IntakeIOKraken.java | 15 +++++++++------ 2 files changed, 19 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index eacbdc2..32be6e7 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -311,22 +311,26 @@ private void configureBindings() { Commands.run( () -> m_intake.runPivotVolts( - driverController.getRightTriggerAxis() - - driverController.getLeftTriggerAxis()))); + driverController.getRightTriggerAxis() - driverController.getLeftTriggerAxis()), + m_intake)); driverController .rightBumper() - .whileTrue(new IntakeCommand(m_intake, 0.9, 0.1, 0)) - .whileFalse(new IntakeCommand(m_intake, 0.4, 0, 0).until(leftBumper)); + .whileTrue(new IntakeCommand(m_intake, 0.25, -0.35, 0)) + .whileFalse(new IntakeCommand(m_intake, 0.9, 0, 0).until(leftBumper)); driverController .leftBumper() - .whileTrue(new IntakeCommand(m_intake, 0.6, -0.5, 0)) - .whileFalse(new IntakeCommand(m_intake, 0.4, 0, 0).until(rightBumper)); + .whileTrue( + new IntakeCommand(m_intake, 0.75, 0, 0) + .withTimeout(0.075) + .andThen(new IntakeCommand(m_intake, 0.75, 0.7, 0))) + .whileFalse(new IntakeCommand(m_intake, 0.9, 0, 0).until(rightBumper)); driverController.a().whileTrue(new IntakeCommand(m_intake, 0.9, 0, 1)); // Press Y button --> Manually Re-Zero the Gyro + driverController .y() .onTrue( diff --git a/src/main/java/frc/robot/subsystems/Intake/IntakeIOKraken.java b/src/main/java/frc/robot/subsystems/Intake/IntakeIOKraken.java index 52dc6de..4aab918 100644 --- a/src/main/java/frc/robot/subsystems/Intake/IntakeIOKraken.java +++ b/src/main/java/frc/robot/subsystems/Intake/IntakeIOKraken.java @@ -18,9 +18,9 @@ public class IntakeIOKraken implements IntakeIO { new TalonFX(CANandPowerPorts.INTAKE_ROLLER.getDeviceNumber()); private final TalonFX intakePivot = new TalonFX(CANandPowerPorts.INTAKE_PIVOT.getDeviceNumber()); - private final CANcoder encoderActual = new CANcoder(1); + private final CANcoder encoderActual = new CANcoder(23); - PIDController pivotPID = new PIDController(0, 0, 0); + PIDController pivotPID = new PIDController(1.5, 0, 0); public final int[] powerPorts = { CANandPowerPorts.INTAKE_PIVOT.getPowerPort(), CANandPowerPorts.INTAKE_ROLLER.getPowerPort() @@ -35,9 +35,11 @@ public IntakeIOKraken() {} @Override public void updateInputs(IntakeIOInputs inputs) { - BaseStatusSignal.refreshAll(pivotPosition, pivotVelocity, pivotAppliedVolts, pivotCurrent); + BaseStatusSignal.refreshAll( + encoderActual.getAbsolutePosition(), pivotVelocity, pivotAppliedVolts, pivotCurrent); inputs.positionRad = - Units.rotationsToRadians(pivotPosition.getValueAsDouble()); // 21.42857; // gear ratio + Units.rotationsToRadians( + encoderActual.getAbsolutePosition().getValueAsDouble()); // 21.42857; // gear ratio inputs.velocityRadPerSec = Units.rotationsToRadians(pivotVelocity.getValueAsDouble()); // 21.42857; // gear ratio inputs.appliedVolts = pivotAppliedVolts.getValueAsDouble(); @@ -62,7 +64,8 @@ public void stop() { @Override public void setPivotPosition(double position) { - intakePivot.set(-pivotPID.calculate(pivotPosition.getValueAsDouble(), position)); + intakePivot.set( + -pivotPID.calculate(encoderActual.getAbsolutePosition().getValueAsDouble(), position)); } @Override @@ -79,6 +82,6 @@ public void configure(double kP, double kI, double kD) { @Override public double getEncoder() { - return pivotPosition.getValueAsDouble(); + return encoderActual.getAbsolutePosition().getValueAsDouble(); } }