From 313776ddf0f8715e26e32b0e0e5af662f9f5fcd5 Mon Sep 17 00:00:00 2001 From: Abhishrek05 <90010729+Abhishrek05@users.noreply.github.com> Date: Wed, 21 Feb 2024 00:03:15 -0700 Subject: [PATCH] AHHHHHHHHHHHHHHHHH (Im tweakin) --- src/main/java/frc4388/robot/Constants.java | 2 +- .../java/frc4388/robot/RobotContainer.java | 130 ++++---- src/main/java/frc4388/robot/RobotMap.java | 6 + .../commands/Swerve/JoystickRecorder.java | 6 +- .../java/frc4388/robot/subsystems/Intake.java | 286 ++++++++++-------- .../frc4388/robot/subsystems/Shooter.java | 2 +- .../frc4388/robot/subsystems/SwerveDrive.java | 5 + .../Configurable/ConfigurableDouble.java | 2 +- src/main/java/frc4388/utility/RobotGyro.java | 10 + .../configurable/ConfigurableDouble.java | 2 +- 10 files changed, 246 insertions(+), 205 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index f870106..7e099c0 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -169,7 +169,7 @@ public final class Constants { public static final int LEFT_SHOOTER_ID = 15; // final public static final int RIGHT_SHOOTER_ID = 16; // final public static final double SHOOTER_SPEED = 0.35; // final - public static final double SHOOTER_IDLE = 0.4; // final + public static final double SHOOTER_IDLE = 0.35; // final public static final double SHOOTER_IDLE_LIMELIGHT = 0.8; } diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 4e11195..585e3c3 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -76,20 +76,20 @@ public class RobotContainer { private Command intakeToShootStuff = new ArmIntakeIn(m_robotIntake, m_robotShooter); private SequentialCommandGroup intakeToShoot = new SequentialCommandGroup( - new InstantCommand(() -> m_robotIntake.talonPIDIn()), - new InstantCommand(() -> m_robotShooter.spin()) + new InstantCommand(() -> m_robotIntake.talonPIDIn()) + //new InstantCommand(() -> m_robotShooter.spin()) ); - private SequentialCommandGroup outtakeToShootFull = new SequentialCommandGroup( - new InstantCommand(() -> m_robotShooter.spin()), - new InstantCommand(() -> m_robotIntake.handoff()) - ); + // private SequentialCommandGroup outtakeToShootFull = new SequentialCommandGroup( + // new InstantCommand(() -> m_robotShooter.spin()), + // new InstantCommand(() -> m_robotIntake.handoff()) + // ); - private SequentialCommandGroup intakeInToOut = new SequentialCommandGroup( - new InstantCommand(() -> m_robotIntake.rotateArmOut2(), m_robotIntake), - new RunCommand(() -> m_robotIntake.limitNote(), m_robotIntake).until(m_robotIntake.getArmFowardLimitState()), - new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter) - ); + // private SequentialCommandGroup intakeInToOut = new SequentialCommandGroup( + // new InstantCommand(() -> m_robotIntake.rotateArmOut2(), m_robotIntake), + // new RunCommand(() -> m_robotIntake.limitNote(), m_robotIntake).until(m_robotIntake.getArmFowardLimitState()), + // new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter) + // ); private AutoAlign autoAlign = new AutoAlign(m_robotSwerveDrive, limelight); @@ -99,7 +99,7 @@ public class RobotContainer { autoAlign, new InstantCommand(() -> m_robotShooter.spin()), new WaitCommand(3.0), - new InstantCommand(() -> m_robotIntake.handoff(), m_robotIntake), + new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake), new WaitCommand(3.0), new InstantCommand(() -> m_robotShooter.idle()), new InstantCommand(() -> autoAlign.reverse()), @@ -112,8 +112,9 @@ public class RobotContainer { ); private SequentialCommandGroup ejectToShoot = new SequentialCommandGroup( - new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter), - new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake) + new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter) + // new WaitCommand(0.75), + //new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake) ); private SequentialCommandGroup turnOffShoot = new SequentialCommandGroup( @@ -190,7 +191,7 @@ public class RobotContainer { */ public RobotContainer() { configureButtonBindings(); - configureVirtualButtonBindings(); + // configureVirtualButtonBindings(); DriverStation.silenceJoystickConnectionWarning(true); @@ -218,43 +219,44 @@ public class RobotContainer { private void configureButtonBindings() { /* Driver Buttons */ new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON) - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive)); + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroFlip(), m_robotSwerveDrive)); /* Auto Recording */ - // new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) - // .whileTrue(new JoystickRecorder(m_robotSwerveDrive, - // () -> getDeadbandedDriverController().getLeftX(), - // () -> getDeadbandedDriverController().getLeftY(), - // () -> getDeadbandedDriverController().getRightX(), - // () -> getDeadbandedDriverController().getRightY(), - // () -> getDeadbandedOperatorController().getLeftBumper(), - // () -> getDeadbandedOperatorController().getRightBumper(), - // "TwoNotePrt1.txt")) + new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) + .whileTrue(new JoystickRecorder(m_robotSwerveDrive, m_robotShooter, m_robotIntake, + () -> getDeadbandedDriverController().getLeftX(), + () -> getDeadbandedDriverController().getLeftY(), + () -> getDeadbandedDriverController().getRightX(), + () -> getDeadbandedDriverController().getRightY(), + () -> getDeadbandedOperatorController().getLeftBumper(), + () -> getDeadbandedOperatorController().getRightBumper(), + "Taxi.txt")) + .onFalse(new InstantCommand()); + + new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) + .onTrue(new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt")) + .onFalse(new InstantCommand()); + + // new JoystickButton(m_autoRecorderXbox, XboxController.LEFT_BUMPER_BUTTON) + // .whileTrue(new neoJoystickRecorder(m_robotSwerveDrive, + // new DeadbandedXboxController[]{getDeadbandedDriverController(), getDeadbandedOperatorController()}, + // "2note.auto")) + // .onFalse(new InstantCommand()); + + // new JoystickButton(m_autoRecorderXbox, XboxController.RIGHT_BUMPER_BUTTON) + // .onTrue(new neoJoystickPlayback(m_robotSwerveDrive, + // "2note.auto", + // new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()}, + // true, false)) // .onFalse(new InstantCommand()); - // new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) - // .onTrue(new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt")) - // .onFalse(new InstantCommand()); - new JoystickButton(m_autoRecorderXbox, XboxController.LEFT_BUMPER_BUTTON) - .whileTrue(new neoJoystickRecorder(m_robotSwerveDrive, - new DeadbandedXboxController[]{getDeadbandedDriverController(), getDeadbandedOperatorController()}, - "2note.auto")) - .onFalse(new InstantCommand()); - - new JoystickButton(m_autoRecorderXbox, XboxController.RIGHT_BUMPER_BUTTON) - .onTrue(new neoJoystickPlayback(m_robotSwerveDrive, - "2note.auto", - new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()}, - true, false)) - .onFalse(new InstantCommand()); - // /* Speed */ - // new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final - // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp())); - // // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.setToFast())); + new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp())); + // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.setToFast())); - // new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final - // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown())); + new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown())); @@ -264,18 +266,18 @@ public class RobotContainer { .onTrue(new InstantCommand(() -> m_robotIntake.talonPIDIn())) .onFalse(new InstantCommand(() -> m_robotIntake.talonStopArmMotor())); - new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) + new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON) .onTrue(new InstantCommand(() -> m_robotIntake.talonPIDOut())) .onFalse(new InstantCommand(() -> m_robotIntake.talonStopArmMotor())); - new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON) - .onTrue(new InstantCommand(() -> m_robotIntake.talonSpinIntakeMotor())) + new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) + .onTrue(new InstantCommand(() -> m_robotIntake.talonHandoff())) .onFalse(new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors())); // Override Intake Position encoder: out new JoystickButton(getDeadbandedOperatorController(), XboxController.BACK_BUTTON) - .onTrue(new InstantCommand(() -> m_robotIntake.talonSetPivotEncoderPosition(-53), m_robotIntake)); + .onTrue(new InstantCommand(() -> m_robotIntake.talonSetPivotEncoderPosition(-57), m_robotIntake)); // Override Intake Position encoder: in new JoystickButton(getDeadbandedOperatorController(), XboxController.START_BUTTON) @@ -287,7 +289,7 @@ public class RobotContainer { .onFalse(new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter)); new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON) - .onTrue(ejectToShoot) + .onTrue(new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter)) .onFalse(turnOffShoot); @@ -301,7 +303,7 @@ public class RobotContainer { } - private void configureVirtualButtonBindings() { + private void configureVirtualButtonBindings() { /* Driver Buttons */ new JoystickButton(getVirtualDriverController(), XboxController.A_BUTTON) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive)); @@ -319,21 +321,21 @@ public class RobotContainer { /* Operator Buttons */ new JoystickButton(getVirtualOperatorController(), XboxController.Y_BUTTON) - .onTrue(new InstantCommand(() -> m_robotIntake.pidIn())) - .onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor())); + .onTrue(new InstantCommand(() -> m_robotIntake.talonPIDIn())) + .onFalse(new InstantCommand(() -> m_robotIntake.talonStopArmMotor())); new JoystickButton(getVirtualOperatorController(), XboxController.A_BUTTON) - .onTrue(new InstantCommand(() -> m_robotIntake.pidOut())) - .onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor())); + .onTrue(new InstantCommand(() -> m_robotIntake.talonPIDOut())) + .onFalse(new InstantCommand(() -> m_robotIntake.talonStopArmMotor())); // Override Intake Position encoder: out new JoystickButton(getVirtualOperatorController(), XboxController.BACK_BUTTON) - .onTrue(new InstantCommand(() -> m_robotIntake.setPivotEncoderPosition(-53), m_robotIntake)); + .onTrue(new InstantCommand(() -> m_robotIntake.talonSetPivotEncoderPosition(-53), m_robotIntake)); // Override Intake Position encoder: in new JoystickButton(getVirtualOperatorController(), XboxController.START_BUTTON) - .onTrue(new InstantCommand(() -> m_robotIntake.setPivotEncoderPosition(0), m_robotIntake)); + .onTrue(new InstantCommand(() -> m_robotIntake.talonSetPivotEncoderPosition(0), m_robotIntake)); //Spin Shooter Motors new JoystickButton(getVirtualOperatorController(), XboxController.X_BUTTON) @@ -347,7 +349,7 @@ public class RobotContainer { new JoystickButton(getVirtualOperatorController(), XboxController.RIGHT_BUMPER_BUTTON) .onTrue(i) - .onFalse(new InstantCommand(() -> m_robotIntake.pidIn())); + .onFalse(new InstantCommand(() -> m_robotIntake.talonPIDIn())); new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_JOYSTICK_BUTTON) @@ -361,11 +363,11 @@ public class RobotContainer { */ public Command getAutonomousCommand() { //no auto - return new neoJoystickPlayback(m_robotSwerveDrive, - "2note.auto", - new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()}, - true, false); - //return playbackChooser.getCommand(); + // return new neoJoystickPlayback(m_robotSwerveDrive, + // "2note.auto", + // new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()}, + // true, false); + return playbackChooser.getCommand(); } /** diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 4f4d20f..60cc5f5 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -11,6 +11,7 @@ import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.CANCoder; import com.ctre.phoenix.sensors.WPI_Pigeon2; +import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.hardware.TalonFX; import com.revrobotics.CANSparkLowLevel; import com.revrobotics.CANSparkMax; @@ -76,6 +77,11 @@ public class RobotMap { void configureLEDMotorControllers() { } + void configureIntakeMotorControllers() { + intakeMotor.getConfigurator().apply(new TalonFXConfiguration()); + pivotMotor.getConfigurator().apply(new TalonFXConfiguration()); + } + void configureDriveMotorControllers() { // config factory default leftFrontWheel.configFactoryDefault(); diff --git a/src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java b/src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java index 56275b8..c4c527b 100644 --- a/src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java +++ b/src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java @@ -65,7 +65,7 @@ public class JoystickRecorder extends Command { intakeToShootStuff = new ArmIntakeIn(m_robotIntake, m_robotShooter); intakeToShoot = new SequentialCommandGroup( - new InstantCommand(() -> m_robotIntake.pidIn()), + new InstantCommand(() -> m_robotIntake.talonPIDIn()), new InstantCommand(() -> m_robotShooter.spin()) ); i = new SequentialCommandGroup( @@ -105,14 +105,14 @@ public class JoystickRecorder extends Command { if(lastOPLB != inputs.OPLB && inputs.OPLB == true){ m_robotShooter.spin(); - m_robotIntake.handoff(); + m_robotIntake.talonHandoff(); }else if(lastOPLB != inputs.OPLB && inputs.OPLB == false){ } if(lastOPRB != inputs.OPRB){ m_robotShooter.spin(); - m_robotIntake.handoff(); + m_robotIntake.talonHandoff(); } System.out.println("RECORDING"); diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java index c9c87b3..8ba0205 100644 --- a/src/main/java/frc4388/robot/subsystems/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/Intake.java @@ -51,6 +51,8 @@ public class Intake extends SubsystemBase { private TalonFX talonPivot; private CANcoder encoder; + private boolean r; + private HardwareLimitSwitchConfigs l; TalonFXConfiguration doodooController = new TalonFXConfiguration(); @@ -65,33 +67,33 @@ public class Intake extends SubsystemBase { /** Creates a new Intake. */ //For NEO - public Intake(CANSparkMax intakeMotor, CANSparkMax pivot) { - this.intakeMotor = intakeMotor; - this.pivot = pivot; + // public Intake(CANSparkMax intakeMotor, CANSparkMax pivot) { + // this.intakeMotor = intakeMotor; + // this.pivot = pivot; - pivot.restoreFactoryDefaults(); - //pivot.setInverted(true); + // pivot.restoreFactoryDefaults(); + // //pivot.setInverted(true); - forwardLimit = pivot.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); - reverseLimit = pivot.getReverseLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); - forwardLimit.enableLimitSwitch(true); - reverseLimit.enableLimitSwitch(true); + // forwardLimit = pivot.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); + // reverseLimit = pivot.getReverseLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); + // forwardLimit.enableLimitSwitch(true); + // reverseLimit.enableLimitSwitch(true); - intakeMotor.restoreFactoryDefaults(); + // intakeMotor.restoreFactoryDefaults(); - intakeforwardLimit = intakeMotor.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); - intakereverseLimit = intakeMotor.getReverseLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); - intakeforwardLimit.enableLimitSwitch(true); - intakereverseLimit.enableLimitSwitch(false); + // intakeforwardLimit = intakeMotor.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); + // intakereverseLimit = intakeMotor.getReverseLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); + // intakeforwardLimit.enableLimitSwitch(true); + // intakereverseLimit.enableLimitSwitch(false); - //Arm PID - m_spedController = pivot.getPIDController(); - m_spedController.setP(armGains.kP); - m_spedController.setI(armGains.kI); - m_spedController.setD(armGains.kD); + // //Arm PID + // m_spedController = pivot.getPIDController(); + // m_spedController.setP(armGains.kP); + // m_spedController.setI(armGains.kI); + // m_spedController.setD(armGains.kD); - SmartDashboard.putNumber("Outtake Speed", IntakeConstants.INTAKE_OUT_SPEED_UNPRESSED); - } + // SmartDashboard.putNumber("Outtake Speed", IntakeConstants.INTAKE_OUT_SPEED_UNPRESSED); + // } //For Talon public Intake(TalonFX talonIntake, TalonFX talonPivot) { @@ -115,9 +117,9 @@ public class Intake extends SubsystemBase { // in init function, set slot 0 gains var slot0Configs = new Slot0Configs(); - slot0Configs.kP = 0.05; // An error of 0.5 rotations results in 12 V output + 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.0; // A velocity of 1 rps results in 0.1 V output + slot0Configs.kD = 0.06; // A velocity of 1 rps results in 0.1 V output talonPivot.getConfigurator().apply(slot0Configs); @@ -126,13 +128,13 @@ public class Intake extends SubsystemBase { // ! Talon Methods public void talonPIDIn() { - PositionVoltage request = new PositionVoltage(0).withSlot(0); - talonPivot.setControl(request.withPosition(53)); //TODO: Find actual value + PositionVoltage request = new PositionVoltage(-59); + talonPivot.setControl(request.withPosition(0)); //TODO: Find actual value } public void talonPIDOut() { - PositionVoltage request = new PositionVoltage(53).withSlot(53); - talonPivot.setControl(request.withPosition(0)); //TODO: Find actual value + PositionVoltage request = new PositionVoltage(0); + talonPivot.setControl(request.withPosition(-59)); //TODO: Find actual value } public void talonHandoff() { @@ -144,8 +146,10 @@ public class Intake extends SubsystemBase { } public boolean getTalonIntakeLimitSwitchState() { - var r = talonIntake.getForwardLimit().getValue().value == 1; - return r; + if(r = talonIntake.getForwardLimit().getValue().value == 0) { + return true; + } + return false; } public void talonSetPivotEncoderPosition(int val) { @@ -160,144 +164,158 @@ public class Intake extends SubsystemBase { talonPivot.set(0); } + public double getArmPos() { + return talonPivot.getPosition().getValue(); + } + + public void resetArmPosition() { + if(getTalonIntakeLimitSwitchState()){ + // talonPivot.setPosition(0); + } + } + // ! NEO Methods //hanoff - public void spinIntakeMotor() { - intakeMotor.set(IntakeConstants.INTAKE_SPEED); - } + // public void spinIntakeMotor() { + // intakeMotor.set(IntakeConstants.INTAKE_SPEED); + // } - //Rotate robot in for handoff - public void rotateArmIn() { - pivot.set(IntakeConstants.PIVOT_SPEED); - } + // //Rotate robot in for handoff + // public void rotateArmIn() { + // pivot.set(IntakeConstants.PIVOT_SPEED); + // } - //Rotates robot out for intake - public void rotateArmOut() { - pivot.set(-IntakeConstants.PIVOT_SPEED); + // //Rotates robot out for intake + // public void rotateArmOut() { + // pivot.set(-IntakeConstants.PIVOT_SPEED); - } + // } - public void pidIn() { - m_spedController.setReference(2.5, CANSparkMax.ControlType.kPosition); - //SmartDashboard.putNumber("Velocity Output", pivot.getEncoder().getVelocity()); - } + // public void pidIn() { + // m_spedController.setReference(2.5, CANSparkMax.ControlType.kPosition); + // //SmartDashboard.putNumber("Velocity Output", pivot.getEncoder().getVelocity()); + // } - public void pidOut() { - m_spedController.setReference(-53, CANSparkMax.ControlType.kPosition); - } + // public void pidOut() { + // m_spedController.setReference(-53, CANSparkMax.ControlType.kPosition); + // } - public void limitNote() { - if (intakeforwardLimit.isPressed()) { - rotateArmIn2(); - } else { - spinIntakeMotor(); - } - } + // public void limitNote() { + // if (intakeforwardLimit.isPressed()) { + // rotateArmIn2(); + // } else { + // spinIntakeMotor(); + // } + // } - public void rotateArmOut2() { - if(reverseLimit.isPressed()){ - stopArmMotor(); - } else { - pidOut(); - } - } + // public void rotateArmOut2() { + // if(reverseLimit.isPressed()){ + // stopArmMotor(); + // } else { + // pidOut(); + // } + // } - public void rotateArmIn2() { - if(forwardLimit.isPressed()){ - stopArmMotor(); - } else { - pidIn(); - } - } + // public void rotateArmIn2() { + // if(forwardLimit.isPressed()){ + // stopArmMotor(); + // } else { + // pidIn(); + // } + // } - public void handoff() { - intakeMotor.set(-IntakeConstants.INTAKE_OUT_SPEED_UNPRESSED); - } + // public void handoff() { + // intakeMotor.set(-IntakeConstants.INTAKE_OUT_SPEED_UNPRESSED); + // } - public void handoff2() { - if(intakeforwardLimit.isPressed()) { - intakeMotor.set(-smartDashboardOuttakeValue); - } else { - intakeMotor.set(-smartDashboardOuttakeValue); - } - } + // public void handoff2() { + // if(intakeforwardLimit.isPressed()) { + // intakeMotor.set(-smartDashboardOuttakeValue); + // } else { + // intakeMotor.set(-smartDashboardOuttakeValue); + // } + // } - public void stopIntakeMotors() { - intakeMotor.set(0); - } + // public void stopIntakeMotors() { + // intakeMotor.set(0); + // } - public void stopArmMotor() { - pivot.set(0); - } + // public void stopArmMotor() { + // pivot.set(0); + // } - public RelativeEncoder getEncoder() { - return pivot.getEncoder(); - } + // public RelativeEncoder getEncoder() { + // return pivot.getEncoder(); + // } - public boolean getForwardLimitSwitchState() { - return forwardLimit.isPressed(); - } + // public boolean getForwardLimitSwitchState() { + // return forwardLimit.isPressed(); + // } - public boolean getReverseLimitSwitchState() { - return reverseLimit.isPressed(); - } + // public boolean getReverseLimitSwitchState() { + // return reverseLimit.isPressed(); + // } - public boolean getIntakeLimitSwtichState() { - return intakeforwardLimit.isPressed(); - } + // public boolean getIntakeLimitSwtichState() { + // return intakeforwardLimit.isPressed(); + // } - public void setVoltage(double voltage) { - pivot.setVoltage(voltage); - } + // public void setVoltage(double voltage) { + // pivot.setVoltage(voltage); + // } - public double getVelocity() { - return pivot.getEncoder().getVelocity(); - } + // public double getVelocity() { + // return pivot.getEncoder().getVelocity(); + // } - public void setPivotEncoderPosition(int val) { - pivot.getEncoder().setPosition(val); - } + // public void setPivotEncoderPosition(int val) { + // pivot.getEncoder().setPosition(val); + // } - public void resetPosition() { - if(forwardLimit.isPressed()) { - setPivotEncoderPosition(0); - } - } + // public void resetPosition() { + // if(forwardLimit.isPressed()) { + // setPivotEncoderPosition(0); + // } + // } - public double getPos() { - return pivot.getEncoder().getPosition(); - } + // public double getPos() { + // return pivot.getEncoder().getPosition(); + // } - public double getIntakeVelocity() { - return intakeMotor.getEncoder().getVelocity(); - } + // public double getIntakeVelocity() { + // return intakeMotor.getEncoder().getVelocity(); + // } - public void rotateArm() { + // public void rotateArm() { - } + // } - public BooleanSupplier getArmFowardLimitState() { - if(forwardLimit.isPressed()) { - return sup; - } else { - return dup; - } - } + // public BooleanSupplier getArmFowardLimitState() { + // if(forwardLimit.isPressed()) { + // return sup; + // } else { + // return dup; + // } + // } - public void changeIntakeNeutralState() { - if(forwardLimit.isPressed()) { - intakeMotor.setIdleMode(CANSparkBase.IdleMode.kCoast); - } - } + // public void changeIntakeNeutralState() { + // if(forwardLimit.isPressed()) { + // intakeMotor.setIdleMode(CANSparkBase.IdleMode.kCoast); + // } + // } @Override public void periodic() { // This method will be called once per scheduler run - SmartDashboard.putNumber("Vel Output", getVelocity()); - SmartDashboard.putNumber("Position", getPos()); - resetPosition(); - changeIntakeNeutralState(); + // SmartDashboard.putNumber("Vel Output", getVelocity()); + // SmartDashboard.putNumber("Position", getPos()); + // resetPosition(); + // changeIntakeNeutralState(); + + resetArmPosition(); + + SmartDashboard.putNumber("Pivot Position", getArmPos()); smartDashboardOuttakeValue = SmartDashboard.getNumber("Outtake Speed", IntakeConstants.INTAKE_OUT_SPEED_UNPRESSED); } diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java index d3100a2..13c48bf 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -49,7 +49,7 @@ public class Shooter extends SubsystemBase { } public void spin() { - spin(smartDashboardShooterSpeed); + spin(ShooterConstants.SHOOTER_SPEED); } public void spin(double speed) { diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 7554671..a96e593 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -139,6 +139,11 @@ public class SwerveDrive extends SubsystemBase { gyro.reset(); rotTarget = 0.0; } + + public void resetGyroFlip() { + gyro.resetFlip(); + rotTarget = 0.0; + } public void stopModules() { for (SwerveModule module : this.modules) { diff --git a/src/main/java/frc4388/utility/Configurable/ConfigurableDouble.java b/src/main/java/frc4388/utility/Configurable/ConfigurableDouble.java index fe15818..c0384db 100644 --- a/src/main/java/frc4388/utility/Configurable/ConfigurableDouble.java +++ b/src/main/java/frc4388/utility/Configurable/ConfigurableDouble.java @@ -1,4 +1,4 @@ -package frc4388.utility.Configurable; +package frc4388.utility.configurable; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; diff --git a/src/main/java/frc4388/utility/RobotGyro.java b/src/main/java/frc4388/utility/RobotGyro.java index af5c075..4d36404 100644 --- a/src/main/java/frc4388/utility/RobotGyro.java +++ b/src/main/java/frc4388/utility/RobotGyro.java @@ -105,6 +105,16 @@ public class RobotGyro implements Gyro { } } + public void resetFlip() { + resetZeroValues(); + + if (m_isGyroAPigeon) { + m_pigeon.setYaw(180); + } else { + m_navX.reset(); + } + } + /** * Get Yaw, Pitch, and Roll data. * diff --git a/src/main/java/frc4388/utility/configurable/ConfigurableDouble.java b/src/main/java/frc4388/utility/configurable/ConfigurableDouble.java index fe15818..c0384db 100644 --- a/src/main/java/frc4388/utility/configurable/ConfigurableDouble.java +++ b/src/main/java/frc4388/utility/configurable/ConfigurableDouble.java @@ -1,4 +1,4 @@ -package frc4388.utility.Configurable; +package frc4388.utility.configurable; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;