From be29d003365d8c054ba774cd1aa061aedc17cc5c Mon Sep 17 00:00:00 2001 From: Abhishrek05 <90010729+Abhishrek05@users.noreply.github.com> Date: Sat, 9 Mar 2024 14:31:26 -0700 Subject: [PATCH] added climber stuff and tweaked auto stuff (im tweakin) --- src/main/java/frc4388/robot/Constants.java | 10 ++++- src/main/java/frc4388/robot/Robot.java | 3 +- .../java/frc4388/robot/RobotContainer.java | 43 +++++++++++++------ src/main/java/frc4388/robot/RobotMap.java | 4 ++ .../commands/Swerve/neoJoystickPlayback.java | 6 ++- .../commands/Swerve/neoJoystickRecorder.java | 16 +++++-- .../frc4388/robot/subsystems/Climber.java | 39 +++++++++++++++++ .../frc4388/robot/subsystems/Limelight.java | 2 +- .../frc4388/robot/subsystems/Shooter.java | 38 ++++++++++++++-- 9 files changed, 136 insertions(+), 25 deletions(-) create mode 100644 src/main/java/frc4388/robot/subsystems/Climber.java diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 7d24243..bea82c4 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -171,8 +171,8 @@ 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.50; // final - public static final double SHOOTER_IDLE = 0.35; // final - public static final double SHOOTER_IDLE_LIMELIGHT = 0.8; + public static final double SHOOTER_IDLE = 0.15; // final + public static final double SHOOTER_IDLE_LIMELIGHT = 0.20; } public static final class IntakeConstants { @@ -189,6 +189,12 @@ 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 class OIConstants { public static final int XBOX_DRIVER_ID = 0; public static final int XBOX_OPERATOR_ID = 1; diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 7a67970..9276ff1 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -43,7 +43,7 @@ public class Robot extends TimedRobot { /** * This function is called every robot packet, no matter the mode. Use * this for items like diagnostics that you want ran during disabled, - * autonomous, teleoperated and test. + * autonomous, teleoperated and test.doubl * *

This runs after the mode specific periodic functions, but before * LiveWindow and SmartDashboard integrated updating. @@ -51,6 +51,7 @@ public class Robot extends TimedRobot { @Override public void robotPeriodic() { m_robotTime.updateTimes(); + 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 4647936..2961630 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -19,6 +19,7 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import edu.wpi.first.wpilibj2.command.button.Trigger; import frc4388.robot.Constants.OIConstants; import frc4388.robot.commands.Autos.AutoAlign; import frc4388.robot.commands.Autos.PlaybackChooser; @@ -32,8 +33,10 @@ import frc4388.robot.subsystems.LED; import frc4388.robot.subsystems.Limelight; import frc4388.robot.subsystems.SwerveDrive; import frc4388.robot.subsystems.Shooter; +import frc4388.robot.subsystems.Climber; import frc4388.robot.subsystems.Intake; import frc4388.utility.DeferredBlock; +import frc4388.utility.configurable.ConfigurableString; import frc4388.utility.controller.DeadbandedXboxController; import frc4388.utility.controller.VirtualController; import frc4388.utility.controller.XboxController; @@ -59,12 +62,14 @@ public class RobotContainer { m_robotMap.gyro); /* Limelight */ - private final Limelight limelight = new Limelight(); + public final Limelight limelight = new Limelight(); - private final Shooter m_robotShooter = new Shooter(m_robotMap.leftShooter, m_robotMap.rightShooter); + 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); /* Controllers */ @@ -115,7 +120,8 @@ public class RobotContainer { private SequentialCommandGroup i = new SequentialCommandGroup( - intakeToShootStuff, intakeToShoot + intakeToShootStuff, intakeToShoot, + new InstantCommand(() -> m_robotShooter.idle()) ); private SequentialCommandGroup ejectToShoot = new SequentialCommandGroup( @@ -145,6 +151,7 @@ public class RobotContainer { private Command startLeftMoveRight = new InstantCommand(); // new JoystickPlayback(m_robotSwerveDrive, "StartLeftMoveRight.txt"); private Command startRightMoveLeft = new InstantCommand(); // new JoystickPlayback(m_robotSwerveDrive, "StartRightMoveLeft.txt"); + private ConfigurableString autoplaybackName = new ConfigurableString("Auto Playback Name", "defualt.auto"); //Help Simplify Shooting // private SequentialCommandGroup pullInArmtoShoot = new SequentialCommandGroup( @@ -316,6 +323,14 @@ 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())); // ! /* Auto Recording */ // new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) @@ -334,29 +349,29 @@ public class RobotContainer { new JoystickButton(m_autoRecorderXbox, XboxController.LEFT_BUMPER_BUTTON) .whileTrue(new neoJoystickRecorder(m_robotSwerveDrive, new DeadbandedXboxController[]{getDeadbandedDriverController(), getDeadbandedOperatorController()}, - "Nuetral_Center_1note_taxi.auto")) + () -> autoplaybackName.get())) .onFalse(new InstantCommand()); new JoystickButton(m_autoRecorderXbox, XboxController.RIGHT_BUMPER_BUTTON) .onTrue(new neoJoystickPlayback(m_robotSwerveDrive, - "Nuetral_Center_1note_taxi.auto", + autoplaybackName.get(), new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()}, true, false)) .onFalse(new InstantCommand()); // ! /* Speed */ - // new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final - // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp())); + // 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.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 */ @@ -407,6 +422,10 @@ public class RobotContainer { new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_JOYSTICK_BUTTON) .onTrue(emergencyRetract); + new Trigger(() -> getDeadbandedOperatorController().getRawAxis(XboxController.RIGHT_TRIGGER_AXIS) > 0.5) + .onTrue(new InstantCommand(() -> m_robotShooter.idle())) + .onFalse(new InstantCommand(() -> m_robotShooter.stop())); + } diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 60cc5f5..cb84990 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -22,6 +22,7 @@ import edu.wpi.first.wpilibj.motorcontrol.Talon; import frc4388.robot.Constants.LEDConstants; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.robot.Constants.ShooterConstants; +import frc4388.robot.Constants.ClimbConstants; import frc4388.robot.Constants.IntakeConstants; import frc4388.robot.subsystems.SwerveModule; import frc4388.utility.RobotGyro; @@ -74,6 +75,9 @@ public class RobotMap { public final TalonFX intakeMotor = new TalonFX(IntakeConstants.INTAKE_MOTOR_ID); public final TalonFX pivotMotor = new TalonFX(IntakeConstants.PIVOT_MOTOR_ID); + /* Climber Subsystem */ + public final TalonFX climbMotor = new TalonFX(ClimbConstants.CLIMB_MOTOR_ID); + void configureLEDMotorControllers() { } diff --git a/src/main/java/frc4388/robot/commands/Swerve/neoJoystickPlayback.java b/src/main/java/frc4388/robot/commands/Swerve/neoJoystickPlayback.java index 206a2b5..c892a3b 100644 --- a/src/main/java/frc4388/robot/commands/Swerve/neoJoystickPlayback.java +++ b/src/main/java/frc4388/robot/commands/Swerve/neoJoystickPlayback.java @@ -2,6 +2,7 @@ package frc4388.robot.commands.Swerve; import java.io.FileInputStream; import java.util.ArrayList; +import java.util.function.Supplier; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.Command; @@ -13,15 +14,16 @@ import frc4388.utility.controller.VirtualController; public class neoJoystickPlayback extends Command { private final SwerveDrive swerve; - private final String filename; private final VirtualController[] controllers; private final ArrayList frames = new ArrayList<>(); + //private final Supplier filenameGetter; + private final String filename; private int frame_index = 0; private long startTime = 0; private long playbackTime = 0; private boolean m_finished = false; // ! There is no better way. private boolean m_shouldfree = false; // should free memory on ending - + private byte m_numAxes = 0; private byte m_numPOVs = 0; private byte m_numControllers = 0; diff --git a/src/main/java/frc4388/robot/commands/Swerve/neoJoystickRecorder.java b/src/main/java/frc4388/robot/commands/Swerve/neoJoystickRecorder.java index 0ad4100..d1870ec 100644 --- a/src/main/java/frc4388/robot/commands/Swerve/neoJoystickRecorder.java +++ b/src/main/java/frc4388/robot/commands/Swerve/neoJoystickRecorder.java @@ -2,6 +2,7 @@ package frc4388.robot.commands.Swerve; import java.io.FileOutputStream; import java.util.ArrayList; +import java.util.function.Supplier; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.XboxController; @@ -15,18 +16,24 @@ import frc4388.utility.controller.DeadbandedXboxController; public class neoJoystickRecorder extends Command { private final SwerveDrive swerve; private final XboxController[] controllers; - private final String filename; - private long startTime = -1; + private String filename; + private final Supplier filenameGetter; + private long startTime = -1; private final ArrayList frames = new ArrayList<>(); - public neoJoystickRecorder(SwerveDrive swerve, DeadbandedXboxController[] controllers, String filename) { + public neoJoystickRecorder(SwerveDrive swerve, DeadbandedXboxController[] controllers, Supplier filenameGetter) { this.swerve = swerve; this.controllers = controllers; - this.filename = filename; + this.filenameGetter = filenameGetter; + this.filename = ""; addRequirements(this.swerve); } + public neoJoystickRecorder(SwerveDrive swerve, DeadbandedXboxController[] controllers, String filename) { + this(swerve, controllers, () -> filename); + } + @Override public void initialize() { frames.clear(); @@ -35,6 +42,7 @@ public class neoJoystickRecorder extends Command { AutoRecordingFrame frame = new AutoRecordingFrame(); frame.controllerFrames = new AutoRecordingControllerFrame[] {new AutoRecordingControllerFrame(), new AutoRecordingControllerFrame()}; frames.add(frame); + this.filename = this.filenameGetter.get(); } diff --git a/src/main/java/frc4388/robot/subsystems/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java new file mode 100644 index 0000000..12f1343 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/Climber.java @@ -0,0 +1,39 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.subsystems; + +import com.ctre.phoenix6.hardware.TalonFX; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.Constants; +import frc4388.robot.Constants.ClimbConstants; + +//! 6.5C Scoring Criteria for Stage + +public class Climber extends SubsystemBase { + /** Creates a new Climber. */ + TalonFX climbMotor; + + public Climber(TalonFX climbMotor) { + this.climbMotor = climbMotor; + } + + public void climbOut() { + climbMotor.set(Constants.ClimbConstants.CLIMB_OUT_SPEED); + } + + public void climbIn() { + climbMotor.set(Constants.ClimbConstants.CLIMB_IN_SPEED); + } + + public void stopClimb() { + climbMotor.set(0.d); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } +} diff --git a/src/main/java/frc4388/robot/subsystems/Limelight.java b/src/main/java/frc4388/robot/subsystems/Limelight.java index 9ece406..9c02f61 100644 --- a/src/main/java/frc4388/robot/subsystems/Limelight.java +++ b/src/main/java/frc4388/robot/subsystems/Limelight.java @@ -71,7 +71,7 @@ public class Limelight extends SubsystemBase { public void periodic() { // This method will be called once per scheduler run - isTag = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tv").getBoolean(false); + isTag = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tv").getDouble(0.0) == 1.0; double[] newPose = NetworkTableInstance.getDefault().getTable("limelight").getEntry("botpose").getDoubleArray(new double[6]); if(newPose != cameraPose){ diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java index d3100a2..026e213 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -11,6 +11,8 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.IntakeConstants; import frc4388.robot.Constants.ShooterConstants; +import frc4388.robot.subsystems.Limelight; + import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.NeutralModeValue; @@ -22,13 +24,24 @@ public class Shooter extends SubsystemBase { private TalonFX leftShooter; private TalonFX rightShooter; + private Limelight limelight; + + private int spinMode = 0; + // 0 = Stop / Coast + // 1 = Idle + // 2 = Idle Near Speaker + // 3 = Spin + // 4 = SingleSpin + private double smartDashboardShooterSpeed; /** Creates a new Shooter. */ - public Shooter(TalonFX leftTalonFX, TalonFX rightTalonFX) { + public Shooter(TalonFX leftTalonFX, TalonFX rightTalonFX, Limelight tmplimelight) { leftShooter = leftTalonFX; rightShooter = rightTalonFX; + limelight = tmplimelight; + leftShooter.setNeutralMode(NeutralModeValue.Coast); rightShooter.setNeutralMode(NeutralModeValue.Coast); SmartDashboard.putNumber("Shooter Speed", ShooterConstants.SHOOTER_SPEED); @@ -42,32 +55,43 @@ public class Shooter extends SubsystemBase { public void singleSpin() { leftShooter.set(1.0); + spinMode = 4; } public void singleSpin(double speed) { leftShooter.set(speed); + spinMode = 4; } public void spin() { spin(smartDashboardShooterSpeed); + spinMode = 3; } public void spin(double speed) { leftShooter.set(-speed); rightShooter.set(-speed); + spinMode = 3; } public void spin(double leftSpeed, double rightSpeed) { leftShooter.set(leftSpeed); rightShooter.set(-rightSpeed); + spinMode = 3; } public void stop() { spin(0.d); + spinMode = 0; } public void idle() { - spin(ShooterConstants.SHOOTER_IDLE); + spin(ShooterConstants.SHOOTER_IDLE); + spinMode = 1; + } + + public int getMode(){ + return spinMode; } @Override @@ -77,6 +101,14 @@ public class Shooter extends SubsystemBase { //SmartDashboard.putNumber("Right Shooter RPM", rightShooter.getRotorVelocity().getValue()); 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. + if(limelight.isNearSpeaker() && (spinMode == 0 || spinMode == 1)){ + leftShooter.set(-ShooterConstants.SHOOTER_IDLE_LIMELIGHT); + rightShooter.set(-ShooterConstants.SHOOTER_IDLE_LIMELIGHT); + spinMode = 2; + }else if(!limelight.isNearSpeaker() && spinMode == 2){ + idle(); + } } }