diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index b653677..2781eb9 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -7,12 +7,10 @@ package frc4388.robot; -import edu.wpi.first.math.trajectory.TrapezoidProfile; - import edu.wpi.first.math.geometry.Translation2d; - -import frc4388.utility.LEDPatterns; +import edu.wpi.first.math.trajectory.TrapezoidProfile; import frc4388.utility.Gains; +import frc4388.utility.LEDPatterns; /** * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean @@ -40,10 +38,10 @@ public final class Constants { public static final double TURBO_SPEED = 4.0; public static final class DefaultSwerveRotOffsets { - public static final double FRONT_LEFT_ROT_OFFSET = 216.869140625; - public static final double FRONT_RIGHT_ROT_OFFSET = 229.4826875;//-212.695 + 90; //77.783 + 45 + 90 ;//-202.588; - public static final double BACK_LEFT_ROT_OFFSET = -277.646484375; - public static final double BACK_RIGHT_ROT_OFFSET = 140.009765625; + public static final double FRONT_LEFT_ROT_OFFSET = 220; + public static final double FRONT_RIGHT_ROT_OFFSET = 136.365;//-212.695 + 90; //77.783 + 45 + 90 ;//-202.588; + public static final double BACK_LEFT_ROT_OFFSET = -279.08349884; + public static final double BACK_RIGHT_ROT_OFFSET = 140.8887656; } public static final class IDs { @@ -191,8 +189,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 30d5cac..51b613f 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -87,9 +87,9 @@ public class RobotContainer { private ParallelCommandGroup intakeToShoot = new ParallelCommandGroup( new InstantCommand(() -> m_robotIntake.talonPIDIn()), - new InstantCommand(() -> m_robotShooter.idle()), - new InstantCommand(() -> m_driverXbox.setRumble(null, 1.0)).andThen(new WaitCommand(1)).andThen(new InstantCommand(() -> m_driverXbox.setRumble(null, 0.0))) - //new InstantCommand(() -> m_robotShooter.spin()) + new InstantCommand(() -> m_robotShooter.idle()) + // new InstantCommand(() -> m_driverXbox.setRumble(RumbleType.kRightRumble, 1.0)).andThen(new WaitCommand(0.2)).andThen(new InstantCommand(() -> m_driverXbox.setRumble(RumbleType.kRightRumble, 0.0))), + // new InstantCommand(() -> m_robotShooter.spin()) ); // private SequentialCommandGroup outtakeToShootFull = new SequentialCommandGroup( @@ -133,8 +133,8 @@ public class RobotContainer { ); private SequentialCommandGroup turnOffShoot = new SequentialCommandGroup( - new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter), - new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake) + new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter) + // new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake) ); private SequentialCommandGroup emergencyRetract = new SequentialCommandGroup( @@ -327,34 +327,46 @@ 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())); new Trigger(() -> getDeadbandedDriverController().getPOV() == 180) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroRightAmp())); + + // * /* D-Pad Stuff */ + new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.TOP_BOTTOM_DPAD_AXIS) > 0.9) + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 1), + new Translation2d(0, 0), + true))) + .onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0), + new Translation2d(0, 0), + true))); + + new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.TOP_BOTTOM_DPAD_AXIS) > -0.9) + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, -1), + new Translation2d(0, 0), + true))) + .onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0), + new Translation2d(0, 0), + true))); + + new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.LEFT_RIGHT_DPAD_AXIS) > 0.9) + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(1, 0), + new Translation2d(0, 0), + true))) + .onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0), + new Translation2d(0, 0), + true))); + + new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.LEFT_RIGHT_DPAD_AXIS) > -0.9) + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(-1, 0), + new Translation2d(0, 0), + true))) + .onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0), + new Translation2d(0, 0), + true))); // ! /* Auto Recording */ - // new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) - // .whileTrue(new JoystickRecorder(m_robotSwerveDrive, - // () -> getDeadbandedDriverController().getLeftX(), - // () -> getDeadbandedDriverController().getLeftY(), - // () -> getDeadbandedDriverController().getRightX(), - // () -> getDeadbandedDriverController().getRightY(), - // "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()}, @@ -368,19 +380,31 @@ public class RobotContainer { true, false)) .onFalse(new InstantCommand()); + // new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) + // .whileTrue(new JoystickRecorder(m_robotSwerveDrive, + // () -> getDeadbandedDriverController().getLeftX(), + // () -> getDeadbandedDriverController().getLeftY(), + // () -> getDeadbandedDriverController().getRightX(), + // () -> getDeadbandedDriverController().getRightY(), + // "Taxi.txt")) + // .onFalse(new InstantCommand()); + + // new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) + // .onTrue(new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt")) + // .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())); - // new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) - // .whileTrue(new InstantCommand(() -> - // m_robotSwerveDrive.driveWithInput(new Translation2d(0, 1), - // new Translation2d(0, 0), - // true), m_robotSwerveDrive)); + new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) + .whileTrue(new InstantCommand(() -> + m_robotSwerveDrive.driveWithInput(new Translation2d(0, 1), + new Translation2d(0, 0), + true), m_robotSwerveDrive)); //? /* Operator Buttons */ @@ -430,8 +454,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/commands/Autos/neoPlaybackChooser.java b/src/main/java/frc4388/robot/commands/Autos/neoPlaybackChooser.java new file mode 100644 index 0000000..86bc7b2 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/Autos/neoPlaybackChooser.java @@ -0,0 +1,107 @@ +// package frc4388.robot.commands.Autos; + +// import java.io.File; +// import java.util.ArrayList; +// import java.util.HashMap; + +// import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; +// import edu.wpi.first.wpilibj.shuffleboard.ComplexWidget; +// import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +// import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +// import edu.wpi.first.wpilibj2.command.Command; +// import edu.wpi.first.wpilibj2.command.InstantCommand; +// import frc4388.robot.commands.Swerve.JoystickPlayback; +// import frc4388.robot.commands.Swerve.neoJoystickPlayback; +// import frc4388.robot.subsystems.SwerveDrive; +// import frc4388.utility.controller.VirtualController; + +// public class neoPlaybackChooser { +// private final SendableChooser m_teamChosser = new SendableChooser(); +// private final SendableChooser m_possitionChosser = new SendableChooser(); +// private final SendableChooser m_autoNameChosser = new SendableChooser(); + +// private final SwerveDrive m_swerve; +// private final VirtualController[] m_controllers; +// // private final ArrayList> m_choosers = new ArrayList<>(); +// // private SendableChooser m_playback = null; +// private final ArrayList m_widgets = new ArrayList<>(); +// // private final HashMap m_commandPool = new HashMap<>(); + +// // private final File m_dir = new File("/home/lvuser/autos/"); +// // private int m_cmdNum = 0; + +// // commands +// private Command m_noAuto = new InstantCommand(); + +// public neoPlaybackChooser(SwerveDrive swerve, VirtualController[] controllers) { +// m_teamChosser.addOption("Red", "red"); +// m_teamChosser.setDefaultOption("Blue", "blue"); +// m_teamChosser.addOption("Nuetral", "nuetral"); +// m_possitionChosser.addOption("AMP", "amp"); +// m_possitionChosser.setDefaultOption("Center", "center"); +// m_possitionChosser.addOption("Source", "source"); +// m_swerve = swerve; +// m_controllers = controllers; +// } + +// public neoPlaybackChooser addOption(String name, String option) { +// m_autoNameChosser.addOption(name, option); +// return this; +// } + +// // public PlaybackChooser buildDisplay() { +// // for (int i = 0; i < 10; i++) { +// // appendCommand(); +// // } +// // m_playback = m_choosers.get(0); +// // nextChooser(); + +// // // ! This does not work, why? +// // Shuffleboard.getTab("Auto Chooser") +// // .add("Add Sequence", new InstantCommand(() -> nextChooser())) +// // .withPosition(4, 0); +// // return this; +// // } + +// // This will be bound to a button for the time being +// public void render() { +// // var chooser = new SendableChooser(); +// // // chooser.setDefaultOption("No Auto", m_noAuto); + +// // m_choosers.add(chooser); +// ComplexWidget widget = Shuffleboard.getTab("Neo Auto Chooser") +// .add("Command: " + m_choosers.size(), chooser) +// .withSize(4, 1) +// .withPosition(0, m_choosers.size() - 1) +// .withWidget(BuiltInWidgets.kSplitButtonChooser) +// .withWidget(BuiltInWidgets.kComboBoxChooser); + + +// m_widgets.add(widget); +// } + +// // public void nextChooser() { +// // SendableChooser chooser = m_choosers.get(m_cmdNum++); + +// // String[] dirs = m_dir.list(); + +// // if(dirs != null){ // Fix funny error +// // for (String auto : dirs) { +// // chooser.addOption(auto, new JoystickPlayback(m_swerve, auto)); +// // } +// // } + + +// // for (var cmd_name : m_commandPool.keySet()) { +// // chooser.addOption(cmd_name, m_commandPool.get(cmd_name)); +// // } +// // } + +// public String autoName() { +// return m_teamChosser.getSelected() + "_" + m_possitionChosser.getSelected() + "_" + m_autoNameChosser.getSelected() + ".auto"; +// } + +// public Command getCommand() { +// return new neoJoystickPlayback(m_swerve, autoName(), m_controllers, true, true); +// } +// } diff --git a/src/main/java/frc4388/robot/commands/Swerve/neoJoystickPlayback.java b/src/main/java/frc4388/robot/commands/Swerve/neoJoystickPlayback.java index a5ab006..8b5afdf 100644 --- a/src/main/java/frc4388/robot/commands/Swerve/neoJoystickPlayback.java +++ b/src/main/java/frc4388/robot/commands/Swerve/neoJoystickPlayback.java @@ -12,6 +12,11 @@ import frc4388.utility.UtilityStructs.AutoRecordingControllerFrame; import frc4388.utility.UtilityStructs.AutoRecordingFrame; import frc4388.utility.controller.VirtualController; + +/** + * The NEO autonomus playback system, designed based the old {@link JoystickPlayback} System but with {@link VirtualController}s + * @author Zachary Wilke + */ public class neoJoystickPlayback extends Command { private final SwerveDrive swerve; private final VirtualController[] controllers; @@ -29,6 +34,14 @@ public class neoJoystickPlayback extends Command { private byte m_numControllers = 0; private short m_numFrames = -1; + /** + * Creates an new NEO Joystick Playback with specifyed pramiters. + * @param swerve m_robotSwerveDrive + * @param filenameGetter a String Supplier, designed for quickly changing auto names in shuffle board. + * @param controllers an Order-Specific Array of Virtual controllers, index 0 means driver, index 1 means operator, etc. + * @param shouldfree Unloads the auto on compleation or intruption. + * @param instantload Load the auto on object instantiation + */ public neoJoystickPlayback(SwerveDrive swerve, Supplier filenameGetter, VirtualController[] controllers, boolean shouldfree, boolean instantload) { this.swerve = swerve; this.filenameGetter = filenameGetter; @@ -38,18 +51,44 @@ public class neoJoystickPlayback extends Command { if (instantload) loadAuto(); addRequirements(this.swerve); } + + /** + * Creates an new NEO Joystick Playback with specifyed pramiters. + * @param swerve m_robotSwerveDrive + * @param filename a String containing the name of the auto file you wish to playback. + * @param controllers an Order-Specific Array of Virtual controllers, index 0 means driver, index 1 means operator, etc. + * @param shouldfree unloads the auto on compleation or intruption. + * @param instantload load the auto on object instantiation + */ public neoJoystickPlayback(SwerveDrive swerve, String filename, VirtualController[] controllers, boolean shouldfree, boolean instantload) { this(swerve, () -> filename, controllers, shouldfree, instantload); } + /** + * Creates an new NEO Joystick Playback with specifyed pramiters. + * @param swerve m_robotSwerveDrive + * @param filenameGetter a String Supplier, designed for quickly changing auto names in shuffle board. + * @param controllers an Order-Specific Array of Virtual controllers, index 0 means driver, index 1 means operator, etc. + */ public neoJoystickPlayback(SwerveDrive swerve, Supplier filenameGetter, VirtualController[] controllers) { this(swerve, filenameGetter, controllers, true, false); } + /** + * Creates an new NEO Joystick Playback with specifyed pramiters. + * @param swerve m_robotSwerveDrive + * @param filename a String containing the name of the auto file you wish to playback. + * @param controllers an Order-Specific Array of Virtual controllers, index 0 means driver, index 1 means operator, etc. + */ public neoJoystickPlayback(SwerveDrive swerve, String filename, VirtualController[] controllers) { this(swerve, () -> filename, controllers, true, false); } + /** + * Load the auto file from disk into memory + * @return Returns true if loading was successful, else wise; return false + * @implNote if the auto is already loaded, it will return true. + */ public boolean loadAuto() { filename = filenameGetter.get(); try (FileInputStream stream = new FileInputStream("/home/lvuser/autos/" + filename)) { @@ -101,6 +140,9 @@ public class neoJoystickPlayback extends Command { } } + /** + * Unloads the auto. + */ public void unloadAuto() { System.out.println("AUTOPLAYBACK: Auto unloaded"); frames.clear(); diff --git a/src/main/java/frc4388/robot/commands/Swerve/neoJoystickRecorder.java b/src/main/java/frc4388/robot/commands/Swerve/neoJoystickRecorder.java index d1870ec..7f48a6c 100644 --- a/src/main/java/frc4388/robot/commands/Swerve/neoJoystickRecorder.java +++ b/src/main/java/frc4388/robot/commands/Swerve/neoJoystickRecorder.java @@ -13,6 +13,10 @@ import frc4388.utility.UtilityStructs.AutoRecordingControllerFrame; import frc4388.utility.UtilityStructs.AutoRecordingFrame; import frc4388.utility.controller.DeadbandedXboxController; +/** + * The NEO autonomus recording system, designed based the old {@link JoystickRecorder} System but with {@link frc4388.utility.controller.VirtualController VirtualController}s + * @author Zachary Wilke + */ public class neoJoystickRecorder extends Command { private final SwerveDrive swerve; private final XboxController[] controllers; @@ -21,6 +25,12 @@ public class neoJoystickRecorder extends Command { private long startTime = -1; private final ArrayList frames = new ArrayList<>(); + /** + * Creates an new NEO Joystick Playback with specifyed pramiters. + * @param swerve m_robotSwerveDrive + * @param controllers an Order-Specific Array of Virtual controllers, index 0 means driver, index 1 means operator, etc. + * @param filenameGetter a String Supplier, designed for quickly changing auto names in shuffle board. + */ public neoJoystickRecorder(SwerveDrive swerve, DeadbandedXboxController[] controllers, Supplier filenameGetter) { this.swerve = swerve; this.controllers = controllers; @@ -30,6 +40,12 @@ public class neoJoystickRecorder extends Command { addRequirements(this.swerve); } + /** + * Creates an new NEO Joystick Playback with specifyed pramiters. + * @param swerve m_robotSwerveDrive + * @param controllers an Order-Specific Array of Virtual controllers, index 0 means driver, index 1 means operator, etc. + * @param filename a String containing the name of the auto file you wish to playback. + */ public neoJoystickRecorder(SwerveDrive swerve, DeadbandedXboxController[] controllers, String filename) { this(swerve, controllers, () -> filename); } 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()); } } diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java index 986ed14..e8d48e2 100644 --- a/src/main/java/frc4388/robot/subsystems/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/Intake.java @@ -333,7 +333,7 @@ public class Intake extends SubsystemBase { resetArmPosition(); - SmartDashboard.putNumber("Pivot Position", getArmPos()); + //SmartDashboard.putNumber("Pivot Position", getArmPos()); smartDashboardOuttakeValue = SmartDashboard.getNumber("Outtake Speed", IntakeConstants.INTAKE_OUT_SPEED_UNPRESSED); } diff --git a/src/main/java/frc4388/robot/subsystems/Limelight.java b/src/main/java/frc4388/robot/subsystems/Limelight.java index dc5e0e7..44ce9ad 100644 --- a/src/main/java/frc4388/robot/subsystems/Limelight.java +++ b/src/main/java/frc4388/robot/subsystems/Limelight.java @@ -55,8 +55,8 @@ public class Limelight extends SubsystemBase { isNearSpeaker = distance <= VisionConstants.SpeakerBubbleDistance; - SmartDashboard.putBoolean("nearSpeaker", isNearSpeaker); - SmartDashboard.putNumber("speakerDistance", distance); + //SmartDashboard.putBoolean("nearSpeaker", isNearSpeaker); + //SmartDashboard.putNumber("speakerDistance", distance); } public Pose2d getPose() { diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java index 026e213..ba8ed12 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -99,7 +99,7 @@ public class Shooter extends SubsystemBase { // This method will be called once per scheduler run // SmartDashboard.putNumber("Left Shooter RPM", leftShooter.getRotorVelocity().getValue()); //SmartDashboard.putNumber("Right Shooter RPM", rightShooter.getRotorVelocity().getValue()); - smartDashboardShooterSpeed = SmartDashboard.getNumber("Shooter Speed", ShooterConstants.SHOOTER_SPEED); + //smartDashboardShooterSpeed = SmartDashboard.getNumber("Shooter Speed", ShooterConstants.SHOOTER_SPEED); // If the robot is near the speaker, and is stopped, or idled, set to limelight idle speed. // Else if the robot is not near the speaker, then set the speed back to idle. diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index e2f3da9..f979565 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -81,7 +81,7 @@ public class SwerveDrive extends SubsystemBase { // Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00)); // Convert field-relative speeds to robot-relative speeds. - chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d());//.times(-1)); + chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), rightStick.getX(), gyro.getRotation2d());//.times(-1)); } else { // Create robot-relative speeds. chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED); } @@ -218,7 +218,7 @@ public class SwerveDrive extends SubsystemBase { @Override public void periodic() { // This method will be called once per scheduler run\ - SmartDashboard.putNumber("Gyro", getGyroAngle()); + // SmartDashboard.putNumber("Gyro", getGyroAngle()); } public void shiftDown() { diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index 1ddab51..7e112f9 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -4,26 +4,34 @@ package frc4388.robot.subsystems; +import javax.swing.text.StyleContext.SmallAttributeSet; + +import com.ctre.phoenix.ErrorCode; import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.RemoteSensorSource; import com.ctre.phoenix.motorcontrol.TalonFXControlMode; import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.CANCoder; +import com.ctre.phoenix.sensors.SensorInitializationStrategy; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.utility.Gains; +import frc4388.utility.configurable.ConfigurableDouble; public class SwerveModule extends SubsystemBase { private WPI_TalonFX driveMotor; private WPI_TalonFX angleMotor; private CANCoder encoder; - + private int selfid; + private ConfigurableDouble offsetGetter; + private static int swerveId = 0; public static Gains swerveGains = SwerveDriveConstants.PIDConstants.SWERVE_GAINS; /** Creates a new SwerveModule. */ @@ -31,7 +39,9 @@ public class SwerveModule extends SubsystemBase { this.driveMotor = driveMotor; this.angleMotor = angleMotor; this.encoder = encoder; - + this.offsetGetter = new ConfigurableDouble("Swerve id " + swerveId, offset); + this.selfid = swerveId; + swerveId++; TalonFXConfiguration angleConfig = new TalonFXConfiguration(); angleConfig.slot0.kP = swerveGains.kP; angleConfig.slot0.kI = swerveGains.kI; @@ -42,13 +52,24 @@ public class SwerveModule extends SubsystemBase { angleConfig.remoteFilter0.remoteSensorSource = RemoteSensorSource.CANCoder; angleConfig.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0; angleMotor.configAllSettings(angleConfig); - + + //encoder.configSensorInitializationStrategy(SensorInitializationStrategy.BootToAbsolutePosition); + reset(0); encoder.configMagnetOffset(offset); driveMotor.setSelectedSensorPosition(0); driveMotor.config_kP(0, 0.2); } - + + @Override + public void periodic() { + //encoder.configMagnetOffset(offsetGetter.get()); + //SmartDashboard.putString("Error Code: " + selfid, getstuff()); + // SmartDashboard.putNumber("Angular Position: " + selfid, getAngle().getDegrees()); + // SmartDashboard.putNumber("Angular Velocity: " + selfid, getAngularVel()); + // SmartDashboard.putNumber("Drive Position: " + selfid, getDrivePos()); + // SmartDashboard.putNumber("Drive Velocity: " + selfid, getDriveVel()); + } /** * Get the drive motor of the SwerveModule * @return the drive motor of the SwerveModule @@ -158,4 +179,9 @@ public class SwerveModule extends SubsystemBase { public double getVoltage() { return (Math.abs(angleMotor.getMotorOutputVoltage()) + Math.abs(driveMotor.getMotorOutputVoltage())); } + + // public String getstuff() { + // encoder.getPosition(); + // return "" + encoder.getLastError().value; + // } } diff --git a/src/main/java/frc4388/utility/Configurable/ConfigurableDouble.java b/src/main/java/frc4388/utility/Configurable/ConfigurableDouble.java deleted file mode 100644 index c0384db..0000000 --- a/src/main/java/frc4388/utility/Configurable/ConfigurableDouble.java +++ /dev/null @@ -1,23 +0,0 @@ -package frc4388.utility.configurable; - -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - -public class ConfigurableDouble { - private double defualtValue; - private String name; - - /** - * Creates an new ConfigurableDouble through Smart Dashboard. - * @param name the name of the Smart Dashboard key. - * @param defualtValue the initilization value - */ - public ConfigurableDouble(String name, double defualtValue) { - this.name = name; - this.defualtValue = defualtValue; - SmartDashboard.putNumber(name, defualtValue); - } - - public double get() { - return SmartDashboard.getNumber(name, defualtValue); - } -} diff --git a/src/main/java/frc4388/utility/controller/VirtualController.java b/src/main/java/frc4388/utility/controller/VirtualController.java index 12e98cf..709bc1d 100644 --- a/src/main/java/frc4388/utility/controller/VirtualController.java +++ b/src/main/java/frc4388/utility/controller/VirtualController.java @@ -2,22 +2,39 @@ package frc4388.utility.controller; import edu.wpi.first.wpilibj.GenericHID; +/** + * A virtual controller that can be bound like an standard controller. + * @author Zachary Wilke + */ public class VirtualController extends GenericHID { private short m_buttonStates = 0; private short m_buttonStatesLastFrame = 0; private double[] m_axes = new double[6]; private short[] m_pov = new short[1]; + /** + * Create an virtual controller + * @param port virtual port (merely a formality). + */ public VirtualController(int port) { super(port); } + /** + * Set the curent inputs to the new frames. + * @param axes joystick axes, (i.e. joysticks and triggers). + * @param buttonFlags the bit packed button states. + * @param pov the array of dpads. + */ public void setFrame(double[] axes, short buttonFlags, short[] pov) { m_axes = axes; setOutputs(buttonFlags); m_pov = pov; } + /** + * Zero outs the controls. + */ public void zeroControls() { m_axes = new double[6]; m_buttonStates = 0; @@ -25,6 +42,12 @@ public class VirtualController extends GenericHID { m_pov = new short[1]; } + /** + * Gets the value of a bitflag from an int + * @param value int to search + * @param index index of bit + * @return if the bit is set + */ public static boolean getFlag(int value, int index) { return ((value & 1 << index) != 0); } @@ -90,6 +113,10 @@ public class VirtualController extends GenericHID { Hopefully this isn't a problem */ } + /** + * Use {@link VirtualController#setFrame} or {@link VirtualController#setOutputs}. + * this is an no-op overide. + */ @Override public void setOutput(int outputNumber, boolean value) { // do not use @@ -97,12 +124,20 @@ public class VirtualController extends GenericHID { //m_buttonStates[outputNumber - 1] = value; } + /** + * Set buttons from a packed int, if you want to set joysticks and dpad use {@link VirtualController#SetFrame} + */ @Override public void setOutputs(int value) { m_buttonStatesLastFrame = m_buttonStates; m_buttonStates = (short) value; } + /** + * Why are you Setting rumble on an virtual controller? + * @param type the rumble type (even though it won't do anything) + * @param value the rumble strength (always multiplyed by 0.0) + */ @Override public void setRumble(RumbleType type, double value) { System.out.println("Why are you Setting rumble on an virtual controller?");