From ba9dce23af55c4b6b598179862b31290adb0e5cb Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 12 Mar 2022 22:19:42 -0700 Subject: [PATCH] shhhtufff --- src/main/java/frc4388/robot/Constants.java | 8 ++-- .../java/frc4388/robot/RobotContainer.java | 44 ++++++++++++++----- src/main/java/frc4388/robot/RobotMap.java | 10 ++--- .../java/frc4388/robot/subsystems/Hood.java | 4 +- .../java/frc4388/robot/subsystems/Intake.java | 2 +- .../java/frc4388/robot/subsystems/Turret.java | 10 +++-- 6 files changed, 51 insertions(+), 27 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index f3dfd97..6393625 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -183,7 +183,7 @@ public final class Constants { true, 27, 0, 0); public static final int SHOOTER_FALCON_LEFT_CAN_ID = 21; public static final int SHOOTER_FALCON_RIGHT_CAN_ID = 22; - public static final double TURRET_SPEED_MULTIPLIER = 0.4d; + public static final double TURRET_SPEED_MULTIPLIER = 0.75d; public static final int DEGREES_PER_ROT = 0; public static final int TURRET_MOTOR_POS_AT_ZERO_ROT = 0; public static final int TURRET_MOTOR_ROTS_PER_ROT = 0; @@ -198,8 +198,8 @@ public final class Constants { public static final Gains SHOOTER_TURRET_GAINS = new Gains(0.6, 0.0, 0.0, 0.0, 0, 1.0); public static final Gains SHOOTER_ANGLE_GAINS = new Gains(0.1, 0.0, 0.0, 0.0, 0, 0.7); public static final double SHOOTER_TURRET_MIN = -1.0; - public static final double TURRET_FORWARD_LIMIT = 55.0; // TODO: find - public static final double TURRET_REVERSE_LIMIT = -55.0; // TODO: find + public static final double TURRET_FORWARD_LIMIT = 17.0; + public static final double TURRET_REVERSE_LIMIT = -105.0; public static final Gains DRUM_SHOOTER_GAINS = new Gains(0, 0, 0, 0, 0, 0); // TODO: tune values @@ -207,7 +207,7 @@ public final class Constants { public static final int SHOOTER_ANGLE_ADJUST_ID = 20; public static final double HOOD_MOTOR_ROTS_PER_ROT = 1; // TODO: Find public static final double HOOD_MOTOR_POS_AT_ZERO_ROT = 0; // TODO: Find - public static final double HOOD_FORWARD_LIMIT = 0.0; // TODO: find + public static final double HOOD_FORWARD_LIMIT = -0.0; // TODO: find public static final double HOOD_REVERSE_LIMIT = -150; // TODO: find } diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index c9a3ea2..2498c7c 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -105,7 +105,7 @@ public class RobotContainer { public final Storage m_robotStorage = new Storage(m_robotMap.storageMotor); // private final LED m_robotLED = new LED(m_robotMap.LEDController); public final BoomBoom m_robotBoomBoom = new BoomBoom(m_robotMap.shooterFalconLeft, m_robotMap.shooterFalconRight); - // public final Hood m_robotHood = new Hood(m_robotMap.angleAdjusterMotor); + public final Hood m_robotHood = new Hood(m_robotMap.angleAdjusterMotor); public final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret); private final VisionOdometry m_robotVisionOdometry = new VisionOdometry(m_robotSwerveDrive, m_robotTurret); private final WPI_TalonFX testElbowMotor = new WPI_TalonFX(30); @@ -134,6 +134,7 @@ public class RobotContainer { public boolean manual = false; + public static boolean softLimits = true; /** * The container for the robot. Contains subsystems, OI devices, and commands. @@ -174,8 +175,8 @@ public class RobotContainer { m_robotTurret.setDefaultCommand( new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()), m_robotTurret).withName("Turret runTurretWithInput defaultCommand")); - // m_robotHood.setDefaultCommand( - // new RunCommand(() -> m_robotHood.runHood(getOperatorController().getLeftY() * 0.15), m_robotHood)); + m_robotHood.setDefaultCommand( + new RunCommand(() -> m_robotHood.runHood(getOperatorController().getLeftY()), m_robotHood)); // m_robotTurret.setDefaultCommand( // new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry)); @@ -228,11 +229,17 @@ public class RobotContainer { /* Operator Buttons */ // X > Extend Intake - new JoystickButton(getOperatorController(), XboxController.Button.kX.value) - .whenPressed(new InstantCommand(() -> m_robotVisionOdometry.setLEDs(true), m_robotVisionOdometry)); - // Y > Retract Intake + // new JoystickButton(getOperatorController(), XboxController.Button.kX.value) + // .whenPressed(new InstantCommand(() -> m_robotVisionOdometry.setLEDs(true), m_robotVisionOdometry)); + + // new JoystickButton(getOperatorController(), XboxController.Button.kX.value) + // .whenPressed(new InstantCommand(() -> m_robotTurret.m_boomBoomRotateEncoder.setPosition(0), m_robotTurret)); + + new JoystickButton(getOperatorController(), XboxController.Button.kY.value) + .whenPressed(new InstantCommand(() -> m_robotVisionOdometry.setLEDs(false), m_robotVisionOdometry)); + // new JoystickButton(getOperatorController(), XboxController.Button.kY.value) - // .whenPressed(() -> m_robotIntake.runExtender(false)); + // .whenPressed(new InstantCommand(() -> m_robotVisionOdometry.setLEDs(false), m_robotVisionOdometry)); // new JoystickButton(getOperatorController(), XboxController.Button.kA.value) //8ft // .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.425))) @@ -252,7 +259,7 @@ public class RobotContainer { // .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0))); new JoystickButton(getOperatorController(), XboxController.Button.kX.value) //20ft - .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.1))) + .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.2))) // .whileHeld(new RunCommand(() -> m_robotHood.runAngleAdjustPID(-96))) .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0))); @@ -289,12 +296,21 @@ public class RobotContainer { // .whenPressed(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotSwerveDrive, m_robotVisionOdometry)); /* Button Box Buttons */ + new JoystickButton(getButtonBox(), ButtonBox.Button.kLeftSwitch.value) - .whileHeld(new RunCommand(() -> RunMiddleSwitch.setManual(true))) - .whenReleased(new RunCommand(() -> RunMiddleSwitch.setManual(false))); + .whenPressed(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(false), m_robotTurret)) + .whenPressed(new InstantCommand(() -> m_robotHood.setHoodSoftLimits(false), m_robotHood)) + .whenReleased(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(true), m_robotTurret)) + .whenReleased(new InstantCommand(() -> m_robotHood.setHoodSoftLimits(true), m_robotHood)) + .whenReleased(new InstantCommand(() -> m_robotTurret.m_boomBoomRotateEncoder.setPosition(0), m_robotTurret)) + .whenReleased(new InstantCommand(() -> m_robotHood.m_angleEncoder.setPosition(0), m_robotHood)); + + // new JoystickButton(getButtonBox(), ButtonBox.Button.kLeftSwitch.value) + // .whileHeld(new RunCommand(() -> RunMiddleSwitch.setManual(true))) + // .whenReleased(new RunCommand(() -> RunMiddleSwitch.setManual(false))); - new JoystickButton(getButtonBox(), ButtonBox.Button.kMiddleSwitch.value) - .whileHeld(new RunMiddleSwitch()); + // new JoystickButton(getButtonBox(), ButtonBox.Button.kMiddleSwitch.value) + // .whileHeld(new RunMiddleSwitch()); new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value) .whileHeld(new RunCommand(() -> System.out.println("RightSwitch"))); @@ -350,6 +366,10 @@ public class RobotContainer { this.manual = set; } + public static void setSoftLimits(boolean set) { + softLimits = set; + } + /** * Get odometry. * diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 5caebaa..86701be 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -236,7 +236,7 @@ public class RobotMap { ShooterConstants.SHOOTER_TIMEOUT_MS); shooterFalconRight.follow(shooterFalconLeft); - } +// } // // /* Turret Subsytem */ // shooterFalconRight.configStatorCurrentLimit(new @@ -247,10 +247,10 @@ public class RobotMap { // // numbers out of our ass anymore // // hood subsystem -// angleAdjusterMotor.restoreFactoryDefaults(); -// angleAdjusterMotor.setIdleMode(IdleMode.kBrake); -// angleAdjusterMotor.setInverted(true); -// } + angleAdjusterMotor.restoreFactoryDefaults(); + angleAdjusterMotor.setIdleMode(IdleMode.kBrake); + angleAdjusterMotor.setInverted(true); + } diff --git a/src/main/java/frc4388/robot/subsystems/Hood.java b/src/main/java/frc4388/robot/subsystems/Hood.java index 057bf2e..9ab9a1f 100644 --- a/src/main/java/frc4388/robot/subsystems/Hood.java +++ b/src/main/java/frc4388/robot/subsystems/Hood.java @@ -50,13 +50,14 @@ public class Hood extends SubsystemBase { m_angleAdjusterMotor.setSoftLimit(SoftLimitDirection.kForward, (float) ShooterConstants.HOOD_FORWARD_LIMIT); m_angleAdjusterMotor.setSoftLimit(SoftLimitDirection.kReverse, (float) ShooterConstants.HOOD_REVERSE_LIMIT); - setHoodSoftLimits(false); + setHoodSoftLimits(true); } @Override public void periodic() { // This method will be called once per scheduler run + SmartDashboard.putNumber("Hood Angle", m_angleEncoder.getPosition()); } /** @@ -87,7 +88,6 @@ public class Hood extends SubsystemBase { */ public void runHood(double input) { m_angleAdjusterMotor.set(input); - SmartDashboard.putNumber("Hood Angle", m_angleAdjusterMotor.getAlternateEncoder(1024).getPosition()); } public void resetGyroAngleAdj(){ diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java index 5d6f001..cb4a73d 100644 --- a/src/main/java/frc4388/robot/subsystems/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/Intake.java @@ -35,7 +35,7 @@ public class Intake extends SubsystemBase { * @param rightTrigger Right Trigger to Run Outward */ public void runWithTriggers(double leftTrigger, double rightTrigger) { - m_intakeMotor.set((rightTrigger - leftTrigger) * 0.3); + m_intakeMotor.set((rightTrigger - leftTrigger) * 0.4); SmartDashboard.putNumber("Intake Current Supply", m_intakeMotor.getSupplyCurrent()); SmartDashboard.putNumber("Intake Current Stator", m_intakeMotor.getStatorCurrent()); } diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index 2fdfef4..9ca1c32 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -7,7 +7,7 @@ package frc4388.robot.subsystems; import com.revrobotics.CANSparkMax.ControlType; import com.revrobotics.CANSparkMax.IdleMode; import com.revrobotics.CANSparkMax.SoftLimitDirection; - +import com.revrobotics.SparkMaxRelativeEncoder.Type; import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkMaxLimitSwitch; @@ -56,10 +56,12 @@ public class Turret extends SubsystemBase { m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kForward, (float) ShooterConstants.TURRET_FORWARD_LIMIT); m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kReverse, (float) ShooterConstants.TURRET_REVERSE_LIMIT); - setTurretSoftLimits(false); + setTurretSoftLimits(true); m_boomBoomRotateMotor.setInverted(true); + // m_boomBoomRotateMotor.getAlternateEncoder(4096).setPosition(0); + m_boomBoomRotatePIDController.setP(m_shooterTGains.kP); m_boomBoomRotatePIDController.setI(m_shooterTGains.kI); m_boomBoomRotatePIDController.setD(m_shooterTGains.kD); @@ -71,6 +73,7 @@ public class Turret extends SubsystemBase { @Override public void periodic() { // This method will be called once per scheduler run + SmartDashboard.putNumber("Turret Angle", m_boomBoomRotateEncoder.getPosition()); } /** @@ -89,7 +92,6 @@ public class Turret extends SubsystemBase { public void runTurretWithInput(double input) { m_boomBoomRotateMotor.set(input * ShooterConstants.TURRET_SPEED_MULTIPLIER); - SmartDashboard.putNumber("Turret Angle", m_boomBoomRotateMotor.getAlternateEncoder(1024).getPosition()); } public void runshooterRotatePID(double targetAngle) { @@ -102,10 +104,12 @@ public class Turret extends SubsystemBase { } public double getboomBoomRotatePosition() { + // return 0.0; return m_boomBoomRotateEncoder.getPosition(); } public double getBoomBoomAngleDegrees() { + // return 0.0; return (m_boomBoomRotateEncoder.getPosition() - ShooterConstants.TURRET_MOTOR_POS_AT_ZERO_ROT) * 360 / ShooterConstants.TURRET_MOTOR_ROTS_PER_ROT; }