From a649496d488eda3a64115114cf0c81e3db8f397c Mon Sep 17 00:00:00 2001 From: Jack Nelson Date: Wed, 7 Feb 2024 15:53:16 -0500 Subject: [PATCH 01/58] Update ArmSubsystem.java --- .../frc/robot/subsystems/ArmSubsystem.java | 37 +++++++++++++++---- 1 file changed, 29 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java index 34ef757..c55166d 100644 --- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java @@ -25,16 +25,19 @@ public class ArmSubsystem extends SubsystemBase { private final SparkPIDController m_armMainPIDController; private final RelativeEncoder m_MainEncoder; private final double kP, kI, kD, kIz, kMaxOutput, kMinOutput; - private final double kminArmAngle = - Units.degreesToRadians( - 0); // this is needed for the feedforward control, basically min angle relative to flat on - // floor. private static double kDt = 0.02; // 20ms (update rate for wpilib) private final double ksArmVolts = 0.0; private final double kgArmGravityGain = 0.0; private final double kvArmVoltSecondsPerMeter = 0.0; private final double kaArmVoltSecondsSquaredPerMeter = 0.0; - private final double kLoweredArmPositionRadians = Units.degreesToRadians(45); + private final double kMinArmAngleRadians = Units.degreesToRadians(0); + private final double kMaxArmAngleRadians = Units.degreesToRadians(150); + private final double kArmLoadAngleRadians = + Units.degreesToRadians(45); // angle to be when recieving ring + private final double kArmSpeakerAngleRadians = + Units.degreesToRadians(50); // angle to be when shooting into speaker + private final double kArmAmpAngleRadians = + Units.degreesToRadians(45); // angle to be when shooting into amp private final double karmMaxVelocity = 2; // m/s private final double karmMaxAcceleration = 1; // m/s^2 // general drive constants @@ -124,7 +127,24 @@ public void MoveArmRelative(double radians) { public void lowerArm() { // move to set lowered arm position - MoveArmToPosition(kLoweredArmPositionRadians); + MoveArmToPosition(kMinArmAngleRadians); + } + + public void rasieArm() { + // move to set raised arm position + MoveArmToPosition(kMaxArmAngleRadians); + } + + public void moveArmToLoad() { + MoveArmToPosition(kArmLoadAngleRadians); + } + + public void moveArmToAmp() { + MoveArmToPosition(kArmAmpAngleRadians); + } + + public void moveArmToSpeaker() { + MoveArmToPosition(kArmSpeakerAngleRadians); } /* @@ -134,8 +154,9 @@ public void MoveArmToPosition(double radians) { m_stopped = false; // update the motion profile with new goal // add minimum starting angle to the target angle to get the real angle - double total_radians = radians + kminArmAngle; - m_goal = new TrapezoidProfile.State(total_radians, 0); // set the goal + double final_radians = Math.max(radians, kMinArmAngleRadians); + final_radians = Math.min(final_radians, kMaxArmAngleRadians); + m_goal = new TrapezoidProfile.State(radians, 0); // set the goal } /* From 4336e09495538e7c8cd23b73a4b5156c9583946b Mon Sep 17 00:00:00 2001 From: Iooob <94081903+Iooob@users.noreply.github.com> Date: Wed, 7 Feb 2024 16:31:47 -0500 Subject: [PATCH 02/58] add buttons --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 24 ++++++++++++++++++--- 2 files changed, 22 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 318f2c5..a5d769e 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -42,7 +42,7 @@ public static final class CANConstants { // Joystick buttons public static final int AIMBUTTON = 12; - public static final int FIREBUTTON = 0; + public static final int TRIGGER = 0; // Analog Ports /// Ultrasonic Sensors and ports. diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9ad2067..825785c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -89,9 +89,16 @@ public class RobotContainer { private Trigger m_toggleBrakeButton; private Trigger m_lifterUpButton; private Trigger m_lifterDownButton; - private JoystickButton m_aimButton; - private JoystickButton m_fireButton; private Trigger m_driveToSpeakerButton; + //joystick buttons + private JoystickButton m_aimButton; + private JoystickButton m_armSpeakerButton; + private JoystickButton m_armAmpButton; + private JoystickButton m_armLoadButton; + private JoystickButton m_armLowerButton; + private JoystickButton m_armRaiseButton; + private JoystickButton m_enableAxisButton; + private JoystickButton m_shooterTrigger; // Init For Autonomous // private RamseteAutoBuilder autoBuilder; private SendableChooser autoDashboardChooser = new SendableChooser(); @@ -139,7 +146,18 @@ private void setupTriggers() { // Joystick buttons m_aimButton = new JoystickButton(m_flightStick, Constants.AIMBUTTON); - m_fireButton = new JoystickButton(m_flightStick, Constants.FIREBUTTON); + // arm raise buttons + m_armLoadButton = new JoystickButton(m_flightStick, Constants.ARMLOADBUTTON); + m_armSpeakerButton = new JoystickButton(m_flightStick, Constants.ARMSPEAKERBUTTON); + m_armAmpButton = new JoystickButton(m_flightStick, Constants.ARMAMPBUTTON); + m_armLowerButton = new JoystickButton(m_flightStick, Constants.ARMLOWERBUTTON); + m_armRaiseButton = new JoystickButton(m_flightStick, Constants.ARMRAISEBUTTON); + m_enableAxisButton = new JoystickButton(m_flightStick, Constants.ENABLEAXISBUTTON); + // load and shoot buttons + m_shooterTrigger = new JoystickButton(m_flightStick, Constants.TRIGGER); + + + } private void bindCommands() { From 221896aa0cd75dc9430a09f17c3303835f530cb1 Mon Sep 17 00:00:00 2001 From: Jack Nelson Date: Thu, 8 Feb 2024 14:06:28 -0500 Subject: [PATCH 03/58] add full state + fun stuff haha die Co-Authored-By: Max Aitel <91815420+TimeToDie2@users.noreply.github.com> --- src/main/java/frc/robot/Constants.java | 10 +++++ src/main/java/frc/robot/RobotContainer.java | 34 ++++++++-------- src/main/java/frc/robot/ShooterState.java | 34 ++++++++++++++++ .../java/frc/robot/commands/ArmCommand.java | 39 +++++++++++++++---- .../frc/robot/commands/ShooterCommand.java | 13 +++++-- 5 files changed, 104 insertions(+), 26 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index a5d769e..8948704 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -43,10 +43,20 @@ public static final class CANConstants { // Joystick buttons public static final int AIMBUTTON = 12; public static final int TRIGGER = 0; + public static final int ENABLEAXISBUTTON = 10; + public static final int ARMSPEAKERBUTTON = 4; + public static final int ARMAMPBUTTON = 3; + public static final int ARMLOADBUTTON = 6; // Analog Ports /// Ultrasonic Sensors and ports. public static final int ULTRASONICSHOOTERPORT = 0; // Digital Ports + + // Shooter Speeds (M/s) + public static final double SHOOTERSOURCE = -5.0; + public static final double SHOOTERAMP = 3.0; + public static final double SHOOTERSPEAKER = 5.0; + public static final double SHOOTERDEFAULT = 5.0; } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 825785c..5f6b53f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -78,7 +78,8 @@ public class RobotContainer { new UltrasonicShooterCommand(m_ultrasonicShooterSubsystem, m_shooterState); private final ArmCommand m_armCommand = new ArmCommand(m_armSubsystem, m_shooterState, this::GetFlightStickY); - private final ShooterCommand m_shooterCommand = new ShooterCommand(m_shooterSubsytem); + private final ShooterCommand m_shooterCommand = + new ShooterCommand(m_shooterSubsytem, m_shooterState); private final LifterUpCommand m_lifterUpCommand = new LifterUpCommand(m_lifterSubsystem); private final LifterDownCommand m_lifterDownCommand = new LifterDownCommand(m_lifterSubsystem); @@ -90,13 +91,11 @@ public class RobotContainer { private Trigger m_lifterUpButton; private Trigger m_lifterDownButton; private Trigger m_driveToSpeakerButton; - //joystick buttons + // joystick buttons private JoystickButton m_aimButton; - private JoystickButton m_armSpeakerButton; - private JoystickButton m_armAmpButton; - private JoystickButton m_armLoadButton; - private JoystickButton m_armLowerButton; - private JoystickButton m_armRaiseButton; + private JoystickButton m_armRaiseToSpeakerButton; + private JoystickButton m_armRaiseToAmpButton; + private JoystickButton m_armWheelLoadButton; private JoystickButton m_enableAxisButton; private JoystickButton m_shooterTrigger; // Init For Autonomous @@ -147,17 +146,12 @@ private void setupTriggers() { // Joystick buttons m_aimButton = new JoystickButton(m_flightStick, Constants.AIMBUTTON); // arm raise buttons - m_armLoadButton = new JoystickButton(m_flightStick, Constants.ARMLOADBUTTON); - m_armSpeakerButton = new JoystickButton(m_flightStick, Constants.ARMSPEAKERBUTTON); - m_armAmpButton = new JoystickButton(m_flightStick, Constants.ARMAMPBUTTON); - m_armLowerButton = new JoystickButton(m_flightStick, Constants.ARMLOWERBUTTON); - m_armRaiseButton = new JoystickButton(m_flightStick, Constants.ARMRAISEBUTTON); + m_armWheelLoadButton = new JoystickButton(m_flightStick, Constants.ARMLOADBUTTON); + m_armRaiseToSpeakerButton = new JoystickButton(m_flightStick, Constants.ARMSPEAKERBUTTON); + m_armRaiseToAmpButton = new JoystickButton(m_flightStick, Constants.ARMAMPBUTTON); m_enableAxisButton = new JoystickButton(m_flightStick, Constants.ENABLEAXISBUTTON); // load and shoot buttons m_shooterTrigger = new JoystickButton(m_flightStick, Constants.TRIGGER); - - - } private void bindCommands() { @@ -165,11 +159,19 @@ private void bindCommands() { m_balanceButton.whileTrue(m_balanceCommand); m_straightButton.whileTrue(m_straightCommand); m_aimButton.whileTrue(m_aimCommand); - m_fireButton.whileTrue(m_shooterCommand); m_driveToSpeakerButton.whileTrue(m_driveToSpeaker); m_lifterUpButton.whileTrue(m_lifterUpCommand); m_lifterDownButton.whileTrue(m_lifterDownCommand); m_toggleBrakeButton.whileTrue(new InstantCommand(() -> m_driveSubsystem.SwitchBrakemode())); + // shooter + arm commands + m_shooterTrigger.whileTrue(m_shooterCommand); + m_armRaiseToSpeakerButton.whileTrue( + new InstantCommand(() -> m_shooterState.setMode(ShooterState.ShooterMode.SPEAKER))); + m_armRaiseToAmpButton.whileTrue( + new InstantCommand(() -> m_shooterState.setMode(ShooterState.ShooterMode.AMP))); + m_armWheelLoadButton.whileTrue( + new InstantCommand(() -> m_shooterState.setMode(ShooterState.ShooterMode.SOURCE))); + m_enableAxisButton.whileTrue(new InstantCommand(() -> m_shooterState.toggleAxis())); } private void bindDriveSysIDCommands() { diff --git a/src/main/java/frc/robot/ShooterState.java b/src/main/java/frc/robot/ShooterState.java index fd4358c..e494f7e 100644 --- a/src/main/java/frc/robot/ShooterState.java +++ b/src/main/java/frc/robot/ShooterState.java @@ -4,9 +4,20 @@ * ShooterState.java * Tracks the status of a ring loaded in the shooter */ + public class ShooterState { + public final class ShooterMode { + public static final int DEFAULT = 0; + public static final int SOURCE = 1; + public static final int AMP = 2; + public static final int SPEAKER = 3; + } + public boolean isLoaded = false; public boolean isLowered = false; + public int mode = ShooterMode.SOURCE; + public boolean axisEnabled = false; + public boolean shooting = false; public ShooterState() {} @@ -14,12 +25,35 @@ public void setLoaded() { isLoaded = true; } + public void setMode(int newMode) { + mode = newMode; + } + + public void toggleAxis() { + axisEnabled = !axisEnabled; + } + public void setFired() { isLoaded = false; isLowered = false; + mode = ShooterMode.SOURCE; } public void setLowered() { isLowered = true; + mode = ShooterMode.DEFAULT; + } + + public double getShooterSpeed() { + switch (mode) { + case ShooterMode.SOURCE: // TODO + return Constants.SHOOTERSOURCE; + case ShooterMode.AMP: + return Constants.SHOOTERAMP; + case ShooterMode.SPEAKER: + return Constants.SHOOTERSPEAKER; + default: + return Constants.SHOOTERDEFAULT; + } } } diff --git a/src/main/java/frc/robot/commands/ArmCommand.java b/src/main/java/frc/robot/commands/ArmCommand.java index 5e2e4db..7ffaa26 100644 --- a/src/main/java/frc/robot/commands/ArmCommand.java +++ b/src/main/java/frc/robot/commands/ArmCommand.java @@ -20,6 +20,7 @@ public class ArmCommand extends Command { private final ShooterState m_shooterState; private final DoubleSupplier m_yAxis; private final double kMaxRadiansPerInput = Units.degreesToRadians(5); + private final int lastMode = 0; /** * Creates a new ExampleCommand. @@ -41,14 +42,38 @@ public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if (!HelperFunctions.inDeadzone(m_yAxis.getAsDouble(), Constants.CONTROLLERDEADZONE)) { - m_ArmSubsystem.MoveArmRelative(m_yAxis.getAsDouble() * kMaxRadiansPerInput); + if (!m_shooterState.shooting) { + if (!HelperFunctions.inDeadzone(m_yAxis.getAsDouble(), Constants.CONTROLLERDEADZONE) + && m_shooterState.axisEnabled) { + m_ArmSubsystem.MoveArmRelative(m_yAxis.getAsDouble() * kMaxRadiansPerInput); - } else if (m_shooterState.isLoaded & !m_shooterState.isLowered) { - m_ArmSubsystem.lowerArm(); - m_shooterState.setLowered(); - } else { - m_ArmSubsystem.stop(); + } else if (m_shooterState.isLoaded & !m_shooterState.isLowered) { + m_ArmSubsystem.lowerArm(); + m_shooterState.setLowered(); + } else if (!m_shooterState.isLoaded & !m_shooterState.isLowered) { + m_ArmSubsystem.lowerArm(); + } else if (lastMode != m_shooterState.mode) { + followState(); + } else { + m_ArmSubsystem.stop(); + } + } + } + + private void followState() { + switch (m_shooterState.mode) { + case ShooterState.ShooterMode.SOURCE: + m_ArmSubsystem.moveArmToLoad(); + break; + case ShooterState.ShooterMode.AMP: + m_ArmSubsystem.moveArmToAmp(); + break; + case ShooterState.ShooterMode.SPEAKER: + m_ArmSubsystem.moveArmToSpeaker(); + break; + default: + m_ArmSubsystem.lowerArm(); + break; } } diff --git a/src/main/java/frc/robot/commands/ShooterCommand.java b/src/main/java/frc/robot/commands/ShooterCommand.java index 2d14be6..075ce57 100644 --- a/src/main/java/frc/robot/commands/ShooterCommand.java +++ b/src/main/java/frc/robot/commands/ShooterCommand.java @@ -5,6 +5,7 @@ package frc.robot.commands; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.ShooterState; import frc.robot.subsystems.ShooterSubsystem; /** A Shooter Command that uses an example subsystem. */ @@ -12,13 +13,16 @@ public class ShooterCommand extends Command { @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) private final ShooterSubsystem m_shooterSubsystem; + private final ShooterState m_shooterState; + /** * Creates a new ShooterCommand. * * @param subsystem The subsystem used by this command. */ - public ShooterCommand(ShooterSubsystem s_subsystem) { + public ShooterCommand(ShooterSubsystem s_subsystem, ShooterState shooterState) { m_shooterSubsystem = s_subsystem; + m_shooterState = shooterState; // Use addRequirements() here to declare subsystem dependencies. addRequirements(s_subsystem); } @@ -32,12 +36,15 @@ public void initialize() {} public void execute() { // TODO: Finish Implementing ShooterCommand System.out.println("ShooterCommand Activated"); - m_shooterSubsystem.SpinShooterFull(); + m_shooterSubsystem.SpinShooter(m_shooterState.getShooterSpeed()); + m_shooterState.shooting = true; } // Called once the command ends or is interrupted. @Override - public void end(boolean interrupted) {} + public void end(boolean interrupted) { + m_shooterState.shooting = false; + } // Returns true when the command should end. @Override From 39ad0e6b3a97b341f46aa35cee3f5dc7e0c79705 Mon Sep 17 00:00:00 2001 From: Jack Nelson Date: Thu, 8 Feb 2024 14:08:37 -0500 Subject: [PATCH 04/58] Update CameraSubsystem.java --- src/main/java/frc/robot/subsystems/CameraSubsystem.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/CameraSubsystem.java b/src/main/java/frc/robot/subsystems/CameraSubsystem.java index f518a12..31c4a9b 100644 --- a/src/main/java/frc/robot/subsystems/CameraSubsystem.java +++ b/src/main/java/frc/robot/subsystems/CameraSubsystem.java @@ -22,7 +22,7 @@ public class CameraSubsystem extends SubsystemBase { private final DriveSubsystem m_driveSubsystem; public final AprilTagFieldLayout aprilTagFieldLayout; - private final String frontCameraName = "OV5647"; + private final String frontCameraName = "cam"; private final PhotonCamera frontCamera; public PhotonPipelineResult frontCameraResult; // Physical location of camera relative to center From 7883a4fb1e31d0a0dbb53f4dbc18294f16151371 Mon Sep 17 00:00:00 2001 From: Jack Nelson Date: Mon, 12 Feb 2024 16:22:18 -0500 Subject: [PATCH 05/58] vendor updates --- vendordeps/PathplannerLib.json | 6 +++--- vendordeps/REVLib.json | 10 +++++----- vendordeps/photonlib.json | 10 +++++----- 3 files changed, 13 insertions(+), 13 deletions(-) diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json index cae1363..ff15fab 100644 --- a/vendordeps/PathplannerLib.json +++ b/vendordeps/PathplannerLib.json @@ -1,7 +1,7 @@ { "fileName": "PathplannerLib.json", "name": "PathplannerLib", - "version": "2024.1.6", + "version": "2024.2.3", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2024", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2024.1.6" + "version": "2024.2.3" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2024.1.6", + "version": "2024.2.3", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json index 0f3520e..a829581 100644 --- a/vendordeps/REVLib.json +++ b/vendordeps/REVLib.json @@ -1,7 +1,7 @@ { "fileName": "REVLib.json", "name": "REVLib", - "version": "2024.2.0", + "version": "2024.2.1", "frcYear": "2024", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2024.2.0" + "version": "2024.2.1" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2024.2.0", + "version": "2024.2.1", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -37,7 +37,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2024.2.0", + "version": "2024.2.1", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -55,7 +55,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2024.2.0", + "version": "2024.2.1", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json index d4b0b67..46211fc 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -1,7 +1,7 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "v2024.2.3", + "version": "v2024.2.4", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", "frcYear": "2024", "mavenUrls": [ @@ -14,7 +14,7 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-cpp", - "version": "v2024.2.3", + "version": "v2024.2.4", "libName": "photonlib", "headerClassifier": "headers", "sharedLibrary": true, @@ -29,7 +29,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2024.2.3", + "version": "v2024.2.4", "libName": "photontargeting", "headerClassifier": "headers", "sharedLibrary": true, @@ -46,12 +46,12 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-java", - "version": "v2024.2.3" + "version": "v2024.2.4" }, { "groupId": "org.photonvision", "artifactId": "photontargeting-java", - "version": "v2024.2.3" + "version": "v2024.2.4" } ] } \ No newline at end of file From 354690ba62407f65c15c56e39fcfbb925e0fcce0 Mon Sep 17 00:00:00 2001 From: Jack Nelson Date: Mon, 12 Feb 2024 19:00:11 -0500 Subject: [PATCH 06/58] Update ArmSubsystem.java --- src/main/java/frc/robot/subsystems/ArmSubsystem.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java index c55166d..3d72f5f 100644 --- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java @@ -43,7 +43,7 @@ public class ArmSubsystem extends SubsystemBase { // general drive constants // https://www.chiefdelphi.com/t/encoders-velocity-to-m-s/390332/2 // https://sciencing.com/convert-rpm-linear-speed-8232280.html - private final double kGearRatio = 1; // TBD + private final double kGearRatio = 48; // TBD, 48:1 / kGearRatio:1 // basically converted from rotations to radians by multiplying by 2 pi, then adjusting for the // gear ratio by dividing by the gear ratio. // remember that 2pi radians in 360 degrees. From 67681f4aaaaaf543c1e1d78d526fe6996ba7ab25 Mon Sep 17 00:00:00 2001 From: Iooob <94081903+Iooob@users.noreply.github.com> Date: Wed, 14 Feb 2024 15:19:39 -0500 Subject: [PATCH 07/58] Add default mode joystick button Added button for arm to go flat (default mode) --- src/main/java/frc/robot/Constants.java | 13 +++++++++---- src/main/java/frc/robot/RobotContainer.java | 4 ++++ 2 files changed, 13 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 8948704..0147e1e 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -41,12 +41,17 @@ public static final class CANConstants { // Now In RobotContainer as Native Triggers. // Joystick buttons - public static final int AIMBUTTON = 12; - public static final int TRIGGER = 0; - public static final int ENABLEAXISBUTTON = 10; + public static final int TRIGGER = 1; + public static final int ARMAMPBUTTON = 3; public static final int ARMSPEAKERBUTTON = 4; - public static final int ARMAMPBUTTON = 3; + public static final int ARMDEFAULTBUTTON = 5; public static final int ARMLOADBUTTON = 6; + public static final int ENABLEAXISBUTTON = 10; + public static final int AIMBUTTON = 12; + + + + // Analog Ports /// Ultrasonic Sensors and ports. diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5f6b53f..e6ed755 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -14,6 +14,7 @@ import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.simulation.JoystickSim; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; @@ -98,6 +99,7 @@ public class RobotContainer { private JoystickButton m_armWheelLoadButton; private JoystickButton m_enableAxisButton; private JoystickButton m_shooterTrigger; + private JoystickButton m_armDefault; // Init For Autonomous // private RamseteAutoBuilder autoBuilder; private SendableChooser autoDashboardChooser = new SendableChooser(); @@ -150,6 +152,8 @@ private void setupTriggers() { m_armRaiseToSpeakerButton = new JoystickButton(m_flightStick, Constants.ARMSPEAKERBUTTON); m_armRaiseToAmpButton = new JoystickButton(m_flightStick, Constants.ARMAMPBUTTON); m_enableAxisButton = new JoystickButton(m_flightStick, Constants.ENABLEAXISBUTTON); + m_armDefault = new JoystickButton(m_flightStick, Constants.ARMDEFAULTBUTTON); + // load and shoot buttons m_shooterTrigger = new JoystickButton(m_flightStick, Constants.TRIGGER); } From 52594117fb8b7ba322e2e3bfe1285a6473857bbc Mon Sep 17 00:00:00 2001 From: Jack Nelson Date: Wed, 14 Feb 2024 15:35:43 -0500 Subject: [PATCH 08/58] add shooterstate to dash fix pid --- src/main/java/frc/robot/Constants.java | 10 +++------- src/main/java/frc/robot/RobotContainer.java | 5 ++--- src/main/java/frc/robot/ShooterState.java | 10 ++++++++++ .../java/frc/robot/subsystems/ArmSubsystem.java | 13 +++++++++---- 4 files changed, 24 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 0147e1e..6739926 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -41,17 +41,13 @@ public static final class CANConstants { // Now In RobotContainer as Native Triggers. // Joystick buttons - public static final int TRIGGER = 1; - public static final int ARMAMPBUTTON = 3; + public static final int TRIGGER = 1; + public static final int ARMAMPBUTTON = 3; public static final int ARMSPEAKERBUTTON = 4; public static final int ARMDEFAULTBUTTON = 5; public static final int ARMLOADBUTTON = 6; - public static final int ENABLEAXISBUTTON = 10; + public static final int ENABLEAXISBUTTON = 10; public static final int AIMBUTTON = 12; - - - - // Analog Ports /// Ultrasonic Sensors and ports. diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e6ed755..632a81a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -14,7 +14,6 @@ import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.XboxController; -import edu.wpi.first.wpilibj.simulation.JoystickSim; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; @@ -63,7 +62,7 @@ public class RobotContainer { private final DriveSubsystem m_driveSubsystem = new DriveSubsystem(); private final CameraSubsystem m_cameraSubsystem = new CameraSubsystem(m_driveSubsystem); - private final ArmSubsystem m_armSubsystem = new ArmSubsystem(); + private final ArmSubsystem m_armSubsystem = new ArmSubsystem(m_shooterState); private final ShooterSubsystem m_shooterSubsytem = new ShooterSubsystem(); private final LifterSubsystem m_lifterSubsystem = new LifterSubsystem(); // The robots commands are defined here.. @@ -153,7 +152,7 @@ private void setupTriggers() { m_armRaiseToAmpButton = new JoystickButton(m_flightStick, Constants.ARMAMPBUTTON); m_enableAxisButton = new JoystickButton(m_flightStick, Constants.ENABLEAXISBUTTON); m_armDefault = new JoystickButton(m_flightStick, Constants.ARMDEFAULTBUTTON); - + // load and shoot buttons m_shooterTrigger = new JoystickButton(m_flightStick, Constants.TRIGGER); } diff --git a/src/main/java/frc/robot/ShooterState.java b/src/main/java/frc/robot/ShooterState.java index e494f7e..bbcf47c 100644 --- a/src/main/java/frc/robot/ShooterState.java +++ b/src/main/java/frc/robot/ShooterState.java @@ -1,5 +1,7 @@ package frc.robot; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + /* * ShooterState.java * Tracks the status of a ring loaded in the shooter @@ -56,4 +58,12 @@ public double getShooterSpeed() { return Constants.SHOOTERDEFAULT; } } + + public void updateDash() { + SmartDashboard.putBoolean("Manual Arm Mode Enabled", axisEnabled); + SmartDashboard.putNumber("Arm Mode", mode); + SmartDashboard.putBoolean("Loaded", isLoaded); + SmartDashboard.putBoolean("Lowered", isLowered); + SmartDashboard.putBoolean("Arm Shooting", shooting); + } } diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java index 3d72f5f..ad7f874 100644 --- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java @@ -19,8 +19,10 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Constants.CANConstants; +import frc.robot.ShooterState; public class ArmSubsystem extends SubsystemBase { + private final ShooterState m_shooterState; private final CANSparkMax m_armMotorMain; private final SparkPIDController m_armMainPIDController; private final RelativeEncoder m_MainEncoder; @@ -61,7 +63,8 @@ public class ArmSubsystem extends SubsystemBase { private final SysIdRoutine m_sysIdRoutine; /** Creates a new ArmSubsystem. */ - public ArmSubsystem() { + public ArmSubsystem(ShooterState shooterState) { + m_shooterState = shooterState; // create the arm motors m_armMotorMain = new CANSparkMax(CANConstants.MOTORARMMAINID, CANSparkMax.MotorType.kBrushless); @@ -75,9 +78,9 @@ public ArmSubsystem() { // setup the encoders m_MainEncoder.setPositionConversionFactor(kRadiansConversionRatio); // PID coefficients - kP = 0.1; - kI = 1e-4; - kD = 1; + kP = 0.0625; + kI = 0; + kD = 0; kIz = 0; kMaxOutput = 1; kMinOutput = -1; @@ -88,6 +91,7 @@ public ArmSubsystem() { m_armMainPIDController.setD(kD); m_armMainPIDController.setIZone(kIz); m_armMainPIDController.setOutputRange(kMinOutput, kMaxOutput); + m_armMotorMain.burnFlash(); // setup SysID for auto profiling m_sysIdRoutine = @@ -187,6 +191,7 @@ public void periodic() { CANSparkBase.ControlType.kPosition, 0, m_armFeedforward.calculate(m_setpoint.position, m_setpoint.velocity)); + m_shooterState.updateDash(); } @Override From 5c145f49cf5157f24ca5bdcd931bcc2e1182654d Mon Sep 17 00:00:00 2001 From: Jack Nelson Date: Wed, 14 Feb 2024 16:19:46 -0500 Subject: [PATCH 09/58] reee --- src/main/java/frc/robot/commands/ArmCommand.java | 4 +--- src/main/java/frc/robot/subsystems/ArmSubsystem.java | 2 +- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/commands/ArmCommand.java b/src/main/java/frc/robot/commands/ArmCommand.java index 7ffaa26..7e9d0f3 100644 --- a/src/main/java/frc/robot/commands/ArmCommand.java +++ b/src/main/java/frc/robot/commands/ArmCommand.java @@ -43,10 +43,8 @@ public void initialize() {} @Override public void execute() { if (!m_shooterState.shooting) { - if (!HelperFunctions.inDeadzone(m_yAxis.getAsDouble(), Constants.CONTROLLERDEADZONE) - && m_shooterState.axisEnabled) { + if ( (!HelperFunctions.inDeadzone(m_yAxis.getAsDouble(), Constants.CONTROLLERDEADZONE)) && m_shooterState.axisEnabled) { m_ArmSubsystem.MoveArmRelative(m_yAxis.getAsDouble() * kMaxRadiansPerInput); - } else if (m_shooterState.isLoaded & !m_shooterState.isLowered) { m_ArmSubsystem.lowerArm(); m_shooterState.setLowered(); diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java index ad7f874..00e8a76 100644 --- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java @@ -78,7 +78,7 @@ public ArmSubsystem(ShooterState shooterState) { // setup the encoders m_MainEncoder.setPositionConversionFactor(kRadiansConversionRatio); // PID coefficients - kP = 0.0625; + kP = 0.9; kI = 0; kD = 0; kIz = 0; From 4488b498b9526e74da9936694f3a318547617b77 Mon Sep 17 00:00:00 2001 From: Jack Nelson Date: Wed, 14 Feb 2024 16:19:50 -0500 Subject: [PATCH 10/58] Update RobotContainer.java --- src/main/java/frc/robot/RobotContainer.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 632a81a..ecf73f5 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -114,8 +114,8 @@ public RobotContainer() { setupTriggers(); // Bind the commands to the triggers if (enableAutoProfiling) { - bindDriveSysIDCommands(); - // bindArmSysIDCommands(); + // bindDriveSysIDCommands(); + bindArmSysIDCommands(); // bindShooterSysIDCommands(); } else { bindCommands(); From 20d21a8e1c98f2408e98a2d3ab70cce808f30e9f Mon Sep 17 00:00:00 2001 From: Jack Nelson Date: Fri, 16 Feb 2024 19:00:21 -0500 Subject: [PATCH 11/58] add default arm cmd --- src/main/java/frc/robot/RobotContainer.java | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ecf73f5..2afd835 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -98,7 +98,7 @@ public class RobotContainer { private JoystickButton m_armWheelLoadButton; private JoystickButton m_enableAxisButton; private JoystickButton m_shooterTrigger; - private JoystickButton m_armDefault; + private JoystickButton m_armRaiseToDefault; // Init For Autonomous // private RamseteAutoBuilder autoBuilder; private SendableChooser autoDashboardChooser = new SendableChooser(); @@ -151,7 +151,7 @@ private void setupTriggers() { m_armRaiseToSpeakerButton = new JoystickButton(m_flightStick, Constants.ARMSPEAKERBUTTON); m_armRaiseToAmpButton = new JoystickButton(m_flightStick, Constants.ARMAMPBUTTON); m_enableAxisButton = new JoystickButton(m_flightStick, Constants.ENABLEAXISBUTTON); - m_armDefault = new JoystickButton(m_flightStick, Constants.ARMDEFAULTBUTTON); + m_armRaiseToDefault = new JoystickButton(m_flightStick, Constants.ARMDEFAULTBUTTON); // load and shoot buttons m_shooterTrigger = new JoystickButton(m_flightStick, Constants.TRIGGER); @@ -174,6 +174,8 @@ private void bindCommands() { new InstantCommand(() -> m_shooterState.setMode(ShooterState.ShooterMode.AMP))); m_armWheelLoadButton.whileTrue( new InstantCommand(() -> m_shooterState.setMode(ShooterState.ShooterMode.SOURCE))); + m_armRaiseToDefault.whileTrue( + new InstantCommand(() -> m_shooterState.setMode(ShooterState.ShooterMode.DEFAULT))); m_enableAxisButton.whileTrue(new InstantCommand(() -> m_shooterState.toggleAxis())); } From d1b083041b0bada99e5ec982c3e97462c5b6f989 Mon Sep 17 00:00:00 2001 From: Jack Nelson Date: Fri, 16 Feb 2024 19:01:48 -0500 Subject: [PATCH 12/58] change arm gear and lockout values --- src/main/java/frc/robot/subsystems/ArmSubsystem.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java index 00e8a76..be0cf78 100644 --- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java @@ -33,7 +33,7 @@ public class ArmSubsystem extends SubsystemBase { private final double kvArmVoltSecondsPerMeter = 0.0; private final double kaArmVoltSecondsSquaredPerMeter = 0.0; private final double kMinArmAngleRadians = Units.degreesToRadians(0); - private final double kMaxArmAngleRadians = Units.degreesToRadians(150); + private final double kMaxArmAngleRadians = Units.degreesToRadians(190); private final double kArmLoadAngleRadians = Units.degreesToRadians(45); // angle to be when recieving ring private final double kArmSpeakerAngleRadians = @@ -45,7 +45,7 @@ public class ArmSubsystem extends SubsystemBase { // general drive constants // https://www.chiefdelphi.com/t/encoders-velocity-to-m-s/390332/2 // https://sciencing.com/convert-rpm-linear-speed-8232280.html - private final double kGearRatio = 48; // TBD, 48:1 / kGearRatio:1 + private final double kGearRatio = 120; // TBD, 48:1 / kGearRatio:1 // basically converted from rotations to radians by multiplying by 2 pi, then adjusting for the // gear ratio by dividing by the gear ratio. // remember that 2pi radians in 360 degrees. From c339edffcfb43642ca8f96580bfd1af4cb8e0965 Mon Sep 17 00:00:00 2001 From: Jack Nelson Date: Fri, 16 Feb 2024 19:02:55 -0500 Subject: [PATCH 13/58] add code for 2 shooters --- src/main/java/frc/robot/Constants.java | 3 ++- src/main/java/frc/robot/commands/ArmCommand.java | 3 ++- .../java/frc/robot/subsystems/LifterSubsystem.java | 4 ++++ .../java/frc/robot/subsystems/ShooterSubsystem.java | 12 +++++++++--- 4 files changed, 17 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 6739926..be3c6ed 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -22,7 +22,8 @@ public static final class CANConstants { /// Arm Motors public static final int MOTORARMMAINID = 21; /// Shooter Motors - public static final int MOTORSHOOTERID = 31; + public static final int MOTORSHOOTERLEFTID = 31; + public static final int MOTORSHOOTERRIGHTID = 32; /// Lifter Motors public static final int MOTORLIFTERLEFTID = 41; public static final int MOTORLIFTERRIGHTID = 42; diff --git a/src/main/java/frc/robot/commands/ArmCommand.java b/src/main/java/frc/robot/commands/ArmCommand.java index 7e9d0f3..90d2bb1 100644 --- a/src/main/java/frc/robot/commands/ArmCommand.java +++ b/src/main/java/frc/robot/commands/ArmCommand.java @@ -43,7 +43,8 @@ public void initialize() {} @Override public void execute() { if (!m_shooterState.shooting) { - if ( (!HelperFunctions.inDeadzone(m_yAxis.getAsDouble(), Constants.CONTROLLERDEADZONE)) && m_shooterState.axisEnabled) { + if ((!HelperFunctions.inDeadzone(m_yAxis.getAsDouble(), Constants.CONTROLLERDEADZONE)) + && m_shooterState.axisEnabled) { m_ArmSubsystem.MoveArmRelative(m_yAxis.getAsDouble() * kMaxRadiansPerInput); } else if (m_shooterState.isLoaded & !m_shooterState.isLowered) { m_ArmSubsystem.lowerArm(); diff --git a/src/main/java/frc/robot/subsystems/LifterSubsystem.java b/src/main/java/frc/robot/subsystems/LifterSubsystem.java index 938d386..6acafea 100644 --- a/src/main/java/frc/robot/subsystems/LifterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/LifterSubsystem.java @@ -16,6 +16,10 @@ public class LifterSubsystem extends SubsystemBase { public LifterSubsystem() { m_left = new CANSparkMax(CANConstants.MOTORLIFTERLEFTID, CANSparkMax.MotorType.kBrushless); m_right = new CANSparkMax(CANConstants.MOTORLIFTERRIGHTID, CANSparkMax.MotorType.kBrushless); + m_left.setIdleMode(CANSparkMax.IdleMode.kBrake); + m_right.setIdleMode(CANSparkMax.IdleMode.kBrake); + m_left.burnFlash(); + m_right.burnFlash(); } public void leftUp(double speed) { diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 5cdff47..8089f2d 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -21,6 +21,7 @@ public class ShooterSubsystem extends SubsystemBase { private final CANSparkMax m_ShooterMotorMain; + private final CANSparkMax m_ShooterMotorSecondary; private final SparkPIDController m_ShooterMainPIDController; private RelativeEncoder m_ShooterMainEncoder; private final double kP, kI, kD, kIz, kMaxOutput, kMinOutput, kMaxSpeed; @@ -28,7 +29,7 @@ public class ShooterSubsystem extends SubsystemBase { // https://www.chiefdelphi.com/t/encoders-velocity-to-m-s/390332/2 // https://sciencing.com/convert-rpm-linear-speed-8232280.html private final double kWheelDiameter = Units.inchesToMeters(6); // meters - private final double kGearRatio = 1; // TBD + private final double kGearRatio = 4; // TBD // basically converted from rotations to to radians to then meters using the wheel diameter. // the diameter is already *2 so we don't need to multiply by 2 again. private final double kVelocityConversionRatio = (Math.PI * kWheelDiameter) / kGearRatio / 60; @@ -49,9 +50,12 @@ public class ShooterSubsystem extends SubsystemBase { public ShooterSubsystem() { // create the shooter motors m_ShooterMotorMain = - new CANSparkMax(CANConstants.MOTORSHOOTERID, CANSparkMax.MotorType.kBrushless); + new CANSparkMax(CANConstants.MOTORSHOOTERLEFTID, CANSparkMax.MotorType.kBrushless); + m_ShooterMotorSecondary = new CANSparkMax(CANConstants.MOTORSHOOTERRIGHTID, CANSparkMax.MotorType.kBrushless); // set the idle mode to coast - m_ShooterMotorMain.setIdleMode(CANSparkMax.IdleMode.kCoast); + m_ShooterMotorMain.setIdleMode(CANSparkMax.IdleMode.kBrake); + m_ShooterMotorSecondary.setIdleMode(CANSparkMax.IdleMode.kBrake); + m_ShooterMotorSecondary.follow(m_ShooterMotorMain, true); // connect to built in PID controller m_ShooterMainPIDController = m_ShooterMotorMain.getPIDController(); @@ -82,6 +86,8 @@ public ShooterSubsystem() { (voltage) -> this.setVoltage(voltage), null, // No log consumer, since data is recorded by URCL this)); + m_ShooterMotorMain.burnFlash(); + m_ShooterMotorSecondary.burnFlash(); } public void setVoltage(Measure voltage) { From b68d7195c03771db504a1490c4666258c3c390c4 Mon Sep 17 00:00:00 2001 From: Jack Nelson Date: Mon, 19 Feb 2024 12:25:12 -0500 Subject: [PATCH 14/58] update ver --- build.gradle | 2 +- .../java/frc/robot/subsystems/ShooterSubsystem.java | 3 ++- vendordeps/URCL.json | 12 ++++++------ 3 files changed, 9 insertions(+), 8 deletions(-) diff --git a/build.gradle b/build.gradle index 1ba507c..ff5a9b0 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2024.2.1" + id "edu.wpi.first.GradleRIO" version "2024.3.1" id 'com.diffplug.spotless' version '6.20.0' id "com.peterabeles.gversion" version "1.10" } diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 8089f2d..97074fe 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -51,7 +51,8 @@ public ShooterSubsystem() { // create the shooter motors m_ShooterMotorMain = new CANSparkMax(CANConstants.MOTORSHOOTERLEFTID, CANSparkMax.MotorType.kBrushless); - m_ShooterMotorSecondary = new CANSparkMax(CANConstants.MOTORSHOOTERRIGHTID, CANSparkMax.MotorType.kBrushless); + m_ShooterMotorSecondary = + new CANSparkMax(CANConstants.MOTORSHOOTERRIGHTID, CANSparkMax.MotorType.kBrushless); // set the idle mode to coast m_ShooterMotorMain.setIdleMode(CANSparkMax.IdleMode.kBrake); m_ShooterMotorSecondary.setIdleMode(CANSparkMax.IdleMode.kBrake); diff --git a/vendordeps/URCL.json b/vendordeps/URCL.json index d2c0e48..998c261 100644 --- a/vendordeps/URCL.json +++ b/vendordeps/URCL.json @@ -1,25 +1,25 @@ { "fileName": "URCL.json", "name": "URCL", - "version": "2024.0.1", + "version": "2024.1.0", "frcYear": "2024", "uuid": "84246d17-a797-4d1e-bd9f-c59cd8d2477c", "mavenUrls": [ - "https://raw.githubusercontent.com/Mechanical-Advantage/URCL/2024.0.1" + "https://raw.githubusercontent.com/Mechanical-Advantage/URCL/2024.1.0" ], "jsonUrl": "https://raw.githubusercontent.com/Mechanical-Advantage/URCL/maven/URCL.json", "javaDependencies": [ { "groupId": "org.littletonrobotics.urcl", "artifactId": "URCL-java", - "version": "2024.0.1" + "version": "2024.1.0" } ], "jniDependencies": [ { "groupId": "org.littletonrobotics.urcl", "artifactId": "URCL-driver", - "version": "2024.0.1", + "version": "2024.1.0", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -34,7 +34,7 @@ { "groupId": "org.littletonrobotics.urcl", "artifactId": "URCL-cpp", - "version": "2024.0.1", + "version": "2024.1.0", "libName": "URCL", "headerClassifier": "headers", "sharedLibrary": false, @@ -49,7 +49,7 @@ { "groupId": "org.littletonrobotics.urcl", "artifactId": "URCL-driver", - "version": "2024.0.1", + "version": "2024.1.0", "libName": "URCLDriver", "headerClassifier": "headers", "sharedLibrary": false, From 842973f4146a97e65a0bf47420c2c4ac167220dc Mon Sep 17 00:00:00 2001 From: Jack Nelson Date: Thu, 22 Feb 2024 12:25:53 -0500 Subject: [PATCH 15/58] update deps + calibrate shooter --- src/main/java/frc/robot/Constants.java | 8 ++++---- src/main/java/frc/robot/commands/ShooterCommand.java | 1 + .../java/frc/robot/subsystems/ShooterSubsystem.java | 10 +++++----- vendordeps/photonlib.json | 10 +++++----- 4 files changed, 15 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index be3c6ed..b6b724f 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -57,8 +57,8 @@ public static final class CANConstants { // Digital Ports // Shooter Speeds (M/s) - public static final double SHOOTERSOURCE = -5.0; - public static final double SHOOTERAMP = 3.0; - public static final double SHOOTERSPEAKER = 5.0; - public static final double SHOOTERDEFAULT = 5.0; + public static final double SHOOTERSOURCE = -20.0; + public static final double SHOOTERAMP = 20.0; + public static final double SHOOTERSPEAKER = 8.0; + public static final double SHOOTERDEFAULT = 8.0; } diff --git a/src/main/java/frc/robot/commands/ShooterCommand.java b/src/main/java/frc/robot/commands/ShooterCommand.java index 075ce57..533b702 100644 --- a/src/main/java/frc/robot/commands/ShooterCommand.java +++ b/src/main/java/frc/robot/commands/ShooterCommand.java @@ -44,6 +44,7 @@ public void execute() { @Override public void end(boolean interrupted) { m_shooterState.shooting = false; + m_shooterSubsystem.StopShooter(); } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 97074fe..2bdacfe 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -28,16 +28,16 @@ public class ShooterSubsystem extends SubsystemBase { // general drive constants // https://www.chiefdelphi.com/t/encoders-velocity-to-m-s/390332/2 // https://sciencing.com/convert-rpm-linear-speed-8232280.html - private final double kWheelDiameter = Units.inchesToMeters(6); // meters + private final double kWheelDiameter = Units.inchesToMeters(4); // meters private final double kGearRatio = 4; // TBD // basically converted from rotations to to radians to then meters using the wheel diameter. // the diameter is already *2 so we don't need to multiply by 2 again. private final double kVelocityConversionRatio = (Math.PI * kWheelDiameter) / kGearRatio / 60; // setup feedforward - private final double ksShooterVolts = 0.0; - private final double kvDriveVoltSecondsPerMeter = 0.0; - private final double kaDriveVoltSecondsSquaredPerMeter = 0.0; + private final double ksShooterVolts = 0.17875; + private final double kvDriveVoltSecondsPerMeter = 1.5442; + private final double kaDriveVoltSecondsSquaredPerMeter = 0.019079; SimpleMotorFeedforward m_shooterFeedForward = new SimpleMotorFeedforward( @@ -65,7 +65,7 @@ public ShooterSubsystem() { m_ShooterMainEncoder = m_ShooterMotorMain.getEncoder(); m_ShooterMainEncoder.setVelocityConversionFactor(kVelocityConversionRatio); // PID coefficients - kP = 6e-5; + kP = 0.00032423; kI = 0; kD = 0; kIz = 0; diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json index 46211fc..8b1044d 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -1,7 +1,7 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "v2024.2.4", + "version": "v2024.2.6", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", "frcYear": "2024", "mavenUrls": [ @@ -14,7 +14,7 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-cpp", - "version": "v2024.2.4", + "version": "v2024.2.6", "libName": "photonlib", "headerClassifier": "headers", "sharedLibrary": true, @@ -29,7 +29,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2024.2.4", + "version": "v2024.2.6", "libName": "photontargeting", "headerClassifier": "headers", "sharedLibrary": true, @@ -46,12 +46,12 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-java", - "version": "v2024.2.4" + "version": "v2024.2.6" }, { "groupId": "org.photonvision", "artifactId": "photontargeting-java", - "version": "v2024.2.4" + "version": "v2024.2.6" } ] } \ No newline at end of file From ba0afc6383d8d7339279d6f6739c5861d54acc17 Mon Sep 17 00:00:00 2001 From: Jack Nelson Date: Thu, 22 Feb 2024 12:42:25 -0500 Subject: [PATCH 16/58] fix arm trying to kill us --- src/main/java/frc/robot/RobotContainer.java | 1 + .../frc/robot/subsystems/ArmSubsystem.java | 25 +++++++++++-------- 2 files changed, 16 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2afd835..052b74d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -193,6 +193,7 @@ private void bindArmSysIDCommands() { m_controller1.x().whileTrue(m_armSubsystem.sysIdDynamic(SysIdRoutine.Direction.kForward)); m_controller1.y().whileTrue(m_armSubsystem.sysIdDynamic(SysIdRoutine.Direction.kReverse)); m_controller1.leftTrigger().whileTrue(new InstantCommand(() -> DataLogManager.stop())); + m_armSubsystem.disablePID(); } private void bindShooterSysIDCommands() { diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java index be0cf78..6c14ee4 100644 --- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java @@ -42,6 +42,7 @@ public class ArmSubsystem extends SubsystemBase { Units.degreesToRadians(45); // angle to be when shooting into amp private final double karmMaxVelocity = 2; // m/s private final double karmMaxAcceleration = 1; // m/s^2 + private boolean kPIDEnabled = true; // general drive constants // https://www.chiefdelphi.com/t/encoders-velocity-to-m-s/390332/2 // https://sciencing.com/convert-rpm-linear-speed-8232280.html @@ -178,19 +179,23 @@ public void zeroEncoders() { m_MainEncoder.setPosition(0); } + public void disablePID() { + kPIDEnabled = false; + } + @Override public void periodic() { // This method will be called once per scheduler run - - // update the setpoint - m_setpoint = m_armTrapezoidProfile.calculate(kDt, m_setpoint, m_goal); - - // Call the controller and feedforward with the target position and velocity - m_armMainPIDController.setReference( - m_setpoint.position, - CANSparkBase.ControlType.kPosition, - 0, - m_armFeedforward.calculate(m_setpoint.position, m_setpoint.velocity)); + if (kPIDEnabled) { + // update the setpoint + m_setpoint = m_armTrapezoidProfile.calculate(kDt, m_setpoint, m_goal); + // Call the controller and feedforward with the target position and velocity + m_armMainPIDController.setReference( + m_setpoint.position, + CANSparkBase.ControlType.kPosition, + 0, + m_armFeedforward.calculate(m_setpoint.position, m_setpoint.velocity)); + } m_shooterState.updateDash(); } From 08bed35c7851c5307a4db14a73ae33711d99f71e Mon Sep 17 00:00:00 2001 From: Jack Nelson Date: Thu, 22 Feb 2024 13:13:03 -0500 Subject: [PATCH 17/58] better numbers --- .../frc/robot/subsystems/ArmSubsystem.java | 21 ++++++++++--------- 1 file changed, 11 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java index 6c14ee4..38985b1 100644 --- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java @@ -28,10 +28,10 @@ public class ArmSubsystem extends SubsystemBase { private final RelativeEncoder m_MainEncoder; private final double kP, kI, kD, kIz, kMaxOutput, kMinOutput; private static double kDt = 0.02; // 20ms (update rate for wpilib) - private final double ksArmVolts = 0.0; - private final double kgArmGravityGain = 0.0; - private final double kvArmVoltSecondsPerMeter = 0.0; - private final double kaArmVoltSecondsSquaredPerMeter = 0.0; + private final double ksArmVolts = 0.08521; + private final double kgArmGravityGain = 0.1711; + private final double kvArmVoltSecondsPerMeter = 0.0022383; + private final double kaArmVoltSecondsSquaredPerMeter = 0.00041162; private final double kMinArmAngleRadians = Units.degreesToRadians(0); private final double kMaxArmAngleRadians = Units.degreesToRadians(190); private final double kArmLoadAngleRadians = @@ -40,8 +40,8 @@ public class ArmSubsystem extends SubsystemBase { Units.degreesToRadians(50); // angle to be when shooting into speaker private final double kArmAmpAngleRadians = Units.degreesToRadians(45); // angle to be when shooting into amp - private final double karmMaxVelocity = 2; // m/s - private final double karmMaxAcceleration = 1; // m/s^2 + private final double karmMaxVelocity = 0.025; // m/s + private final double karmMaxAcceleration = 0.005; // m/s^2 private boolean kPIDEnabled = true; // general drive constants // https://www.chiefdelphi.com/t/encoders-velocity-to-m-s/390332/2 @@ -71,6 +71,7 @@ public ArmSubsystem(ShooterState shooterState) { // set the idle mode to brake m_armMotorMain.setIdleMode(CANSparkMax.IdleMode.kBrake); + m_armMotorMain.setInverted(true); // connect to built in PID controller m_armMainPIDController = m_armMotorMain.getPIDController(); @@ -79,12 +80,12 @@ public ArmSubsystem(ShooterState shooterState) { // setup the encoders m_MainEncoder.setPositionConversionFactor(kRadiansConversionRatio); // PID coefficients - kP = 0.9; + kP = 1.0935; kI = 0; - kD = 0; + kD = 0.0042903; kIz = 0; - kMaxOutput = 1; - kMinOutput = -1; + kMaxOutput = 0.25; + kMinOutput = -0.25; // set PID coefficients m_armMainPIDController.setP(kP); From 6d8cf2a8020d827169cf8b313e3ca608bcc895c7 Mon Sep 17 00:00:00 2001 From: Jack Nelson Date: Fri, 23 Feb 2024 20:04:04 -0500 Subject: [PATCH 18/58] update photonlib --- vendordeps/photonlib.json | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json index 8b1044d..26a28d5 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -1,7 +1,7 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "v2024.2.6", + "version": "v2024.2.7", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", "frcYear": "2024", "mavenUrls": [ @@ -14,7 +14,7 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-cpp", - "version": "v2024.2.6", + "version": "v2024.2.7", "libName": "photonlib", "headerClassifier": "headers", "sharedLibrary": true, @@ -29,7 +29,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2024.2.6", + "version": "v2024.2.7", "libName": "photontargeting", "headerClassifier": "headers", "sharedLibrary": true, @@ -46,12 +46,12 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-java", - "version": "v2024.2.6" + "version": "v2024.2.7" }, { "groupId": "org.photonvision", "artifactId": "photontargeting-java", - "version": "v2024.2.6" + "version": "v2024.2.7" } ] } \ No newline at end of file From bce6588697bb9c4a34f9f8d5d3b5b1b7f4e1e9ea Mon Sep 17 00:00:00 2001 From: Jack Nelson Date: Fri, 23 Feb 2024 20:04:41 -0500 Subject: [PATCH 19/58] Update Constants.java --- src/main/java/frc/robot/Constants.java | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index b6b724f..bcd48da 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -45,7 +45,7 @@ public static final class CANConstants { public static final int TRIGGER = 1; public static final int ARMAMPBUTTON = 3; public static final int ARMSPEAKERBUTTON = 4; - public static final int ARMDEFAULTBUTTON = 5; + public static final int ARMDEFAULTBUTTON = 2; public static final int ARMLOADBUTTON = 6; public static final int ENABLEAXISBUTTON = 10; public static final int AIMBUTTON = 12; @@ -56,9 +56,15 @@ public static final class CANConstants { // Digital Ports + // Shooter Angles + public static final double ARMANGLEOFFSET = 22.5; // WHY MaTH HURT + public static final double ARMLOADANGLE = 45; + public static final double ARMSPEAKERANGLE = + 70; // our start is like 40 degrees or something lol - ma + public static final double ARMAMPANGLE = 50; // Shooter Speeds (M/s) - public static final double SHOOTERSOURCE = -20.0; - public static final double SHOOTERAMP = 20.0; + public static final double SHOOTERSOURCE = -8.0; + public static final double SHOOTERAMP = 3.0; public static final double SHOOTERSPEAKER = 8.0; - public static final double SHOOTERDEFAULT = 8.0; + public static final double SHOOTERDEFAULT = 8.0; // somewhere around 8 is the cap - ma } From 012e5b0d1bda1c55030961de9c4c0b6701d3289b Mon Sep 17 00:00:00 2001 From: Jack Nelson Date: Fri, 23 Feb 2024 20:05:59 -0500 Subject: [PATCH 20/58] shooterstate refactor --- src/main/java/frc/robot/ShooterState.java | 27 ++++++++++--------- .../java/frc/robot/commands/ArmCommand.java | 16 +++++------ 2 files changed, 23 insertions(+), 20 deletions(-) diff --git a/src/main/java/frc/robot/ShooterState.java b/src/main/java/frc/robot/ShooterState.java index bbcf47c..f7714ff 100644 --- a/src/main/java/frc/robot/ShooterState.java +++ b/src/main/java/frc/robot/ShooterState.java @@ -8,16 +8,19 @@ */ public class ShooterState { - public final class ShooterMode { - public static final int DEFAULT = 0; - public static final int SOURCE = 1; - public static final int AMP = 2; - public static final int SPEAKER = 3; - } public boolean isLoaded = false; public boolean isLowered = false; - public int mode = ShooterMode.SOURCE; + + public enum ShooterMode { + DEFAULT, + SOURCE, + AMP, + SPEAKER, + STOP + }; + + public ShooterMode mode = ShooterMode.DEFAULT; public boolean axisEnabled = false; public boolean shooting = false; @@ -27,7 +30,7 @@ public void setLoaded() { isLoaded = true; } - public void setMode(int newMode) { + public void setMode(ShooterMode newMode) { mode = newMode; } @@ -48,11 +51,11 @@ public void setLowered() { public double getShooterSpeed() { switch (mode) { - case ShooterMode.SOURCE: // TODO + case SOURCE: // TODO return Constants.SHOOTERSOURCE; - case ShooterMode.AMP: + case AMP: return Constants.SHOOTERAMP; - case ShooterMode.SPEAKER: + case SPEAKER: return Constants.SHOOTERSPEAKER; default: return Constants.SHOOTERDEFAULT; @@ -61,7 +64,7 @@ public double getShooterSpeed() { public void updateDash() { SmartDashboard.putBoolean("Manual Arm Mode Enabled", axisEnabled); - SmartDashboard.putNumber("Arm Mode", mode); + SmartDashboard.putString("Arm Mode", mode.name()); SmartDashboard.putBoolean("Loaded", isLoaded); SmartDashboard.putBoolean("Lowered", isLowered); SmartDashboard.putBoolean("Arm Shooting", shooting); diff --git a/src/main/java/frc/robot/commands/ArmCommand.java b/src/main/java/frc/robot/commands/ArmCommand.java index 90d2bb1..a9750ca 100644 --- a/src/main/java/frc/robot/commands/ArmCommand.java +++ b/src/main/java/frc/robot/commands/ArmCommand.java @@ -20,7 +20,6 @@ public class ArmCommand extends Command { private final ShooterState m_shooterState; private final DoubleSupplier m_yAxis; private final double kMaxRadiansPerInput = Units.degreesToRadians(5); - private final int lastMode = 0; /** * Creates a new ExampleCommand. @@ -51,28 +50,29 @@ public void execute() { m_shooterState.setLowered(); } else if (!m_shooterState.isLoaded & !m_shooterState.isLowered) { m_ArmSubsystem.lowerArm(); - } else if (lastMode != m_shooterState.mode) { - followState(); } else { - m_ArmSubsystem.stop(); + followState(); } } } private void followState() { switch (m_shooterState.mode) { - case ShooterState.ShooterMode.SOURCE: + case SOURCE: m_ArmSubsystem.moveArmToLoad(); break; - case ShooterState.ShooterMode.AMP: + case AMP: m_ArmSubsystem.moveArmToAmp(); break; - case ShooterState.ShooterMode.SPEAKER: + case SPEAKER: m_ArmSubsystem.moveArmToSpeaker(); break; - default: + case DEFAULT: m_ArmSubsystem.lowerArm(); break; + case STOP: + m_ArmSubsystem.stop(); + break; } } From 7170236776ce32d0d9b63d08de7773617117899a Mon Sep 17 00:00:00 2001 From: Jack Nelson Date: Fri, 23 Feb 2024 20:10:55 -0500 Subject: [PATCH 21/58] fix arm subsystem --- src/main/java/frc/robot/Constants.java | 2 +- .../frc/robot/subsystems/ArmSubsystem.java | 23 ++++++++++--------- 2 files changed, 13 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index bcd48da..9888b7d 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -57,7 +57,7 @@ public static final class CANConstants { // Digital Ports // Shooter Angles - public static final double ARMANGLEOFFSET = 22.5; // WHY MaTH HURT + public static final double ARMSTARTINGANGLE = 22.5; // WHY MaTH HURT public static final double ARMLOADANGLE = 45; public static final double ARMSPEAKERANGLE = 70; // our start is like 40 degrees or something lol - ma diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java index 38985b1..be37229 100644 --- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java @@ -15,9 +15,11 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.units.Measure; import edu.wpi.first.units.Voltage; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.robot.Constants; import frc.robot.Constants.CANConstants; import frc.robot.ShooterState; @@ -32,16 +34,16 @@ public class ArmSubsystem extends SubsystemBase { private final double kgArmGravityGain = 0.1711; private final double kvArmVoltSecondsPerMeter = 0.0022383; private final double kaArmVoltSecondsSquaredPerMeter = 0.00041162; - private final double kMinArmAngleRadians = Units.degreesToRadians(0); - private final double kMaxArmAngleRadians = Units.degreesToRadians(190); + private final double kMinArmAngleRadians = Units.degreesToRadians(Constants.ARMSTARTINGANGLE); + private final double kMaxArmAngleRadians = Units.degreesToRadians(90); private final double kArmLoadAngleRadians = - Units.degreesToRadians(45); // angle to be when recieving ring + Units.degreesToRadians(Constants.ARMLOADANGLE); // angle to be when recieving ring private final double kArmSpeakerAngleRadians = - Units.degreesToRadians(50); // angle to be when shooting into speaker + Units.degreesToRadians(Constants.ARMSPEAKERANGLE); // angle to be when shooting into speaker private final double kArmAmpAngleRadians = - Units.degreesToRadians(45); // angle to be when shooting into amp - private final double karmMaxVelocity = 0.025; // m/s - private final double karmMaxAcceleration = 0.005; // m/s^2 + Units.degreesToRadians(Constants.ARMAMPANGLE); // angle to be when shooting into amp + private final double karmMaxVelocity = 1.0; // m/s + private final double karmMaxAcceleration = 0.5; // m/s^2 private boolean kPIDEnabled = true; // general drive constants // https://www.chiefdelphi.com/t/encoders-velocity-to-m-s/390332/2 @@ -79,6 +81,7 @@ public ArmSubsystem(ShooterState shooterState) { m_MainEncoder = m_armMotorMain.getEncoder(); // setup the encoders m_MainEncoder.setPositionConversionFactor(kRadiansConversionRatio); + m_MainEncoder.setPosition(Units.degreesToRadians(Constants.ARMSTARTINGANGLE)); // PID coefficients kP = 1.0935; kI = 0; @@ -176,10 +179,6 @@ public void stop() { } } - public void zeroEncoders() { - m_MainEncoder.setPosition(0); - } - public void disablePID() { kPIDEnabled = false; } @@ -198,6 +197,8 @@ public void periodic() { m_armFeedforward.calculate(m_setpoint.position, m_setpoint.velocity)); } m_shooterState.updateDash(); + SmartDashboard.putNumber( + "Current Arm Angle (Degrees)", Units.radiansToDegrees(m_MainEncoder.getPosition())); } @Override From 42ae4451a1055c424729a52b249af46154e7eaed Mon Sep 17 00:00:00 2001 From: Jack Nelson Date: Fri, 23 Feb 2024 20:12:59 -0500 Subject: [PATCH 22/58] rewrite lifters to left and right also add direction switch button --- src/main/java/frc/robot/RobotContainer.java | 40 +++++++++------ ...ownCommand.java => LeftLifterCommand.java} | 16 +++--- ...UpCommand.java => RightLifterCommand.java} | 20 +++----- .../robot/subsystems/LeftLifterSubsystem.java | 49 +++++++++++++++++++ ...bsystem.java => RightLifterSubsystem.java} | 29 +++++------ 5 files changed, 98 insertions(+), 56 deletions(-) rename src/main/java/frc/robot/commands/{LifterDownCommand.java => LeftLifterCommand.java} (73%) rename src/main/java/frc/robot/commands/{LifterUpCommand.java => RightLifterCommand.java} (67%) create mode 100644 src/main/java/frc/robot/subsystems/LeftLifterSubsystem.java rename src/main/java/frc/robot/subsystems/{LifterSubsystem.java => RightLifterSubsystem.java} (61%) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 052b74d..35faeb3 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -26,15 +26,16 @@ import frc.robot.commands.ArmCommand; import frc.robot.commands.BalanceCommand; import frc.robot.commands.DefaultDrive; -import frc.robot.commands.LifterDownCommand; -import frc.robot.commands.LifterUpCommand; +import frc.robot.commands.LeftLifterCommand; +import frc.robot.commands.RightLifterCommand; import frc.robot.commands.ShooterCommand; import frc.robot.commands.StraightCommand; import frc.robot.commands.UltrasonicShooterCommand; import frc.robot.subsystems.ArmSubsystem; import frc.robot.subsystems.CameraSubsystem; import frc.robot.subsystems.DriveSubsystem; -import frc.robot.subsystems.LifterSubsystem; +import frc.robot.subsystems.LeftLifterSubsystem; +import frc.robot.subsystems.RightLifterSubsystem; import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.UltrasonicSubsystem; @@ -64,7 +65,8 @@ public class RobotContainer { private final CameraSubsystem m_cameraSubsystem = new CameraSubsystem(m_driveSubsystem); private final ArmSubsystem m_armSubsystem = new ArmSubsystem(m_shooterState); private final ShooterSubsystem m_shooterSubsytem = new ShooterSubsystem(); - private final LifterSubsystem m_lifterSubsystem = new LifterSubsystem(); + private final LeftLifterSubsystem m_leftLifterSubsystem = new LeftLifterSubsystem(); + private final RightLifterSubsystem m_rightLifterSubsystem = new RightLifterSubsystem(); // The robots commands are defined here.. // private final ExampleCommand m_autoCommand = new ExampleCommand(m_exampleSubsystem); @@ -80,17 +82,20 @@ public class RobotContainer { new ArmCommand(m_armSubsystem, m_shooterState, this::GetFlightStickY); private final ShooterCommand m_shooterCommand = new ShooterCommand(m_shooterSubsytem, m_shooterState); - private final LifterUpCommand m_lifterUpCommand = new LifterUpCommand(m_lifterSubsystem); - private final LifterDownCommand m_lifterDownCommand = new LifterDownCommand(m_lifterSubsystem); + private final LeftLifterCommand m_LeftLifterCommand = + new LeftLifterCommand(m_leftLifterSubsystem); + private final RightLifterCommand m_RightLifterCommand = + new RightLifterCommand(m_rightLifterSubsystem); private Command m_driveToSpeaker; // Init Buttons - private Trigger m_balanceButton; + // private Trigger m_balanceButton; private Trigger m_straightButton; private Trigger m_toggleBrakeButton; - private Trigger m_lifterUpButton; - private Trigger m_lifterDownButton; + private Trigger m_lifterRightButton; + private Trigger m_lifterLeftButton; private Trigger m_driveToSpeakerButton; + private Trigger m_lifterDirectionButton; // joystick buttons private JoystickButton m_aimButton; private JoystickButton m_armRaiseToSpeakerButton; @@ -138,11 +143,11 @@ public RobotContainer() { private void setupTriggers() { // Controller buttons m_toggleBrakeButton = m_controller1.x(); - m_lifterUpButton = m_controller1.a(); - m_lifterDownButton = m_controller1.b(); - m_balanceButton = m_controller1.rightBumper(); - m_straightButton = m_controller1.rightTrigger(); + m_straightButton = m_controller1.rightBumper(); + m_lifterRightButton = m_controller1.rightTrigger(); + m_lifterLeftButton = m_controller1.leftTrigger(); m_driveToSpeakerButton = m_controller1.y(); + m_lifterDirectionButton = m_controller1.a(); // Joystick buttons m_aimButton = new JoystickButton(m_flightStick, Constants.AIMBUTTON); @@ -159,12 +164,15 @@ private void setupTriggers() { private void bindCommands() { // commands - m_balanceButton.whileTrue(m_balanceCommand); + // m_balanceButton.whileTrue(m_balanceCommand); m_straightButton.whileTrue(m_straightCommand); m_aimButton.whileTrue(m_aimCommand); m_driveToSpeakerButton.whileTrue(m_driveToSpeaker); - m_lifterUpButton.whileTrue(m_lifterUpCommand); - m_lifterDownButton.whileTrue(m_lifterDownCommand); + m_lifterRightButton.whileTrue(m_RightLifterCommand); + m_lifterLeftButton.whileTrue(m_LeftLifterCommand); + m_lifterDirectionButton.whileTrue( + new InstantCommand(() -> m_leftLifterSubsystem.changeDirection()) + .andThen(new InstantCommand(() -> m_rightLifterSubsystem.changeDirection()))); m_toggleBrakeButton.whileTrue(new InstantCommand(() -> m_driveSubsystem.SwitchBrakemode())); // shooter + arm commands m_shooterTrigger.whileTrue(m_shooterCommand); diff --git a/src/main/java/frc/robot/commands/LifterDownCommand.java b/src/main/java/frc/robot/commands/LeftLifterCommand.java similarity index 73% rename from src/main/java/frc/robot/commands/LifterDownCommand.java rename to src/main/java/frc/robot/commands/LeftLifterCommand.java index 0ccaf25..666fa44 100644 --- a/src/main/java/frc/robot/commands/LifterDownCommand.java +++ b/src/main/java/frc/robot/commands/LeftLifterCommand.java @@ -5,22 +5,19 @@ package frc.robot.commands; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.Constants; -import frc.robot.subsystems.LifterSubsystem; +import frc.robot.subsystems.LeftLifterSubsystem; /** Lifter Command using the Lifter Subsystem. */ -public class LifterDownCommand extends Command { +public class LeftLifterCommand extends Command { @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) - private final LifterSubsystem m_lifterSubsystem; - - private final double speed = Constants.LIFTERSPEED; + private final LeftLifterSubsystem m_lifterSubsystem; /** * Creates a new LifterDownCommand. * * @param subsystem The subsystem used by this command. */ - public LifterDownCommand(LifterSubsystem l_subsystem) { + public LeftLifterCommand(LeftLifterSubsystem l_subsystem) { m_lifterSubsystem = l_subsystem; // Use addRequirements() here to declare subsystem dependencies. addRequirements(l_subsystem); @@ -33,14 +30,13 @@ public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - m_lifterSubsystem.leftDown(speed); - m_lifterSubsystem.rightDown(speed); + m_lifterSubsystem.activateLeft(); } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - m_lifterSubsystem.stop(); + m_lifterSubsystem.stopLeft(); } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/commands/LifterUpCommand.java b/src/main/java/frc/robot/commands/RightLifterCommand.java similarity index 67% rename from src/main/java/frc/robot/commands/LifterUpCommand.java rename to src/main/java/frc/robot/commands/RightLifterCommand.java index b7c35a0..c733159 100644 --- a/src/main/java/frc/robot/commands/LifterUpCommand.java +++ b/src/main/java/frc/robot/commands/RightLifterCommand.java @@ -5,22 +5,19 @@ package frc.robot.commands; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.Constants; -import frc.robot.subsystems.LifterSubsystem; +import frc.robot.subsystems.RightLifterSubsystem; /** Lifter Command using the Lifter Subsystem. */ -public class LifterUpCommand extends Command { +public class RightLifterCommand extends Command { @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) - private final LifterSubsystem m_lifterSubsystem; - - private final double speed = Constants.LIFTERSPEED; + private final RightLifterSubsystem m_lifterSubsystem; /** - * Creates a new LifterUpCommand. + * Creates a new LifterDownCommand. * - * @param l_subsystem The subsystem used by this command. + * @param subsystem The subsystem used by this command. */ - public LifterUpCommand(LifterSubsystem l_subsystem) { + public RightLifterCommand(RightLifterSubsystem l_subsystem) { m_lifterSubsystem = l_subsystem; // Use addRequirements() here to declare subsystem dependencies. addRequirements(l_subsystem); @@ -33,14 +30,13 @@ public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - m_lifterSubsystem.leftUp(speed); - m_lifterSubsystem.rightUp(speed); + m_lifterSubsystem.activateRight(); } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - m_lifterSubsystem.stop(); + m_lifterSubsystem.stopRight(); } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/subsystems/LeftLifterSubsystem.java b/src/main/java/frc/robot/subsystems/LeftLifterSubsystem.java new file mode 100644 index 0000000..052c0f9 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/LeftLifterSubsystem.java @@ -0,0 +1,49 @@ +// 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 frc.robot.subsystems; + +import com.revrobotics.CANSparkMax; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; +import frc.robot.Constants.CANConstants; + +public class LeftLifterSubsystem extends SubsystemBase { + private final CANSparkMax m_left; // Motor for Left + private double kCurrentSpeed = Constants.LIFTERSPEED; + + /** Creates a new LifterSubsystem. */ + public LeftLifterSubsystem() { + m_left = new CANSparkMax(CANConstants.MOTORLIFTERLEFTID, CANSparkMax.MotorType.kBrushless); + m_left.setIdleMode(CANSparkMax.IdleMode.kBrake); + m_left.setInverted(false); + m_left.burnFlash(); + } + + public void leftMove(double speed) { + m_left.set(speed); + } + + public void activateLeft() { + leftMove(kCurrentSpeed); + } + + public void changeDirection() { + kCurrentSpeed = -kCurrentSpeed; + } + + public void stopLeft() { + m_left.set(0); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } + + @Override + public void simulationPeriodic() { + // This method will be called once per scheduler run during simulation + } +} diff --git a/src/main/java/frc/robot/subsystems/LifterSubsystem.java b/src/main/java/frc/robot/subsystems/RightLifterSubsystem.java similarity index 61% rename from src/main/java/frc/robot/subsystems/LifterSubsystem.java rename to src/main/java/frc/robot/subsystems/RightLifterSubsystem.java index 6acafea..68a08a0 100644 --- a/src/main/java/frc/robot/subsystems/LifterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/RightLifterSubsystem.java @@ -6,40 +6,33 @@ import com.revrobotics.CANSparkMax; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; import frc.robot.Constants.CANConstants; -public class LifterSubsystem extends SubsystemBase { - private final CANSparkMax m_left; // Motor for Left +public class RightLifterSubsystem extends SubsystemBase { private final CANSparkMax m_right; // Motor for right + private double kCurrentSpeed = Constants.LIFTERSPEED; /** Creates a new LifterSubsystem. */ - public LifterSubsystem() { - m_left = new CANSparkMax(CANConstants.MOTORLIFTERLEFTID, CANSparkMax.MotorType.kBrushless); + public RightLifterSubsystem() { m_right = new CANSparkMax(CANConstants.MOTORLIFTERRIGHTID, CANSparkMax.MotorType.kBrushless); - m_left.setIdleMode(CANSparkMax.IdleMode.kBrake); m_right.setIdleMode(CANSparkMax.IdleMode.kBrake); - m_left.burnFlash(); m_right.burnFlash(); } - public void leftUp(double speed) { - m_left.set(speed); - } - - public void leftDown(double speed) { - m_left.set(-speed); + public void rightMove(double speed) { + m_right.set(speed); } - public void rightUp(double speed) { - m_right.set(speed); + public void activateRight() { + rightMove(kCurrentSpeed); } - public void rightDown(double speed) { - m_right.set(-speed); + public void changeDirection() { + kCurrentSpeed = -kCurrentSpeed; } - public void stop() { - m_left.set(0); + public void stopRight() { m_right.set(0); } From 873814c858390c3bdc521679f5311c1d217c6be5 Mon Sep 17 00:00:00 2001 From: Jack Nelson Date: Sun, 25 Feb 2024 13:12:20 -0500 Subject: [PATCH 23/58] update constants --- src/main/java/frc/robot/Constants.java | 12 ++++++------ src/main/java/frc/robot/subsystems/ArmSubsystem.java | 8 ++++---- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 9888b7d..172ed6d 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -46,7 +46,7 @@ public static final class CANConstants { public static final int ARMAMPBUTTON = 3; public static final int ARMSPEAKERBUTTON = 4; public static final int ARMDEFAULTBUTTON = 2; - public static final int ARMLOADBUTTON = 6; + public static final int ARMLOADBUTTON = 11; public static final int ENABLEAXISBUTTON = 10; public static final int AIMBUTTON = 12; @@ -58,13 +58,13 @@ public static final class CANConstants { // Shooter Angles public static final double ARMSTARTINGANGLE = 22.5; // WHY MaTH HURT - public static final double ARMLOADANGLE = 45; + public static final double ARMLOADANGLE = 65; public static final double ARMSPEAKERANGLE = - 70; // our start is like 40 degrees or something lol - ma - public static final double ARMAMPANGLE = 50; + 60; // our start is like 40 degrees or something lol - ma + public static final double ARMAMPANGLE = 98; // Shooter Speeds (M/s) - public static final double SHOOTERSOURCE = -8.0; - public static final double SHOOTERAMP = 3.0; + public static final double SHOOTERSOURCE = -6.0; + public static final double SHOOTERAMP = 2.4; public static final double SHOOTERSPEAKER = 8.0; public static final double SHOOTERDEFAULT = 8.0; // somewhere around 8 is the cap - ma } diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java index be37229..f793be0 100644 --- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java @@ -83,12 +83,12 @@ public ArmSubsystem(ShooterState shooterState) { m_MainEncoder.setPositionConversionFactor(kRadiansConversionRatio); m_MainEncoder.setPosition(Units.degreesToRadians(Constants.ARMSTARTINGANGLE)); // PID coefficients - kP = 1.0935; + kP = 1.7496; kI = 0; - kD = 0.0042903; + kD = 0.0068645; kIz = 0; - kMaxOutput = 0.25; - kMinOutput = -0.25; + kMaxOutput = 0.4; + kMinOutput = -0.4; // set PID coefficients m_armMainPIDController.setP(kP); From bc51bc48db4cc5bae2a306c025d370f639bd0180 Mon Sep 17 00:00:00 2001 From: Jack Nelson Date: Sun, 25 Feb 2024 14:07:30 -0500 Subject: [PATCH 24/58] absolute working --- src/main/java/frc/robot/Constants.java | 9 ++++++--- .../frc/robot/subsystems/ArmSubsystem.java | 20 +++++++++++++++---- 2 files changed, 22 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 172ed6d..8a74019 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -57,11 +57,14 @@ public static final class CANConstants { // Digital Ports // Shooter Angles + public static final double ARMENCODEROFFSET = 0.8457221; public static final double ARMSTARTINGANGLE = 22.5; // WHY MaTH HURT - public static final double ARMLOADANGLE = 65; + public static final double ARMMINRELATVESTART = 0.0; + public static final double ARMLOADANGLE = 65 - ARMSTARTINGANGLE; public static final double ARMSPEAKERANGLE = - 60; // our start is like 40 degrees or something lol - ma - public static final double ARMAMPANGLE = 98; + 60 - ARMSTARTINGANGLE; // our start is like 40 degrees or something lol - ma + public static final double ARMAMPANGLE = 98 - ARMSTARTINGANGLE; + public static final double ARMMAXRELATIVE = 100 - ARMSTARTINGANGLE; // Shooter Speeds (M/s) public static final double SHOOTERSOURCE = -6.0; public static final double SHOOTERAMP = 2.4; diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java index f793be0..4a37c92 100644 --- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java @@ -6,6 +6,7 @@ import static edu.wpi.first.units.Units.Volts; +import com.revrobotics.AbsoluteEncoder; import com.revrobotics.CANSparkBase; import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; @@ -28,14 +29,15 @@ public class ArmSubsystem extends SubsystemBase { private final CANSparkMax m_armMotorMain; private final SparkPIDController m_armMainPIDController; private final RelativeEncoder m_MainEncoder; + private final AbsoluteEncoder m_AbsoluteEncoder; private final double kP, kI, kD, kIz, kMaxOutput, kMinOutput; private static double kDt = 0.02; // 20ms (update rate for wpilib) private final double ksArmVolts = 0.08521; private final double kgArmGravityGain = 0.1711; private final double kvArmVoltSecondsPerMeter = 0.0022383; private final double kaArmVoltSecondsSquaredPerMeter = 0.00041162; - private final double kMinArmAngleRadians = Units.degreesToRadians(Constants.ARMSTARTINGANGLE); - private final double kMaxArmAngleRadians = Units.degreesToRadians(90); + private final double kMinArmAngleRadians = Units.degreesToRadians(Constants.ARMMINRELATVESTART); + private final double kMaxArmAngleRadians = Units.degreesToRadians(Constants.ARMMAXRELATIVE); private final double kArmLoadAngleRadians = Units.degreesToRadians(Constants.ARMLOADANGLE); // angle to be when recieving ring private final double kArmSpeakerAngleRadians = @@ -53,6 +55,7 @@ public class ArmSubsystem extends SubsystemBase { // gear ratio by dividing by the gear ratio. // remember that 2pi radians in 360 degrees. private final double kRadiansConversionRatio = (Math.PI * 2) / kGearRatio; + private final double kAbsoluteRadiansConversionRatio = (Math.PI * 2); private final ArmFeedforward m_armFeedforward = new ArmFeedforward( ksArmVolts, kgArmGravityGain, kvArmVoltSecondsPerMeter, kaArmVoltSecondsSquaredPerMeter); @@ -79,9 +82,13 @@ public ArmSubsystem(ShooterState shooterState) { m_armMainPIDController = m_armMotorMain.getPIDController(); // allow us to read the encoder m_MainEncoder = m_armMotorMain.getEncoder(); + m_AbsoluteEncoder = m_armMotorMain.getAbsoluteEncoder(); + m_AbsoluteEncoder.setInverted(false); + m_AbsoluteEncoder.setPositionConversionFactor(kAbsoluteRadiansConversionRatio); + // m_AbsoluteEncoder.setZeroOffset(Constants.ARMENCODEROFFSET); // setup the encoders m_MainEncoder.setPositionConversionFactor(kRadiansConversionRatio); - m_MainEncoder.setPosition(Units.degreesToRadians(Constants.ARMSTARTINGANGLE)); + m_MainEncoder.setPosition(Units.degreesToRadians(Constants.ARMMINRELATVESTART)); // PID coefficients kP = 1.7496; kI = 0; @@ -96,6 +103,7 @@ public ArmSubsystem(ShooterState shooterState) { m_armMainPIDController.setD(kD); m_armMainPIDController.setIZone(kIz); m_armMainPIDController.setOutputRange(kMinOutput, kMaxOutput); + m_armMainPIDController.setFeedbackDevice(m_AbsoluteEncoder); m_armMotorMain.burnFlash(); // setup SysID for auto profiling @@ -198,7 +206,11 @@ public void periodic() { } m_shooterState.updateDash(); SmartDashboard.putNumber( - "Current Arm Angle (Degrees)", Units.radiansToDegrees(m_MainEncoder.getPosition())); + "Current Arm Angle (Degrees) (Relative)", + Units.radiansToDegrees(m_MainEncoder.getPosition())); + SmartDashboard.putNumber( + "Current Arm Angle (Degrees) (Absolute)", + Units.radiansToDegrees(m_AbsoluteEncoder.getPosition())); } @Override From 21e0d8e02274aca25d2686a293b3ae0d07188b31 Mon Sep 17 00:00:00 2001 From: Jack Nelson Date: Mon, 26 Feb 2024 16:56:13 -0500 Subject: [PATCH 25/58] fix gyro drive + drive constants --- src/main/java/frc/robot/DriveConstants.java | 16 ++++++++-------- .../frc/robot/subsystems/DriveSubsystem.java | 7 ++++--- 2 files changed, 12 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/DriveConstants.java b/src/main/java/frc/robot/DriveConstants.java index 947f9f4..cc5b575 100644 --- a/src/main/java/frc/robot/DriveConstants.java +++ b/src/main/java/frc/robot/DriveConstants.java @@ -31,21 +31,21 @@ public final class DriveConstants { // The Robot Characterization Toolsuite provides a convenient tool for obtaining these // values for your robot. // Feed Forward Constants - public static final double ksDriveVolts = 0.16213; - public static final double kvDriveVoltSecondsPerMeter = 2.2194; - public static final double kaDriveVoltSecondsSquaredPerMeter = 0.33599; + public static final double ksDriveVolts = 0.19676; + public static final double kvDriveVoltSecondsPerMeter = 2.2623; + public static final double kaDriveVoltSecondsSquaredPerMeter = 0.43785; // Max speed Constants - public static final double kMaxOutputDrive = 1.0; - public static final double kMinOutputDrive = -1.0; + public static final double kMaxOutputDrive = 0.8; + public static final double kMinOutputDrive = -0.8; // Feed Back / PID Constants - public static final double kPDriveVel = 0.00034388; + public static final double kPDriveVel = 0.00088622; public static final double kIDriveVel = 0.0; public static final double kDDriveVel = 0.0; public static final double kIzDriveVel = 0.0; // error before integral takes effect - public static final double kPDrivePos = 5.7745; + public static final double kPDrivePos = 4.6269; public static final double kIDrivePos = 0.0; - public static final double kDDrivePos = 0.55289; + public static final double kDDrivePos = 0.49649; public static final double kIzDrivePos = 0.0; // error before integral takes effect // Helper class that converts a chassis velocity (dx and dtheta components) to left and right // wheel velocities for a differential drive. diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index 69506c9..9ab6d2d 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -31,6 +31,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Constants.CANConstants; +import frc.robot.Constants; import frc.robot.DriveConstants; /** This Subsystem is what allows the code to interact with the drivetrain of the robot. */ @@ -76,8 +77,8 @@ public class DriveSubsystem extends SubsystemBase { static final double turn_P = 0.1; static final double turn_I = 0.00; static final double turn_D = 0.00; - static final double MaxTurnRateDegPerS = 20; - static final double MaxTurnAccelerationDegPerSSquared = 50; + static final double MaxTurnRateDegPerS = 5; + static final double MaxTurnAccelerationDegPerSSquared = 10; static final double TurnToleranceDeg = 3; // max diff in degrees static final double TurnRateToleranceDegPerS = 10; // degrees per second // false when inactive, true when active / a target is set. @@ -370,7 +371,7 @@ public void driveStraight( } double leftStickValue = joystickMagnitude + angleRate; double rightStickValue = joystickMagnitude - angleRate; - this.tankDrive(leftStickValue, rightStickValue); + this.tankDrive((leftStickValue * Constants.MAX_SPEED), (rightStickValue * Constants.MAX_SPEED)); } /* From b52816afc28628082f055b824192342c71884bfa Mon Sep 17 00:00:00 2001 From: Jack Nelson Date: Mon, 26 Feb 2024 18:14:36 -0500 Subject: [PATCH 26/58] Update Constants.java --- src/main/java/frc/robot/Constants.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 8a74019..6c75e81 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -53,6 +53,8 @@ public static final class CANConstants { // Analog Ports /// Ultrasonic Sensors and ports. public static final int ULTRASONICSHOOTERPORT = 0; + public static final int ARMLIMITSWITCHBACK = 1; + public static final int ARMLIMITSWITCHFRONT = 2; // Digital Ports From a74f32a008a82c3b4a7f9c37d831bc3f5be763b9 Mon Sep 17 00:00:00 2001 From: Jack Nelson Date: Tue, 27 Feb 2024 13:20:44 -0500 Subject: [PATCH 27/58] fix conversions --- src/main/java/frc/robot/Constants.java | 6 +++--- src/main/java/frc/robot/subsystems/ArmSubsystem.java | 6 ++++++ src/main/java/frc/robot/subsystems/DriveSubsystem.java | 2 +- src/main/java/frc/robot/subsystems/ShooterSubsystem.java | 4 +++- src/main/java/frc/robot/subsystems/UltrasonicSubsystem.java | 2 ++ 5 files changed, 15 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 6c75e81..35d529d 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -53,13 +53,13 @@ public static final class CANConstants { // Analog Ports /// Ultrasonic Sensors and ports. public static final int ULTRASONICSHOOTERPORT = 0; - public static final int ARMLIMITSWITCHBACK = 1; - public static final int ARMLIMITSWITCHFRONT = 2; // Digital Ports + public static final int ARMLIMITSWITCHBACK = 0; + public static final int ARMLIMITSWITCHFRONT = 1; // Shooter Angles - public static final double ARMENCODEROFFSET = 0.8457221; + public static final double ARMENCODEROFFSET = 2.8378526; public static final double ARMSTARTINGANGLE = 22.5; // WHY MaTH HURT public static final double ARMMINRELATVESTART = 0.0; public static final double ARMLOADANGLE = 65 - ARMSTARTINGANGLE; diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java index 4a37c92..95817fe 100644 --- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java @@ -55,7 +55,9 @@ public class ArmSubsystem extends SubsystemBase { // gear ratio by dividing by the gear ratio. // remember that 2pi radians in 360 degrees. private final double kRadiansConversionRatio = (Math.PI * 2) / kGearRatio; + private final double kVelocityConversionRatio = kRadiansConversionRatio / 60; private final double kAbsoluteRadiansConversionRatio = (Math.PI * 2); + private final double kAbsoluteVelocityConversionRatio = kAbsoluteRadiansConversionRatio / 60; private final ArmFeedforward m_armFeedforward = new ArmFeedforward( ksArmVolts, kgArmGravityGain, kvArmVoltSecondsPerMeter, kaArmVoltSecondsSquaredPerMeter); @@ -85,9 +87,11 @@ public ArmSubsystem(ShooterState shooterState) { m_AbsoluteEncoder = m_armMotorMain.getAbsoluteEncoder(); m_AbsoluteEncoder.setInverted(false); m_AbsoluteEncoder.setPositionConversionFactor(kAbsoluteRadiansConversionRatio); + m_AbsoluteEncoder.setVelocityConversionFactor(kAbsoluteVelocityConversionRatio); // m_AbsoluteEncoder.setZeroOffset(Constants.ARMENCODEROFFSET); // setup the encoders m_MainEncoder.setPositionConversionFactor(kRadiansConversionRatio); + m_MainEncoder.setVelocityConversionFactor(kVelocityConversionRatio); m_MainEncoder.setPosition(Units.degreesToRadians(Constants.ARMMINRELATVESTART)); // PID coefficients kP = 1.7496; @@ -104,6 +108,8 @@ public ArmSubsystem(ShooterState shooterState) { m_armMainPIDController.setIZone(kIz); m_armMainPIDController.setOutputRange(kMinOutput, kMaxOutput); m_armMainPIDController.setFeedbackDevice(m_AbsoluteEncoder); + // m_armMotorMain.setSoftLimit(SoftLimitDirection.kReverse, kMinArmAngleRadians)); + // m_armMotorMain.setSoftLimit(SoftLimitDirection.kForward, kMaxArmAngleRadians); m_armMotorMain.burnFlash(); // setup SysID for auto profiling diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index 9ab6d2d..97bd541 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -30,8 +30,8 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import frc.robot.Constants.CANConstants; import frc.robot.Constants; +import frc.robot.Constants.CANConstants; import frc.robot.DriveConstants; /** This Subsystem is what allows the code to interact with the drivetrain of the robot. */ diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 2bdacfe..8dc2c65 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -32,7 +32,8 @@ public class ShooterSubsystem extends SubsystemBase { private final double kGearRatio = 4; // TBD // basically converted from rotations to to radians to then meters using the wheel diameter. // the diameter is already *2 so we don't need to multiply by 2 again. - private final double kVelocityConversionRatio = (Math.PI * kWheelDiameter) / kGearRatio / 60; + private final double kPositionConversionRatio = (Math.PI * kWheelDiameter) / kGearRatio; + private final double kVelocityConversionRatio = kPositionConversionRatio / 60; // setup feedforward private final double ksShooterVolts = 0.17875; @@ -63,6 +64,7 @@ public ShooterSubsystem() { // allow us to read the encoder m_ShooterMainEncoder = m_ShooterMotorMain.getEncoder(); + m_ShooterMainEncoder.setPositionConversionFactor(kPositionConversionRatio); m_ShooterMainEncoder.setVelocityConversionFactor(kVelocityConversionRatio); // PID coefficients kP = 0.00032423; diff --git a/src/main/java/frc/robot/subsystems/UltrasonicSubsystem.java b/src/main/java/frc/robot/subsystems/UltrasonicSubsystem.java index e27792c..ef7566f 100644 --- a/src/main/java/frc/robot/subsystems/UltrasonicSubsystem.java +++ b/src/main/java/frc/robot/subsystems/UltrasonicSubsystem.java @@ -5,6 +5,7 @@ import edu.wpi.first.wpilibj.AnalogInput; import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; /** This Subsystem is what translates the output of the ultrasonic sensor to standard units. */ @@ -34,6 +35,7 @@ public void periodic() { // SmartDashboard.putNumber("Ultrasonic Sensor Distance", getRangeInches(ultrasonic1)); // Calculate what percentage of 5 Volts we are actually at voltageScaleFactor = 5 / RobotController.getVoltage5V(); + SmartDashboard.putNumber("Ring Distance", DistanceIn()); } @Override From 2356082a218cee20a0b31436d6c63f73cfc3d57d Mon Sep 17 00:00:00 2001 From: Jack Nelson Date: Tue, 27 Feb 2024 13:21:11 -0500 Subject: [PATCH 28/58] formatting --- src/main/java/frc/robot/subsystems/ShooterSubsystem.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 8dc2c65..7337ead 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -32,8 +32,8 @@ public class ShooterSubsystem extends SubsystemBase { private final double kGearRatio = 4; // TBD // basically converted from rotations to to radians to then meters using the wheel diameter. // the diameter is already *2 so we don't need to multiply by 2 again. - private final double kPositionConversionRatio = (Math.PI * kWheelDiameter) / kGearRatio; - private final double kVelocityConversionRatio = kPositionConversionRatio / 60; + private final double kPositionConversionRatio = (Math.PI * kWheelDiameter) / kGearRatio; + private final double kVelocityConversionRatio = kPositionConversionRatio / 60; // setup feedforward private final double ksShooterVolts = 0.17875; From 7d86c6b41d7151e19bcdc8e7e5a11d373c0a1be0 Mon Sep 17 00:00:00 2001 From: Jack Nelson Date: Tue, 27 Feb 2024 18:08:07 -0500 Subject: [PATCH 29/58] changes --- src/main/java/frc/robot/Constants.java | 3 ++- src/main/java/frc/robot/RobotContainer.java | 2 +- src/main/java/frc/robot/commands/ArmCommand.java | 10 ++++++---- .../java/frc/robot/subsystems/ArmSubsystem.java | 16 ++++++++-------- .../frc/robot/subsystems/CameraSubsystem.java | 10 +++++----- .../frc/robot/subsystems/ShooterSubsystem.java | 8 ++++---- 6 files changed, 26 insertions(+), 23 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 35d529d..f26b678 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -60,7 +60,8 @@ public static final class CANConstants { // Shooter Angles public static final double ARMENCODEROFFSET = 2.8378526; - public static final double ARMSTARTINGANGLE = 22.5; // WHY MaTH HURT + public static final double ARMSTARTINGANGLEOFFSET = -2.2; + public static final double ARMSTARTINGANGLE = 22.5 + ARMSTARTINGANGLEOFFSET; // WHY MaTH HURT public static final double ARMMINRELATVESTART = 0.0; public static final double ARMLOADANGLE = 65 - ARMSTARTINGANGLE; public static final double ARMSPEAKERANGLE = diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 35faeb3..12e80cb 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -107,7 +107,7 @@ public class RobotContainer { // Init For Autonomous // private RamseteAutoBuilder autoBuilder; private SendableChooser autoDashboardChooser = new SendableChooser(); - public final boolean enableAutoProfiling = false; + public final boolean enableAutoProfiling = true; /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { diff --git a/src/main/java/frc/robot/commands/ArmCommand.java b/src/main/java/frc/robot/commands/ArmCommand.java index a9750ca..3ff3bf7 100644 --- a/src/main/java/frc/robot/commands/ArmCommand.java +++ b/src/main/java/frc/robot/commands/ArmCommand.java @@ -42,10 +42,12 @@ public void initialize() {} @Override public void execute() { if (!m_shooterState.shooting) { - if ((!HelperFunctions.inDeadzone(m_yAxis.getAsDouble(), Constants.CONTROLLERDEADZONE)) - && m_shooterState.axisEnabled) { - m_ArmSubsystem.MoveArmRelative(m_yAxis.getAsDouble() * kMaxRadiansPerInput); - } else if (m_shooterState.isLoaded & !m_shooterState.isLowered) { + if (m_shooterState.axisEnabled) { + if ((!HelperFunctions.inDeadzone(m_yAxis.getAsDouble(), Constants.CONTROLLERDEADZONE))) { + m_ArmSubsystem.MoveArmRelative(m_yAxis.getAsDouble() * kMaxRadiansPerInput); + } + } + else if (m_shooterState.isLoaded & !m_shooterState.isLowered) { m_ArmSubsystem.lowerArm(); m_shooterState.setLowered(); } else if (!m_shooterState.isLoaded & !m_shooterState.isLowered) { diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java index 95817fe..7394aa9 100644 --- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java @@ -32,10 +32,10 @@ public class ArmSubsystem extends SubsystemBase { private final AbsoluteEncoder m_AbsoluteEncoder; private final double kP, kI, kD, kIz, kMaxOutput, kMinOutput; private static double kDt = 0.02; // 20ms (update rate for wpilib) - private final double ksArmVolts = 0.08521; - private final double kgArmGravityGain = 0.1711; - private final double kvArmVoltSecondsPerMeter = 0.0022383; - private final double kaArmVoltSecondsSquaredPerMeter = 0.00041162; + private final double ksArmVolts = 0.62208; + private final double kgArmGravityGain = 0.23678; + private final double kvArmVoltSecondsPerMeter = 1.4195; + private final double kaArmVoltSecondsSquaredPerMeter = 0.44504; private final double kMinArmAngleRadians = Units.degreesToRadians(Constants.ARMMINRELATVESTART); private final double kMaxArmAngleRadians = Units.degreesToRadians(Constants.ARMMAXRELATIVE); private final double kArmLoadAngleRadians = @@ -44,8 +44,8 @@ public class ArmSubsystem extends SubsystemBase { Units.degreesToRadians(Constants.ARMSPEAKERANGLE); // angle to be when shooting into speaker private final double kArmAmpAngleRadians = Units.degreesToRadians(Constants.ARMAMPANGLE); // angle to be when shooting into amp - private final double karmMaxVelocity = 1.0; // m/s - private final double karmMaxAcceleration = 0.5; // m/s^2 + private final double karmMaxVelocity = 0.25; // rad/s + private final double karmMaxAcceleration = 0.10; // rad/s^2 private boolean kPIDEnabled = true; // general drive constants // https://www.chiefdelphi.com/t/encoders-velocity-to-m-s/390332/2 @@ -94,9 +94,9 @@ public ArmSubsystem(ShooterState shooterState) { m_MainEncoder.setVelocityConversionFactor(kVelocityConversionRatio); m_MainEncoder.setPosition(Units.degreesToRadians(Constants.ARMMINRELATVESTART)); // PID coefficients - kP = 1.7496; + kP = 2.3142; kI = 0; - kD = 0.0068645; + kD = 0.0; kIz = 0; kMaxOutput = 0.4; kMinOutput = -0.4; diff --git a/src/main/java/frc/robot/subsystems/CameraSubsystem.java b/src/main/java/frc/robot/subsystems/CameraSubsystem.java index 31c4a9b..5c143b6 100644 --- a/src/main/java/frc/robot/subsystems/CameraSubsystem.java +++ b/src/main/java/frc/robot/subsystems/CameraSubsystem.java @@ -26,15 +26,15 @@ public class CameraSubsystem extends SubsystemBase { private final PhotonCamera frontCamera; public PhotonPipelineResult frontCameraResult; // Physical location of camera relative to center - private final double CameraLocationXMeters = 0.5; - private final double CameraLocationYMeters = 0.0; - private final double CameraLocationZMeters = 0.5; + private final double CameraLocationXMeters = Units.inchesToMeters(6); + private final double CameraLocationYMeters = Units.inchesToMeters(9.3); + private final double CameraLocationZMeters = Units.inchesToMeters(10.5); // angle of camera / orientation // Cam mounted facing forward, half a meter forward of center, half a meter up from center. - private final double CameraRollRadians = Units.degreesToRadians(0.5); + private final double CameraRollRadians = Units.degreesToRadians(90); private final double CameraPitchRadians = Units.degreesToRadians(0.0); - private final double CameraYawRadians = Units.degreesToRadians(0.5); + private final double CameraYawRadians = Units.degreesToRadians(0); private final Transform3d frontCameraLocation = new Transform3d( diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 7337ead..83e0bb5 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -36,9 +36,9 @@ public class ShooterSubsystem extends SubsystemBase { private final double kVelocityConversionRatio = kPositionConversionRatio / 60; // setup feedforward - private final double ksShooterVolts = 0.17875; - private final double kvDriveVoltSecondsPerMeter = 1.5442; - private final double kaDriveVoltSecondsSquaredPerMeter = 0.019079; + private final double ksShooterVolts = 0.2063; + private final double kvDriveVoltSecondsPerMeter = 1.5611; + private final double kaDriveVoltSecondsSquaredPerMeter = 0.1396; SimpleMotorFeedforward m_shooterFeedForward = new SimpleMotorFeedforward( @@ -67,7 +67,7 @@ public ShooterSubsystem() { m_ShooterMainEncoder.setPositionConversionFactor(kPositionConversionRatio); m_ShooterMainEncoder.setVelocityConversionFactor(kVelocityConversionRatio); // PID coefficients - kP = 0.00032423; + kP = 0.00013373; kI = 0; kD = 0; kIz = 0; From 95593e9d63599ac2606d8a67c274250e98e3b83e Mon Sep 17 00:00:00 2001 From: Jack Nelson Date: Tue, 27 Feb 2024 20:39:47 -0500 Subject: [PATCH 30/58] final ver --- src/main/java/frc/robot/Constants.java | 16 ++--- src/main/java/frc/robot/RobotContainer.java | 2 +- .../java/frc/robot/commands/ArmCommand.java | 7 +- .../frc/robot/subsystems/ArmSubsystem.java | 65 +++++++++++++++---- .../robot/subsystems/ShooterSubsystem.java | 24 ++----- 5 files changed, 72 insertions(+), 42 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index f26b678..893270c 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -55,22 +55,20 @@ public static final class CANConstants { public static final int ULTRASONICSHOOTERPORT = 0; // Digital Ports - public static final int ARMLIMITSWITCHBACK = 0; - public static final int ARMLIMITSWITCHFRONT = 1; + public static final int ARMLIMITSWITCHFRONT = 0; + public static final int ARMLIMITSWITCHBACK = 1; // Shooter Angles - public static final double ARMENCODEROFFSET = 2.8378526; - public static final double ARMSTARTINGANGLEOFFSET = -2.2; - public static final double ARMSTARTINGANGLE = 22.5 + ARMSTARTINGANGLEOFFSET; // WHY MaTH HURT + public static final double ARMENCODEROFFSET = -2.2; + public static final double ARMSTARTINGANGLE = 22.5 + ARMENCODEROFFSET; // WHY MaTH HURT public static final double ARMMINRELATVESTART = 0.0; - public static final double ARMLOADANGLE = 65 - ARMSTARTINGANGLE; - public static final double ARMSPEAKERANGLE = - 60 - ARMSTARTINGANGLE; // our start is like 40 degrees or something lol - ma + public static final double ARMLOADANGLE = 50 - ARMSTARTINGANGLE; + public static final double ARMSPEAKERANGLE = 75 - ARMSTARTINGANGLE; // to go to 75 you just put 75 public static final double ARMAMPANGLE = 98 - ARMSTARTINGANGLE; public static final double ARMMAXRELATIVE = 100 - ARMSTARTINGANGLE; // Shooter Speeds (M/s) public static final double SHOOTERSOURCE = -6.0; public static final double SHOOTERAMP = 2.4; - public static final double SHOOTERSPEAKER = 8.0; + public static final double SHOOTERSPEAKER = 20.0; public static final double SHOOTERDEFAULT = 8.0; // somewhere around 8 is the cap - ma } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 12e80cb..35faeb3 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -107,7 +107,7 @@ public class RobotContainer { // Init For Autonomous // private RamseteAutoBuilder autoBuilder; private SendableChooser autoDashboardChooser = new SendableChooser(); - public final boolean enableAutoProfiling = true; + public final boolean enableAutoProfiling = false; /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { diff --git a/src/main/java/frc/robot/commands/ArmCommand.java b/src/main/java/frc/robot/commands/ArmCommand.java index 3ff3bf7..57f27c7 100644 --- a/src/main/java/frc/robot/commands/ArmCommand.java +++ b/src/main/java/frc/robot/commands/ArmCommand.java @@ -43,11 +43,10 @@ public void initialize() {} public void execute() { if (!m_shooterState.shooting) { if (m_shooterState.axisEnabled) { - if ((!HelperFunctions.inDeadzone(m_yAxis.getAsDouble(), Constants.CONTROLLERDEADZONE))) { - m_ArmSubsystem.MoveArmRelative(m_yAxis.getAsDouble() * kMaxRadiansPerInput); - } + if ((!HelperFunctions.inDeadzone(m_yAxis.getAsDouble(), Constants.CONTROLLERDEADZONE))) { + m_ArmSubsystem.MoveArmRelative(m_yAxis.getAsDouble() * kMaxRadiansPerInput); } - else if (m_shooterState.isLoaded & !m_shooterState.isLowered) { + } else if (m_shooterState.isLoaded & !m_shooterState.isLowered) { m_ArmSubsystem.lowerArm(); m_shooterState.setLowered(); } else if (!m_shooterState.isLoaded & !m_shooterState.isLowered) { diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java index 7394aa9..b7eecf1 100644 --- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java @@ -12,10 +12,12 @@ import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkPIDController; import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; import edu.wpi.first.units.Measure; import edu.wpi.first.units.Voltage; +import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -23,6 +25,7 @@ import frc.robot.Constants; import frc.robot.Constants.CANConstants; import frc.robot.ShooterState; +import frc.robot.utils.HelperFunctions; public class ArmSubsystem extends SubsystemBase { private final ShooterState m_shooterState; @@ -32,10 +35,10 @@ public class ArmSubsystem extends SubsystemBase { private final AbsoluteEncoder m_AbsoluteEncoder; private final double kP, kI, kD, kIz, kMaxOutput, kMinOutput; private static double kDt = 0.02; // 20ms (update rate for wpilib) - private final double ksArmVolts = 0.62208; - private final double kgArmGravityGain = 0.23678; - private final double kvArmVoltSecondsPerMeter = 1.4195; - private final double kaArmVoltSecondsSquaredPerMeter = 0.44504; + private final double ksArmVolts = 0.31531; + private final double kgArmGravityGain = 0.19588; + private final double kvArmVoltSecondsPerMeter = 2.0638; + private final double kaArmVoltSecondsSquaredPerMeter = 0.37994; private final double kMinArmAngleRadians = Units.degreesToRadians(Constants.ARMMINRELATVESTART); private final double kMaxArmAngleRadians = Units.degreesToRadians(Constants.ARMMAXRELATIVE); private final double kArmLoadAngleRadians = @@ -44,8 +47,8 @@ public class ArmSubsystem extends SubsystemBase { Units.degreesToRadians(Constants.ARMSPEAKERANGLE); // angle to be when shooting into speaker private final double kArmAmpAngleRadians = Units.degreesToRadians(Constants.ARMAMPANGLE); // angle to be when shooting into amp - private final double karmMaxVelocity = 0.25; // rad/s - private final double karmMaxAcceleration = 0.10; // rad/s^2 + private final double karmMaxVelocity = 1.0; // rad/s + private final double karmMaxAcceleration = 0.5; // rad/s^2 private boolean kPIDEnabled = true; // general drive constants // https://www.chiefdelphi.com/t/encoders-velocity-to-m-s/390332/2 @@ -70,12 +73,21 @@ public class ArmSubsystem extends SubsystemBase { // setup SysID for auto profiling private final SysIdRoutine m_sysIdRoutine; + // setup front limit switch for rest + private final DigitalInput m_frontLimit; + + private boolean m_frontLimitState = false; + private final Debouncer m_frontLimitDebouncer = new Debouncer(0.2, Debouncer.DebounceType.kBoth); + /** Creates a new ArmSubsystem. */ public ArmSubsystem(ShooterState shooterState) { m_shooterState = shooterState; // create the arm motors m_armMotorMain = new CANSparkMax(CANConstants.MOTORARMMAINID, CANSparkMax.MotorType.kBrushless); + // front limit switch + m_frontLimit = new DigitalInput(Constants.ARMLIMITSWITCHFRONT); + // set the idle mode to brake m_armMotorMain.setIdleMode(CANSparkMax.IdleMode.kBrake); m_armMotorMain.setInverted(true); @@ -92,11 +104,11 @@ public ArmSubsystem(ShooterState shooterState) { // setup the encoders m_MainEncoder.setPositionConversionFactor(kRadiansConversionRatio); m_MainEncoder.setVelocityConversionFactor(kVelocityConversionRatio); - m_MainEncoder.setPosition(Units.degreesToRadians(Constants.ARMMINRELATVESTART)); + matchEncoders(); // PID coefficients kP = 2.3142; kI = 0; - kD = 0.0; + kD = 0.23128; kIz = 0; kMaxOutput = 0.4; kMinOutput = -0.4; @@ -107,7 +119,7 @@ public ArmSubsystem(ShooterState shooterState) { m_armMainPIDController.setD(kD); m_armMainPIDController.setIZone(kIz); m_armMainPIDController.setOutputRange(kMinOutput, kMaxOutput); - m_armMainPIDController.setFeedbackDevice(m_AbsoluteEncoder); + m_armMainPIDController.setFeedbackDevice(m_MainEncoder); // m_armMotorMain.setSoftLimit(SoftLimitDirection.kReverse, kMinArmAngleRadians)); // m_armMotorMain.setSoftLimit(SoftLimitDirection.kForward, kMaxArmAngleRadians); m_armMotorMain.burnFlash(); @@ -197,6 +209,23 @@ public void disablePID() { kPIDEnabled = false; } + public void matchEncoders() { + m_MainEncoder.setPosition(m_AbsoluteEncoder.getPosition()); + } + + public double getError() { + return m_AbsoluteEncoder.getPosition() - m_MainEncoder.getPosition(); + } + + private boolean atGoal() { + return HelperFunctions.inDeadzone( + m_goal.position - m_MainEncoder.getPosition(), Units.degreesToRadians(3)); + } + + private boolean ArmStopped() { + return HelperFunctions.inDeadzone(m_goal.velocity, 0.01); + } + @Override public void periodic() { // This method will be called once per scheduler run @@ -209,14 +238,28 @@ public void periodic() { CANSparkBase.ControlType.kPosition, 0, m_armFeedforward.calculate(m_setpoint.position, m_setpoint.velocity)); + if (atGoal() && ArmStopped() && !m_stopped) { + m_stopped = true; + double cur_error = getError(); + if (!HelperFunctions.inDeadzone(cur_error, Units.degreesToRadians(3))) { + matchEncoders(); + } + } + } + if (m_frontLimitState != !m_frontLimitDebouncer.calculate(m_frontLimit.get())) { + m_frontLimitState = !m_frontLimitDebouncer.calculate(m_frontLimit.get()); + if (m_frontLimitState) { + matchEncoders(); + } } m_shooterState.updateDash(); SmartDashboard.putNumber( "Current Arm Angle (Degrees) (Relative)", - Units.radiansToDegrees(m_MainEncoder.getPosition())); + Units.radiansToDegrees(m_MainEncoder.getPosition()) + Constants.ARMSTARTINGANGLE); SmartDashboard.putNumber( "Current Arm Angle (Degrees) (Absolute)", - Units.radiansToDegrees(m_AbsoluteEncoder.getPosition())); + Units.radiansToDegrees(m_AbsoluteEncoder.getPosition()) + Constants.ARMSTARTINGANGLE); + SmartDashboard.putBoolean("Front Limit Switch Pressed", !m_frontLimit.get()); } @Override diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 83e0bb5..ede3767 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -14,6 +14,7 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.units.Measure; import edu.wpi.first.units.Voltage; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; @@ -24,7 +25,7 @@ public class ShooterSubsystem extends SubsystemBase { private final CANSparkMax m_ShooterMotorSecondary; private final SparkPIDController m_ShooterMainPIDController; private RelativeEncoder m_ShooterMainEncoder; - private final double kP, kI, kD, kIz, kMaxOutput, kMinOutput, kMaxSpeed; + private final double kP, kI, kD, kIz, kMaxOutput, kMinOutput; // general drive constants // https://www.chiefdelphi.com/t/encoders-velocity-to-m-s/390332/2 // https://sciencing.com/convert-rpm-linear-speed-8232280.html @@ -73,8 +74,6 @@ public ShooterSubsystem() { kIz = 0; kMaxOutput = 1; kMinOutput = -1; - kMaxSpeed = 5; - // set PID coefficients m_ShooterMainPIDController.setP(kP); m_ShooterMainPIDController.setI(kI); @@ -113,6 +112,10 @@ public void SpinShooter(double speed) { speed, CANSparkBase.ControlType.kVelocity, 0, m_shooterFeedForward.calculate(speed)); } + public void SpinAtFull() { + m_ShooterMotorMain.set(1); + } + /* * Stop the shooter */ @@ -120,13 +123,6 @@ public void StopShooter() { SpinShooter(0); } - /* - * Spin Shooter at max Speed - */ - public void SpinShooterFull() { - SpinShooter(kMaxSpeed); - } - /* * Check if shooter is at a given Speed */ @@ -135,16 +131,10 @@ public Boolean isAtSpeedTolerance(double speed) { && m_ShooterMainEncoder.getVelocity() < speed + 0.1); } - /* - * Check if shooter is at max Speed - */ - public Boolean isAtMaxSpeed() { - return isAtSpeedTolerance(kMaxSpeed); - } - @Override public void periodic() { // This method will be called once per scheduler run + SmartDashboard.putNumber("Shooter Motor Speed m/s", m_ShooterMainEncoder.getVelocity()); } @Override From 8f4f17657a6270da9a2ca2c29955b08fc22bb199 Mon Sep 17 00:00:00 2001 From: Jack Nelson Date: Wed, 28 Feb 2024 00:22:54 -0500 Subject: [PATCH 31/58] add goal update criteria --- src/main/java/frc/robot/ShooterState.java | 6 +++ .../frc/robot/subsystems/ArmSubsystem.java | 38 +++++++++++++++---- 2 files changed, 36 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/ShooterState.java b/src/main/java/frc/robot/ShooterState.java index f7714ff..7fe9738 100644 --- a/src/main/java/frc/robot/ShooterState.java +++ b/src/main/java/frc/robot/ShooterState.java @@ -62,6 +62,12 @@ public double getShooterSpeed() { } } + /** + * Updates the values on the SmartDashboard related to the shooter state. This method puts the + * values of various shooter state variables onto the SmartDashboard. The variables include + * whether the manual arm mode is enabled, the current arm mode, whether the shooter is loaded, + * whether the arm is lowered, and whether the arm is shooting. + */ public void updateDash() { SmartDashboard.putBoolean("Manual Arm Mode Enabled", axisEnabled); SmartDashboard.putString("Arm Mode", mode.name()); diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java index b7eecf1..247734e 100644 --- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java @@ -68,7 +68,7 @@ public class ArmSubsystem extends SubsystemBase { new TrapezoidProfile(new TrapezoidProfile.Constraints(karmMaxVelocity, karmMaxAcceleration)); private TrapezoidProfile.State m_goal = new TrapezoidProfile.State(); private TrapezoidProfile.State m_setpoint = new TrapezoidProfile.State(); - private boolean m_stopped = true; + private boolean m_newGoal = true; // setup SysID for auto profiling private final SysIdRoutine m_sysIdRoutine; @@ -186,11 +186,13 @@ public void moveArmToSpeaker() { * Move arm to global position (by updating goal) */ public void MoveArmToPosition(double radians) { - m_stopped = false; // update the motion profile with new goal // add minimum starting angle to the target angle to get the real angle double final_radians = Math.max(radians, kMinArmAngleRadians); final_radians = Math.min(final_radians, kMaxArmAngleRadians); + if (final_radians != m_goal.position) { + m_newGoal = true; + } m_goal = new TrapezoidProfile.State(radians, 0); // set the goal } @@ -198,10 +200,9 @@ public void MoveArmToPosition(double radians) { * attempt to hold arm at current location */ public void stop() { - if (!m_stopped) { + if (!atGoal()) { // update the PID controller with current encoder position MoveArmToPosition(getAverageEncoderPosition()); - m_stopped = true; } } @@ -209,23 +210,44 @@ public void disablePID() { kPIDEnabled = false; } + /** Matches the position of the main encoder with the absolute encoder. */ public void matchEncoders() { m_MainEncoder.setPosition(m_AbsoluteEncoder.getPosition()); } + /** + * Calculates the error between the position reported by the absolute encoder and the main + * encoder. + * + * @return The error between the two encoder positions. + */ public double getError() { return m_AbsoluteEncoder.getPosition() - m_MainEncoder.getPosition(); } + /** + * Checks if the arm is at the goal position. + * + * @return true if the arm is at the goal position, false otherwise. + */ private boolean atGoal() { return HelperFunctions.inDeadzone( m_goal.position - m_MainEncoder.getPosition(), Units.degreesToRadians(3)); } + /** + * Checks if the arm is stopped. + * + * @return true if the arm is stopped, false otherwise. + */ private boolean ArmStopped() { return HelperFunctions.inDeadzone(m_goal.velocity, 0.01); } + private boolean getFrontLimit() { + return !m_frontLimitDebouncer.calculate(m_frontLimit.get()); + } + @Override public void periodic() { // This method will be called once per scheduler run @@ -238,16 +260,16 @@ public void periodic() { CANSparkBase.ControlType.kPosition, 0, m_armFeedforward.calculate(m_setpoint.position, m_setpoint.velocity)); - if (atGoal() && ArmStopped() && !m_stopped) { - m_stopped = true; + if (atGoal() && ArmStopped() && m_newGoal) { + m_newGoal = false; // reset the new goal flag, so that we dont try resyncing encoders again double cur_error = getError(); if (!HelperFunctions.inDeadzone(cur_error, Units.degreesToRadians(3))) { matchEncoders(); } } } - if (m_frontLimitState != !m_frontLimitDebouncer.calculate(m_frontLimit.get())) { - m_frontLimitState = !m_frontLimitDebouncer.calculate(m_frontLimit.get()); + if (m_frontLimitState != getFrontLimit()) { + m_frontLimitState = getFrontLimit(); if (m_frontLimitState) { matchEncoders(); } From 6dc757021f43a884280dd3c5097207fdb1a192d0 Mon Sep 17 00:00:00 2001 From: Jack Nelson Date: Wed, 28 Feb 2024 00:23:01 -0500 Subject: [PATCH 32/58] Update PathplannerLib.json --- vendordeps/PathplannerLib.json | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json index ff15fab..787450f 100644 --- a/vendordeps/PathplannerLib.json +++ b/vendordeps/PathplannerLib.json @@ -1,7 +1,7 @@ { "fileName": "PathplannerLib.json", "name": "PathplannerLib", - "version": "2024.2.3", + "version": "2024.2.4", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2024", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2024.2.3" + "version": "2024.2.4" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2024.2.3", + "version": "2024.2.4", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, From e570248e99d931004a9e84aca44d85def2a14807 Mon Sep 17 00:00:00 2001 From: Jack Nelson Date: Wed, 28 Feb 2024 09:31:58 -0500 Subject: [PATCH 33/58] fix encoder offset --- .../java/frc/robot/subsystems/ArmSubsystem.java | 16 ++++++++++++++-- 1 file changed, 14 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java index 247734e..8681382 100644 --- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java @@ -79,6 +79,9 @@ public class ArmSubsystem extends SubsystemBase { private boolean m_frontLimitState = false; private final Debouncer m_frontLimitDebouncer = new Debouncer(0.2, Debouncer.DebounceType.kBoth); + // offset to match the absolute encoder with the main encoder + private double m_curOffset = 0.0; + /** Creates a new ArmSubsystem. */ public ArmSubsystem(ShooterState shooterState) { m_shooterState = shooterState; @@ -192,8 +195,9 @@ public void MoveArmToPosition(double radians) { final_radians = Math.min(final_radians, kMaxArmAngleRadians); if (final_radians != m_goal.position) { m_newGoal = true; + resetOffset(); } - m_goal = new TrapezoidProfile.State(radians, 0); // set the goal + m_goal = new TrapezoidProfile.State(radians + m_curOffset, 0); // set the goal } /* @@ -215,6 +219,14 @@ public void matchEncoders() { m_MainEncoder.setPosition(m_AbsoluteEncoder.getPosition()); } + private void SetOffsetWithEncoder() { + m_curOffset = getError(); + } + + private void resetOffset() { + m_curOffset = 0.0; + } + /** * Calculates the error between the position reported by the absolute encoder and the main * encoder. @@ -264,7 +276,7 @@ public void periodic() { m_newGoal = false; // reset the new goal flag, so that we dont try resyncing encoders again double cur_error = getError(); if (!HelperFunctions.inDeadzone(cur_error, Units.degreesToRadians(3))) { - matchEncoders(); + SetOffsetWithEncoder(); } } } From 03df9c4bb6c991f89ab67d358fa151912bcbb479 Mon Sep 17 00:00:00 2001 From: Jack Nelson Date: Wed, 28 Feb 2024 09:33:34 -0500 Subject: [PATCH 34/58] fix breaky --- src/main/java/frc/robot/subsystems/ArmSubsystem.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java index 8681382..2cdd379 100644 --- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java @@ -193,7 +193,7 @@ public void MoveArmToPosition(double radians) { // add minimum starting angle to the target angle to get the real angle double final_radians = Math.max(radians, kMinArmAngleRadians); final_radians = Math.min(final_radians, kMaxArmAngleRadians); - if (final_radians != m_goal.position) { + if (final_radians != (m_goal.position - m_curOffset))) { m_newGoal = true; resetOffset(); } From e52a61008346c7feda5f08643a494bdc90c38baa Mon Sep 17 00:00:00 2001 From: Jack Nelson Date: Wed, 28 Feb 2024 09:37:23 -0500 Subject: [PATCH 35/58] lmfao --- src/main/java/frc/robot/Constants.java | 6 +++--- src/main/java/frc/robot/subsystems/ArmSubsystem.java | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 893270c..951856f 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -62,9 +62,9 @@ public static final class CANConstants { public static final double ARMENCODEROFFSET = -2.2; public static final double ARMSTARTINGANGLE = 22.5 + ARMENCODEROFFSET; // WHY MaTH HURT public static final double ARMMINRELATVESTART = 0.0; - public static final double ARMLOADANGLE = 50 - ARMSTARTINGANGLE; - public static final double ARMSPEAKERANGLE = 75 - ARMSTARTINGANGLE; // to go to 75 you just put 75 - public static final double ARMAMPANGLE = 98 - ARMSTARTINGANGLE; + public static final double ARMLOADANGLE = 45 - ARMSTARTINGANGLE; + public static final double ARMSPEAKERANGLE = 65 - ARMSTARTINGANGLE; // to go to 75 you just put 75 + public static final double ARMAMPANGLE = 100 - ARMSTARTINGANGLE; public static final double ARMMAXRELATIVE = 100 - ARMSTARTINGANGLE; // Shooter Speeds (M/s) public static final double SHOOTERSOURCE = -6.0; diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java index 2cdd379..b3e123f 100644 --- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java @@ -193,7 +193,7 @@ public void MoveArmToPosition(double radians) { // add minimum starting angle to the target angle to get the real angle double final_radians = Math.max(radians, kMinArmAngleRadians); final_radians = Math.min(final_radians, kMaxArmAngleRadians); - if (final_radians != (m_goal.position - m_curOffset))) { + if (final_radians != (m_goal.position - m_curOffset)) { m_newGoal = true; resetOffset(); } From 38152e700d3292090b812333da355f57049e2ae7 Mon Sep 17 00:00:00 2001 From: Jack Nelson Date: Wed, 28 Feb 2024 15:59:22 -0500 Subject: [PATCH 36/58] working arm error correction --- src/main/java/frc/robot/Constants.java | 4 +-- .../frc/robot/subsystems/ArmSubsystem.java | 31 ++++++++++++++----- .../frc/robot/subsystems/DriveSubsystem.java | 4 +-- .../java/frc/robot/utils/HelperFunctions.java | 2 +- 4 files changed, 28 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 951856f..e74bbf7 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -63,8 +63,8 @@ public static final class CANConstants { public static final double ARMSTARTINGANGLE = 22.5 + ARMENCODEROFFSET; // WHY MaTH HURT public static final double ARMMINRELATVESTART = 0.0; public static final double ARMLOADANGLE = 45 - ARMSTARTINGANGLE; - public static final double ARMSPEAKERANGLE = 65 - ARMSTARTINGANGLE; // to go to 75 you just put 75 - public static final double ARMAMPANGLE = 100 - ARMSTARTINGANGLE; + public static final double ARMSPEAKERANGLE = 75 - ARMSTARTINGANGLE; // to go to 75 you just put 75 + public static final double ARMAMPANGLE = 105 - ARMSTARTINGANGLE; public static final double ARMMAXRELATIVE = 100 - ARMSTARTINGANGLE; // Shooter Speeds (M/s) public static final double SHOOTERSOURCE = -6.0; diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java index b3e123f..0915947 100644 --- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java @@ -68,18 +68,26 @@ public class ArmSubsystem extends SubsystemBase { new TrapezoidProfile(new TrapezoidProfile.Constraints(karmMaxVelocity, karmMaxAcceleration)); private TrapezoidProfile.State m_goal = new TrapezoidProfile.State(); private TrapezoidProfile.State m_setpoint = new TrapezoidProfile.State(); + + // track if goal was changed private boolean m_newGoal = true; + // last goal pre offset + private double m_lastGoal = 0.0; + // setup SysID for auto profiling private final SysIdRoutine m_sysIdRoutine; // setup front limit switch for rest private final DigitalInput m_frontLimit; + // last measured state of limit switch private boolean m_frontLimitState = false; + + // makes limit switch input less finnicky private final Debouncer m_frontLimitDebouncer = new Debouncer(0.2, Debouncer.DebounceType.kBoth); - // offset to match the absolute encoder with the main encoder + // offset to match the absolute encoder with the main encoder by adding offset to goal private double m_curOffset = 0.0; /** Creates a new ArmSubsystem. */ @@ -193,11 +201,12 @@ public void MoveArmToPosition(double radians) { // add minimum starting angle to the target angle to get the real angle double final_radians = Math.max(radians, kMinArmAngleRadians); final_radians = Math.min(final_radians, kMaxArmAngleRadians); - if (final_radians != (m_goal.position - m_curOffset)) { + if (final_radians != m_lastGoal) { m_newGoal = true; + m_lastGoal = final_radians; resetOffset(); } - m_goal = new TrapezoidProfile.State(radians + m_curOffset, 0); // set the goal + m_goal = new TrapezoidProfile.State(final_radians + m_curOffset, 0); // set the goal } /* @@ -234,7 +243,7 @@ private void resetOffset() { * @return The error between the two encoder positions. */ public double getError() { - return m_AbsoluteEncoder.getPosition() - m_MainEncoder.getPosition(); + return m_MainEncoder.getPosition() - m_AbsoluteEncoder.getPosition(); } /** @@ -244,7 +253,7 @@ public double getError() { */ private boolean atGoal() { return HelperFunctions.inDeadzone( - m_goal.position - m_MainEncoder.getPosition(), Units.degreesToRadians(3)); + m_goal.position - m_MainEncoder.getPosition(), Units.degreesToRadians(1)); } /** @@ -253,7 +262,7 @@ private boolean atGoal() { * @return true if the arm is stopped, false otherwise. */ private boolean ArmStopped() { - return HelperFunctions.inDeadzone(m_goal.velocity, 0.01); + return HelperFunctions.inDeadzone(m_goal.velocity, 0.0001); } private boolean getFrontLimit() { @@ -273,10 +282,11 @@ public void periodic() { 0, m_armFeedforward.calculate(m_setpoint.position, m_setpoint.velocity)); if (atGoal() && ArmStopped() && m_newGoal) { - m_newGoal = false; // reset the new goal flag, so that we dont try resyncing encoders again double cur_error = getError(); - if (!HelperFunctions.inDeadzone(cur_error, Units.degreesToRadians(3))) { + if (!HelperFunctions.inDeadzone(cur_error, Units.degreesToRadians(5))) { SetOffsetWithEncoder(); + m_newGoal = false; // reset the new goal flag, so that we dont try resyncing encoders again + m_goal = new TrapezoidProfile.State(m_lastGoal + m_curOffset, 0); // set the goal } } } @@ -294,6 +304,11 @@ public void periodic() { "Current Arm Angle (Degrees) (Absolute)", Units.radiansToDegrees(m_AbsoluteEncoder.getPosition()) + Constants.ARMSTARTINGANGLE); SmartDashboard.putBoolean("Front Limit Switch Pressed", !m_frontLimit.get()); + SmartDashboard.putNumber("Current angle Offset", Units.radiansToDegrees(m_curOffset)); + SmartDashboard.putNumber("Requested Angle", Units.radiansToDegrees(m_lastGoal) + Constants.ARMSTARTINGANGLE); + SmartDashboard.putNumber("Current Goal Angle", Units.radiansToDegrees(m_goal.position) + Constants.ARMSTARTINGANGLE); + SmartDashboard.putBoolean("At Goal", atGoal()); + } @Override diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index 97bd541..323b70d 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -78,8 +78,8 @@ public class DriveSubsystem extends SubsystemBase { static final double turn_I = 0.00; static final double turn_D = 0.00; static final double MaxTurnRateDegPerS = 5; - static final double MaxTurnAccelerationDegPerSSquared = 10; - static final double TurnToleranceDeg = 3; // max diff in degrees + static final double MaxTurnAccelerationDegPerSSquared = 5; + static final double TurnToleranceDeg = 10; // max diff in degrees static final double TurnRateToleranceDegPerS = 10; // degrees per second // false when inactive, true when active / a target is set. private boolean turnControllerEnabled = false; diff --git a/src/main/java/frc/robot/utils/HelperFunctions.java b/src/main/java/frc/robot/utils/HelperFunctions.java index c003f67..d371e77 100644 --- a/src/main/java/frc/robot/utils/HelperFunctions.java +++ b/src/main/java/frc/robot/utils/HelperFunctions.java @@ -5,4 +5,4 @@ public final class HelperFunctions { public static boolean inDeadzone(double value, double deadzone) { return Math.abs(value) < deadzone; } -} +} \ No newline at end of file From 5b4927d62e09daf3f9faedef2a2819e00cb75869 Mon Sep 17 00:00:00 2001 From: Jack Nelson Date: Wed, 28 Feb 2024 22:16:54 -0500 Subject: [PATCH 37/58] spotless --- src/main/java/frc/robot/subsystems/ArmSubsystem.java | 12 +++++++----- src/main/java/frc/robot/utils/HelperFunctions.java | 2 +- 2 files changed, 8 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java index 0915947..7a4804e 100644 --- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java @@ -68,7 +68,7 @@ public class ArmSubsystem extends SubsystemBase { new TrapezoidProfile(new TrapezoidProfile.Constraints(karmMaxVelocity, karmMaxAcceleration)); private TrapezoidProfile.State m_goal = new TrapezoidProfile.State(); private TrapezoidProfile.State m_setpoint = new TrapezoidProfile.State(); - + // track if goal was changed private boolean m_newGoal = true; @@ -285,7 +285,8 @@ public void periodic() { double cur_error = getError(); if (!HelperFunctions.inDeadzone(cur_error, Units.degreesToRadians(5))) { SetOffsetWithEncoder(); - m_newGoal = false; // reset the new goal flag, so that we dont try resyncing encoders again + m_newGoal = + false; // reset the new goal flag, so that we dont try resyncing encoders again m_goal = new TrapezoidProfile.State(m_lastGoal + m_curOffset, 0); // set the goal } } @@ -305,10 +306,11 @@ public void periodic() { Units.radiansToDegrees(m_AbsoluteEncoder.getPosition()) + Constants.ARMSTARTINGANGLE); SmartDashboard.putBoolean("Front Limit Switch Pressed", !m_frontLimit.get()); SmartDashboard.putNumber("Current angle Offset", Units.radiansToDegrees(m_curOffset)); - SmartDashboard.putNumber("Requested Angle", Units.radiansToDegrees(m_lastGoal) + Constants.ARMSTARTINGANGLE); - SmartDashboard.putNumber("Current Goal Angle", Units.radiansToDegrees(m_goal.position) + Constants.ARMSTARTINGANGLE); + SmartDashboard.putNumber( + "Requested Angle", Units.radiansToDegrees(m_lastGoal) + Constants.ARMSTARTINGANGLE); + SmartDashboard.putNumber( + "Current Goal Angle", Units.radiansToDegrees(m_goal.position) + Constants.ARMSTARTINGANGLE); SmartDashboard.putBoolean("At Goal", atGoal()); - } @Override diff --git a/src/main/java/frc/robot/utils/HelperFunctions.java b/src/main/java/frc/robot/utils/HelperFunctions.java index d371e77..c003f67 100644 --- a/src/main/java/frc/robot/utils/HelperFunctions.java +++ b/src/main/java/frc/robot/utils/HelperFunctions.java @@ -5,4 +5,4 @@ public final class HelperFunctions { public static boolean inDeadzone(double value, double deadzone) { return Math.abs(value) < deadzone; } -} \ No newline at end of file +} From 1ee4d110d99ab23d8eabd7a5eded605ea5c784c3 Mon Sep 17 00:00:00 2001 From: Jack Nelson Date: Wed, 28 Feb 2024 22:18:32 -0500 Subject: [PATCH 38/58] add driver cam --- src/main/java/frc/robot/RobotContainer.java | 2 + .../subsystems/DriverCameraSubsystem.java | 37 +++++++++++++++++++ 2 files changed, 39 insertions(+) create mode 100644 src/main/java/frc/robot/subsystems/DriverCameraSubsystem.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 35faeb3..c3930d2 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -34,6 +34,7 @@ import frc.robot.subsystems.ArmSubsystem; import frc.robot.subsystems.CameraSubsystem; import frc.robot.subsystems.DriveSubsystem; +import frc.robot.subsystems.DriverCameraSubsystem; import frc.robot.subsystems.LeftLifterSubsystem; import frc.robot.subsystems.RightLifterSubsystem; import frc.robot.subsystems.ShooterSubsystem; @@ -67,6 +68,7 @@ public class RobotContainer { private final ShooterSubsystem m_shooterSubsytem = new ShooterSubsystem(); private final LeftLifterSubsystem m_leftLifterSubsystem = new LeftLifterSubsystem(); private final RightLifterSubsystem m_rightLifterSubsystem = new RightLifterSubsystem(); + private final DriverCameraSubsystem m_DriverCameraSubsystem = new DriverCameraSubsystem(); // The robots commands are defined here.. // private final ExampleCommand m_autoCommand = new ExampleCommand(m_exampleSubsystem); diff --git a/src/main/java/frc/robot/subsystems/DriverCameraSubsystem.java b/src/main/java/frc/robot/subsystems/DriverCameraSubsystem.java new file mode 100644 index 0000000..4100039 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/DriverCameraSubsystem.java @@ -0,0 +1,37 @@ +package frc.robot.subsystems; + +import edu.wpi.first.cameraserver.CameraServer; +import edu.wpi.first.cscore.UsbCamera; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class DriverCameraSubsystem extends SubsystemBase { + private final UsbCamera m_DriverCamera1; + private final int k_FrontCameraID = 0; + private NetworkTableEntry cameraSelection; + + // Initalizes DriverCameraSubsystem + public DriverCameraSubsystem() { + // connect to network tables + cameraSelection = NetworkTableInstance.getDefault().getTable("").getEntry("CameraSelection"); + + // Connect to Cameras + m_DriverCamera1 = CameraServer.startAutomaticCapture(k_FrontCameraID); + + // set the camera selection to the front camera + cameraSelection.setString(m_DriverCamera1.getName()); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + SmartDashboard.putNumber("Driver Camera FPS", m_DriverCamera1.getActualFPS()); + } + + @Override + public void simulationPeriodic() { + // This method will be called once per scheduler run during simulation + } +} From a8b4ebdbe17c03e323b348b829784076e35abb91 Mon Sep 17 00:00:00 2001 From: Jack Nelson Date: Wed, 28 Feb 2024 22:45:50 -0500 Subject: [PATCH 39/58] expand auto --- src/main/java/frc/robot/RobotContainer.java | 28 ++++++++- .../robot/commands/auto/AimAmpCommand.java | 58 +++++++++++++++++++ .../commands/auto/AimSpeakerCommand.java | 58 +++++++++++++++++++ .../frc/robot/subsystems/ArmSubsystem.java | 8 ++- 4 files changed, 147 insertions(+), 5 deletions(-) create mode 100644 src/main/java/frc/robot/commands/auto/AimAmpCommand.java create mode 100644 src/main/java/frc/robot/commands/auto/AimSpeakerCommand.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c3930d2..57bd757 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -31,6 +31,8 @@ import frc.robot.commands.ShooterCommand; import frc.robot.commands.StraightCommand; import frc.robot.commands.UltrasonicShooterCommand; +import frc.robot.commands.auto.AimAmpCommand; +import frc.robot.commands.auto.AimSpeakerCommand; import frc.robot.subsystems.ArmSubsystem; import frc.robot.subsystems.CameraSubsystem; import frc.robot.subsystems.DriveSubsystem; @@ -89,7 +91,15 @@ public class RobotContainer { private final RightLifterCommand m_RightLifterCommand = new RightLifterCommand(m_rightLifterSubsystem); + // these commands are used by autonomous only + private final AimAmpCommand m_AimAmpCommand = new AimAmpCommand(m_armSubsystem, m_shooterState); + private final AimSpeakerCommand m_AimSpeakerCommand = + new AimSpeakerCommand(m_armSubsystem, m_shooterState); + + // this command is used by on the fly path planning private Command m_driveToSpeaker; + private Command m_driveToAmp; + private Command m_driveToSource; // Init Buttons // private Trigger m_balanceButton; private Trigger m_straightButton; @@ -97,6 +107,8 @@ public class RobotContainer { private Trigger m_lifterRightButton; private Trigger m_lifterLeftButton; private Trigger m_driveToSpeakerButton; + private Trigger m_driveToSourceButton; + private Trigger m_driveToAmpButton; private Trigger m_lifterDirectionButton; // joystick buttons private JoystickButton m_aimButton; @@ -149,6 +161,7 @@ private void setupTriggers() { m_lifterRightButton = m_controller1.rightTrigger(); m_lifterLeftButton = m_controller1.leftTrigger(); m_driveToSpeakerButton = m_controller1.y(); + m_driveToSourceButton = m_controller1.b(); m_lifterDirectionButton = m_controller1.a(); // Joystick buttons @@ -170,6 +183,8 @@ private void bindCommands() { m_straightButton.whileTrue(m_straightCommand); m_aimButton.whileTrue(m_aimCommand); m_driveToSpeakerButton.whileTrue(m_driveToSpeaker); + // m_driveToAmpButton.whileTrue(m_driveToAmp); // TODO: Need to bind button + m_driveToSourceButton.whileTrue(m_driveToSource); m_lifterRightButton.whileTrue(m_RightLifterCommand); m_lifterLeftButton.whileTrue(m_LeftLifterCommand); m_lifterDirectionButton.whileTrue( @@ -220,9 +235,9 @@ private void bindShooterSysIDCommands() { private void initializeAutonomous() { // Network Table Routine Options - autoDashboardChooser.setDefaultOption("Auto With Balancing", "FullAuto"); - autoDashboardChooser.setDefaultOption("DriveForward", "DriveForward"); - autoDashboardChooser.addOption("End at cones", "EndAtCones"); + autoDashboardChooser.setDefaultOption("ShootSpeaker", "SpeakerAuto"); + autoDashboardChooser.addOption("ShootAmp", "AmpAuto"); + autoDashboardChooser.addOption("DriveForward", "DriveForward"); autoDashboardChooser.addOption("Do Nothing", "DoNothing"); SmartDashboard.putData(autoDashboardChooser); @@ -233,6 +248,9 @@ private void initializeAutonomous() { NamedCommands.registerCommand("BalanceRobot", m_balanceCommand); NamedCommands.registerCommand( "BrakeCommand", new InstantCommand(() -> m_driveSubsystem.SetBrakemode())); + NamedCommands.registerCommand("ShooterCommand", m_shooterCommand); + NamedCommands.registerCommand("AimSpeakerCommand", m_AimSpeakerCommand); + NamedCommands.registerCommand("AimAmpCommand", m_AimAmpCommand); // autoBuilder = // new RamseteAutoBuilder( @@ -264,8 +282,12 @@ private void configureTeleopPaths() { new PathConstraints(3.0, 4.0, Units.degreesToRadians(540), Units.degreesToRadians(720)); PathPlannerPath speakerPath = PathPlannerPath.fromPathFile("TeleopSpeakerPath"); + PathPlannerPath ampPath = PathPlannerPath.fromPathFile("TeleopAmpPath"); + PathPlannerPath sourcePath = PathPlannerPath.fromPathFile("TeleopSourcePath"); m_driveToSpeaker = AutoBuilder.pathfindThenFollowPath(speakerPath, constraints); + m_driveToAmp = AutoBuilder.pathfindThenFollowPath(ampPath, constraints); + m_driveToSource = AutoBuilder.pathfindThenFollowPath(sourcePath, constraints); } public double getControllerRightY() { diff --git a/src/main/java/frc/robot/commands/auto/AimAmpCommand.java b/src/main/java/frc/robot/commands/auto/AimAmpCommand.java new file mode 100644 index 0000000..e13633b --- /dev/null +++ b/src/main/java/frc/robot/commands/auto/AimAmpCommand.java @@ -0,0 +1,58 @@ +// 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 frc.robot.commands.auto; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.ShooterState; +import frc.robot.subsystems.ArmSubsystem; + +/** An example command that uses an example subsystem. */ +public class AimAmpCommand extends Command { + @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) + private final ArmSubsystem m_ArmSubsystem; + + private final ShooterState m_shooterState; + + /** + * Creates a new ShootAmpCommand. + * + * @param subsystem The subsystem used by this command. + */ + public AimAmpCommand(ArmSubsystem a_subsystem, ShooterState shooterState) { + m_ArmSubsystem = a_subsystem; + m_shooterState = shooterState; + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(m_ArmSubsystem); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + // this bypasses the shooter state and just moves the arm to the speaker position, as the arm + // subsystem gets paused during the auto shoot command + m_shooterState.setMode(ShooterState.ShooterMode.AMP); + m_ArmSubsystem.moveArmToAmp(); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + // if the arm is stopped, at goal, and the offset is set, then the command is finished, aka at + // correct angle + if (m_ArmSubsystem.ArmStopped() && m_ArmSubsystem.atGoal() && m_ArmSubsystem.isOffsetSet()) { + return true; + } else { + return false; + } + } +} diff --git a/src/main/java/frc/robot/commands/auto/AimSpeakerCommand.java b/src/main/java/frc/robot/commands/auto/AimSpeakerCommand.java new file mode 100644 index 0000000..98f79de --- /dev/null +++ b/src/main/java/frc/robot/commands/auto/AimSpeakerCommand.java @@ -0,0 +1,58 @@ +// 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 frc.robot.commands.auto; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.ShooterState; +import frc.robot.subsystems.ArmSubsystem; + +/** An example command that uses an example subsystem. */ +public class AimSpeakerCommand extends Command { + @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) + private final ArmSubsystem m_ArmSubsystem; + + private final ShooterState m_shooterState; + + /** + * Creates a new ShootSpeakerCommand. + * + * @param subsystem The subsystem used by this command. + */ + public AimSpeakerCommand(ArmSubsystem a_subsystem, ShooterState shooterState) { + m_ArmSubsystem = a_subsystem; + m_shooterState = shooterState; + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(m_ArmSubsystem); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + // this bypasses the shooter state and just moves the arm to the speaker position, as the arm + // subsystem gets paused during the auto shoot command + m_shooterState.setMode(ShooterState.ShooterMode.SPEAKER); + m_ArmSubsystem.moveArmToSpeaker(); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + // if the arm is stopped, at goal, and the offset is set, then the command is finished, aka at + // correct angle + if (m_ArmSubsystem.ArmStopped() && m_ArmSubsystem.atGoal() && m_ArmSubsystem.isOffsetSet()) { + return true; + } else { + return false; + } + } +} diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java index 7a4804e..d00d545 100644 --- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java @@ -251,7 +251,7 @@ public double getError() { * * @return true if the arm is at the goal position, false otherwise. */ - private boolean atGoal() { + public boolean atGoal() { return HelperFunctions.inDeadzone( m_goal.position - m_MainEncoder.getPosition(), Units.degreesToRadians(1)); } @@ -261,10 +261,14 @@ private boolean atGoal() { * * @return true if the arm is stopped, false otherwise. */ - private boolean ArmStopped() { + public boolean ArmStopped() { return HelperFunctions.inDeadzone(m_goal.velocity, 0.0001); } + public boolean isOffsetSet() { + return m_curOffset != 0.0; + } + private boolean getFrontLimit() { return !m_frontLimitDebouncer.calculate(m_frontLimit.get()); } From 874110d8345707f87740de83136a82350ad6a89f Mon Sep 17 00:00:00 2001 From: Jack Nelson Date: Thu, 29 Feb 2024 10:11:02 -0500 Subject: [PATCH 40/58] set new angles --- src/main/java/frc/robot/Constants.java | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index e74bbf7..b8d30da 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -62,13 +62,15 @@ public static final class CANConstants { public static final double ARMENCODEROFFSET = -2.2; public static final double ARMSTARTINGANGLE = 22.5 + ARMENCODEROFFSET; // WHY MaTH HURT public static final double ARMMINRELATVESTART = 0.0; - public static final double ARMLOADANGLE = 45 - ARMSTARTINGANGLE; - public static final double ARMSPEAKERANGLE = 75 - ARMSTARTINGANGLE; // to go to 75 you just put 75 - public static final double ARMAMPANGLE = 105 - ARMSTARTINGANGLE; - public static final double ARMMAXRELATIVE = 100 - ARMSTARTINGANGLE; + public static final double ARMLOADANGLE = 35 - ARMSTARTINGANGLE; + public static final double ARMSPEAKERANGLE = 55 - ARMSTARTINGANGLE; // to go to 75 you just put 75 + public static final double ARMAMPANGLE = 110 - ARMSTARTINGANGLE; + public static final double ARMTRAPANGLE = 45 - ARMSTARTINGANGLE; + public static final double ARMMAXRELATIVE = 120 - ARMSTARTINGANGLE; // Shooter Speeds (M/s) public static final double SHOOTERSOURCE = -6.0; - public static final double SHOOTERAMP = 2.4; + public static final double SHOOTERAMP = 3; public static final double SHOOTERSPEAKER = 20.0; + public static final double SHOOTERTRAP = 8.0; public static final double SHOOTERDEFAULT = 8.0; // somewhere around 8 is the cap - ma } From 92ab93392cfc11d4000f28a0f3a3fcd1f651b056 Mon Sep 17 00:00:00 2001 From: Jack Nelson Date: Thu, 29 Feb 2024 10:11:27 -0500 Subject: [PATCH 41/58] add trap --- src/main/java/frc/robot/ShooterState.java | 3 +++ src/main/java/frc/robot/commands/ArmCommand.java | 4 ++++ src/main/java/frc/robot/subsystems/ArmSubsystem.java | 8 +++++++- 3 files changed, 14 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/ShooterState.java b/src/main/java/frc/robot/ShooterState.java index 7fe9738..5376b08 100644 --- a/src/main/java/frc/robot/ShooterState.java +++ b/src/main/java/frc/robot/ShooterState.java @@ -17,6 +17,7 @@ public enum ShooterMode { SOURCE, AMP, SPEAKER, + TRAP, STOP }; @@ -57,6 +58,8 @@ public double getShooterSpeed() { return Constants.SHOOTERAMP; case SPEAKER: return Constants.SHOOTERSPEAKER; + case TRAP: + return Constants.SHOOTERTRAP; default: return Constants.SHOOTERDEFAULT; } diff --git a/src/main/java/frc/robot/commands/ArmCommand.java b/src/main/java/frc/robot/commands/ArmCommand.java index 57f27c7..74a9545 100644 --- a/src/main/java/frc/robot/commands/ArmCommand.java +++ b/src/main/java/frc/robot/commands/ArmCommand.java @@ -43,6 +43,7 @@ public void initialize() {} public void execute() { if (!m_shooterState.shooting) { if (m_shooterState.axisEnabled) { + m_ArmSubsystem.resetOffset(); if ((!HelperFunctions.inDeadzone(m_yAxis.getAsDouble(), Constants.CONTROLLERDEADZONE))) { m_ArmSubsystem.MoveArmRelative(m_yAxis.getAsDouble() * kMaxRadiansPerInput); } @@ -68,6 +69,9 @@ private void followState() { case SPEAKER: m_ArmSubsystem.moveArmToSpeaker(); break; + case TRAP: + m_ArmSubsystem.moveArmToTrap(); + break; case DEFAULT: m_ArmSubsystem.lowerArm(); break; diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java index d00d545..021cfd4 100644 --- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java @@ -47,6 +47,9 @@ public class ArmSubsystem extends SubsystemBase { Units.degreesToRadians(Constants.ARMSPEAKERANGLE); // angle to be when shooting into speaker private final double kArmAmpAngleRadians = Units.degreesToRadians(Constants.ARMAMPANGLE); // angle to be when shooting into amp + private final double kArmTrapAngleRadians = + Units.degreesToRadians(Constants.ARMTRAPANGLE); // angle to be when shooting into trap + private final double karmMaxVelocity = 1.0; // rad/s private final double karmMaxAcceleration = 0.5; // rad/s^2 private boolean kPIDEnabled = true; @@ -192,6 +195,9 @@ public void moveArmToAmp() { public void moveArmToSpeaker() { MoveArmToPosition(kArmSpeakerAngleRadians); } + public void moveArmToTrap() { + MoveArmToPosition(kArmTrapAngleRadians); + } /* * Move arm to global position (by updating goal) @@ -232,7 +238,7 @@ private void SetOffsetWithEncoder() { m_curOffset = getError(); } - private void resetOffset() { + public void resetOffset() { m_curOffset = 0.0; } From 5cc392328e3c4cc176455b2a7daad6c7b18b06ec Mon Sep 17 00:00:00 2001 From: Jack Nelson Date: Thu, 29 Feb 2024 10:11:35 -0500 Subject: [PATCH 42/58] add fake paths --- .../pathplanner/paths/TeleopAmpPath.path | 49 +++++++++++++++++++ .../pathplanner/paths/TeleopSourcePath.path | 49 +++++++++++++++++++ src/main/java/frc/robot/RobotContainer.java | 2 +- 3 files changed, 99 insertions(+), 1 deletion(-) create mode 100644 src/main/deploy/pathplanner/paths/TeleopAmpPath.path create mode 100644 src/main/deploy/pathplanner/paths/TeleopSourcePath.path diff --git a/src/main/deploy/pathplanner/paths/TeleopAmpPath.path b/src/main/deploy/pathplanner/paths/TeleopAmpPath.path new file mode 100644 index 0000000..09f662d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/TeleopAmpPath.path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 5.0, + "y": 5.0 + }, + "prevControl": null, + "nextControl": { + "x": 6.0, + "y": 4.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.0, + "y": 1.0 + }, + "prevControl": { + "x": 6.75, + "y": 2.5 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/TeleopSourcePath.path b/src/main/deploy/pathplanner/paths/TeleopSourcePath.path new file mode 100644 index 0000000..09f662d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/TeleopSourcePath.path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 5.0, + "y": 5.0 + }, + "prevControl": null, + "nextControl": { + "x": 6.0, + "y": 4.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.0, + "y": 1.0 + }, + "prevControl": { + "x": 6.75, + "y": 2.5 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 57bd757..d2b80b9 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -279,7 +279,7 @@ private void initializeAutonomous() { private void configureTeleopPaths() { // Limits for all Paths PathConstraints constraints = - new PathConstraints(3.0, 4.0, Units.degreesToRadians(540), Units.degreesToRadians(720)); + new PathConstraints(3.0, 2.0, Units.degreesToRadians(540), Units.degreesToRadians(720)); PathPlannerPath speakerPath = PathPlannerPath.fromPathFile("TeleopSpeakerPath"); PathPlannerPath ampPath = PathPlannerPath.fromPathFile("TeleopAmpPath"); From dbca1344202b741a8fe99fe5aea3451cd8ff4f26 Mon Sep 17 00:00:00 2001 From: Jack Nelson Date: Thu, 29 Feb 2024 10:11:46 -0500 Subject: [PATCH 43/58] remove fps --- src/main/java/frc/robot/subsystems/DriverCameraSubsystem.java | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/DriverCameraSubsystem.java b/src/main/java/frc/robot/subsystems/DriverCameraSubsystem.java index 4100039..ee329c4 100644 --- a/src/main/java/frc/robot/subsystems/DriverCameraSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriverCameraSubsystem.java @@ -22,12 +22,14 @@ public DriverCameraSubsystem() { // set the camera selection to the front camera cameraSelection.setString(m_DriverCamera1.getName()); + // start telemetry + //m_DriverCamera1.set } @Override public void periodic() { // This method will be called once per scheduler run - SmartDashboard.putNumber("Driver Camera FPS", m_DriverCamera1.getActualFPS()); + //SmartDashboard.putNumber("Driver Camera FPS", m_DriverCamera1.getActualFPS()); } @Override From feb96d1652c15b7b353fe989212c74b378db02e3 Mon Sep 17 00:00:00 2001 From: Jack Nelson Date: Thu, 29 Feb 2024 10:19:32 -0500 Subject: [PATCH 44/58] enable + disable offset --- src/main/java/frc/robot/commands/ArmCommand.java | 8 ++++++-- .../java/frc/robot/subsystems/ArmSubsystem.java | 14 +++++++++++++- 2 files changed, 19 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/commands/ArmCommand.java b/src/main/java/frc/robot/commands/ArmCommand.java index 74a9545..0066962 100644 --- a/src/main/java/frc/robot/commands/ArmCommand.java +++ b/src/main/java/frc/robot/commands/ArmCommand.java @@ -43,16 +43,20 @@ public void initialize() {} public void execute() { if (!m_shooterState.shooting) { if (m_shooterState.axisEnabled) { - m_ArmSubsystem.resetOffset(); + m_ArmSubsystem.disableOffset(); if ((!HelperFunctions.inDeadzone(m_yAxis.getAsDouble(), Constants.CONTROLLERDEADZONE))) { m_ArmSubsystem.MoveArmRelative(m_yAxis.getAsDouble() * kMaxRadiansPerInput); } - } else if (m_shooterState.isLoaded & !m_shooterState.isLowered) { + } + else if (m_shooterState.isLoaded & !m_shooterState.isLowered) { + m_ArmSubsystem.enableOffset(); m_ArmSubsystem.lowerArm(); m_shooterState.setLowered(); } else if (!m_shooterState.isLoaded & !m_shooterState.isLowered) { + m_ArmSubsystem.enableOffset(); m_ArmSubsystem.lowerArm(); } else { + m_ArmSubsystem.enableOffset(); followState(); } } diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java index 021cfd4..d113314 100644 --- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java @@ -75,6 +75,9 @@ public class ArmSubsystem extends SubsystemBase { // track if goal was changed private boolean m_newGoal = true; + // enable or disable offset + private boolean m_offsetEnabled = true; + // last goal pre offset private double m_lastGoal = 0.0; @@ -279,6 +282,15 @@ private boolean getFrontLimit() { return !m_frontLimitDebouncer.calculate(m_frontLimit.get()); } + public void enableOffset() { + m_offsetEnabled = true; + } + + public void disableOffset() { + m_offsetEnabled = true; + } + + @Override public void periodic() { // This method will be called once per scheduler run @@ -291,7 +303,7 @@ public void periodic() { CANSparkBase.ControlType.kPosition, 0, m_armFeedforward.calculate(m_setpoint.position, m_setpoint.velocity)); - if (atGoal() && ArmStopped() && m_newGoal) { + if (atGoal() && ArmStopped() && m_newGoal && m_offsetEnabled) { double cur_error = getError(); if (!HelperFunctions.inDeadzone(cur_error, Units.degreesToRadians(5))) { SetOffsetWithEncoder(); From 0190cab8a349859ad5352f1cb29115ba796e958f Mon Sep 17 00:00:00 2001 From: Jack Nelson Date: Thu, 29 Feb 2024 10:27:48 -0500 Subject: [PATCH 45/58] correct angle and add button --- src/main/java/frc/robot/Constants.java | 3 ++- src/main/java/frc/robot/RobotContainer.java | 4 ++++ 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index b8d30da..e731783 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -47,6 +47,7 @@ public static final class CANConstants { public static final int ARMSPEAKERBUTTON = 4; public static final int ARMDEFAULTBUTTON = 2; public static final int ARMLOADBUTTON = 11; + public static final int ARMTRAPBUTTON = 9; public static final int ENABLEAXISBUTTON = 10; public static final int AIMBUTTON = 12; @@ -65,7 +66,7 @@ public static final class CANConstants { public static final double ARMLOADANGLE = 35 - ARMSTARTINGANGLE; public static final double ARMSPEAKERANGLE = 55 - ARMSTARTINGANGLE; // to go to 75 you just put 75 public static final double ARMAMPANGLE = 110 - ARMSTARTINGANGLE; - public static final double ARMTRAPANGLE = 45 - ARMSTARTINGANGLE; + public static final double ARMTRAPANGLE = 60 - ARMSTARTINGANGLE; public static final double ARMMAXRELATIVE = 120 - ARMSTARTINGANGLE; // Shooter Speeds (M/s) public static final double SHOOTERSOURCE = -6.0; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d2b80b9..606704c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -114,6 +114,7 @@ public class RobotContainer { private JoystickButton m_aimButton; private JoystickButton m_armRaiseToSpeakerButton; private JoystickButton m_armRaiseToAmpButton; + private JoystickButton m_armRaiseToTrapButton; private JoystickButton m_armWheelLoadButton; private JoystickButton m_enableAxisButton; private JoystickButton m_shooterTrigger; @@ -170,6 +171,7 @@ private void setupTriggers() { m_armWheelLoadButton = new JoystickButton(m_flightStick, Constants.ARMLOADBUTTON); m_armRaiseToSpeakerButton = new JoystickButton(m_flightStick, Constants.ARMSPEAKERBUTTON); m_armRaiseToAmpButton = new JoystickButton(m_flightStick, Constants.ARMAMPBUTTON); + m_armRaiseToTrapButton = new JoystickButton(m_flightStick, Constants.ARMTRAPBUTTON); m_enableAxisButton = new JoystickButton(m_flightStick, Constants.ENABLEAXISBUTTON); m_armRaiseToDefault = new JoystickButton(m_flightStick, Constants.ARMDEFAULTBUTTON); @@ -199,6 +201,8 @@ private void bindCommands() { new InstantCommand(() -> m_shooterState.setMode(ShooterState.ShooterMode.AMP))); m_armWheelLoadButton.whileTrue( new InstantCommand(() -> m_shooterState.setMode(ShooterState.ShooterMode.SOURCE))); + m_armRaiseToTrapButton.whileTrue( + new InstantCommand(() -> m_shooterState.setMode(ShooterState.ShooterMode.TRAP))); m_armRaiseToDefault.whileTrue( new InstantCommand(() -> m_shooterState.setMode(ShooterState.ShooterMode.DEFAULT))); m_enableAxisButton.whileTrue(new InstantCommand(() -> m_shooterState.toggleAxis())); From c04d34d89e0152edc85487311a40a7178ace2578 Mon Sep 17 00:00:00 2001 From: Jack Nelson Date: Thu, 29 Feb 2024 10:52:25 -0500 Subject: [PATCH 46/58] Update Constants.java --- src/main/java/frc/robot/Constants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index e731783..0eab475 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -66,7 +66,7 @@ public static final class CANConstants { public static final double ARMLOADANGLE = 35 - ARMSTARTINGANGLE; public static final double ARMSPEAKERANGLE = 55 - ARMSTARTINGANGLE; // to go to 75 you just put 75 public static final double ARMAMPANGLE = 110 - ARMSTARTINGANGLE; - public static final double ARMTRAPANGLE = 60 - ARMSTARTINGANGLE; + public static final double ARMTRAPANGLE = 63 - ARMSTARTINGANGLE; public static final double ARMMAXRELATIVE = 120 - ARMSTARTINGANGLE; // Shooter Speeds (M/s) public static final double SHOOTERSOURCE = -6.0; From 810d900e03cf0a68f2f10314b72e5a8f4ae54f61 Mon Sep 17 00:00:00 2001 From: Jack Nelson Date: Thu, 29 Feb 2024 12:06:32 -0500 Subject: [PATCH 47/58] format + path --- .../deploy/pathplanner/autos/DoNothing.auto | 25 +++++++++++++++++++ .../pathplanner/paths/TeleopSourcePath.path | 16 ++++++------ src/main/java/frc/robot/RobotContainer.java | 2 +- src/main/java/frc/robot/ShooterState.java | 2 +- .../java/frc/robot/commands/ArmCommand.java | 3 +-- .../frc/robot/subsystems/ArmSubsystem.java | 4 +-- .../subsystems/DriverCameraSubsystem.java | 5 ++-- 7 files changed, 40 insertions(+), 17 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/DoNothing.auto diff --git a/src/main/deploy/pathplanner/autos/DoNothing.auto b/src/main/deploy/pathplanner/autos/DoNothing.auto new file mode 100644 index 0000000..acf233e --- /dev/null +++ b/src/main/deploy/pathplanner/autos/DoNothing.auto @@ -0,0 +1,25 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 2, + "y": 2 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "AimSpeakerCommand" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/TeleopSourcePath.path b/src/main/deploy/pathplanner/paths/TeleopSourcePath.path index 09f662d..9593bf1 100644 --- a/src/main/deploy/pathplanner/paths/TeleopSourcePath.path +++ b/src/main/deploy/pathplanner/paths/TeleopSourcePath.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 5.0, - "y": 5.0 + "x": 8.231349922415486, + "y": 2.441390285624172 }, "prevControl": null, "nextControl": { - "x": 6.0, - "y": 4.0 + "x": 9.231349922415486, + "y": 1.441390285624172 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.0, - "y": 1.0 + "x": 1.0725321924577496, + "y": 1.0796586522082987 }, "prevControl": { - "x": 6.75, - "y": 2.5 + "x": -0.4481584401168075, + "y": 1.0796586522082987 }, "nextControl": null, "isLocked": false, diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 606704c..ea94787 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -202,7 +202,7 @@ private void bindCommands() { m_armWheelLoadButton.whileTrue( new InstantCommand(() -> m_shooterState.setMode(ShooterState.ShooterMode.SOURCE))); m_armRaiseToTrapButton.whileTrue( - new InstantCommand(() -> m_shooterState.setMode(ShooterState.ShooterMode.TRAP))); + new InstantCommand(() -> m_shooterState.setMode(ShooterState.ShooterMode.TRAP))); m_armRaiseToDefault.whileTrue( new InstantCommand(() -> m_shooterState.setMode(ShooterState.ShooterMode.DEFAULT))); m_enableAxisButton.whileTrue(new InstantCommand(() -> m_shooterState.toggleAxis())); diff --git a/src/main/java/frc/robot/ShooterState.java b/src/main/java/frc/robot/ShooterState.java index 5376b08..3f184d7 100644 --- a/src/main/java/frc/robot/ShooterState.java +++ b/src/main/java/frc/robot/ShooterState.java @@ -58,7 +58,7 @@ public double getShooterSpeed() { return Constants.SHOOTERAMP; case SPEAKER: return Constants.SHOOTERSPEAKER; - case TRAP: + case TRAP: return Constants.SHOOTERTRAP; default: return Constants.SHOOTERDEFAULT; diff --git a/src/main/java/frc/robot/commands/ArmCommand.java b/src/main/java/frc/robot/commands/ArmCommand.java index 0066962..82a7d81 100644 --- a/src/main/java/frc/robot/commands/ArmCommand.java +++ b/src/main/java/frc/robot/commands/ArmCommand.java @@ -47,8 +47,7 @@ public void execute() { if ((!HelperFunctions.inDeadzone(m_yAxis.getAsDouble(), Constants.CONTROLLERDEADZONE))) { m_ArmSubsystem.MoveArmRelative(m_yAxis.getAsDouble() * kMaxRadiansPerInput); } - } - else if (m_shooterState.isLoaded & !m_shooterState.isLowered) { + } else if (m_shooterState.isLoaded & !m_shooterState.isLowered) { m_ArmSubsystem.enableOffset(); m_ArmSubsystem.lowerArm(); m_shooterState.setLowered(); diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java index d113314..2949d03 100644 --- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java @@ -49,7 +49,7 @@ public class ArmSubsystem extends SubsystemBase { Units.degreesToRadians(Constants.ARMAMPANGLE); // angle to be when shooting into amp private final double kArmTrapAngleRadians = Units.degreesToRadians(Constants.ARMTRAPANGLE); // angle to be when shooting into trap - + private final double karmMaxVelocity = 1.0; // rad/s private final double karmMaxAcceleration = 0.5; // rad/s^2 private boolean kPIDEnabled = true; @@ -198,6 +198,7 @@ public void moveArmToAmp() { public void moveArmToSpeaker() { MoveArmToPosition(kArmSpeakerAngleRadians); } + public void moveArmToTrap() { MoveArmToPosition(kArmTrapAngleRadians); } @@ -290,7 +291,6 @@ public void disableOffset() { m_offsetEnabled = true; } - @Override public void periodic() { // This method will be called once per scheduler run diff --git a/src/main/java/frc/robot/subsystems/DriverCameraSubsystem.java b/src/main/java/frc/robot/subsystems/DriverCameraSubsystem.java index ee329c4..63f4eac 100644 --- a/src/main/java/frc/robot/subsystems/DriverCameraSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriverCameraSubsystem.java @@ -4,7 +4,6 @@ import edu.wpi.first.cscore.UsbCamera; import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class DriverCameraSubsystem extends SubsystemBase { @@ -23,13 +22,13 @@ public DriverCameraSubsystem() { // set the camera selection to the front camera cameraSelection.setString(m_DriverCamera1.getName()); // start telemetry - //m_DriverCamera1.set + // m_DriverCamera1.set } @Override public void periodic() { // This method will be called once per scheduler run - //SmartDashboard.putNumber("Driver Camera FPS", m_DriverCamera1.getActualFPS()); + // SmartDashboard.putNumber("Driver Camera FPS", m_DriverCamera1.getActualFPS()); } @Override From b7c387c4517228c7d7b10025bccbc3708e966113 Mon Sep 17 00:00:00 2001 From: Jack Nelson Date: Thu, 29 Feb 2024 14:16:29 -0500 Subject: [PATCH 48/58] add autos --- .pathplanner/settings.json | 7 +- .../deploy/pathplanner/autos/DoNothing.auto | 9 +-- .../pathplanner/autos/ShootSpeakerClose.auto | 70 +++++++++++++++++ .../pathplanner/autos/ShootSpeakerFar.auto | 76 +++++++++++++++++++ .../pathplanner/paths/ShootSpeakerPath.path | 52 +++++++++++++ .../{Example Path.path => SpeakerToLine.path} | 43 ++++++----- .../pathplanner/paths/TeleopAmpPath.path | 34 ++++++--- .../pathplanner/paths/TeleopSourcePath.path | 8 +- .../pathplanner/paths/TeleopSpeakerPath.path | 32 ++++++-- src/main/java/frc/robot/RobotContainer.java | 9 ++- .../robot/commands/auto/LowerArmCommand.java | 58 ++++++++++++++ 11 files changed, 343 insertions(+), 55 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/ShootSpeakerClose.auto create mode 100644 src/main/deploy/pathplanner/autos/ShootSpeakerFar.auto create mode 100644 src/main/deploy/pathplanner/paths/ShootSpeakerPath.path rename src/main/deploy/pathplanner/paths/{Example Path.path => SpeakerToLine.path} (55%) create mode 100644 src/main/java/frc/robot/commands/auto/LowerArmCommand.java diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index acd4977..4e38b3e 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -6,10 +6,9 @@ "New Folder" ], "autoFolders": [], - "defaultMaxVel": 3.0, - "defaultMaxAccel": 3.0, + "defaultMaxVel": 2.0, + "defaultMaxAccel": 1.0, "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, - "maxModuleSpeed": 4.5, - "choreoProjectPath": null + "maxModuleSpeed": 4.5 } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/DoNothing.auto b/src/main/deploy/pathplanner/autos/DoNothing.auto index acf233e..07d4e33 100644 --- a/src/main/deploy/pathplanner/autos/DoNothing.auto +++ b/src/main/deploy/pathplanner/autos/DoNothing.auto @@ -10,14 +10,7 @@ "command": { "type": "sequential", "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "AimSpeakerCommand" - } - } - ] + "commands": [] } }, "folder": null, diff --git a/src/main/deploy/pathplanner/autos/ShootSpeakerClose.auto b/src/main/deploy/pathplanner/autos/ShootSpeakerClose.auto new file mode 100644 index 0000000..36e4957 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/ShootSpeakerClose.auto @@ -0,0 +1,70 @@ +{ + "version": 1.0, + "startingPose": null, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "AimSpeakerCommand" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "ArmCommand" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "ShooterCommand" + } + }, + { + "type": "wait", + "data": { + "waitTime": 5.0 + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "AimLowerCommand" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "SpeakerToLine" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/ShootSpeakerFar.auto b/src/main/deploy/pathplanner/autos/ShootSpeakerFar.auto new file mode 100644 index 0000000..88315f5 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/ShootSpeakerFar.auto @@ -0,0 +1,76 @@ +{ + "version": 1.0, + "startingPose": null, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "ShootSpeakerPath" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "AimSpeakerCommand" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "ArmCommand" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "ShooterCommand" + } + }, + { + "type": "wait", + "data": { + "waitTime": 5.0 + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "AimLowerCommand" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "SpeakerToLine" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ShootSpeakerPath.path b/src/main/deploy/pathplanner/paths/ShootSpeakerPath.path new file mode 100644 index 0000000..308e73e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/ShootSpeakerPath.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.2, + "y": 1.75 + }, + "prevControl": null, + "nextControl": { + "x": 2.200000000000002, + "y": 1.75 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.4, + "y": 5.5 + }, + "prevControl": { + "x": 2.4, + "y": 5.5 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 1.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/SpeakerToLine.path similarity index 55% rename from src/main/deploy/pathplanner/paths/Example Path.path rename to src/main/deploy/pathplanner/paths/SpeakerToLine.path index a3fe7a4..14229a1 100644 --- a/src/main/deploy/pathplanner/paths/Example Path.path +++ b/src/main/deploy/pathplanner/paths/SpeakerToLine.path @@ -3,41 +3,41 @@ "waypoints": [ { "anchor": { - "x": 2.0, - "y": 7.0 + "x": 2.118024005101129, + "y": 4.635503404956002 }, "prevControl": null, "nextControl": { - "x": 3.0, - "y": 6.5 + "x": 1.5654159830259637, + "y": 3.041080915880077 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.0, - "y": 5.0 + "x": 2.5, + "y": 2.199257239161131 }, "prevControl": { - "x": 4.0, - "y": 6.0 + "x": 1.3231422106608646, + "y": 3.5454931848085423 }, "nextControl": { - "x": 6.0, - "y": 4.0 + "x": 3.676857789339134, + "y": 0.85302129351372 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.0, - "y": 1.0 + "x": 7.957431157356543, + "y": 1.3650562174103558 }, "prevControl": { - "x": 6.75, - "y": 2.5 + "x": 6.957431157356543, + "y": 1.3650562174103558 }, "nextControl": null, "isLocked": false, @@ -48,18 +48,21 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 2.0, + "maxAcceleration": 1.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, "goalEndState": { - "velocity": 0.0, - "rotation": 0, + "velocity": 0, + "rotation": -4.382714348011046, "rotateFast": false }, "reversed": false, "folder": null, - "previewStartingState": null, - "useDefaultConstraints": false + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/TeleopAmpPath.path b/src/main/deploy/pathplanner/paths/TeleopAmpPath.path index 09f662d..dcfd3d5 100644 --- a/src/main/deploy/pathplanner/paths/TeleopAmpPath.path +++ b/src/main/deploy/pathplanner/paths/TeleopAmpPath.path @@ -3,25 +3,41 @@ "waypoints": [ { "anchor": { - "x": 5.0, - "y": 5.0 + "x": 8.298695211705768, + "y": 4.3985144783186625 }, "prevControl": null, "nextControl": { - "x": 6.0, - "y": 4.0 + "x": 7.438598433871163, + "y": 5.888245796954481 + }, + "isLocked": false, + "linkedName": "MidPoint" + }, + { + "anchor": { + "x": 6.222672214394183, + "y": 6.540894375087692 + }, + "prevControl": { + "x": 8.13247757990561, + "y": 5.745679746402094 + }, + "nextControl": { + "x": 4.3128668488827575, + "y": 7.336109003773291 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.0, - "y": 1.0 + "x": 1.8051986219412255, + "y": 7.422493182165216 }, "prevControl": { - "x": 6.75, - "y": 2.5 + "x": 1.8051986219412255, + "y": 5.90180254959067 }, "nextControl": null, "isLocked": false, @@ -39,7 +55,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0, + "rotation": -3.1134387480333885, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/TeleopSourcePath.path b/src/main/deploy/pathplanner/paths/TeleopSourcePath.path index 9593bf1..14d7952 100644 --- a/src/main/deploy/pathplanner/paths/TeleopSourcePath.path +++ b/src/main/deploy/pathplanner/paths/TeleopSourcePath.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 1.0725321924577496, - "y": 1.0796586522082987 + "x": 15.398883453656053, + "y": 1.1470264049073142 }, "prevControl": { - "x": -0.4481584401168075, - "y": 1.0796586522082987 + "x": 14.995556428276537, + "y": 1.1470264049073142 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/TeleopSpeakerPath.path b/src/main/deploy/pathplanner/paths/TeleopSpeakerPath.path index 09f662d..c33b7a8 100644 --- a/src/main/deploy/pathplanner/paths/TeleopSpeakerPath.path +++ b/src/main/deploy/pathplanner/paths/TeleopSpeakerPath.path @@ -3,25 +3,41 @@ "waypoints": [ { "anchor": { - "x": 5.0, - "y": 5.0 + "x": 8.298695211705768, + "y": 4.3985144783186625 }, "prevControl": null, "nextControl": { - "x": 6.0, - "y": 4.0 + "x": 9.712908774078864, + "y": 4.3985144783186625 + }, + "isLocked": false, + "linkedName": "MidPoint" + }, + { + "anchor": { + "x": 5.303155179058627, + "y": 4.806135432132297 + }, + "prevControl": { + "x": 7.312495893755382, + "y": 4.221098166502594 + }, + "nextControl": { + "x": 3.293814464361872, + "y": 5.391172697762 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.0, - "y": 1.0 + "x": 0.9994362713898646, + "y": 5.545540883229574 }, "prevControl": { - "x": 6.75, - "y": 2.5 + "x": 1.681964380095043, + "y": 5.469704426706775 }, "nextControl": null, "isLocked": false, diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ea94787..cc22487 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -33,6 +33,7 @@ import frc.robot.commands.UltrasonicShooterCommand; import frc.robot.commands.auto.AimAmpCommand; import frc.robot.commands.auto.AimSpeakerCommand; +import frc.robot.commands.auto.LowerArmCommand; import frc.robot.subsystems.ArmSubsystem; import frc.robot.subsystems.CameraSubsystem; import frc.robot.subsystems.DriveSubsystem; @@ -93,6 +94,8 @@ public class RobotContainer { // these commands are used by autonomous only private final AimAmpCommand m_AimAmpCommand = new AimAmpCommand(m_armSubsystem, m_shooterState); + private final LowerArmCommand m_AimLowerCommand = + new LowerArmCommand(m_armSubsystem, m_shooterState); private final AimSpeakerCommand m_AimSpeakerCommand = new AimSpeakerCommand(m_armSubsystem, m_shooterState); @@ -239,8 +242,8 @@ private void bindShooterSysIDCommands() { private void initializeAutonomous() { // Network Table Routine Options - autoDashboardChooser.setDefaultOption("ShootSpeaker", "SpeakerAuto"); - autoDashboardChooser.addOption("ShootAmp", "AmpAuto"); + autoDashboardChooser.setDefaultOption("ShootSpeakerClose", "ShootSpeakerClose"); + autoDashboardChooser.addOption("ShootSpeakerFar", "ShootSpeakerFar"); autoDashboardChooser.addOption("DriveForward", "DriveForward"); autoDashboardChooser.addOption("Do Nothing", "DoNothing"); SmartDashboard.putData(autoDashboardChooser); @@ -254,7 +257,9 @@ private void initializeAutonomous() { "BrakeCommand", new InstantCommand(() -> m_driveSubsystem.SetBrakemode())); NamedCommands.registerCommand("ShooterCommand", m_shooterCommand); NamedCommands.registerCommand("AimSpeakerCommand", m_AimSpeakerCommand); + NamedCommands.registerCommand("ArmCommand", m_armCommand); NamedCommands.registerCommand("AimAmpCommand", m_AimAmpCommand); + NamedCommands.registerCommand("AimLowerCommand", m_AimLowerCommand); // autoBuilder = // new RamseteAutoBuilder( diff --git a/src/main/java/frc/robot/commands/auto/LowerArmCommand.java b/src/main/java/frc/robot/commands/auto/LowerArmCommand.java new file mode 100644 index 0000000..11a52db --- /dev/null +++ b/src/main/java/frc/robot/commands/auto/LowerArmCommand.java @@ -0,0 +1,58 @@ +// 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 frc.robot.commands.auto; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.ShooterState; +import frc.robot.subsystems.ArmSubsystem; + +/** An example command that uses an example subsystem. */ +public class LowerArmCommand extends Command { + @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) + private final ArmSubsystem m_ArmSubsystem; + + private final ShooterState m_shooterState; + + /** + * Creates a new ShootSpeakerCommand. + * + * @param subsystem The subsystem used by this command. + */ + public LowerArmCommand(ArmSubsystem a_subsystem, ShooterState shooterState) { + m_ArmSubsystem = a_subsystem; + m_shooterState = shooterState; + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(m_ArmSubsystem); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + // this bypasses the shooter state and just moves the arm to the speaker position, as the arm + // subsystem gets paused during the auto shoot command + m_shooterState.setMode(ShooterState.ShooterMode.DEFAULT); + m_ArmSubsystem.lowerArm(); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + // if the arm is stopped, at goal, and the offset is set, then the command is finished, aka at + // correct angle + if (m_ArmSubsystem.ArmStopped() && m_ArmSubsystem.atGoal() && m_ArmSubsystem.isOffsetSet()) { + return true; + } else { + return false; + } + } +} From ef57b6189d5f4fb48903b46d347511692d62f6b9 Mon Sep 17 00:00:00 2001 From: Jack Nelson Date: Thu, 29 Feb 2024 18:29:57 -0500 Subject: [PATCH 49/58] big push --- .../deploy/pathplanner/autos/ShootSpeakerClose.auto | 10 ++++++++-- src/main/deploy/pathplanner/autos/ShootSpeakerFar.auto | 2 +- .../deploy/pathplanner/paths/ShootSpeakerPath.path | 8 ++++---- src/main/java/frc/robot/commands/ShooterCommand.java | 2 -- .../java/frc/robot/commands/auto/LowerArmCommand.java | 2 +- src/main/java/frc/robot/subsystems/ArmSubsystem.java | 2 +- 6 files changed, 15 insertions(+), 11 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/ShootSpeakerClose.auto b/src/main/deploy/pathplanner/autos/ShootSpeakerClose.auto index 36e4957..97d8340 100644 --- a/src/main/deploy/pathplanner/autos/ShootSpeakerClose.auto +++ b/src/main/deploy/pathplanner/autos/ShootSpeakerClose.auto @@ -1,6 +1,12 @@ { "version": 1.0, - "startingPose": null, + "startingPose": { + "position": { + "x": 1.5, + "y": 5.5 + }, + "rotation": 180.0 + }, "command": { "type": "sequential", "data": { @@ -38,7 +44,7 @@ { "type": "wait", "data": { - "waitTime": 5.0 + "waitTime": 2.0 } } ] diff --git a/src/main/deploy/pathplanner/autos/ShootSpeakerFar.auto b/src/main/deploy/pathplanner/autos/ShootSpeakerFar.auto index 88315f5..e43ebcc 100644 --- a/src/main/deploy/pathplanner/autos/ShootSpeakerFar.auto +++ b/src/main/deploy/pathplanner/autos/ShootSpeakerFar.auto @@ -44,7 +44,7 @@ { "type": "wait", "data": { - "waitTime": 5.0 + "waitTime": 2.0 } } ] diff --git a/src/main/deploy/pathplanner/paths/ShootSpeakerPath.path b/src/main/deploy/pathplanner/paths/ShootSpeakerPath.path index 308e73e..e7640ee 100644 --- a/src/main/deploy/pathplanner/paths/ShootSpeakerPath.path +++ b/src/main/deploy/pathplanner/paths/ShootSpeakerPath.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.2, - "y": 1.75 + "x": 1.4, + "y": 1.7313445053430379 }, "prevControl": null, "nextControl": { - "x": 2.200000000000002, - "y": 1.75 + "x": 2.439730735385641, + "y": 1.722415665210475 }, "isLocked": false, "linkedName": null diff --git a/src/main/java/frc/robot/commands/ShooterCommand.java b/src/main/java/frc/robot/commands/ShooterCommand.java index 533b702..cee33d0 100644 --- a/src/main/java/frc/robot/commands/ShooterCommand.java +++ b/src/main/java/frc/robot/commands/ShooterCommand.java @@ -34,8 +34,6 @@ public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - // TODO: Finish Implementing ShooterCommand - System.out.println("ShooterCommand Activated"); m_shooterSubsystem.SpinShooter(m_shooterState.getShooterSpeed()); m_shooterState.shooting = true; } diff --git a/src/main/java/frc/robot/commands/auto/LowerArmCommand.java b/src/main/java/frc/robot/commands/auto/LowerArmCommand.java index 11a52db..caaa386 100644 --- a/src/main/java/frc/robot/commands/auto/LowerArmCommand.java +++ b/src/main/java/frc/robot/commands/auto/LowerArmCommand.java @@ -49,7 +49,7 @@ public void end(boolean interrupted) {} public boolean isFinished() { // if the arm is stopped, at goal, and the offset is set, then the command is finished, aka at // correct angle - if (m_ArmSubsystem.ArmStopped() && m_ArmSubsystem.atGoal() && m_ArmSubsystem.isOffsetSet()) { + if (m_ArmSubsystem.ArmStopped() && m_ArmSubsystem.getFrontLimit()) { return true; } else { return false; diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java index 2949d03..cebdcb5 100644 --- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java @@ -279,7 +279,7 @@ public boolean isOffsetSet() { return m_curOffset != 0.0; } - private boolean getFrontLimit() { + public boolean getFrontLimit() { return !m_frontLimitDebouncer.calculate(m_frontLimit.get()); } From 3500d0d7db20b2b1dc7dc1988945cc12309a9676 Mon Sep 17 00:00:00 2001 From: Jack Nelson Date: Thu, 29 Feb 2024 18:32:54 -0500 Subject: [PATCH 50/58] Create shuffleboard.json --- shuffleboard.json | 426 ++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 426 insertions(+) create mode 100644 shuffleboard.json diff --git a/shuffleboard.json b/shuffleboard.json new file mode 100644 index 0000000..0a49bf2 --- /dev/null +++ b/shuffleboard.json @@ -0,0 +1,426 @@ +{ + "tabPane": [ + { + "title": "SmartDashboard", + "autoPopulate": true, + "autoPopulatePrefix": "SmartDashboard/", + "widgetPane": { + "gridSize": 256.0, + "showGrid": true, + "hgap": 16.0, + "vgap": 16.0, + "titleType": 0, + "tiles": { + "1,0": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Branch", + "_title": "Branch", + "_glyph": 148, + "_showGlyph": false + } + }, + "2,0": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Commit", + "_title": "Commit", + "_glyph": 148, + "_showGlyph": false + } + }, + "3,0": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Left Encoder Speed (M/s)", + "_title": "Left Encoder Speed (M/s)", + "_glyph": 148, + "_showGlyph": false + } + }, + "0,1": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Right Encoder Speed (M/s)", + "_title": "Right Encoder Speed (M/s)", + "_glyph": 148, + "_showGlyph": false + } + }, + "1,1": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Distance L", + "_title": "Distance L", + "_glyph": 148, + "_showGlyph": false + } + }, + "2,1": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Distance R", + "_title": "Distance R", + "_glyph": 148, + "_showGlyph": false + } + }, + "3,1": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Current Robot Location X axis", + "_title": "Current Robot Location X axis", + "_glyph": 148, + "_showGlyph": false + } + }, + "0,0": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Grid Layout", + "_title": "Grid Layout", + "_glyph": 149, + "_showGlyph": false, + "Layout/Number of columns": 3, + "Layout/Number of rows": 3, + "Layout/Label position": "BOTTOM", + "_children": { + "0,0": { + "_type": "Subsystem Layout", + "_title": "Subsystem Layout", + "_glyph": 149, + "_showGlyph": false, + "Layout/Label position": "BOTTOM", + "_children": [ + { + "_type": "Split Button Chooser", + "_source0": "network_table:///SmartDashboard/SendableChooser[0]", + "_title": "Auto Mode", + "_glyph": 148, + "_showGlyph": false + } + ] + } + } + } + } + } + } + }, + { + "title": "LiveWindow", + "autoPopulate": true, + "autoPopulatePrefix": "LiveWindow/", + "widgetPane": { + "gridSize": 256.0, + "showGrid": true, + "hgap": 16.0, + "vgap": 16.0, + "titleType": 0, + "tiles": {} + } + }, + { + "title": "Main Dashboard", + "autoPopulate": false, + "autoPopulatePrefix": "", + "widgetPane": { + "gridSize": 128.0, + "showGrid": true, + "hgap": 16.0, + "vgap": 16.0, + "titleType": 0, + "tiles": { + "1,1": { + "size": [ + 2, + 1 + ], + "content": { + "_type": "Field", + "_title": "Field", + "_glyph": 148, + "_showGlyph": false, + "Game/Game": "k2024Crescendo", + "Visuals/Robot Icon Size": 50.0, + "Visuals/Show Outside Circles": false + } + }, + "3,0": { + "size": [ + 3, + 2 + ], + "content": { + "_type": "Differential Drivebase", + "_title": "Differential Drivebase", + "_glyph": 148, + "_showGlyph": false, + "Wheels/Number of wheels": 4, + "Wheels/Wheel diameter": 40.0, + "Visuals/Show velocity vectors": true + } + }, + "0,0": { + "size": [ + 3, + 1 + ], + "content": { + "_type": "Basic FMS Info", + "_title": "Basic FMS Info", + "_glyph": 148, + "_showGlyph": false + } + }, + "6,0": { + "size": [ + 3, + 3 + ], + "content": { + "_type": "Camera Stream", + "_title": "Camera Stream", + "_glyph": 148, + "_showGlyph": false, + "Crosshair/Show crosshair": true, + "Crosshair/Crosshair color": "#FFFFFFFF", + "Controls/Show controls": true, + "Controls/Rotation": "NONE", + "compression": -1.0, + "fps": -1, + "imageWidth": -1, + "imageHeight": -1 + } + }, + "9,0": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "ComboBox Chooser", + "_source0": "network_table:///SmartDashboard/SendableChooser[0]", + "_title": "/SmartDashboard/SendableChooser[0]", + "_glyph": 148, + "_showGlyph": false + } + }, + "9,1": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "ComboBox Chooser", + "_source0": "network_table:///SmartDashboard/SendableChooser[0]", + "_title": "/SmartDashboard/SendableChooser[0]", + "_glyph": 148, + "_showGlyph": false + } + }, + "0,3": { + "size": [ + 3, + 1 + ], + "content": { + "_type": "Split Button Chooser", + "_source0": "network_table:///SmartDashboard/SendableChooser[0]", + "_title": "/SmartDashboard/SendableChooser[0]", + "_glyph": 148, + "_showGlyph": false + } + }, + "4,2": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Boolean Box", + "_source0": "network_table:///SmartDashboard/At Goal", + "_title": "/SmartDashboard/At Goal", + "_glyph": 148, + "_showGlyph": false, + "Colors/Color when true": "#7CFC00FF", + "Colors/Color when false": "#8B0000FF" + } + } + } + } + }, + { + "title": "Driver Dashboard", + "autoPopulate": false, + "autoPopulatePrefix": "", + "widgetPane": { + "gridSize": 128.0, + "showGrid": true, + "hgap": 16.0, + "vgap": 16.0, + "titleType": 0, + "tiles": { + "0,0": { + "size": [ + 4, + 3 + ], + "content": { + "_type": "Field", + "_title": "Field", + "_glyph": 148, + "_showGlyph": false, + "Game/Game": "k2024Crescendo", + "Visuals/Robot Icon Size": 50.0, + "Visuals/Show Outside Circles": false + } + }, + "4,2": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Arm Mode", + "_title": "/SmartDashboard/Arm Mode", + "_glyph": 148, + "_showGlyph": false + } + }, + "4,3": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Boolean Box", + "_source0": "network_table:///SmartDashboard/At Goal", + "_title": "/SmartDashboard/At Goal", + "_glyph": 148, + "_showGlyph": false, + "Colors/Color when true": "#7CFC00FF", + "Colors/Color when false": "#8B0000FF" + } + }, + "4,1": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Boolean Box", + "_source0": "network_table:///SmartDashboard/Manual Arm Mode Enabled", + "_title": "/SmartDashboard/Manual Arm Mode Enabled", + "_glyph": 148, + "_showGlyph": false, + "Colors/Color when true": "#7CFC00FF", + "Colors/Color when false": "#8B0000FF" + } + }, + "4,0": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Simple Dial", + "_source0": "network_table:///SmartDashboard/Current Arm Angle (Degrees) (Absolute)", + "_title": "Arm Angle", + "_glyph": 148, + "_showGlyph": true, + "Range/Min": 20.0, + "Range/Max": 110.0, + "Visuals/Show value": true + } + }, + "5,0": { + "size": [ + 4, + 4 + ], + "content": { + "_type": "Camera Stream", + "_source0": "camera_server://photonvision_Port_1183_Input_MJPEG_Server", + "_title": "photonvision_Port_1183_Input_MJPEG_Server", + "_glyph": 80, + "_showGlyph": true, + "Crosshair/Show crosshair": false, + "Crosshair/Crosshair color": "#FFFFFFFF", + "Controls/Show controls": false, + "Controls/Rotation": "NONE", + "compression": -1.0, + "fps": -1, + "imageWidth": -1, + "imageHeight": -1 + } + }, + "3,3": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Current Goal Angle", + "_title": "/SmartDashboard/Current Goal Angle", + "_glyph": 148, + "_showGlyph": false + } + }, + "2,3": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Current Arm Angle (Degrees) (Relative)", + "_title": "/SmartDashboard/Current Arm Angle (Degrees) (Relative)", + "_glyph": 148, + "_showGlyph": false + } + } + } + } + } + ], + "windowGeometry": { + "x": -7.333333492279053, + "y": -7.333333492279053, + "width": 1294.6666259765625, + "height": 694.6666870117188 + } +} \ No newline at end of file From f1124c04acf33a515aed5f33b3dc07fc9c355a22 Mon Sep 17 00:00:00 2001 From: Jack Nelson Date: Fri, 1 Mar 2024 06:23:38 -0500 Subject: [PATCH 51/58] Update Constants.java --- src/main/java/frc/robot/Constants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 0eab475..da15804 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -64,7 +64,7 @@ public static final class CANConstants { public static final double ARMSTARTINGANGLE = 22.5 + ARMENCODEROFFSET; // WHY MaTH HURT public static final double ARMMINRELATVESTART = 0.0; public static final double ARMLOADANGLE = 35 - ARMSTARTINGANGLE; - public static final double ARMSPEAKERANGLE = 55 - ARMSTARTINGANGLE; // to go to 75 you just put 75 + public static final double ARMSPEAKERANGLE = 60 - ARMSTARTINGANGLE; // to go to 75 you just put 75 public static final double ARMAMPANGLE = 110 - ARMSTARTINGANGLE; public static final double ARMTRAPANGLE = 63 - ARMSTARTINGANGLE; public static final double ARMMAXRELATIVE = 120 - ARMSTARTINGANGLE; From 70490e7de3be97b44d6c54eace7ad88a08b7925a Mon Sep 17 00:00:00 2001 From: Iooob <94081903+Iooob@users.noreply.github.com> Date: Fri, 1 Mar 2024 10:57:23 -0500 Subject: [PATCH 52/58] Revert "Update Constants.java" This reverts commit f1124c04acf33a515aed5f33b3dc07fc9c355a22. --- src/main/java/frc/robot/Constants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index da15804..0eab475 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -64,7 +64,7 @@ public static final class CANConstants { public static final double ARMSTARTINGANGLE = 22.5 + ARMENCODEROFFSET; // WHY MaTH HURT public static final double ARMMINRELATVESTART = 0.0; public static final double ARMLOADANGLE = 35 - ARMSTARTINGANGLE; - public static final double ARMSPEAKERANGLE = 60 - ARMSTARTINGANGLE; // to go to 75 you just put 75 + public static final double ARMSPEAKERANGLE = 55 - ARMSTARTINGANGLE; // to go to 75 you just put 75 public static final double ARMAMPANGLE = 110 - ARMSTARTINGANGLE; public static final double ARMTRAPANGLE = 63 - ARMSTARTINGANGLE; public static final double ARMMAXRELATIVE = 120 - ARMSTARTINGANGLE; From be4d6aef30e2e4ef2a261284d34c5a88037fc627 Mon Sep 17 00:00:00 2001 From: Iooob <94081903+Iooob@users.noreply.github.com> Date: Fri, 1 Mar 2024 11:50:35 -0500 Subject: [PATCH 53/58] fix --- shuffleboard.json | 68 ++++++++++++------- src/main/java/frc/robot/Constants.java | 2 +- .../frc/robot/subsystems/ArmSubsystem.java | 2 +- 3 files changed, 45 insertions(+), 27 deletions(-) diff --git a/shuffleboard.json b/shuffleboard.json index 0a49bf2..d4a0526 100644 --- a/shuffleboard.json +++ b/shuffleboard.json @@ -134,6 +134,32 @@ } } } + }, + "4,0": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Ring Distance", + "_title": "Ring Distance", + "_glyph": 148, + "_showGlyph": false + } + }, + "4,1": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Average Distance Traveled", + "_title": "Average Distance Traveled", + "_glyph": 148, + "_showGlyph": false + } } } } @@ -366,27 +392,6 @@ "Visuals/Show value": true } }, - "5,0": { - "size": [ - 4, - 4 - ], - "content": { - "_type": "Camera Stream", - "_source0": "camera_server://photonvision_Port_1183_Input_MJPEG_Server", - "_title": "photonvision_Port_1183_Input_MJPEG_Server", - "_glyph": 80, - "_showGlyph": true, - "Crosshair/Show crosshair": false, - "Crosshair/Crosshair color": "#FFFFFFFF", - "Controls/Show controls": false, - "Controls/Rotation": "NONE", - "compression": -1.0, - "fps": -1, - "imageWidth": -1, - "imageHeight": -1 - } - }, "3,3": { "size": [ 1, @@ -412,15 +417,28 @@ "_glyph": 148, "_showGlyph": false } + }, + "5,3": { + "size": [ + 2, + 1 + ], + "content": { + "_type": "ComboBox Chooser", + "_source0": "network_table:///SmartDashboard/SendableChooser[0]", + "_title": "SmartDashboard/SendableChooser[0]", + "_glyph": 148, + "_showGlyph": false + } } } } } ], "windowGeometry": { - "x": -7.333333492279053, - "y": -7.333333492279053, - "width": 1294.6666259765625, - "height": 694.6666870117188 + "x": 0.800000011920929, + "y": 0.800000011920929, + "width": 1534.4000244140625, + "height": 814.4000244140625 } } \ No newline at end of file diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 0eab475..4e466b8 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -64,7 +64,7 @@ public static final class CANConstants { public static final double ARMSTARTINGANGLE = 22.5 + ARMENCODEROFFSET; // WHY MaTH HURT public static final double ARMMINRELATVESTART = 0.0; public static final double ARMLOADANGLE = 35 - ARMSTARTINGANGLE; - public static final double ARMSPEAKERANGLE = 55 - ARMSTARTINGANGLE; // to go to 75 you just put 75 + public static final double ARMSPEAKERANGLE = 59 - ARMSTARTINGANGLE; // to go to 75 you just put 75 public static final double ARMAMPANGLE = 110 - ARMSTARTINGANGLE; public static final double ARMTRAPANGLE = 63 - ARMSTARTINGANGLE; public static final double ARMMAXRELATIVE = 120 - ARMSTARTINGANGLE; diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java index cebdcb5..e09571a 100644 --- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java @@ -305,7 +305,7 @@ public void periodic() { m_armFeedforward.calculate(m_setpoint.position, m_setpoint.velocity)); if (atGoal() && ArmStopped() && m_newGoal && m_offsetEnabled) { double cur_error = getError(); - if (!HelperFunctions.inDeadzone(cur_error, Units.degreesToRadians(5))) { + if (!HelperFunctions.inDeadzone(cur_error, Units.degreesToRadians(3))) { SetOffsetWithEncoder(); m_newGoal = false; // reset the new goal flag, so that we dont try resyncing encoders again From 91aa6a7f24529bc5740abfc1afbeb657724c1d7c Mon Sep 17 00:00:00 2001 From: Iooob <94081903+Iooob@users.noreply.github.com> Date: Fri, 1 Mar 2024 15:27:59 -0500 Subject: [PATCH 54/58] lifter limits --- src/main/java/frc/robot/subsystems/LeftLifterSubsystem.java | 1 + src/main/java/frc/robot/subsystems/RightLifterSubsystem.java | 1 + 2 files changed, 2 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/LeftLifterSubsystem.java b/src/main/java/frc/robot/subsystems/LeftLifterSubsystem.java index 052c0f9..30a5594 100644 --- a/src/main/java/frc/robot/subsystems/LeftLifterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/LeftLifterSubsystem.java @@ -18,6 +18,7 @@ public LeftLifterSubsystem() { m_left = new CANSparkMax(CANConstants.MOTORLIFTERLEFTID, CANSparkMax.MotorType.kBrushless); m_left.setIdleMode(CANSparkMax.IdleMode.kBrake); m_left.setInverted(false); + m_left.setSmartCurrentLimit(60); m_left.burnFlash(); } diff --git a/src/main/java/frc/robot/subsystems/RightLifterSubsystem.java b/src/main/java/frc/robot/subsystems/RightLifterSubsystem.java index 68a08a0..d677abc 100644 --- a/src/main/java/frc/robot/subsystems/RightLifterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/RightLifterSubsystem.java @@ -17,6 +17,7 @@ public class RightLifterSubsystem extends SubsystemBase { public RightLifterSubsystem() { m_right = new CANSparkMax(CANConstants.MOTORLIFTERRIGHTID, CANSparkMax.MotorType.kBrushless); m_right.setIdleMode(CANSparkMax.IdleMode.kBrake); + m_right.setSmartCurrentLimit(60); m_right.burnFlash(); } From dc92575e06a0116baeabe66868799e7f48acbb18 Mon Sep 17 00:00:00 2001 From: Iooob <94081903+Iooob@users.noreply.github.com> Date: Sat, 2 Mar 2024 08:59:18 -0500 Subject: [PATCH 55/58] add new autos --- ...akerClose.auto => ShootSpeakerCenter.auto} | 4 +- .../pathplanner/autos/ShootSpeakerLeft.auto | 76 +++++++++++++++++++ .../pathplanner/autos/ShootSpeakerRight.auto | 76 +++++++++++++++++++ src/main/java/frc/robot/RobotContainer.java | 4 +- 4 files changed, 157 insertions(+), 3 deletions(-) rename src/main/deploy/pathplanner/autos/{ShootSpeakerClose.auto => ShootSpeakerCenter.auto} (97%) create mode 100644 src/main/deploy/pathplanner/autos/ShootSpeakerLeft.auto create mode 100644 src/main/deploy/pathplanner/autos/ShootSpeakerRight.auto diff --git a/src/main/deploy/pathplanner/autos/ShootSpeakerClose.auto b/src/main/deploy/pathplanner/autos/ShootSpeakerCenter.auto similarity index 97% rename from src/main/deploy/pathplanner/autos/ShootSpeakerClose.auto rename to src/main/deploy/pathplanner/autos/ShootSpeakerCenter.auto index 97d8340..fdee053 100644 --- a/src/main/deploy/pathplanner/autos/ShootSpeakerClose.auto +++ b/src/main/deploy/pathplanner/autos/ShootSpeakerCenter.auto @@ -2,8 +2,8 @@ "version": 1.0, "startingPose": { "position": { - "x": 1.5, - "y": 5.5 + "x": 1.3959954881393342, + "y": 5.55 }, "rotation": 180.0 }, diff --git a/src/main/deploy/pathplanner/autos/ShootSpeakerLeft.auto b/src/main/deploy/pathplanner/autos/ShootSpeakerLeft.auto new file mode 100644 index 0000000..75e9d22 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/ShootSpeakerLeft.auto @@ -0,0 +1,76 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.7167669684840067, + "y": 6.745441850370149 + }, + "rotation": -122.66091272167378 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "AimSpeakerCommand" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "ArmCommand" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "ShooterCommand" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "AimLowerCommand" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "SpeakerToLine" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/ShootSpeakerRight.auto b/src/main/deploy/pathplanner/autos/ShootSpeakerRight.auto new file mode 100644 index 0000000..5654769 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/ShootSpeakerRight.auto @@ -0,0 +1,76 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.787031987758696, + "y": 4.356431195030721 + }, + "rotation": 120.96375653207355 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "AimSpeakerCommand" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "ArmCommand" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "ShooterCommand" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "AimLowerCommand" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "SpeakerToLine" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index cc22487..6988f52 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -242,7 +242,9 @@ private void bindShooterSysIDCommands() { private void initializeAutonomous() { // Network Table Routine Options - autoDashboardChooser.setDefaultOption("ShootSpeakerClose", "ShootSpeakerClose"); + autoDashboardChooser.setDefaultOption("ShootSpeakerCenter", "ShootSpeakerCenter"); + autoDashboardChooser.addOption("ShootSpeakerLeft", "ShootSpeakerLeft"); + autoDashboardChooser.addOption("ShootSpeakerRight", "ShootSpeakerRight"); autoDashboardChooser.addOption("ShootSpeakerFar", "ShootSpeakerFar"); autoDashboardChooser.addOption("DriveForward", "DriveForward"); autoDashboardChooser.addOption("Do Nothing", "DoNothing"); From 13f590518cba3eb3e2e0f8bdc73ffaf5f3c6fa8f Mon Sep 17 00:00:00 2001 From: Iooob <94081903+Iooob@users.noreply.github.com> Date: Sat, 2 Mar 2024 09:05:09 -0500 Subject: [PATCH 56/58] fix offset --- src/main/java/frc/robot/Constants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 4e466b8..748162d 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -60,7 +60,7 @@ public static final class CANConstants { public static final int ARMLIMITSWITCHBACK = 1; // Shooter Angles - public static final double ARMENCODEROFFSET = -2.2; + public static final double ARMENCODEROFFSET = -2.45; public static final double ARMSTARTINGANGLE = 22.5 + ARMENCODEROFFSET; // WHY MaTH HURT public static final double ARMMINRELATVESTART = 0.0; public static final double ARMLOADANGLE = 35 - ARMSTARTINGANGLE; From e8c8babf2a1b0bf2929b575ab546eb36cef011ed Mon Sep 17 00:00:00 2001 From: Iooob <94081903+Iooob@users.noreply.github.com> Date: Sat, 2 Mar 2024 10:19:42 -0500 Subject: [PATCH 57/58] changed current limit --- src/main/java/frc/robot/subsystems/ShooterSubsystem.java | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index ede3767..bd6d5ad 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -48,6 +48,9 @@ public class ShooterSubsystem extends SubsystemBase { // setup SysID for auto profiling private final SysIdRoutine m_sysIdRoutine; + // current limit + private final int k_CurrentLimit = 80; + /** Creates a new ShooterSubsystem. */ public ShooterSubsystem() { // create the shooter motors @@ -59,6 +62,8 @@ public ShooterSubsystem() { m_ShooterMotorMain.setIdleMode(CANSparkMax.IdleMode.kBrake); m_ShooterMotorSecondary.setIdleMode(CANSparkMax.IdleMode.kBrake); m_ShooterMotorSecondary.follow(m_ShooterMotorMain, true); + m_ShooterMotorMain.setSmartCurrentLimit(k_CurrentLimit); + m_ShooterMotorSecondary.setSmartCurrentLimit(k_CurrentLimit); // connect to built in PID controller m_ShooterMainPIDController = m_ShooterMotorMain.getPIDController(); From 89a0f4c6aad06b04121d06990bfb8a418c4c0ec7 Mon Sep 17 00:00:00 2001 From: Iooob <94081903+Iooob@users.noreply.github.com> Date: Sun, 3 Mar 2024 09:51:14 -0500 Subject: [PATCH 58/58] Mid-Regionals changes --- .pathplanner/settings.json | 2 +- shuffleboard.json | 23 ++++- src/main/deploy/pathplanner/autos/SFR.auto | 88 +++++++++++++++++++ .../pathplanner/autos/ShootSpeakerCenter.auto | 4 +- src/main/deploy/pathplanner/paths/SFR1.path | 68 ++++++++++++++ src/main/deploy/pathplanner/paths/SFR2.path | 52 +++++++++++ .../pathplanner/paths/ShootSpeakerPath.path | 2 +- .../pathplanner/paths/SpeakerToLine.path | 2 +- src/main/java/frc/robot/Constants.java | 1 + src/main/java/frc/robot/RobotContainer.java | 6 +- 10 files changed, 241 insertions(+), 7 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/SFR.auto create mode 100644 src/main/deploy/pathplanner/paths/SFR1.path create mode 100644 src/main/deploy/pathplanner/paths/SFR2.path diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index 4e38b3e..e5c98a5 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -6,7 +6,7 @@ "New Folder" ], "autoFolders": [], - "defaultMaxVel": 2.0, + "defaultMaxVel": 1.5, "defaultMaxAccel": 1.0, "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, diff --git a/shuffleboard.json b/shuffleboard.json index d4a0526..3716c61 100644 --- a/shuffleboard.json +++ b/shuffleboard.json @@ -430,6 +430,27 @@ "_glyph": 148, "_showGlyph": false } + }, + "5,0": { + "size": [ + 4, + 3 + ], + "content": { + "_type": "Camera Stream", + "_source0": "camera_server://USB Camera 0", + "_title": "USB Camera 0", + "_glyph": 148, + "_showGlyph": false, + "Crosshair/Show crosshair": true, + "Crosshair/Crosshair color": "#FFFFFFFF", + "Controls/Show controls": false, + "Controls/Rotation": "QUARTER_CW", + "compression": -1.0, + "fps": -1, + "imageWidth": -1, + "imageHeight": -1 + } } } } @@ -438,7 +459,7 @@ "windowGeometry": { "x": 0.800000011920929, "y": 0.800000011920929, - "width": 1534.4000244140625, + "width": 1500.0, "height": 814.4000244140625 } } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/SFR.auto b/src/main/deploy/pathplanner/autos/SFR.auto new file mode 100644 index 0000000..fbe87c2 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/SFR.auto @@ -0,0 +1,88 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.75, + "y": 4.35 + }, + "rotation": 120.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "BrakeCommand" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "AimSpeakerCommand" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "ArmCommand" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "ShooterCommand" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "AimLowerCommand" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "SFR1" + } + }, + { + "type": "path", + "data": { + "pathName": "SFR2" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/ShootSpeakerCenter.auto b/src/main/deploy/pathplanner/autos/ShootSpeakerCenter.auto index fdee053..910c880 100644 --- a/src/main/deploy/pathplanner/autos/ShootSpeakerCenter.auto +++ b/src/main/deploy/pathplanner/autos/ShootSpeakerCenter.auto @@ -2,8 +2,8 @@ "version": 1.0, "startingPose": { "position": { - "x": 1.3959954881393342, - "y": 5.55 + "x": 0.6727105761903848, + "y": 4.635503404956002 }, "rotation": 180.0 }, diff --git a/src/main/deploy/pathplanner/paths/SFR1.path b/src/main/deploy/pathplanner/paths/SFR1.path new file mode 100644 index 0000000..e23f40d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/SFR1.path @@ -0,0 +1,68 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.75, + "y": 4.35 + }, + "prevControl": null, + "nextControl": { + "x": 1.8567437134823315, + "y": 2.969949786055847 + }, + "isLocked": false, + "linkedName": "SpeakerStart" + }, + { + "anchor": { + "x": 2.3500908540278633, + "y": 1.7464488775131404 + }, + "prevControl": { + "x": 0.9374285533592253, + "y": 1.7464488775131404 + }, + "nextControl": { + "x": 4.138203120548443, + "y": 1.7464488775131404 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.957431157356543, + "y": 1.3650562174103558 + }, + "prevControl": { + "x": 6.957431157356543, + "y": 1.3650562174103558 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "MidStart" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.5, + "maxAcceleration": 1.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -4.382714348011046, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 120.02871427926773, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SFR2.path b/src/main/deploy/pathplanner/paths/SFR2.path new file mode 100644 index 0000000..b104aa3 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/SFR2.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 7.957431157356543, + "y": 1.3650562174103558 + }, + "prevControl": null, + "nextControl": { + "x": 7.957431157356543, + "y": 2.2351584729206 + }, + "isLocked": false, + "linkedName": "MidStart" + }, + { + "anchor": { + "x": 7.957431157356543, + "y": 7.222602137511967 + }, + "prevControl": { + "x": 7.957431157356543, + "y": 6.272602137511967 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "MidEnd" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.5, + "maxAcceleration": 1.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ShootSpeakerPath.path b/src/main/deploy/pathplanner/paths/ShootSpeakerPath.path index e7640ee..6c0dff1 100644 --- a/src/main/deploy/pathplanner/paths/ShootSpeakerPath.path +++ b/src/main/deploy/pathplanner/paths/ShootSpeakerPath.path @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, + "maxVelocity": 1.5, "maxAcceleration": 1.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 diff --git a/src/main/deploy/pathplanner/paths/SpeakerToLine.path b/src/main/deploy/pathplanner/paths/SpeakerToLine.path index 14229a1..87901a8 100644 --- a/src/main/deploy/pathplanner/paths/SpeakerToLine.path +++ b/src/main/deploy/pathplanner/paths/SpeakerToLine.path @@ -48,7 +48,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, + "maxVelocity": 1.5, "maxAcceleration": 1.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 748162d..cadf0ae 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -49,6 +49,7 @@ public static final class CANConstants { public static final int ARMLOADBUTTON = 11; public static final int ARMTRAPBUTTON = 9; public static final int ENABLEAXISBUTTON = 10; + public static final int ARMDISABLEENCODER = 8; public static final int AIMBUTTON = 12; // Analog Ports diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6988f52..b7dc170 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -122,6 +122,7 @@ public class RobotContainer { private JoystickButton m_enableAxisButton; private JoystickButton m_shooterTrigger; private JoystickButton m_armRaiseToDefault; + private JoystickButton m_armDisableEncoderButton; // Init For Autonomous // private RamseteAutoBuilder autoBuilder; private SendableChooser autoDashboardChooser = new SendableChooser(); @@ -172,6 +173,7 @@ private void setupTriggers() { m_aimButton = new JoystickButton(m_flightStick, Constants.AIMBUTTON); // arm raise buttons m_armWheelLoadButton = new JoystickButton(m_flightStick, Constants.ARMLOADBUTTON); + m_armDisableEncoderButton = new JoystickButton(m_flightStick, Constants.ARMDISABLEENCODER); m_armRaiseToSpeakerButton = new JoystickButton(m_flightStick, Constants.ARMSPEAKERBUTTON); m_armRaiseToAmpButton = new JoystickButton(m_flightStick, Constants.ARMAMPBUTTON); m_armRaiseToTrapButton = new JoystickButton(m_flightStick, Constants.ARMTRAPBUTTON); @@ -198,6 +200,7 @@ private void bindCommands() { m_toggleBrakeButton.whileTrue(new InstantCommand(() -> m_driveSubsystem.SwitchBrakemode())); // shooter + arm commands m_shooterTrigger.whileTrue(m_shooterCommand); + m_armDisableEncoderButton.whileTrue(new InstantCommand(() -> m_armSubsystem.disableOffset())); m_armRaiseToSpeakerButton.whileTrue( new InstantCommand(() -> m_shooterState.setMode(ShooterState.ShooterMode.SPEAKER))); m_armRaiseToAmpButton.whileTrue( @@ -242,7 +245,8 @@ private void bindShooterSysIDCommands() { private void initializeAutonomous() { // Network Table Routine Options - autoDashboardChooser.setDefaultOption("ShootSpeakerCenter", "ShootSpeakerCenter"); + autoDashboardChooser.setDefaultOption("SFR", "SFR"); + autoDashboardChooser.addOption("ShootSpeakerCenter", "ShootSpeakerCenter"); autoDashboardChooser.addOption("ShootSpeakerLeft", "ShootSpeakerLeft"); autoDashboardChooser.addOption("ShootSpeakerRight", "ShootSpeakerRight"); autoDashboardChooser.addOption("ShootSpeakerFar", "ShootSpeakerFar");