From 230b1511d9d6bb695826516c210ac77cc6e120d7 Mon Sep 17 00:00:00 2001 From: Abhishrek05 <90010729+Abhishrek05@users.noreply.github.com> Date: Wed, 13 Mar 2024 14:15:53 -0600 Subject: [PATCH] 3/13's changes --- src/main/java/frc4388/robot/Constants.java | 6 +++--- .../java/frc4388/robot/RobotContainer.java | 16 ++++++---------- src/main/java/frc4388/robot/RobotMap.java | 2 +- .../java/frc4388/robot/subsystems/Climber.java | 18 ++++++++++++------ 4 files changed, 22 insertions(+), 20 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 44f08d6..3e30175 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -67,7 +67,7 @@ public final class Constants { public static final class PIDConstants { public static final int SWERVE_SLOT_IDX = 0; public static final int SWERVE_PID_LOOP_IDX = 1; - public static final Gains SWERVE_GAINS = new Gains(1.2, 0.0, 0.2, 0.0, 0, 0.0); + public static final Gains SWERVE_GAINS = new Gains(1.2, 0.0, 0.1, 0.2, 0, 0.0); } public static final class AutoConstants { @@ -191,8 +191,8 @@ public final class Constants { public static final class ClimbConstants { public static final int CLIMB_MOTOR_ID = 19; - public static final double CLIMB_OUT_SPEED = 0.3; - public static final double CLIMB_IN_SPEED = -0.3; + public static final double CLIMB_IN_SPEED = -1.0; + public static final double CLIMB_OUT_SPEED = 0.87; } public static final class OIConstants { diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index fa06eec..8e64dcf 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -327,14 +327,6 @@ public class RobotContainer { new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroFlip(), m_robotSwerveDrive)); - new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.RIGHT_TRIGGER_AXIS) > 0.5) - .onTrue(new InstantCommand(() -> m_robotClimber.climbOut())) - .onFalse(new InstantCommand(() -> m_robotClimber.stopClimb())); - - new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.LEFT_TRIGGER_AXIS) > 0.5) - .onTrue(new InstantCommand(() -> m_robotClimber.climbIn())) - .onFalse(new InstantCommand(() -> m_robotClimber.stopClimb())); - new Trigger(() -> getDeadbandedDriverController().getPOV() == 0) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroRightBlue())); @@ -430,8 +422,12 @@ public class RobotContainer { .onTrue(emergencyRetract); new Trigger(() -> getDeadbandedOperatorController().getRawAxis(XboxController.RIGHT_TRIGGER_AXIS) > 0.5) - .onTrue(new InstantCommand(() -> m_robotShooter.idle())) - .onFalse(new InstantCommand(() -> m_robotShooter.stop())); + .onTrue(new InstantCommand(() -> m_robotClimber.climbOut())) + .onFalse(new InstantCommand(() -> m_robotClimber.stopClimb())); + + new Trigger(() -> getDeadbandedOperatorController().getRawAxis(XboxController.LEFT_TRIGGER_AXIS) > 0.5) + .onTrue(new InstantCommand(() -> m_robotClimber.climbIn())) + .onFalse(new InstantCommand(() -> m_robotClimber.stopClimb())); new Trigger(() -> getDeadbandedOperatorController().getPOV() == 0) .onTrue(new InstantCommand(() -> m_robotClimber.climbOut())) diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index cb84990..ef03f99 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -149,7 +149,7 @@ public class RobotMap { rightFrontSteer.setNeutralMode(NeutralMode.Brake); leftBackSteer.setNeutralMode(NeutralMode.Brake); rightBackSteer.setNeutralMode(NeutralMode.Brake); - + // initialize SwerveModules this.leftFront = new SwerveModule(leftFrontWheel, leftFrontSteer, leftFrontEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.FRONT_LEFT_ROT_OFFSET); this.rightFront = new SwerveModule(rightFrontWheel, rightFrontSteer, rightFrontEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.FRONT_RIGHT_ROT_OFFSET); diff --git a/src/main/java/frc4388/robot/subsystems/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java index 92d682b..f543373 100644 --- a/src/main/java/frc4388/robot/subsystems/Climber.java +++ b/src/main/java/frc4388/robot/subsystems/Climber.java @@ -7,10 +7,12 @@ package frc4388.robot.subsystems; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.hardware.TalonFX; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants; import frc4388.robot.Constants.ClimbConstants; +import frc4388.robot.Constants.IntakeConstants; //! 6.5C Scoring Criteria for Stage @@ -21,6 +23,7 @@ public class Climber extends SubsystemBase { public Climber(TalonFX climbMotor) { this.climbMotor = climbMotor; this.climbMotor.setInverted(true); + var slot0Configs = new Slot0Configs(); slot0Configs.kP = 0.7; // An error of 0.5 rotations results in 12 V output slot0Configs.kI = 0.0; // no output for integrated error @@ -30,15 +33,17 @@ public class Climber extends SubsystemBase { } public void climbOut() { - PositionVoltage request = new PositionVoltage(0); - climbMotor.setControl(request.withPosition(-520)); - //climbMotor.set(Constants.ClimbConstants.CLIMB_IN_SPEED); + //PositionVoltage request = new PositionVoltage(0); + //climbMotor.setControl(request.withPosition(-520)); + + climbMotor.set(Constants.ClimbConstants.CLIMB_OUT_SPEED); } public void climbIn() { - PositionVoltage request = new PositionVoltage(-520); - climbMotor.setControl(request.withPosition(0)); - // climbMotor.set(Constants.ClimbConstants.CLIMB_IN_SPEED); + //PositionVoltage request = new PositionVoltage(-520); + //climbMotor.setControl(request.withPosition(0)); + climbMotor.set(Constants.ClimbConstants.CLIMB_IN_SPEED); + ; } public void stopClimb() { @@ -48,5 +53,6 @@ public class Climber extends SubsystemBase { @Override public void periodic() { // This method will be called once per scheduler run + SmartDashboard.putNumber("climber pos", climbMotor.getPosition().getValue()); } }