diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index bea82c4..b653677 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -35,7 +35,7 @@ public final class Constants { public static final double CORRECTION_MIN = 10; public static final double CORRECTION_MAX = 50; - public static final double SLOW_SPEED = 0.2; + public static final double SLOW_SPEED = 0.5; public static final double FAST_SPEED = 1.0; public static final double TURBO_SPEED = 4.0; @@ -190,9 +190,9 @@ public final class Constants { } public static final class ClimbConstants { - public static final int CLIMB_MOTOR_ID = 0; - public static final double CLIMB_OUT_SPEED = 1.0; - public static final double CLIMB_IN_SPEED = -1.0; + 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 class OIConstants { diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 9276ff1..83d63a9 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -51,7 +51,7 @@ public class Robot extends TimedRobot { @Override public void robotPeriodic() { m_robotTime.updateTimes(); - System.out.println(m_robotContainer.limelight.isNearSpeaker()); + //System.out.println(m_robotContainer.limelight.isNearSpeaker()); //mled.updateLED(); // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled // commands, running already-scheduled commands, removing finished or interrupted commands, diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index a351460..d8a0a82 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -62,12 +62,10 @@ public class RobotContainer { m_robotMap.gyro); /* Limelight */ - public final Limelight limelight = new Limelight(); + private final Limelight limelight = new Limelight(); private final Shooter m_robotShooter = new Shooter(m_robotMap.leftShooter, m_robotMap.rightShooter, limelight); - private final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor, m_robotMap.pivotMotor); - private final Climber m_robotClimber = new Climber(m_robotMap.climbMotor); //private final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor, m_robotMap.pivotMotor); @@ -76,7 +74,8 @@ public class RobotContainer { private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID); private final DeadbandedXboxController m_autoRecorderXbox = new DeadbandedXboxController(2); - + + private final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor, m_robotMap.pivotMotor, m_driverXbox, m_operatorXbox); /* Virtual Controllers */ private final VirtualController m_virtualDriver = new VirtualController(0); @@ -297,6 +296,7 @@ public class RobotContainer { true); }, m_robotSwerveDrive) .withName("SwerveDrive DefaultCommand")); + m_robotSwerveDrive.setToSlow(); // ! Swerve Drive Default Command (Orientation Rotation) // m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> { @@ -331,6 +331,12 @@ public class RobotContainer { 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())); + + new Trigger(() -> getDeadbandedDriverController().getPOV() == 180) + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroRightAmp())); // ! /* Auto Recording */ // new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) @@ -400,8 +406,6 @@ public class RobotContainer { new JoystickButton(getDeadbandedOperatorController(), XboxController.START_BUTTON) .onTrue(new InstantCommand(() -> m_robotIntake.talonSetPivotEncoderPosition(0), m_robotIntake)); - - new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON) .onTrue(new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter)) .onFalse(turnOffShoot); @@ -426,6 +430,13 @@ public class RobotContainer { .onTrue(new InstantCommand(() -> m_robotShooter.idle())) .onFalse(new InstantCommand(() -> m_robotShooter.stop())); + new Trigger(() -> getDeadbandedOperatorController().getPOV() == 0) + .onTrue(new InstantCommand(() -> m_robotClimber.climbOut())) + .onFalse(new InstantCommand(() -> m_robotClimber.stopClimb())); + + new Trigger(() -> getDeadbandedOperatorController().getPOV() == 180) + .onTrue(new InstantCommand(() -> m_robotClimber.climbIn())) + .onFalse(new InstantCommand(() -> m_robotClimber.stopClimb())); } @@ -442,6 +453,8 @@ public class RobotContainer { // new JoystickButton(getVirtualDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown())); + new Trigger(() -> getDeadbandedDriverController().getPOV() == 0) + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroRightBlue())); // /* Operator Buttons */ diff --git a/src/main/java/frc4388/robot/subsystems/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java index 12f1343..92d682b 100644 --- a/src/main/java/frc4388/robot/subsystems/Climber.java +++ b/src/main/java/frc4388/robot/subsystems/Climber.java @@ -4,6 +4,8 @@ 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.wpilibj2.command.SubsystemBase; @@ -18,14 +20,25 @@ 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 + slot0Configs.kD = 0.06; // A velocity of 1 rps results in 0.1 V output + + climbMotor.getConfigurator().apply(slot0Configs); } public void climbOut() { - climbMotor.set(Constants.ClimbConstants.CLIMB_OUT_SPEED); + PositionVoltage request = new PositionVoltage(0); + climbMotor.setControl(request.withPosition(-520)); + //climbMotor.set(Constants.ClimbConstants.CLIMB_IN_SPEED); } public void climbIn() { - 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() { diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java index 986ed14..d55ff97 100644 --- a/src/main/java/frc4388/robot/subsystems/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/Intake.java @@ -26,6 +26,7 @@ import com.revrobotics.SparkLimitSwitch; import com.revrobotics.SparkPIDController; import com.revrobotics.RelativeEncoder; import edu.wpi.first.wpilibj.CAN; +import edu.wpi.first.wpilibj.GenericHID.RumbleType; import edu.wpi.first.wpilibj.motorcontrol.Spark; import edu.wpi.first.wpilibj.motorcontrol.Talon; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -35,6 +36,7 @@ import frc4388.robot.Constants.IntakeConstants; import frc4388.robot.commands.PID; import frc4388.utility.Gains; import frc4388.utility.configurable.ConfigurableDouble; +import frc4388.utility.controller.DeadbandedXboxController; public class Intake extends SubsystemBase { @@ -52,6 +54,9 @@ public class Intake extends SubsystemBase { private TalonFX talonPivot; private CANcoder encoder; + private DeadbandedXboxController m_driverXbox; + private DeadbandedXboxController m_operatorXbox; + private boolean r; private HardwareLimitSwitchConfigs l; @@ -96,14 +101,17 @@ public class Intake extends SubsystemBase { // } //For Talon - public Intake(TalonFX talonIntake, TalonFX talonPivot) { + public Intake(TalonFX talonIntake, TalonFX talonPivot, DeadbandedXboxController driverXbox, DeadbandedXboxController operatorXbox) { this.talonIntake = talonIntake; this.talonPivot = talonPivot; + this.m_driverXbox = driverXbox; + this.m_operatorXbox = operatorXbox; + talonIntake.getConfigurator().apply(new TalonFXConfiguration()); talonPivot.getConfigurator().apply(new TalonFXConfiguration()); - talonIntake.setNeutralMode(NeutralModeValue.Brake); + talonIntake.setNeutralMode(NeutralModeValue.Coast); talonPivot.setNeutralMode(NeutralModeValue.Brake); // talonPivot.getConfigurator().apply(new HardwareLimitSwitchConfigs()); @@ -323,6 +331,8 @@ public class Intake extends SubsystemBase { // } // } + private int rumbleTime = 0; + @Override public void periodic() { // This method will be called once per scheduler run @@ -336,5 +346,26 @@ public class Intake extends SubsystemBase { SmartDashboard.putNumber("Pivot Position", getArmPos()); smartDashboardOuttakeValue = SmartDashboard.getNumber("Outtake Speed", IntakeConstants.INTAKE_OUT_SPEED_UNPRESSED); + + // if(getTalonIntakeLimitSwitchState()){ + // rumbleTime = 1000; + // m_driverXbox.setRumble(RumbleType.kLeftRumble, 1.0); + // m_driverXbox.setRumble(RumbleType.kRightRumble, 1.0); + // m_operatorXbox.setRumble(RumbleType.kLeftRumble, 1.0); + // m_operatorXbox.setRumble(RumbleType.kRightRumble, 1.0); + // // m_hid.setRumble(RumbleType.kLeftRumble, 0.0); + // // m_hid.setRumble(RumbleType.kRightRumble, 0.0); + // } + // if(rumbleTime > 0){ + // rumbleTime--; + // if(rumbleTime <= 0){ + // rumbleTime = 0; + // m_driverXbox.setRumble(RumbleType.kLeftRumble, 0); + // m_driverXbox.setRumble(RumbleType.kRightRumble, 0); + // m_operatorXbox.setRumble(RumbleType.kLeftRumble, 0); + // m_operatorXbox.setRumble(RumbleType.kRightRumble, 0); + // } + + // } } } diff --git a/src/main/java/frc4388/robot/subsystems/Limelight.java b/src/main/java/frc4388/robot/subsystems/Limelight.java index 9c02f61..dc5e0e7 100644 --- a/src/main/java/frc4388/robot/subsystems/Limelight.java +++ b/src/main/java/frc4388/robot/subsystems/Limelight.java @@ -30,6 +30,7 @@ public class Limelight extends SubsystemBase { } private void update() { + SmartDashboard.putBoolean("Apriltag", isTag); if(!isTag){ return; } @@ -55,7 +56,6 @@ public class Limelight extends SubsystemBase { isNearSpeaker = distance <= VisionConstants.SpeakerBubbleDistance; SmartDashboard.putBoolean("nearSpeaker", isNearSpeaker); - SmartDashboard.putBoolean("Apriltag", isTag); SmartDashboard.putNumber("speakerDistance", distance); } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 3d11490..e2f3da9 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -189,6 +189,16 @@ public class SwerveDrive extends SubsystemBase { gyro.resetFlip(); rotTarget = 0.0; } + + public void resetGyroRightBlue() { + gyro.resetRightSideBlue(); + rotTarget = 0.0; + } + + public void resetGyroRightAmp() { + gyro.resetAmpSide(); + rotTarget = 0.0; + } public void stopModules() { for (SwerveModule module : this.modules) { diff --git a/src/main/java/frc4388/utility/RobotGyro.java b/src/main/java/frc4388/utility/RobotGyro.java index 09f8cf1..dd90c53 100644 --- a/src/main/java/frc4388/utility/RobotGyro.java +++ b/src/main/java/frc4388/utility/RobotGyro.java @@ -139,6 +139,28 @@ public class RobotGyro implements Gyro { } + public void resetRightSideBlue() { + resetZeroValues(); + + if (m_isGyroAPigeon) { + m_pigeon.setYaw(60); + } else { + m_navX.reset(); + } + + } + + public void resetAmpSide() { + resetZeroValues(); + + if (m_isGyroAPigeon) { + m_pigeon.setYaw(-60); + } else { + m_navX.reset(); + } + + } + /** * Get Yaw, Pitch, and Roll data. *