From d0e9ef7b1c9b91c111dc762f0ca6aa0e19b52fe8 Mon Sep 17 00:00:00 2001 From: RavenRain44 <89553123+RavenRain44@users.noreply.github.com> Date: Mon, 5 Feb 2024 15:43:03 -0600 Subject: [PATCH 01/49] Small Changes to Functions --- build.gradle | 2 +- .../frc/robot/subsystems/LEDSubsystem.java | 26 ++++++++++++++----- 2 files changed, 20 insertions(+), 8 deletions(-) diff --git a/build.gradle b/build.gradle index 8ad1f97..31c3b10 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2024.1.1" + id "edu.wpi.first.GradleRIO" version "2024.2.1" } java { diff --git a/src/main/java/frc/robot/subsystems/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/LEDSubsystem.java index 45beeb0..1f3d242 100644 --- a/src/main/java/frc/robot/subsystems/LEDSubsystem.java +++ b/src/main/java/frc/robot/subsystems/LEDSubsystem.java @@ -10,13 +10,15 @@ public class LEDSubsystem extends SubsystemBase { public DigitalOutput AllianceColor = new DigitalOutput(2); // Alliance color - public DigitalOutput FoundNote = new DigitalOutput(3); // Sends if the camera 'OnionRing' sees a note + public DigitalOutput HasNote = new DigitalOutput(3); // Sends if the camera 'OnionRing' sees a note public DigitalOutput Aimed = new DigitalOutput(4); // Sends if the outtake is aimed public DigitalOutput NoteInIntake = new DigitalOutput(5); // Sends if the Note is in the intake/outtake public DigitalOutput NoteOuttaked = new DigitalOutput(6); // Sends if the Note has been outtaked VisionSubsystem vision; + boolean m_hasNote = false; + /** Creates a new LEDSubsystem. */ public LEDSubsystem(VisionSubsystem m_VisionSubsystem) { this.vision = m_VisionSubsystem; @@ -27,9 +29,9 @@ public void periodic() { var results = vision.getLatestResultN(); if (results.hasTargets()) { - FoundNote(true); + HasNote(true); } else { - FoundNote(false); + HasNote(false); } } @@ -37,18 +39,28 @@ public void setAllianceColor() { AllianceColor.set(Robot.getAlliance()); // True is Red, False is Blue } - public void FoundNote(boolean foundNote) { - FoundNote.set(foundNote); + public void HasNote(boolean hasNote) { + m_hasNote = hasNote; + HasNote.set(hasNote); } - public void Aimed(boolean aimed) { - Aimed.set(aimed); + public void HasNote() { + if (m_hasNote) { + m_hasNote = false; + } else { + m_hasNote = true; + } + HasNote.set(m_hasNote); } public void NoteInIntake(boolean noteInIntake) { NoteInIntake.set(noteInIntake); } + public void Aimed(boolean aimed) { + Aimed.set(aimed); + } + public void NoteOuttaked(boolean noteOuttaked) { NoteOuttaked.set(noteOuttaked); } From 84788d9b4f42a1a25090337827167d5e02f80e75 Mon Sep 17 00:00:00 2001 From: RavenRain44 <89553123+RavenRain44@users.noreply.github.com> Date: Mon, 5 Feb 2024 17:08:16 -0600 Subject: [PATCH 02/49] Changed Outputs + Added More Support --- src/main/java/frc/robot/RobotContainer.java | 4 +- .../java/frc/robot/commands/ManRizzt.java | 8 +++- .../frc/robot/subsystems/LEDSubsystem.java | 48 ++++++++++--------- 3 files changed, 34 insertions(+), 26 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 790f274..98f088c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -84,8 +84,8 @@ private void configureButtonBindings() { /* Driver Buttons */ Shoot.whileTrue(new FIREEE(m_Shoota)); zeroGyro.onTrue(new InstantCommand(() -> m_SwerveSubsystem.zeroHeading())); - wristUp.whileTrue(new ManRizzt(m_Shoota, .05)); - wristDown.whileTrue(new ManRizzt(m_Shoota, -.05)); + wristUp.whileTrue(new ManRizzt(m_Shoota, m_LEDSubsystem, .05)); + wristDown.whileTrue(new ManRizzt(m_Shoota, m_LEDSubsystem, -.05)); zeroShooter.onTrue(new InstantCommand(() -> m_Shoota.resetWristEncoder())); } diff --git a/src/main/java/frc/robot/commands/ManRizzt.java b/src/main/java/frc/robot/commands/ManRizzt.java index d20fbdc..fd0ba82 100644 --- a/src/main/java/frc/robot/commands/ManRizzt.java +++ b/src/main/java/frc/robot/commands/ManRizzt.java @@ -6,15 +6,18 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.ShooterSubsystem; +import frc.robot.subsystems.LEDSubsystem; public class ManRizzt extends Command { ShooterSubsystem shooterSubsystem; + LEDSubsystem ledSubsystem; private double speed; - public ManRizzt(ShooterSubsystem subsystem, double speed) { + public ManRizzt(ShooterSubsystem subsystem, LEDSubsystem ledSubsystem, double speed) { // Use addRequirements() here to declare subsystem dependencies. shooterSubsystem = subsystem; + this.ledSubsystem = ledSubsystem; this.speed = speed; addRequirements(shooterSubsystem); } @@ -27,15 +30,18 @@ public void initialize() { @Override public void execute() { shooterSubsystem.manualWristSpeed(speed); + ledSubsystem.ReadyToFire(false); } @Override public boolean isFinished() { + ledSubsystem.ReadyToFire(true); return false; } @Override public void end(boolean interrupted) { shooterSubsystem.manualWristSpeed(0); + ledSubsystem.ReadyToFire(false); } } diff --git a/src/main/java/frc/robot/subsystems/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/LEDSubsystem.java index 1f3d242..b47b05f 100644 --- a/src/main/java/frc/robot/subsystems/LEDSubsystem.java +++ b/src/main/java/frc/robot/subsystems/LEDSubsystem.java @@ -5,19 +5,21 @@ package frc.robot.subsystems; import edu.wpi.first.wpilibj.DigitalOutput; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Robot; public class LEDSubsystem extends SubsystemBase { public DigitalOutput AllianceColor = new DigitalOutput(2); // Alliance color - public DigitalOutput HasNote = new DigitalOutput(3); // Sends if the camera 'OnionRing' sees a note - public DigitalOutput Aimed = new DigitalOutput(4); // Sends if the outtake is aimed - public DigitalOutput NoteInIntake = new DigitalOutput(5); // Sends if the Note is in the intake/outtake - public DigitalOutput NoteOuttaked = new DigitalOutput(6); // Sends if the Note has been outtaked + public DigitalOutput Disconnected = new DigitalOutput(3); // Disconnected color + public DigitalOutput NoteDetected = new DigitalOutput(4); // Note Detected color + public DigitalOutput Intaking = new DigitalOutput(5); // Intaking color + public DigitalOutput ChargingOuttake = new DigitalOutput(6); // Charging the Outtake color + public DigitalOutput ReadyToFire = new DigitalOutput(7); // Ready to Fire color VisionSubsystem vision; - boolean m_hasNote = false; + /** Creates a new LEDSubsystem. */ public LEDSubsystem(VisionSubsystem m_VisionSubsystem) { @@ -29,9 +31,15 @@ public void periodic() { var results = vision.getLatestResultN(); if (results.hasTargets()) { - HasNote(true); + NoteDetected(true); } else { - HasNote(false); + NoteDetected(false); + } + + if (DriverStation.isDSAttached()) { + Disconnected.set(false); + } else { + Disconnected.set(true); } } @@ -39,29 +47,23 @@ public void setAllianceColor() { AllianceColor.set(Robot.getAlliance()); // True is Red, False is Blue } - public void HasNote(boolean hasNote) { - m_hasNote = hasNote; - HasNote.set(hasNote); + public void Disconnected(boolean disconnected) { + Disconnected.set(disconnected); } - public void HasNote() { - if (m_hasNote) { - m_hasNote = false; - } else { - m_hasNote = true; - } - HasNote.set(m_hasNote); + public void NoteDetected(boolean note) { + NoteDetected.set(note); } - public void NoteInIntake(boolean noteInIntake) { - NoteInIntake.set(noteInIntake); + public void Intaking(boolean intaking) { + Intaking.set(intaking); } - public void Aimed(boolean aimed) { - Aimed.set(aimed); + public void ChargingOuttake(boolean chargingOuttake) { + ChargingOuttake.set(chargingOuttake); } - public void NoteOuttaked(boolean noteOuttaked) { - NoteOuttaked.set(noteOuttaked); + public void ReadyToFire(boolean fire) { + ReadyToFire.set(fire); } } From 98fdec50b83908ad151cf7afaef605f4405fba67 Mon Sep 17 00:00:00 2001 From: RavenRain44 <89553123+RavenRain44@users.noreply.github.com> Date: Sat, 10 Feb 2024 14:43:43 -0600 Subject: [PATCH 03/49] Binary Encoder added --- src/main/java/frc/robot/RobotContainer.java | 2 +- .../frc/robot/subsystems/LEDSubsystem.java | 85 +++++++++++++++---- 2 files changed, 70 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 98f088c..dba13b1 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -45,7 +45,7 @@ public class RobotContainer { private final VisionSubsystem m_VisionSubsystem = new VisionSubsystem(); private final SwerveSubsystem m_SwerveSubsystem = new SwerveSubsystem(m_VisionSubsystem); public final ShooterSubsystem m_Shoota = new ShooterSubsystem(m_SwerveSubsystem); - private final LEDSubsystem m_LEDSubsystem = new LEDSubsystem(m_VisionSubsystem); + private final LEDSubsystem m_LEDSubsystem = new LEDSubsystem(m_VisionSubsystem, m_Shoota); private final SendableChooser autoChooser; diff --git a/src/main/java/frc/robot/subsystems/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/LEDSubsystem.java index b47b05f..7af98a4 100644 --- a/src/main/java/frc/robot/subsystems/LEDSubsystem.java +++ b/src/main/java/frc/robot/subsystems/LEDSubsystem.java @@ -4,30 +4,50 @@ package frc.robot.subsystems; +import java.lang.reflect.Array; + import edu.wpi.first.wpilibj.DigitalOutput; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Robot; public class LEDSubsystem extends SubsystemBase { - public DigitalOutput AllianceColor = new DigitalOutput(2); // Alliance color - public DigitalOutput Disconnected = new DigitalOutput(3); // Disconnected color - public DigitalOutput NoteDetected = new DigitalOutput(4); // Note Detected color - public DigitalOutput Intaking = new DigitalOutput(5); // Intaking color - public DigitalOutput ChargingOuttake = new DigitalOutput(6); // Charging the Outtake color - public DigitalOutput ReadyToFire = new DigitalOutput(7); // Ready to Fire color + public DigitalOutput bit1 = new DigitalOutput(2); // Bit 1 (1) + public DigitalOutput bit2 = new DigitalOutput(3); // Bit 2 (2) + public DigitalOutput bit3 = new DigitalOutput(4); // Bit 3 (4) + public DigitalOutput bit4 = new DigitalOutput(5); // Bit 4 (8) - VisionSubsystem vision; + public DigitalOutput[] bits = {bit1, bit2, bit3, bit4}; + + public Boolean[] inputBooleans = { + false, // AllianceColor 0 + false, // Disconnected 1 + false, // NoteDetected 2 + false, // Intaking 3 + false, // ChargingOuttake 4 + false // ReadyToFire 5 + }; + + boolean[] output = new boolean[4]; + VisionSubsystem vision; + ShooterSubsystem shooter; /** Creates a new LEDSubsystem. */ - public LEDSubsystem(VisionSubsystem m_VisionSubsystem) { + public LEDSubsystem(VisionSubsystem m_VisionSubsystem, ShooterSubsystem m_ShooterSubsystem) { this.vision = m_VisionSubsystem; + this.shooter = m_ShooterSubsystem; } @Override public void periodic() { + set(); // Set booleans + encoder(); // Encode the values + send(); // Output to the pins + } + + private void set() { var results = vision.getLatestResultN(); if (results.hasTargets()) { @@ -37,33 +57,66 @@ public void periodic() { } if (DriverStation.isDSAttached()) { - Disconnected.set(false); + inputBooleans[1] = false; + } else { + inputBooleans[1] = true; + } + + if (shooter.getVelocity() > shooter.getCalculatedVelocity() - 0.5) { + ReadyToFire(true); + } else if (shooter.getVelocity() > 0.5 && shooter.getCalculatedVelocity() > shooter.getVelocity()) { + ReadyToFire(false); + ChargingOuttake(true); } else { - Disconnected.set(true); + ReadyToFire(false); + ChargingOuttake(false); + } + } + + private void encoder() { + // Listed in order of priority + if (inputBooleans[5]) { // Ready to fire + output[0] = true; output[1] = false; output[2] = true; output[3] = false; // 1010 + } else if (inputBooleans[3]) { // Intaking + output[0] = true; output[1] = true; output[2] = false; output[3] = false; // 1100 + } else if (inputBooleans[4]) { // Charging the Outtake + output[0] = false; output[1] = false; output[2] = true; output[3] = false; // 0010 + } else if (inputBooleans[2]) { // Note Detected + output[0] = false; output[1] = true; output[2] = false; output[3] = false; // 0100 + } else if (inputBooleans[1]) { // Driver Station Disconnected + output[0] = true; output[1] = false; output[2] = false; output[3] = false; // 1000 + } else if (inputBooleans[0]) { // Alliance color + output[0] = false; output[1] = false; output[2] = false; output[3] = false; // 0000 + } + } + + private void send() { // Redundant + for (int i = 0; i < output.length; i++) { + bits[i].set(output[i]); } } public void setAllianceColor() { - AllianceColor.set(Robot.getAlliance()); // True is Red, False is Blue + inputBooleans[0] = Robot.getAlliance(); // True is Red, False is Blue } public void Disconnected(boolean disconnected) { - Disconnected.set(disconnected); + inputBooleans[1] = disconnected; } public void NoteDetected(boolean note) { - NoteDetected.set(note); + inputBooleans[2] = note; } public void Intaking(boolean intaking) { - Intaking.set(intaking); + inputBooleans[3] = intaking; } public void ChargingOuttake(boolean chargingOuttake) { - ChargingOuttake.set(chargingOuttake); + inputBooleans[4] = chargingOuttake; } public void ReadyToFire(boolean fire) { - ReadyToFire.set(fire); + inputBooleans[5] = fire; } } From bac0225306c68bb6be0841f8196f512f8ce94cf3 Mon Sep 17 00:00:00 2001 From: RavenRain44 <89553123+RavenRain44@users.noreply.github.com> Date: Sat, 17 Feb 2024 10:39:48 -0600 Subject: [PATCH 04/49] Added final encoder code --- .vscode/launch.json | 12 +- .../frc/robot/subsystems/LEDSubsystem.java | 150 ++++++++++++++---- 2 files changed, 125 insertions(+), 37 deletions(-) diff --git a/.vscode/launch.json b/.vscode/launch.json index c9c9713..1f39ec5 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -4,18 +4,24 @@ // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 "version": "0.2.0", "configurations": [ - + { + "type": "java", + "name": "Main", + "request": "launch", + "mainClass": "frc.robot.Main", + "projectName": "2024-Robot" + }, { "type": "wpilib", "name": "WPILib Desktop Debug", "request": "launch", - "desktop": true, + "desktop": true }, { "type": "wpilib", "name": "WPILib roboRIO Debug", "request": "launch", - "desktop": false, + "desktop": false } ] } diff --git a/src/main/java/frc/robot/subsystems/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/LEDSubsystem.java index 7af98a4..6279849 100644 --- a/src/main/java/frc/robot/subsystems/LEDSubsystem.java +++ b/src/main/java/frc/robot/subsystems/LEDSubsystem.java @@ -20,12 +20,16 @@ public class LEDSubsystem extends SubsystemBase { public DigitalOutput[] bits = {bit1, bit2, bit3, bit4}; public Boolean[] inputBooleans = { - false, // AllianceColor 0 - false, // Disconnected 1 - false, // NoteDetected 2 - false, // Intaking 3 - false, // ChargingOuttake 4 - false // ReadyToFire 5 + false, // Amp -0 Rainbow Pattern #1 + false, // Source -1 Rainbow Pattern #2 + false, // Climb -2 Rainbow Pattern #3 + false, // Has Note -3 For Computing + false, // Charging -4 Green/Magenta Pulse (HasNote) + false, // At Speed -5 Green/Magenta BLink (HasNote) + false, // Note in Intake -6 Orange Blink + false, // Note in Shooter -7 Orange Solid + false, // Note Detected -8 White Solid + false, // Alliance Color -9 Red/Blue Wave }; boolean[] output = new boolean[4]; @@ -42,12 +46,12 @@ public LEDSubsystem(VisionSubsystem m_VisionSubsystem, ShooterSubsystem m_Shoote @Override public void periodic() { - set(); // Set booleans - encoder(); // Encode the values - send(); // Output to the pins + set(); // Decimal Phase + encoder(); // Transition Phase + send(); // Send Phase } - private void set() { + private void set() { // Decimal phase var results = vision.getLatestResultN(); if (results.hasTargets()) { @@ -73,43 +77,101 @@ private void set() { } } - private void encoder() { - // Listed in order of priority - if (inputBooleans[5]) { // Ready to fire - output[0] = true; output[1] = false; output[2] = true; output[3] = false; // 1010 - } else if (inputBooleans[3]) { // Intaking - output[0] = true; output[1] = true; output[2] = false; output[3] = false; // 1100 - } else if (inputBooleans[4]) { // Charging the Outtake - output[0] = false; output[1] = false; output[2] = true; output[3] = false; // 0010 - } else if (inputBooleans[2]) { // Note Detected - output[0] = false; output[1] = true; output[2] = false; output[3] = false; // 0100 - } else if (inputBooleans[1]) { // Driver Station Disconnected - output[0] = true; output[1] = false; output[2] = false; output[3] = false; // 1000 - } else if (inputBooleans[0]) { // Alliance color - output[0] = false; output[1] = false; output[2] = false; output[3] = false; // 0000 + private void encoder() { // Transition phase + // Decimal to Binary + int pin_amount = 4; // Amount of pins + + int trueIndex = 0; // Index of selected LED sequence + + for (int i = 0; i < inputBooleans.length; i++) { // Picks the first true sequence based on priority + if (inputBooleans[i]) { + trueIndex = i; + break; + } } + + String binaryString = Integer.toBinaryString(trueIndex); + + int length = binaryString.length(); + + String finalString; + if (length < pin_amount) { + int zerosToAdd = pin_amount - length; + StringBuilder paddedStringBuilder = new StringBuilder(); + + for (int i = 0; i < zerosToAdd; i++) { + paddedStringBuilder.append("0"); + } + + paddedStringBuilder.append(binaryString); + String paddedBinaryString = paddedStringBuilder.toString(); + + finalString = paddedBinaryString; + } else { + finalString = binaryString; + } + + for (int i = pin_amount - 1; i >= 0; i--) { + output[i] = finalString.charAt(i) == '1'; + } + + + // if (inputBooleans[0]) { + // output[0] = false; output[1] = false; output[2] = false; output[3] = false; // 0000 + // } else if (inputBooleans[1]) { + // output[0] = true; output[1] = false; output[2] = false; output[3] = false; // 1000 + // } else if (inputBooleans[2]) { + // output[0] = false; output[1] = true; output[2] = false; output[3] = false; // 0100 + // } else if (inputBooleans[3]) { + // output[0] = true; output[1] = true; output[2] = false; output[3] = false; // 1100 + // } else if (inputBooleans[4]) { + // output[0] = false; output[1] = false; output[2] = true; output[3] = false; // 0010 + // } else if (inputBooleans[5]) { + // output[0] = true; output[1] = false; output[2] = true; output[3] = false; // 1010 + // } else if (inputBooleans[6]) { + // output[0] = false; output[1] = true; output[2] = true; output[3] = false; // 0110 + // } else if (inputBooleans[7]) { + // output[0] = true; output[1] = true; output[2] = true; output[3] = false; // 1110 + // } else if (inputBooleans[8]) { + // output[0] = false; output[1] = false; output[2] = false; output[3] = true; // 0001 + // } else if (inputBooleans[9]) { + // output[0] = true; output[1] = false; output[2] = false; output[3] = true; // 1001 + // } else if (inputBooleans[10]) { + // output[0] = false; output[1] = true; output[2] = false; output[3] = true; // 0101 + // } else if (inputBooleans[11]) { + // output[0] = true; output[1] = true; output[2] = false; output[3] = true; // 1101 + // } else if (inputBooleans[12]) { + // output[0] = false; output[1] = false; output[2] = true; output[3] = true; // 0011 + // } else if (inputBooleans[13]) { + // output[0] = true; output[1] = false; output[2] = true; output[3] = true; // 1011 + // } else if (inputBooleans[14]) { + // output[0] = false; output[1] = true; output[2] = true; output[3] = true; // 0111 + // } else if (inputBooleans[15]) { + // output[0] = true; output[1] = true; output[2] = true; output[3] = true; // 1111 + // } } - private void send() { // Redundant + private void send() { // Send Phase - Redundancy is bliss for (int i = 0; i < output.length; i++) { bits[i].set(output[i]); } } - public void setAllianceColor() { - inputBooleans[0] = Robot.getAlliance(); // True is Red, False is Blue + // SET FUNCTIONS + public void ampTele(boolean amp) { + inputBooleans[0] = amp; } - - public void Disconnected(boolean disconnected) { - inputBooleans[1] = disconnected; + + public void sourceTele(boolean source) { + inputBooleans[1] = source; } - public void NoteDetected(boolean note) { - inputBooleans[2] = note; + public void climbTele(boolean climb) { + inputBooleans[2] = climb; } - public void Intaking(boolean intaking) { - inputBooleans[3] = intaking; + public void HasNote(boolean hasNote) { + inputBooleans[3] = hasNote; } public void ChargingOuttake(boolean chargingOuttake) { @@ -119,4 +181,24 @@ public void ChargingOuttake(boolean chargingOuttake) { public void ReadyToFire(boolean fire) { inputBooleans[5] = fire; } + + public void NoteInIntake(boolean noteInIntake) { + inputBooleans[6] = noteInIntake; + } + + public void NoteInShooter(boolean noteInShooter) { + inputBooleans[7] = noteInShooter; + } + + public void NoteDetected(boolean note) { + inputBooleans[8] = note; + } + + public void setAllianceColor() { + inputBooleans[9] = Robot.getAlliance(); // True is Red, False is Blue + } + + public void Disconnected(boolean disconnected) { // NOT CONFIRMED TO BE IN FINAL + inputBooleans[10] = disconnected; + } } From 5f8769bb20a609db7308520382ae9d684bde9c71 Mon Sep 17 00:00:00 2001 From: RavenRain44 <89553123+RavenRain44@users.noreply.github.com> Date: Thu, 29 Feb 2024 18:40:28 -0600 Subject: [PATCH 05/49] Changed Booleans --- src/main/java/frc/robot/RobotContainer.java | 2 +- .../frc/robot/subsystems/LEDSubsystem.java | 66 ++++++++++++------- 2 files changed, 45 insertions(+), 23 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index dba13b1..8e4ca81 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -79,7 +79,7 @@ public RobotContainer() { * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing * it to a {@link * edu.wpi.first.wpilibj2.command.button.JoystickButton}. - */ + */ private void configureButtonBindings() { /* Driver Buttons */ Shoot.whileTrue(new FIREEE(m_Shoota)); diff --git a/src/main/java/frc/robot/subsystems/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/LEDSubsystem.java index 6279849..808d301 100644 --- a/src/main/java/frc/robot/subsystems/LEDSubsystem.java +++ b/src/main/java/frc/robot/subsystems/LEDSubsystem.java @@ -19,17 +19,24 @@ public class LEDSubsystem extends SubsystemBase { public DigitalOutput[] bits = {bit1, bit2, bit3, bit4}; + public Boolean hasNote = false; + public Boolean chargingOuttake = false; + public Boolean atSpeed = false; + public Boolean[] inputBooleans = { false, // Amp -0 Rainbow Pattern #1 false, // Source -1 Rainbow Pattern #2 false, // Climb -2 Rainbow Pattern #3 - false, // Has Note -3 For Computing - false, // Charging -4 Green/Magenta Pulse (HasNote) - false, // At Speed -5 Green/Magenta BLink (HasNote) - false, // Note in Intake -6 Orange Blink - false, // Note in Shooter -7 Orange Solid - false, // Note Detected -8 White Solid - false, // Alliance Color -9 Red/Blue Wave + false, // Blank/DC -3 None + false, // Note Detected -4 White Solid + false, // Charging -5 Green Pulse (HasNote) + false, // At Speed -6 Green BLink (HasNote) + false, // Charging -7 Magenta Pulse (NoNote) + false, // At Speed -8 Magenta BLink (NoNote) + false, // Note in Intake -9 Orange Blink + false, // Note in Shooter -10 Orange Solid + false, // Alliance Color -11 Red Pulse + false // Alliance Color -12 Blue Pulse }; boolean[] output = new boolean[4]; @@ -66,14 +73,28 @@ private void set() { // Decimal phase inputBooleans[1] = true; } - if (shooter.getVelocity() > shooter.getCalculatedVelocity() - 0.5) { - ReadyToFire(true); - } else if (shooter.getVelocity() > 0.5 && shooter.getCalculatedVelocity() > shooter.getVelocity()) { - ReadyToFire(false); - ChargingOuttake(true); + + + if (hasNote) { // Converts from simple inputs to boolean + if (chargingOuttake) { + inputBooleans[5] = true; + inputBooleans[6] = false; + } else if (atSpeed) { + inputBooleans[5] = false; + inputBooleans[6] = true; + } + inputBooleans[7] = false; + inputBooleans[8] = false; } else { - ReadyToFire(false); - ChargingOuttake(false); + if (chargingOuttake) { + inputBooleans[7] = true; + inputBooleans[8] = false; + } else if (atSpeed) { + inputBooleans[7] = false; + inputBooleans[8] = true; + } + inputBooleans[5] = false; + inputBooleans[6] = false; } } @@ -171,34 +192,35 @@ public void climbTele(boolean climb) { } public void HasNote(boolean hasNote) { - inputBooleans[3] = hasNote; + this.hasNote = hasNote; } public void ChargingOuttake(boolean chargingOuttake) { - inputBooleans[4] = chargingOuttake; + this.chargingOuttake = chargingOuttake; } public void ReadyToFire(boolean fire) { - inputBooleans[5] = fire; + this.atSpeed = fire; } public void NoteInIntake(boolean noteInIntake) { - inputBooleans[6] = noteInIntake; + inputBooleans[9] = noteInIntake; } public void NoteInShooter(boolean noteInShooter) { - inputBooleans[7] = noteInShooter; + inputBooleans[10] = noteInShooter; } public void NoteDetected(boolean note) { - inputBooleans[8] = note; + inputBooleans[4] = note; } public void setAllianceColor() { - inputBooleans[9] = Robot.getAlliance(); // True is Red, False is Blue + inputBooleans[11] = Robot.getAlliance(); // True is Red + inputBooleans[12] = !Robot.getAlliance(); // False is Blue } public void Disconnected(boolean disconnected) { // NOT CONFIRMED TO BE IN FINAL - inputBooleans[10] = disconnected; + inputBooleans[3] = disconnected; } } From 73b7be72425feb920880e83ce4e9b32febde2817 Mon Sep 17 00:00:00 2001 From: RavenRain44 <89553123+RavenRain44@users.noreply.github.com> Date: Thu, 29 Feb 2024 19:58:18 -0600 Subject: [PATCH 06/49] Took new information to set --- src/main/java/frc/robot/RobotContainer.java | 2 +- src/main/java/frc/robot/commands/FIREEE.java | 11 ++++++++++- .../java/frc/robot/commands/ToBreakOrNotToBreak.java | 7 ++++++- src/main/java/frc/robot/subsystems/LEDSubsystem.java | 6 +++--- 4 files changed, 20 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f5a0278..60f1bbb 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -137,7 +137,7 @@ private void configureButtonBindings() { targetAmp.whileTrue(new MissileLock(m_Shoota, "amp")); // Shooter - targetSpeaker.or(targetAmp).and(Shoot).whileTrue(new FIREEE(m_Shoota, m_IntexerSubsystem)); // Main fire + targetSpeaker.or(targetAmp).and(Shoot).whileTrue(new FIREEE(m_Shoota, m_IntexerSubsystem, m_LEDSubsystem)); // Main fire // Reset Odometry resetOdom.onTrue(new InstantCommand(() -> m_SwerveSubsystem.zeroHeading()).alongWith( diff --git a/src/main/java/frc/robot/commands/FIREEE.java b/src/main/java/frc/robot/commands/FIREEE.java index 24fe217..e075c0a 100644 --- a/src/main/java/frc/robot/commands/FIREEE.java +++ b/src/main/java/frc/robot/commands/FIREEE.java @@ -6,15 +6,18 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.IntexerSubsystem; +import frc.robot.subsystems.LEDSubsystem; import frc.robot.subsystems.ShooterSubsystem; public class FIREEE extends Command { private ShooterSubsystem shooter; private IntexerSubsystem intexer; + private LEDSubsystem ledSubsystem; - public FIREEE(ShooterSubsystem shooterSub, IntexerSubsystem intex) { + public FIREEE(ShooterSubsystem shooterSub, IntexerSubsystem intex, LEDSubsystem ledSubsystem) { shooter = shooterSub; intexer = intex; + this.ledSubsystem = ledSubsystem; // Use addRequirements() here to declare subsystem dependencies. addRequirements(intex); } @@ -29,6 +32,12 @@ public void initialize() { public void execute() { if (shooter.shooterAtSpeed()){ intexer.setShooterIntake(.9); + ledSubsystem.atSpeed = true; + ledSubsystem.chargingOuttake = false; + ledSubsystem.hasNote = false; + } else { + ledSubsystem.atSpeed = false; + ledSubsystem.chargingOuttake = true; } } diff --git a/src/main/java/frc/robot/commands/ToBreakOrNotToBreak.java b/src/main/java/frc/robot/commands/ToBreakOrNotToBreak.java index 2e1c3bd..7199e56 100644 --- a/src/main/java/frc/robot/commands/ToBreakOrNotToBreak.java +++ b/src/main/java/frc/robot/commands/ToBreakOrNotToBreak.java @@ -6,14 +6,17 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.IntexerSubsystem; +import frc.robot.subsystems.LEDSubsystem; public class ToBreakOrNotToBreak extends Command { IntexerSubsystem intexer; + LEDSubsystem ledSubsystem; /** Creates a new ToBreakOrNotToBreak. */ - public ToBreakOrNotToBreak(IntexerSubsystem intexer) { + public ToBreakOrNotToBreak(IntexerSubsystem intexer, LEDSubsystem ledSubsystem) { // Use addRequirements() here to declare subsystem dependencies. this.intexer = intexer; + this.ledSubsystem = ledSubsystem; addRequirements(intexer); } @@ -27,8 +30,10 @@ public void initialize() { public void execute() { if (!intexer.shooterBreak()){ intexer.setShooterIntake(.5); + ledSubsystem.hasNote = false; } else { intexer.setShooterIntake(0); + ledSubsystem.hasNote = true; } } diff --git a/src/main/java/frc/robot/subsystems/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/LEDSubsystem.java index 808d301..234cee8 100644 --- a/src/main/java/frc/robot/subsystems/LEDSubsystem.java +++ b/src/main/java/frc/robot/subsystems/LEDSubsystem.java @@ -26,15 +26,15 @@ public class LEDSubsystem extends SubsystemBase { public Boolean[] inputBooleans = { false, // Amp -0 Rainbow Pattern #1 false, // Source -1 Rainbow Pattern #2 - false, // Climb -2 Rainbow Pattern #3 + false, // Climb -2 Rainbow Pattern #3 ASK ANDREW ABOUT HOW TO DO false, // Blank/DC -3 None false, // Note Detected -4 White Solid false, // Charging -5 Green Pulse (HasNote) false, // At Speed -6 Green BLink (HasNote) false, // Charging -7 Magenta Pulse (NoNote) false, // At Speed -8 Magenta BLink (NoNote) - false, // Note in Intake -9 Orange Blink - false, // Note in Shooter -10 Orange Solid + false, // Note in Intake -9 Orange Blink ASK ANDREW ABOUT HOW TO DO + false, // Note in Shooter -10 Orange Solid ASK ANDREW ABOUT HOW TO DO false, // Alliance Color -11 Red Pulse false // Alliance Color -12 Blue Pulse }; From 5c260ca2781a0e173466bb388270241a4628d10a Mon Sep 17 00:00:00 2001 From: Andrew-K961 Date: Sat, 2 Mar 2024 22:41:18 -0600 Subject: [PATCH 07/49] tregedy --- .pathplanner/settings.json | 3 +- CHOREO_TEST.chor | 1377 ----------------- src/main/deploy/choreo/Troll.1.traj | 1030 ++++++++++++ .../pathplanner/autos/4 piece mcnugget.auto | 85 + src/main/deploy/pathplanner/autos/Troll.auto | 25 + .../pathplanner/paths/2nd Note TOP.path | 86 + .../deploy/pathplanner/paths/3N Top 1.path | 58 + .../pathplanner/paths/3N Top Shoot 1.path | 52 + .../pathplanner/paths/3N Top Shoot 2.path | 52 + .../pathplanner/paths/3N Top Shoot 3.path | 52 + .../pathplanner/paths/3rd Note TOP.path | 147 ++ .../pathplanner/paths/Straight Line Top.path | 58 + src/main/java/frc/robot/Constants.java | 4 +- src/main/java/frc/robot/Robot.java | 1 + src/main/java/frc/robot/RobotContainer.java | 16 +- src/main/java/frc/robot/commands/AimBot.java | 86 + .../java/frc/robot/commands/IntexBestHex.java | 2 +- .../robot/commands/IntexForAutosByAutos.java | 20 +- .../java/frc/robot/commands/NoteSniffer.java | 99 ++ .../java/frc/robot/commands/TeleopSwerve.java | 4 +- .../{ZeroWrist.java => ZeroRizz.java} | 4 +- .../robot/subsystems/ShooterSubsystem.java | 2 +- .../frc/robot/subsystems/SwerveSubsystem.java | 6 + 23 files changed, 1872 insertions(+), 1397 deletions(-) create mode 100644 src/main/deploy/choreo/Troll.1.traj create mode 100644 src/main/deploy/pathplanner/autos/4 piece mcnugget.auto create mode 100644 src/main/deploy/pathplanner/autos/Troll.auto create mode 100644 src/main/deploy/pathplanner/paths/2nd Note TOP.path create mode 100644 src/main/deploy/pathplanner/paths/3N Top 1.path create mode 100644 src/main/deploy/pathplanner/paths/3N Top Shoot 1.path create mode 100644 src/main/deploy/pathplanner/paths/3N Top Shoot 2.path create mode 100644 src/main/deploy/pathplanner/paths/3N Top Shoot 3.path create mode 100644 src/main/deploy/pathplanner/paths/3rd Note TOP.path create mode 100644 src/main/deploy/pathplanner/paths/Straight Line Top.path create mode 100644 src/main/java/frc/robot/commands/AimBot.java create mode 100644 src/main/java/frc/robot/commands/NoteSniffer.java rename src/main/java/frc/robot/commands/{ZeroWrist.java => ZeroRizz.java} (94%) diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index 8b73a9d..e5b0f33 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -3,7 +3,8 @@ "robotLength": 0.6604, "holonomicMode": true, "pathFolders": [ - "3 Note Test" + "3 Note Test", + "3 Note Real" ], "autoFolders": [ "Testing" diff --git a/CHOREO_TEST.chor b/CHOREO_TEST.chor index 9e7a53e..f171404 100644 --- a/CHOREO_TEST.chor +++ b/CHOREO_TEST.chor @@ -13,1383 +13,6 @@ "wheelRadius": 0.0508 }, "paths": { - "Choreo Test Path": { - "waypoints": [ - { - "x": 2.098140239715576, - "y": 1.6614248752593994, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 16 - }, - { - "x": 3.049372911453247, - "y": 1.7892024517059326, - "heading": 1.5451611748024034, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 16 - }, - { - "x": 3.929617404937744, - "y": 1.4626601934432983, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 40 - } - ], - "trajectory": [ - { - "x": 2.098140239715576, - "y": 1.6614248752593994, - "heading": 0, - "angularVelocity": -1.1782841342854263e-17, - "velocityX": 0, - "velocityY": 0, - "timestamp": 0 - }, - { - "x": 2.1051233600297774, - "y": 1.6635944045152462, - "heading": -3.951223132837311e-19, - "angularVelocity": -1.1782800630375306e-17, - "velocityX": 0.20824119889647139, - "velocityY": 0.06469677636224515, - "timestamp": 0.033533807676900264 - }, - { - "x": 2.119117786580325, - "y": 1.6678405678016424, - "heading": -7.9021234698948305e-19, - "angularVelocity": -1.1781838033475747e-17, - "velocityX": 0.4173229203609856, - "velocityY": 0.12662335656327253, - "timestamp": 0.06706761535380053 - }, - { - "x": 2.1401546040286137, - "y": 1.6740553731630874, - "heading": -1.1852369347134683e-18, - "angularVelocity": -1.177988639288366e-17, - "velocityX": 0.6273316066871726, - "velocityY": 0.1853295462694014, - "timestamp": 0.1006014230307008 - }, - { - "x": 2.1682680830995276, - "y": 1.6821118294466222, - "heading": -1.5801501749567216e-18, - "angularVelocity": -1.1776565942199235e-17, - "velocityX": 0.8383622683647712, - "velocityY": 0.24024877703001501, - "timestamp": 0.13413523070760106 - }, - { - "x": 2.203495885097548, - "y": 1.6918586180026898, - "heading": -1.97490623007624e-18, - "angularVelocity": -1.1771878578827117e-17, - "velocityX": 1.05051601468707, - "velocityY": 0.29065558704154515, - "timestamp": 0.1676690383845013 - }, - { - "x": 2.2458790364152166, - "y": 1.703112650834629, - "heading": -2.36944701232745e-18, - "angularVelocity": -1.1765459002163076e-17, - "velocityX": 1.263893194773236, - "velocityY": 0.33560259366823547, - "timestamp": 0.20120284606140157 - }, - { - "x": 2.2954613527044936, - "y": 1.7156484118689923, - "heading": -2.7636989943414952e-18, - "angularVelocity": -1.1756846791909453e-17, - "velocityX": 1.4785769861569105, - "velocityY": 0.3738245640085363, - "timestamp": 0.23473665373830183 - }, - { - "x": 2.352287604871465, - "y": 1.729182265786357, - "heading": -3.157524610938865e-18, - "angularVelocity": -1.1744132302464973e-17, - "velocityX": 1.6945958751387369, - "velocityY": 0.4035883442692906, - "timestamp": 0.2682704614152021 - }, - { - "x": 2.4163988172653466, - "y": 1.7433486874743551, - "heading": -3.5508014214230656e-18, - "angularVelocity": -1.1727766554501323e-17, - "velocityX": 1.9118381369511075, - "velocityY": 0.42245192745460064, - "timestamp": 0.3018042690921024 - }, - { - "x": 2.4878208995175837, - "y": 1.7576632912771164, - "heading": -3.94344512657471e-18, - "angularVelocity": -1.1708886946398038e-17, - "velocityX": 2.129853040859288, - "velocityY": 0.42687081469194577, - "timestamp": 0.3353380767690027 - }, - { - "x": 2.5665373086257057, - "y": 1.7714645173374621, - "heading": -4.335271205020906e-18, - "angularVelocity": -1.1684504789304052e-17, - "velocityX": 2.347374621652199, - "velocityY": 0.41156155582810217, - "timestamp": 0.36887188444590296 - }, - { - "x": 2.6524225977886715, - "y": 1.7838242511583042, - "heading": -4.726013111112515e-18, - "angularVelocity": -1.1652174076634648e-17, - "velocityX": 2.561155297080271, - "velocityY": 0.36857531777864144, - "timestamp": 0.40240569212280325 - }, - { - "x": 2.7450834234519204, - "y": 1.7934339266460941, - "heading": -5.115299885471634e-18, - "angularVelocity": -1.1608781113863041e-17, - "velocityX": 2.7632062113565348, - "velocityY": 0.2865667859843989, - "timestamp": 0.43593949979970353 - }, - { - "x": 2.843530303188657, - "y": 1.7985723309277442, - "heading": -5.502770619918827e-18, - "angularVelocity": -1.1554625647246029e-17, - "velocityX": 2.935750114787817, - "velocityY": 0.15323056454418926, - "timestamp": 0.4694733074766038 - }, - { - "x": 2.9458179129472843, - "y": 1.797488238615398, - "heading": -5.8894880430727135e-18, - "angularVelocity": -1.1532161415565022e-17, - "velocityX": 3.0502831871693488, - "velocityY": -0.032328339292479705, - "timestamp": 0.503007115153504 - }, - { - "x": 3.049372911453247, - "y": 1.7892024517059326, - "heading": -6.275464863063295e-18, - "angularVelocity": -1.1510076130028052e-17, - "velocityX": 3.0880775456133938, - "velocityY": -0.24708756575746865, - "timestamp": 0.5365409228304043 - }, - { - "x": 3.1571269598575213, - "y": 1.772490251509115, - "heading": -6.681562648101382e-18, - "angularVelocity": -1.1523095181534151e-17, - "velocityX": 3.0575407451325263, - "velocityY": -0.47421172382188026, - "timestamp": 0.5717829874131273 - }, - { - "x": 3.261123146904745, - "y": 1.7486292178338445, - "heading": -8.55841431876238e-18, - "angularVelocity": -5.3255999561210285e-17, - "velocityX": 2.9509107448319467, - "velocityY": -0.6770611755522685, - "timestamp": 0.6070250519958502 - }, - { - "x": 3.3588334048054063, - "y": 1.719697243767781, - "heading": -9.701173892106415e-18, - "angularVelocity": -3.2426007562861585e-17, - "velocityX": 2.7725463606510936, - "velocityY": -0.8209500325427858, - "timestamp": 0.6422671165785732 - }, - { - "x": 3.44889324038053, - "y": 1.6881771415609788, - "heading": -1.0845266271605606e-17, - "angularVelocity": -3.2463826162489857e-17, - "velocityX": 2.5554642340470015, - "velocityY": -0.8943886398259916, - "timestamp": 0.6775091811612961 - }, - { - "x": 3.5309014136529964, - "y": 1.6560260590968738, - "heading": -1.1990405056824877e-17, - "angularVelocity": -3.2493518099484904e-17, - "velocityX": 2.3269968500275913, - "velocityY": -0.912292819526431, - "timestamp": 0.712751245744019 - }, - { - "x": 3.6048635838845873, - "y": 1.6245744507374444, - "heading": -1.313656485800498e-17, - "angularVelocity": -3.252248961405374e-17, - "velocityX": 2.098690048591799, - "velocityY": -0.8924451144343082, - "timestamp": 0.747993310326742 - }, - { - "x": 3.6709082609882207, - "y": 1.5947168159655363, - "heading": -1.428352643667232e-17, - "angularVelocity": -3.2545240196853133e-17, - "velocityX": 1.874029739336285, - "velocityY": -0.8472158236309923, - "timestamp": 0.7832353749094649 - }, - { - "x": 3.729186240680233, - "y": 1.567074102332766, - "heading": -1.543112572514496e-17, - "angularVelocity": -3.2563335321547424e-17, - "velocityX": 1.653648285991231, - "velocityY": -0.7843670329780381, - "timestamp": 0.8184774394921879 - }, - { - "x": 3.7798396108943755, - "y": 1.5420946209676558, - "heading": -1.3557190662479192e-17, - "angularVelocity": 5.317323937946322e-17, - "velocityX": 1.4372986036401205, - "velocityY": -0.7087973324172524, - "timestamp": 0.8537195040749108 - }, - { - "x": 3.8229940170612227, - "y": 1.5201139765313663, - "heading": -1.1683646818756056e-17, - "angularVelocity": 5.316213847463545e-17, - "velocityX": 1.2245141332611627, - "velocityY": -0.6237047884835205, - "timestamp": 0.8889615686576338 - }, - { - "x": 3.8587583624336923, - "y": 1.5013913212357997, - "heading": -9.810398849655971e-18, - "angularVelocity": 5.315374298099486e-17, - "velocityX": 1.0148198125146835, - "velocityY": -0.531258753346293, - "timestamp": 0.9242036332403567 - }, - { - "x": 3.8872266540166653, - "y": 1.4861320597418657, - "heading": -7.937370756395587e-18, - "angularVelocity": 5.3147503963822577e-17, - "velocityX": 0.8077929576500821, - "velocityY": -0.43298432355222594, - "timestamp": 0.9594456978230796 - }, - { - "x": 3.9084802353875676, - "y": 1.4745025997323902, - "heading": -6.0645124516602805e-18, - "angularVelocity": 5.3142686179864185e-17, - "velocityX": 0.6030742416073348, - "velocityY": -0.3299880454556786, - "timestamp": 0.9946877624058026 - }, - { - "x": 3.9225898514135444, - "y": 1.4666402693172664, - "heading": -4.191768760702976e-18, - "angularVelocity": 5.313943399764024e-17, - "velocityX": 0.40036292405222634, - "velocityY": -0.22309505723391648, - "timestamp": 1.0299298269885255 - }, - { - "x": 3.929617404937744, - "y": 1.4626601934432983, - "heading": -2.0958673938534256e-18, - "angularVelocity": 5.947156826205235e-17, - "velocityX": 0.19940811094378472, - "velocityY": -0.1129353776827106, - "timestamp": 1.0651718915712485 - }, - { - "x": 3.929617404937744, - "y": 1.4626601934432983, - "heading": 0, - "angularVelocity": 5.947060427768327e-17, - "velocityX": 0, - "velocityY": 0, - "timestamp": 1.1004139561539714 - } - ], - "constraints": [ - { - "scope": [ - "first" - ], - "type": "WptZeroVelocity" - }, - { - "scope": [ - "last" - ], - "type": "WptZeroVelocity" - } - ], - "usesControlIntervalGuessing": true, - "defaultControlIntervalCount": 40, - "usesDefaultFieldObstacles": true, - "circleObstacles": [] - }, - "Test 2": { - "waypoints": [ - { - "x": 1.2462917566299438, - "y": 7.013879299163818, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 9 - }, - { - "x": 2.4246835708618164, - "y": 6.99968147277832, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 11 - }, - { - "x": 3.9012229442596436, - "y": 6.304004669189453, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 17 - }, - { - "x": 7.649360656738281, - "y": 7.4256062507629395, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 16 - }, - { - "x": 3.9012229442596436, - "y": 6.942891597747803, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 40 - } - ], - "trajectory": [ - { - "x": 1.2462917566299438, - "y": 7.013879299163818, - "heading": 0, - "angularVelocity": 1.8576957689615527e-38, - "velocityX": 5.650902657479953e-31, - "velocityY": -5.509730104969581e-32, - "timestamp": 0 - }, - { - "x": 1.2728393380635374, - "y": 7.020025993967499, - "heading": -2.1422561939962651e-19, - "angularVelocity": -3.2119799002278703e-18, - "velocityX": 0.3980396549749024, - "velocityY": 0.09216011955865565, - "timestamp": 0.0666958206344134 - }, - { - "x": 1.3262202394266138, - "y": 7.030919145292598, - "heading": -6.179926514323356e-19, - "angularVelocity": -6.0538581091913635e-18, - "velocityX": 0.800363513865061, - "velocityY": 0.1633258459311531, - "timestamp": 0.1333916412688268 - }, - { - "x": 1.4066903645270812, - "y": 7.044766925115804, - "heading": -1.1793044915636138e-18, - "angularVelocity": -8.416002349787475e-18, - "velocityX": 1.2065242519701218, - "velocityY": 0.20762590056762728, - "timestamp": 0.2000874619032402 - }, - { - "x": 1.5144033649690225, - "y": 7.05923241946752, - "heading": -2.051076797499284e-18, - "angularVelocity": -1.3070873776347306e-17, - "velocityX": 1.6149887566774053, - "velocityY": 0.2168875682798883, - "timestamp": 0.2667832825376536 - }, - { - "x": 1.6492537465114516, - "y": 7.071224389847329, - "heading": -2.0856321133302972e-18, - "angularVelocity": -5.180989819451271e-19, - "velocityX": 2.021871539471901, - "velocityY": 0.1798009270420352, - "timestamp": 0.33347910317206697 - }, - { - "x": 1.8105532743625672, - "y": 7.076659046067202, - "heading": -2.1002211194393107e-18, - "angularVelocity": -2.1873166890130693e-19, - "velocityX": 2.4184353129901455, - "velocityY": 0.08148420947829117, - "timestamp": 0.40017492380648034 - }, - { - "x": 1.996442068118288, - "y": 7.070349612577893, - "heading": -1.946519159461839e-18, - "angularVelocity": 2.304534766433042e-18, - "velocityX": 2.7871130752648194, - "velocityY": -0.09460013280231008, - "timestamp": 0.4668707444408937 - }, - { - "x": 2.2031295038232215, - "y": 7.046434155152575, - "heading": -1.5479848716999483e-18, - "angularVelocity": 5.975420933476796e-18, - "velocityX": 3.098956332477912, - "velocityY": -0.3585750530992917, - "timestamp": 0.5335665650753071 - }, - { - "x": 2.4246835708618164, - "y": 6.99968147277832, - "heading": -7.857898560248294e-19, - "angularVelocity": 1.1427955190954808e-17, - "velocityX": 3.3218583253186806, - "velocityY": -0.7009836887796346, - "timestamp": 0.6002623857097205 - }, - { - "x": 2.5541388102121663, - "y": 6.964365766560133, - "heading": -2.0196357456600574e-19, - "angularVelocity": 1.5288290601618025e-17, - "velocityX": 3.389953097093239, - "velocityY": -0.9247875039436507, - "timestamp": 0.6384503003039254 - }, - { - "x": 2.685955086778719, - "y": 6.920434264641045, - "heading": 5.837695378946101e-19, - "angularVelocity": 2.0575490709452673e-17, - "velocityX": 3.4517799143272825, - "velocityY": -1.1504032725046753, - "timestamp": 0.6766382148981303 - }, - { - "x": 2.8197656895488885, - "y": 6.867794774338436, - "heading": 1.246996690820799e-18, - "angularVelocity": 1.7367422203128303e-17, - "velocityX": 3.504003928777101, - "velocityY": -1.3784332258569356, - "timestamp": 0.7148261294923351 - }, - { - "x": 2.9549433705621735, - "y": 6.806327065659224, - "heading": 3.3584531801563643e-18, - "angularVelocity": 5.529118585459454e-17, - "velocityX": 3.539802643045796, - "velocityY": -1.609611557285164, - "timestamp": 0.75301404408654 - }, - { - "x": 3.0902070954482963, - "y": 6.735926337900825, - "heading": 6.144806500333234e-18, - "angularVelocity": 7.296425796261651e-17, - "velocityX": 3.5420558132891666, - "velocityY": -1.8435342308355922, - "timestamp": 0.7912019586807449 - }, - { - "x": 3.222134587082462, - "y": 6.657238540440367, - "heading": 9.630920095804186e-18, - "angularVelocity": 9.128839928842624e-17, - "velocityX": 3.454692224906208, - "velocityY": -2.060541883392227, - "timestamp": 0.8293898732749498 - }, - { - "x": 3.349264802716153, - "y": 6.571014734758072, - "heading": 1.2954476277599915e-17, - "angularVelocity": 8.703162288677359e-17, - "velocityX": 3.329069339983813, - "velocityY": -2.257882018403783, - "timestamp": 0.8675777878691547 - }, - { - "x": 3.481191794816829, - "y": 6.492326078688536, - "heading": 1.7597431411802467e-17, - "angularVelocity": 1.2158179318074916e-16, - "velocityX": 3.4546791439872995, - "velocityY": -2.060564367162037, - "timestamp": 0.9057657024633596 - }, - { - "x": 3.6174699398570356, - "y": 6.421439537112809, - "heading": 2.0225513163692036e-17, - "angularVelocity": 6.881972424538472e-17, - "velocityX": 3.56861971879322, - "velocityY": -1.856255894789645, - "timestamp": 0.9439536170575645 - }, - { - "x": 3.7576383461149243, - "y": 6.358594880649259, - "heading": 2.243630761030922e-17, - "angularVelocity": 5.789251573743686e-17, - "velocityX": 3.6704912469715123, - "velocityY": -1.6456687182535479, - "timestamp": 0.9821415316517694 - }, - { - "x": 3.901222944259643, - "y": 6.304004669189453, - "heading": 2.47929413031983e-17, - "angularVelocity": 6.171150527421755e-17, - "velocityX": 3.759948655764368, - "velocityY": -1.4295153856750347, - "timestamp": 1.0203294462459744 - }, - { - "x": 4.199944605669076, - "y": 6.228799995672069, - "heading": 3.100148897614544e-17, - "angularVelocity": 8.107334480443901e-17, - "velocityX": 3.900809904617719, - "velocityY": -0.9820484190864626, - "timestamp": 1.096908840964896 - }, - { - "x": 4.505390450863175, - "y": 6.18888491371662, - "heading": 3.068102356864592e-17, - "angularVelocity": -4.184747198454113e-18, - "velocityX": 3.9886166026134253, - "velocityY": -0.5212248295105096, - "timestamp": 1.1734882356838174 - }, - { - "x": 4.813406217718833, - "y": 6.184802263219741, - "heading": 2.9566016685455517e-17, - "angularVelocity": -1.4560142284808256e-17, - "velocityX": 4.022175521054888, - "velocityY": -0.05331265039115567, - "timestamp": 1.250067630402739 - }, - { - "x": 5.119802709590309, - "y": 6.216607481698429, - "heading": 3.52377994780859e-17, - "angularVelocity": 7.406408490756321e-17, - "velocityX": 4.001030472964381, - "velocityY": 0.415323450838281, - "timestamp": 1.3266470251216604 - }, - { - "x": 5.420412868385874, - "y": 6.283867538771131, - "heading": 3.848949003766367e-17, - "angularVelocity": 4.246169053061748e-17, - "velocityX": 3.925470551172821, - "velocityY": 0.8783048928329202, - "timestamp": 1.403226419840582 - }, - { - "x": 5.71634502412257, - "y": 6.369392635548687, - "heading": 4.1170423309352994e-17, - "angularVelocity": 3.500854622761714e-17, - "velocityX": 3.8643835828530744, - "velocityY": 1.1168160455134108, - "timestamp": 1.4798058145595034 - }, - { - "x": 6.012277066075067, - "y": 6.454918126038977, - "heading": 5.506942631831843e-17, - "angularVelocity": 1.8149794810968406e-16, - "velocityX": 3.864382097019984, - "velocityY": 1.1168211867493296, - "timestamp": 1.556385209278425 - }, - { - "x": 6.3082087417235275, - "y": 6.540444883990324, - "heading": 4.481737476003156e-17, - "angularVelocity": -1.3387480531183782e-16, - "velocityX": 3.8643773136962514, - "velocityY": 1.116837737690506, - "timestamp": 1.6329646039973464 - }, - { - "x": 6.594966037333605, - "y": 6.6529643659063735, - "heading": 4.841360100358121e-17, - "angularVelocity": 4.6960755653354393e-17, - "velocityX": 3.7445751126979148, - "velocityY": 1.4693179846795974, - "timestamp": 1.709543998716268 - }, - { - "x": 6.848772987308951, - "y": 6.779795600711972, - "heading": 4.204680155814927e-17, - "angularVelocity": -8.313982722738282e-17, - "velocityX": 3.314298198710489, - "velocityY": 1.6562057622889883, - "timestamp": 1.7861233934351894 - }, - { - "x": 7.067412419546155, - "y": 6.899290890343004, - "heading": 3.6458514740789416e-17, - "angularVelocity": -7.297373685909452e-17, - "velocityX": 2.8550686909919953, - "velocityY": 1.5604104742547948, - "timestamp": 1.862702788154111 - }, - { - "x": 7.251170888102313, - "y": 7.010190252281107, - "heading": 2.889241168076903e-17, - "angularVelocity": -9.88007038620393e-17, - "velocityX": 2.3995811044279933, - "velocityY": 1.4481619023706132, - "timestamp": 1.9392821828730324 - }, - { - "x": 7.4001612724406005, - "y": 7.112047913646065, - "heading": 2.1944177837341752e-17, - "angularVelocity": -9.073242257004582e-17, - "velocityX": 1.9455675366074576, - "velocityY": 1.33009227533376, - "timestamp": 2.015861577591954 - }, - { - "x": 7.51444364347692, - "y": 7.204635943834979, - "heading": 1.5592060084324575e-17, - "angularVelocity": -8.294813863311352e-17, - "velocityX": 1.4923383954087404, - "velocityY": 1.209046252312866, - "timestamp": 2.0924409723108757 - }, - { - "x": 7.594055258058767, - "y": 7.2878159439317, - "heading": 9.822855730351304e-18, - "angularVelocity": -7.533626000185252e-17, - "velocityX": 1.0395957669023037, - "velocityY": 1.0861929687794476, - "timestamp": 2.1690203670297974 - }, - { - "x": 7.639021472581016, - "y": 7.361494958394011, - "heading": 4.627623553541244e-18, - "angularVelocity": -6.784113894031215e-17, - "velocityX": 0.5871842508963836, - "velocityY": 0.9621258398956097, - "timestamp": 2.245599761748719 - }, - { - "x": 7.649360656738281, - "y": 7.4256062507629395, - "heading": 0, - "angularVelocity": -6.042911536454736e-17, - "velocityX": 0.13501261266285436, - "velocityY": 0.8371872434424734, - "timestamp": 2.322179156467641 - }, - { - "x": 7.600824522324889, - "y": 7.494356470481976, - "heading": -5.199536290305884e-18, - "angularVelocity": -5.0560214645722654e-17, - "velocityX": -0.47196469577117206, - "velocityY": 0.6685261800522405, - "timestamp": 2.4250176417362526 - }, - { - "x": 7.48991208583228, - "y": 7.545603113049983, - "heading": -9.403502434751289e-18, - "angularVelocity": -4.087930941746883e-17, - "velocityX": -1.0785109893722118, - "velocityY": 0.49832163935617596, - "timestamp": 2.5278561270048643 - }, - { - "x": 7.316689652080842, - "y": 7.579111596852661, - "heading": -1.2640474818606514e-17, - "angularVelocity": -3.147627586763133e-17, - "velocityX": -1.6844125358222435, - "velocityY": 0.32583603030631414, - "timestamp": 2.630694612273476 - }, - { - "x": 7.081267195564433, - "y": 7.594500069269045, - "heading": -1.4957060676591272e-17, - "angularVelocity": -2.2526454180008613e-17, - "velocityX": -2.289244691824056, - "velocityY": 0.14963729168276652, - "timestamp": 2.7335330975420877 - }, - { - "x": 6.7838623866104655, - "y": 7.591037417525899, - "heading": -1.6442803884596208e-17, - "angularVelocity": -1.4447351341853733e-17, - "velocityX": -2.8919602245904463, - "velocityY": -0.03367077737603705, - "timestamp": 2.8363715828106995 - }, - { - "x": 6.4251059318965735, - "y": 7.566762499257423, - "heading": -2.986425944771221e-17, - "angularVelocity": -1.3051006229833453e-16, - "velocityX": -3.488542774398327, - "velocityY": -0.23604896751525264, - "timestamp": 2.939210068079311 - }, - { - "x": 6.016720927626869, - "y": 7.5008443435353165, - "heading": -3.467478724998584e-17, - "angularVelocity": -4.677750542229833e-17, - "velocityX": -3.971130099816352, - "velocityY": -0.640987229148065, - "timestamp": 3.042048553347923 - }, - { - "x": 5.616728238183579, - "y": 7.395348514025283, - "heading": -3.390128241552755e-17, - "angularVelocity": 7.521550173191142e-18, - "velocityX": -3.889523347203425, - "velocityY": -1.02583997843259, - "timestamp": 3.1448870386165346 - }, - { - "x": 5.216735664422249, - "y": 7.289852245902379, - "heading": -2.3452803355527372e-17, - "angularVelocity": 1.0160086501380395e-16, - "velocityX": -3.88952222231366, - "velocityY": -1.025844243498041, - "timestamp": 3.2477255238851463 - }, - { - "x": 4.840874915610426, - "y": 7.190720639628367, - "heading": -1.6752117047626585e-17, - "angularVelocity": 6.515738739653489e-17, - "velocityX": -3.6548646922414014, - "velocityY": -0.9639543602285213, - "timestamp": 3.350564009153758 - }, - { - "x": 4.527657602112525, - "y": 7.108110961714319, - "heading": -1.1168131864503311e-17, - "angularVelocity": 5.429860003544753e-17, - "velocityX": -3.0457207987826376, - "velocityY": -0.8032953587194801, - "timestamp": 3.45340249442237 - }, - { - "x": 4.277083743214221, - "y": 7.0420232172467445, - "heading": -6.700891701543933e-18, - "angularVelocity": 4.343938583567588e-17, - "velocityX": -2.436576717790238, - "velocityY": -0.6426363077492735, - "timestamp": 3.5562409796909815 - }, - { - "x": 4.089153345344071, - "y": 6.99245740792115, - "heading": -3.3504585356800222e-18, - "angularVelocity": 3.257956897302995e-17, - "velocityX": -1.8274325742866233, - "velocityY": -0.48197724029201133, - "timestamp": 3.6590794649595932 - }, - { - "x": 3.9638664117163565, - "y": 6.959413534585286, - "heading": -1.116819200639241e-18, - "angularVelocity": 2.1719880139959676e-17, - "velocityX": -1.2182883995274036, - "velocityY": -0.3213181645912218, - "timestamp": 3.761917950228205 - }, - { - "x": 3.901222944259643, - "y": 6.942891597747803, - "heading": 0, - "angularVelocity": 1.0859935530094074e-17, - "velocityX": -0.6091442060148221, - "velocityY": -0.1606590839443162, - "timestamp": 3.8647564354968167 - }, - { - "x": 3.901222944259643, - "y": 6.942891597747803, - "heading": 0, - "angularVelocity": 0, - "velocityX": -8.630031752099511e-32, - "velocityY": 0, - "timestamp": 3.9675949207654284 - } - ], - "constraints": [ - { - "scope": [ - "first" - ], - "type": "StopPoint" - }, - { - "scope": [ - "last" - ], - "type": "StopPoint" - } - ], - "usesControlIntervalGuessing": true, - "defaultControlIntervalCount": 40, - "usesDefaultFieldObstacles": true, - "circleObstacles": [] - }, - "Test 3": { - "waypoints": [ - { - "x": 1.3740688562393188, - "y": 2.1015472412109375, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 19 - }, - { - "x": 3.077767848968506, - "y": 2.45648455619812, - "heading": 1.5707963267948966, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 27 - }, - { - "x": 4.483319282531738, - "y": 1.4768576622009277, - "heading": -1.5707963267948966, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 40 - } - ], - "trajectory": [ - { - "x": 1.3740688562393188, - "y": 2.1015472412109375, - "heading": 0, - "angularVelocity": 0, - "velocityX": 0, - "velocityY": 0, - "timestamp": 0 - }, - { - "x": 1.3827319927716195, - "y": 2.1056840940342982, - "heading": 0.024187462948881532, - "angularVelocity": 0.5814493745934073, - "velocityX": 0.20825562934773315, - "velocityY": 0.09944699417395882, - "timestamp": 0.04159857075378794 - }, - { - "x": 1.4001445473800622, - "y": 2.1139530067021113, - "heading": 0.07183187897208339, - "angularVelocity": 1.1453378123250968, - "velocityX": 0.41858540552980794, - "velocityY": 0.19877876855005897, - "timestamp": 0.08319714150757589 - }, - { - "x": 1.4264050912023196, - "y": 2.1263405769046955, - "heading": 0.14207148158241312, - "angularVelocity": 1.6885099977538955, - "velocityX": 0.6312847616250105, - "velocityY": 0.2977883609511227, - "timestamp": 0.12479571226136382 - }, - { - "x": 1.461644669528872, - "y": 2.1428235310818113, - "heading": 0.23354680029910396, - "angularVelocity": 2.1990014815116687, - "velocityX": 0.8471343531274942, - "velocityY": 0.3962384735473897, - "timestamp": 0.16639428301515177 - }, - { - "x": 1.5060492101550866, - "y": 2.1633450352713384, - "heading": 0.3440303057594644, - "angularVelocity": 2.655944746609454, - "velocityX": 1.067453516348391, - "velocityY": 0.49332233819105575, - "timestamp": 0.20799285376893972 - }, - { - "x": 1.5598689877360068, - "y": 2.1877699582208265, - "heading": 0.47025292230788457, - "angularVelocity": 3.0343017623157595, - "velocityX": 1.2937891039441975, - "velocityY": 0.587157743813019, - "timestamp": 0.24959142452272767 - }, - { - "x": 1.6233962833017608, - "y": 2.2158406981890693, - "heading": 0.6081456509926965, - "angularVelocity": 3.314842942575804, - "velocityX": 1.5271509192404589, - "velocityY": 0.6748005871294988, - "timestamp": 0.2911899952765156 - }, - { - "x": 1.6969228086864487, - "y": 2.2471577725494125, - "heading": 0.7531530032346565, - "angularVelocity": 3.485873423389107, - "velocityX": 1.767525279170269, - "velocityY": 0.7528401527510284, - "timestamp": 0.33278856603030355 - }, - { - "x": 1.7807044327096728, - "y": 2.2811428510793297, - "heading": 0.8996751634093815, - "angularVelocity": 3.522288326722431, - "velocityX": 2.014050543204923, - "velocityY": 0.8169770719100108, - "timestamp": 0.37438713678409147 - }, - { - "x": 1.874863599501791, - "y": 2.3169249011545388, - "heading": 1.039921254531021, - "angularVelocity": 3.371416098679782, - "velocityX": 2.2635192768863255, - "velocityY": 0.860174987432263, - "timestamp": 0.4159857075378794 - }, - { - "x": 1.9790611887944067, - "y": 2.3530646807512396, - "heading": 1.1622275414761678, - "angularVelocity": 2.9401559892271805, - "velocityX": 2.504835800954579, - "velocityY": 0.8687745502290541, - "timestamp": 0.4575842782916673 - }, - { - "x": 2.0932451980418487, - "y": 2.3882113850013775, - "heading": 1.2660024391339892, - "angularVelocity": 2.494674595239208, - "velocityX": 2.7449022208787723, - "velocityY": 0.8449017265086323, - "timestamp": 0.49918284904545523 - }, - { - "x": 2.217212476082111, - "y": 2.420658735334921, - "heading": 1.3531768415072132, - "angularVelocity": 2.095610517226054, - "velocityX": 2.980085031622671, - "velocityY": 0.7800111817671773, - "timestamp": 0.5407814197992432 - }, - { - "x": 2.3501935564528202, - "y": 2.4482015090574127, - "heading": 1.4262550842141035, - "angularVelocity": 1.756748883021386, - "velocityX": 3.196770416891297, - "velocityY": 0.662108654774838, - "timestamp": 0.5823799905530311 - }, - { - "x": 2.4906536483278, - "y": 2.46851554155244, - "heading": 1.4886926420034476, - "angularVelocity": 1.500954399590967, - "velocityX": 3.3765605243084034, - "velocityY": 0.48833486648020946, - "timestamp": 0.6239785613068191 - }, - { - "x": 2.636556239981357, - "y": 2.4798247504507454, - "heading": 1.5443965976357683, - "angularVelocity": 1.3390834017437327, - "velocityX": 3.507394340949799, - "velocityY": 0.27186532357102605, - "timestamp": 0.665577132060607 - }, - { - "x": 2.784486963341596, - "y": 2.4812509905043107, - "heading": 1.5824822995529222, - "angularVelocity": 0.915553136260199, - "velocityX": 3.55614918204015, - "velocityY": 0.03428579461936428, - "timestamp": 0.7071757028143949 - }, - { - "x": 2.9319879700538403, - "y": 2.4732781401372903, - "heading": 1.5928024946439976, - "angularVelocity": 0.2480901363854538, - "velocityX": 3.5458191000164043, - "velocityY": -0.19166164180201423, - "timestamp": 0.7487742735681828 - }, - { - "x": 3.077767848968506, - "y": 2.45648455619812, - "heading": 1.5707963267948966, - "angularVelocity": -0.5290125946501647, - "velocityX": 3.5044444141473514, - "velocityY": -0.4037057916934574, - "timestamp": 0.7903728443219707 - }, - { - "x": 3.1950861088883977, - "y": 2.4371849871011824, - "heading": 1.5299506752490524, - "angularVelocity": -1.202969278370701, - "velocityX": 3.455208991207777, - "velocityY": -0.5684029469487673, - "timestamp": 0.8243268714787733 - }, - { - "x": 3.309155534819785, - "y": 2.4119403886613155, - "heading": 1.478537222717671, - "angularVelocity": -1.5142077932128373, - "velocityX": 3.359525672891005, - "velocityY": -0.7434934985188447, - "timestamp": 0.8582808986355758 - }, - { - "x": 3.4193096830151886, - "y": 2.380930163774979, - "heading": 1.4211355944324973, - "angularVelocity": -1.6905690750790594, - "velocityX": 3.244214528268367, - "velocityY": -0.9133003499957304, - "timestamp": 0.8922349257923783 - }, - { - "x": 3.5252925044888306, - "y": 2.3443333770341446, - "heading": 1.3578303696691585, - "angularVelocity": -1.8644393630120046, - "velocityX": 3.1213623345506205, - "velocityY": -1.0778334649837304, - "timestamp": 0.9261889529491808 - }, - { - "x": 3.6267984253619163, - "y": 2.3023968893924756, - "heading": 1.2884922471851454, - "angularVelocity": -2.042117777794756, - "velocityX": 2.989510505020297, - "velocityY": -1.2350961330078705, - "timestamp": 0.9601429801059833 - }, - { - "x": 3.7234699768082784, - "y": 2.2554603435851734, - "heading": 1.2127113952283004, - "angularVelocity": -2.2318663882639043, - "velocityX": 2.847130651096655, - "velocityY": -1.3823557833156956, - "timestamp": 0.9940970072627858 - }, - { - "x": 3.814908272910244, - "y": 2.2039836020719794, - "heading": 1.129729146369758, - "angularVelocity": -2.443958958856986, - "velocityX": 2.693002973687319, - "velocityY": -1.5160717541770827, - "timestamp": 1.0280510344195883 - }, - { - "x": 3.9007054057350934, - "y": 2.148565496917068, - "heading": 1.038390775419664, - "angularVelocity": -2.6900600193564195, - "velocityX": 2.5268617601269985, - "velocityY": -1.6321511701343638, - "timestamp": 1.062005061576391 - }, - { - "x": 3.9805012090104195, - "y": 2.0899327454341323, - "heading": 0.9371495335396042, - "angularVelocity": -2.981715288529822, - "velocityX": 2.3501130780991346, - "velocityY": -1.726827607577971, - "timestamp": 1.0959590887331936 - }, - { - "x": 4.053655012390132, - "y": 2.0292588496633805, - "heading": 0.8293051851574991, - "angularVelocity": -3.1761872570754117, - "velocityX": 2.1544956373477353, - "velocityY": -1.786942547066038, - "timestamp": 1.1299131158899962 - }, - { - "x": 4.119927818624713, - "y": 1.9679960836064079, - "heading": 0.7137645375013505, - "angularVelocity": -3.402855488164233, - "velocityX": 1.9518393482026792, - "velocityY": -1.8042857117986315, - "timestamp": 1.1638671430467988 - }, - { - "x": 4.179384944413853, - "y": 1.9075792317237577, - "heading": 0.5896796080209632, - "angularVelocity": -3.654498151485701, - "velocityX": 1.7511067395499393, - "velocityY": -1.779372196522297, - "timestamp": 1.1978211702036015 - }, - { - "x": 4.232333921459595, - "y": 1.8492512775948615, - "heading": 0.45638614526052235, - "angularVelocity": -3.925704074646035, - "velocityX": 1.5594314277127905, - "velocityY": -1.717850841659724, - "timestamp": 1.231775197360404 - }, - { - "x": 4.279204985937824, - "y": 1.7939724784813917, - "heading": 0.3132208792361718, - "angularVelocity": -4.2164443517373975, - "velocityX": 1.3804272542329112, - "velocityY": -1.6280483860824002, - "timestamp": 1.2657292245172067 - }, - { - "x": 4.320441595908879, - "y": 1.7424131364284248, - "heading": 0.15934425300509544, - "angularVelocity": -4.531910913547163, - "velocityX": 1.2144836245965511, - "velocityY": -1.5185044712080482, - "timestamp": 1.2996832516740093 - }, - { - "x": 4.356420236259085, - "y": 1.6949789978909888, - "heading": -0.006355243449938616, - "angularVelocity": -4.8801132098280045, - "velocityX": 1.05962807251248, - "velocityY": -1.3970106791318475, - "timestamp": 1.333637278830812 - }, - { - "x": 4.387409086244814, - "y": 1.651863988262444, - "heading": -0.18526060257567917, - "angularVelocity": -5.269046829093284, - "velocityX": 0.9126708252490286, - "velocityY": -1.269805476374806, - "timestamp": 1.3675913059876146 - }, - { - "x": 4.41359473495854, - "y": 1.6131465719605549, - "heading": -0.37893299146064197, - "angularVelocity": -5.703959297385947, - "velocityX": 0.7712089229593995, - "velocityY": -1.14028937195647, - "timestamp": 1.4015453331444172 - }, - { - "x": 4.4350042238715455, - "y": 1.5791111641775315, - "heading": -0.584927468780437, - "angularVelocity": -6.066864362454111, - "velocityX": 0.6305434349257187, - "velocityY": -1.0023967886331036, - "timestamp": 1.4354993603012198 - }, - { - "x": 4.45163244894447, - "y": 1.5502696896357162, - "heading": -0.7898839519998764, - "angularVelocity": -6.036293788434603, - "velocityX": 0.4897276248306672, - "velocityY": -0.8494272095834752, - "timestamp": 1.4694533874580225 - }, - { - "x": 4.463826845716735, - "y": 1.5265783798021662, - "heading": -0.9844765994557877, - "angularVelocity": -5.731062373152404, - "velocityX": 0.3591443429072621, - "velocityY": -0.6977466833093577, - "timestamp": 1.503407414614825 - }, - { - "x": 4.472183367519406, - "y": 1.5078619625249268, - "heading": -1.1610616366629594, - "angularVelocity": -5.200709665192236, - "velocityX": 0.2461128326282439, - "velocityY": -0.5512282001456157, - "timestamp": 1.5373614417716277 - }, - { - "x": 4.477461351357098, - "y": 1.4939941108118897, - "heading": -1.3141687721041224, - "angularVelocity": -4.509248188250044, - "velocityX": 0.1554450025424584, - "velocityY": -0.40843024743689504, - "timestamp": 1.5713154689284303 - }, - { - "x": 4.480557191442016, - "y": 1.4847418449881848, - "heading": -1.4384150849106647, - "angularVelocity": -3.6592511466260937, - "velocityX": 0.09117740498451485, - "velocityY": -0.2724939159958763, - "timestamp": 1.605269496085233 - }, - { - "x": 4.482401705838399, - "y": 1.4793489828824327, - "heading": -1.5259837396767848, - "angularVelocity": -2.5790358934892232, - "velocityX": 0.05432387704300033, - "velocityY": -0.15882834990084585, - "timestamp": 1.6392235232420356 - }, - { - "x": 4.483319282531738, - "y": 1.4768576622009275, - "heading": -1.5707963267948966, - "angularVelocity": -1.3198018282509576, - "velocityX": 0.02702409022356453, - "velocityY": -0.07337334891149445, - "timestamp": 1.6731775503988382 - }, - { - "x": 4.483319282531738, - "y": 1.4768576622009275, - "heading": -1.5707963267948966, - "angularVelocity": 2.6449030967255686e-30, - "velocityX": 1.3058140867459075e-31, - "velocityY": 0, - "timestamp": 1.7071315775556408 - } - ], - "constraints": [ - { - "scope": [ - "first" - ], - "type": "StopPoint" - }, - { - "scope": [ - "last" - ], - "type": "StopPoint" - } - ], - "usesControlIntervalGuessing": true, - "defaultControlIntervalCount": 40, - "usesDefaultFieldObstacles": true, - "circleObstacles": [] - }, "Troll": { "waypoints": [ { diff --git a/src/main/deploy/choreo/Troll.1.traj b/src/main/deploy/choreo/Troll.1.traj new file mode 100644 index 0000000..4eefac9 --- /dev/null +++ b/src/main/deploy/choreo/Troll.1.traj @@ -0,0 +1,1030 @@ +{ + "samples": [ + { + "x": 1.472737193107605, + "y": 5.55, + "heading": 0, + "angularVelocity": 0, + "velocityX": -8.875549299448591e-33, + "velocityY": 1.6865999682592782e-32, + "timestamp": 0 + }, + { + "x": 1.4954238510421338, + "y": 5.562202684480301, + "heading": 0.04016028608194298, + "angularVelocity": 0.6064807443024526, + "velocityX": 0.34260266876073076, + "velocityY": 0.18427889559851518, + "timestamp": 0.06621856746356154 + }, + { + "x": 1.5408911314828666, + "y": 5.5867292912686075, + "heading": 0.11807101658857888, + "angularVelocity": 1.1765692537747552, + "velocityX": 0.6866243439312181, + "velocityY": 0.37038866480770294, + "timestamp": 0.13243713492712308 + }, + { + "x": 1.6092554887422894, + "y": 5.623729916528466, + "heading": 0.23034305047627052, + "angularVelocity": 1.695476634245707, + "velocityX": 1.0324046544353558, + "velocityY": 0.5587651119184867, + "timestamp": 0.19865570239068464 + }, + { + "x": 1.7006794384120958, + "y": 5.673376134035544, + "heading": 0.3716946261904345, + "angularVelocity": 2.134621468396251, + "velocityX": 1.3806391948922, + "velocityY": 0.7497325811887481, + "timestamp": 0.26487426985424617 + }, + { + "x": 1.8153711212066734, + "y": 5.73583043809041, + "heading": 0.5342344602957644, + "angularVelocity": 2.4545960495864176, + "velocityX": 1.732016973301174, + "velocityY": 0.9431539588837228, + "timestamp": 0.3310928373178077 + }, + { + "x": 1.9535237520205713, + "y": 5.811224596512345, + "heading": 0.7068082719295097, + "angularVelocity": 2.6061242072128543, + "velocityX": 2.0863125873256005, + "velocityY": 1.1385652289053465, + "timestamp": 0.3973114047813692 + }, + { + "x": 2.115146815140512, + "y": 5.899607450474536, + "heading": 0.8720605081292071, + "angularVelocity": 2.4955574022441915, + "velocityX": 2.440751428349426, + "velocityY": 1.334714074127719, + "timestamp": 0.46352997224493075 + }, + { + "x": 2.2991611528231366, + "y": 6.000703985717269, + "heading": 0.9970511976008042, + "angularVelocity": 1.8875474698297623, + "velocityX": 2.778893363766644, + "velocityY": 1.52670979024674, + "timestamp": 0.5297485397084923 + }, + { + "x": 2.5044278422735613, + "y": 6.114387084295501, + "heading": 1.0665576726522747, + "angularVelocity": 1.0496523515057667, + "velocityX": 3.099835851377757, + "velocityY": 1.716785834136148, + "timestamp": 0.5959671071720539 + }, + { + "x": 2.7311192639784148, + "y": 6.24051707801207, + "heading": 1.0817751050892828, + "angularVelocity": 0.22980612568192132, + "velocityX": 3.423381543697635, + "velocityY": 1.904752678106116, + "timestamp": 0.6621856746356155 + }, + { + "x": 2.964942433247541, + "y": 6.368100850146288, + "heading": 1.0817751413957877, + "angularVelocity": 5.482828485290889e-7, + "velocityX": 3.531081662220997, + "velocityY": 1.9267069195422228, + "timestamp": 0.7284042420991771 + }, + { + "x": 3.1987656298041083, + "y": 6.495684572270795, + "heading": 1.081775177702228, + "angularVelocity": 5.482818756667397e-7, + "velocityX": 3.5310820743024163, + "velocityY": 1.926706164320354, + "timestamp": 0.7946228095627387 + }, + { + "x": 3.4325888263613256, + "y": 6.623268294394111, + "heading": 1.0817752140086683, + "angularVelocity": 5.482818741182615e-7, + "velocityX": 3.531082074312233, + "velocityY": 1.9267061643023646, + "timestamp": 0.8608413770263003 + }, + { + "x": 3.6664120229185433, + "y": 6.7508520165174275, + "heading": 1.0817752503151086, + "angularVelocity": 5.482818737417958e-7, + "velocityX": 3.531082074312234, + "velocityY": 1.926706164302361, + "timestamp": 0.9270599444898618 + }, + { + "x": 3.9002352194806806, + "y": 6.878435738631726, + "heading": 1.0817752866215484, + "angularVelocity": 5.482818710733048e-7, + "velocityX": 3.5310820743865343, + "velocityY": 1.9267061641661918, + "timestamp": 0.9932785119534234 + }, + { + "x": 4.13405862259109, + "y": 7.006019082203452, + "heading": 1.0817753229279854, + "angularVelocity": 5.482818239333129e-7, + "velocityX": 3.531085193576202, + "velocityY": 1.926700447603791, + "timestamp": 1.059497079416985 + }, + { + "x": 4.373840182030626, + "y": 7.122017776417369, + "heading": 1.0817753593434176, + "angularVelocity": 5.49927814371679e-7, + "velocityX": 3.6210623186839843, + "velocityY": 1.7517548122397577, + "timestamp": 1.1257156468805465 + }, + { + "x": 4.624085072947641, + "y": 7.213277774894042, + "heading": 1.0817753970756958, + "angularVelocity": 5.698141760636061e-7, + "velocityX": 3.7790743669397395, + "velocityY": 1.3781632851977732, + "timestamp": 1.191934214344108 + }, + { + "x": 4.882248762866474, + "y": 7.278870463039004, + "heading": 1.0817754381512044, + "angularVelocity": 6.203019793980544e-7, + "velocityX": 3.8986601463538673, + "velocityY": 0.990548280602039, + "timestamp": 1.2581527818076697 + }, + { + "x": 5.14570593286406, + "y": 7.318128697944957, + "heading": 1.081775485432278, + "angularVelocity": 7.14015351698796e-7, + "velocityX": 3.9785996600207425, + "velocityY": 0.5928584143345571, + "timestamp": 1.3243713492712312 + }, + { + "x": 5.411777394358682, + "y": 7.330653209159577, + "heading": 1.0817755436800227, + "angularVelocity": 8.796285819324463e-7, + "velocityX": 4.018079395043312, + "velocityY": 0.18913896350161738, + "timestamp": 1.3905899167347928 + }, + { + "x": 5.677757348071688, + "y": 7.316316612470789, + "heading": 1.0817756485534318, + "angularVelocity": 0.000001583746269380313, + "velocityX": 4.016697489859879, + "velocityY": -0.21650418059371343, + "timestamp": 1.4568084841983544 + }, + { + "x": 5.939941346681472, + "y": 7.275392278730094, + "heading": 1.08417598446994, + "angularVelocity": 0.0362486838427761, + "velocityX": 3.959372856473502, + "velocityY": -0.6180190135223776, + "timestamp": 1.523027051661916 + }, + { + "x": 6.1926233871200065, + "y": 7.209386056553619, + "heading": 1.094230473023442, + "angularVelocity": 0.1518379049657743, + "velocityX": 3.8158789916072857, + "velocityY": -0.9967932666739766, + "timestamp": 1.5892456191254776 + }, + { + "x": 6.431036236235606, + "y": 7.120827418505734, + "heading": 1.0882465943881172, + "angularVelocity": -0.09036557063270015, + "velocityX": 3.6003927334549006, + "velocityY": -1.337368678303358, + "timestamp": 1.6554641865890392 + }, + { + "x": 6.652946472167969, + "y": 7.011868953704834, + "heading": 1.0516507294432773, + "angularVelocity": -0.5526526221664004, + "velocityX": 3.351178444844405, + "velocityY": -1.6454367554969764, + "timestamp": 1.7216827540526007 + }, + { + "x": 6.817330150088381, + "y": 6.91398458094159, + "heading": 0.9974973350877399, + "angularVelocity": -1.0346873253086366, + "velocityX": 3.1408134255664746, + "velocityY": -1.8702377025339256, + "timestamp": 1.7740206855522294 + }, + { + "x": 6.96932848776031, + "y": 6.8050318321958345, + "heading": 0.9307984833276803, + "angularVelocity": -1.274388380452764, + "velocityX": 2.9041716651145895, + "velocityY": -2.0817167515787003, + "timestamp": 1.826358617051858 + }, + { + "x": 7.108447801764001, + "y": 6.685502841506335, + "heading": 0.853783246677348, + "angularVelocity": -1.4714994353737287, + "velocityX": 2.6580972922989385, + "velocityY": -2.2837927916648284, + "timestamp": 1.8786965485514866 + }, + { + "x": 7.2341948641942375, + "y": 6.5559531119456285, + "heading": 0.7687330983314559, + "angularVelocity": -1.6250192911520678, + "velocityX": 2.4025990104543773, + "velocityY": -2.475255055917257, + "timestamp": 1.9310344800511152 + }, + { + "x": 7.346097509127973, + "y": 6.41698515308241, + "heading": 0.6779280050822188, + "angularVelocity": -1.7349767300200096, + "velocityX": 2.13807924248075, + "velocityY": -2.6552054099464035, + "timestamp": 1.9833724115507438 + }, + { + "x": 7.44376495093577, + "y": 6.2691546896150365, + "heading": 0.5838410686967184, + "angularVelocity": -1.797681598978898, + "velocityX": 1.8660928892172433, + "velocityY": -2.8245377536256293, + "timestamp": 2.0357103430503725 + }, + { + "x": 7.5267974986172455, + "y": 6.113117237651097, + "heading": 0.48865781043808765, + "angularVelocity": -1.8186285841141039, + "velocityX": 1.586469799290113, + "velocityY": -2.981345412266578, + "timestamp": 2.088048274550001 + }, + { + "x": 7.594641436841627, + "y": 5.9499464596390546, + "heading": 0.3933862357072056, + "angularVelocity": -1.8203160117544566, + "velocityX": 1.29626709119871, + "velocityY": -3.1176390303693813, + "timestamp": 2.1403862060496297 + }, + { + "x": 7.64676744326269, + "y": 5.780900656781067, + "heading": 0.2985965379951544, + "angularVelocity": -1.8111089795882303, + "velocityX": 0.9959508319780062, + "velocityY": -3.2298907888476207, + "timestamp": 2.1927241375492583 + }, + { + "x": 7.682689941029465, + "y": 5.607494331086699, + "heading": 0.204267077341809, + "angularVelocity": -1.8023154135163826, + "velocityX": 0.6863568493727955, + "velocityY": -3.313205560972491, + "timestamp": 2.245062069048887 + }, + { + "x": 7.702012772608416, + "y": 5.431650226927326, + "heading": 0.10944883558979078, + "angularVelocity": -1.8116543591848093, + "velocityX": 0.3691936426468861, + "velocityY": -3.359783222625445, + "timestamp": 2.2974000005485156 + }, + { + "x": 7.709831956706759, + "y": 5.244786482851424, + "heading": 0.04182048420765616, + "angularVelocity": -1.2921479593173169, + "velocityX": 0.14939803454781883, + "velocityY": -3.5703310910792863, + "timestamp": 2.349737932048144 + }, + { + "x": 7.710148320506211, + "y": 5.047944741081597, + "heading": 0.001607732041278722, + "angularVelocity": -0.7683290304788382, + "velocityX": 0.006044637042151999, + "velocityY": -3.760976716690136, + "timestamp": 2.402075863547773 + }, + { + "x": 7.703890826207887, + "y": 4.848684527302479, + "heading": -0.031393890437942266, + "angularVelocity": -0.6305488492500959, + "velocityX": -0.11955944988709234, + "velocityY": -3.807185497587574, + "timestamp": 2.4544137950474014 + }, + { + "x": 7.689965111892734, + "y": 4.6520156160480495, + "heading": -0.07069569449240305, + "angularVelocity": -0.7509239079259387, + "velocityX": -0.2660730738900635, + "velocityY": -3.757674512907042, + "timestamp": 2.50675172654703 + }, + { + "x": 7.6736890071007915, + "y": 4.457429220119764, + "heading": -0.11423745831205336, + "angularVelocity": -0.8319351294951269, + "velocityX": -0.3109810480771227, + "velocityY": -3.7178847224725837, + "timestamp": 2.5590896580466587 + }, + { + "x": 7.657799890486921, + "y": 4.2638591081182335, + "heading": -0.1590132492502008, + "angularVelocity": -0.8555131938767111, + "velocityX": -0.30358701917716197, + "velocityY": -3.6984669904828595, + "timestamp": 2.6114275895462873 + }, + { + "x": 7.642337704419043, + "y": 4.071398640697783, + "heading": -0.20522832712376496, + "angularVelocity": -0.8830130757821877, + "velocityX": -0.29542982737076623, + "velocityY": -3.6772654536760823, + "timestamp": 2.663765521045916 + }, + { + "x": 7.627349314262189, + "y": 3.8801695282505793, + "heading": -0.2531486196328151, + "angularVelocity": -0.9155939322781721, + "velocityX": -0.28637719771100456, + "velocityY": -3.653738444908945, + "timestamp": 2.7161034525455445 + }, + { + "x": 7.612892055649704, + "y": 3.690334816903791, + "heading": -0.30312829452838497, + "angularVelocity": -0.9549417308539411, + "velocityX": -0.27622907895371474, + "velocityY": -3.627096178765391, + "timestamp": 2.768441384045173 + }, + { + "x": 7.599038616037834, + "y": 3.50212217111646, + "heading": -0.35565961313676014, + "angularVelocity": -1.003694970420229, + "velocityX": -0.264692149936568, + "velocityY": -3.596104018529388, + "timestamp": 2.8207793155448018 + }, + { + "x": 7.58588568265439, + "y": 3.3158679152826034, + "heading": -0.4114672226763763, + "angularVelocity": -1.066293755610362, + "velocityX": -0.25130785658842936, + "velocityY": -3.5586858421253695, + "timestamp": 2.8731172470444304 + }, + { + "x": 7.573571145277833, + "y": 3.1321108409056713, + "heading": -0.4717089783444659, + "angularVelocity": -1.1510152186377078, + "velocityX": -0.23528895819364495, + "velocityY": -3.5109731911784747, + "timestamp": 2.925455178544059 + }, + { + "x": 7.562314657485301, + "y": 2.9518321478713343, + "heading": -0.5384896128552801, + "angularVelocity": -1.275950970880255, + "velocityX": -0.21507322643450313, + "velocityY": -3.44451314503355, + "timestamp": 2.9777931100436876 + }, + { + "x": 7.552475081584032, + "y": 2.778276274707623, + "heading": -0.6187626830239448, + "angularVelocity": -1.5337455621300466, + "velocityX": -0.18800085558091725, + "velocityY": -3.3160629048735397, + "timestamp": 3.0301310415433163 + }, + { + "x": 7.5459751353703055, + "y": 2.6199152915908197, + "heading": -0.7306496828630857, + "angularVelocity": -2.1377803178928008, + "velocityX": -0.12419188201534234, + "velocityY": -3.025740196055188, + "timestamp": 3.082468973042945 + }, + { + "x": 7.542089138041763, + "y": 2.4767558634954696, + "heading": -0.8750887967914319, + "angularVelocity": -2.7597405894688265, + "velocityX": -0.07424820234959534, + "velocityY": -2.735290142224239, + "timestamp": 3.1348069045425735 + }, + { + "x": 7.5216270585803455, + "y": 2.331419967454435, + "heading": -1.0120731965665102, + "angularVelocity": -2.6173063369166254, + "velocityX": -0.390960797936048, + "velocityY": -2.776875047919336, + "timestamp": 3.187144836042202 + }, + { + "x": 7.484441535370467, + "y": 2.1871473310520577, + "heading": -1.1444783193039663, + "angularVelocity": -2.5298119154441765, + "velocityX": -0.710488973186514, + "velocityY": -2.756559769722614, + "timestamp": 3.2394827675418307 + }, + { + "x": 7.430689261020509, + "y": 2.045319270385381, + "heading": -1.2712939627154947, + "angularVelocity": -2.4230159614243214, + "velocityX": -1.0270232852121666, + "velocityY": -2.7098522353272774, + "timestamp": 3.2918206990414594 + }, + { + "x": 7.360584286485429, + "y": 1.9069970614078058, + "heading": -1.3909439169907853, + "angularVelocity": -2.286103994693467, + "velocityX": -1.3394678109427585, + "velocityY": -2.642867324218853, + "timestamp": 3.344158630541088 + }, + { + "x": 7.274381021511414, + "y": 1.7730949945870125, + "heading": -1.50169441917269, + "angularVelocity": -2.1160657100614175, + "velocityX": -1.6470514310376692, + "velocityY": -2.558413429497911, + "timestamp": 3.3964965620407166 + }, + { + "x": 7.172363334905187, + "y": 1.6444234151770394, + "heading": -1.6017844735100242, + "angularVelocity": -1.9123807813850309, + "velocityX": -1.9492112829669335, + "velocityY": -2.4584765909383464, + "timestamp": 3.448834493540345 + }, + { + "x": 7.054833854515175, + "y": 1.5217039601454048, + "heading": -1.689495586048226, + "angularVelocity": -1.6758612735550291, + "velocityX": -2.2455889451963866, + "velocityY": -2.344751722419604, + "timestamp": 3.501172425039974 + }, + { + "x": 6.922601222991943, + "y": 1.4058892726898191, + "heading": -1.754739642827425, + "angularVelocity": -1.2465921925031216, + "velocityX": -2.526516194553274, + "velocityY": -2.2128250799595794, + "timestamp": 3.5535103565396025 + }, + { + "x": 6.776206366537654, + "y": 1.2881464891497267, + "heading": -1.7163442600815282, + "angularVelocity": 0.701977704776456, + "velocityX": -2.6765177991575464, + "velocityY": -2.1526757394363294, + "timestamp": 3.6082063710777343 + }, + { + "x": 6.619200799462664, + "y": 1.184169918255188, + "heading": -1.6479685771095909, + "angularVelocity": 1.2501035687758868, + "velocityX": -2.870512018120276, + "velocityY": -1.9009898942829024, + "timestamp": 3.6629023856158662 + }, + { + "x": 6.453145942349176, + "y": 1.096120131949485, + "heading": -1.5766994559842828, + "angularVelocity": 1.303003915863407, + "velocityX": -3.035958991083723, + "velocityY": -1.609802598036071, + "timestamp": 3.717598400153998 + }, + { + "x": 6.278278647893632, + "y": 1.024136466565589, + "heading": -1.5039595806005146, + "angularVelocity": 1.3298935214567948, + "velocityX": -3.197075617522952, + "velocityY": -1.3160678340414054, + "timestamp": 3.77229441469213 + }, + { + "x": 6.0946622707930835, + "y": 0.9682554227258839, + "heading": -1.4319058401965126, + "angularVelocity": 1.3173490063662636, + "velocityX": -3.357033938414989, + "velocityY": -1.0216657339950712, + "timestamp": 3.826990429230262 + }, + { + "x": 5.901723318023782, + "y": 0.9281131404158702, + "heading": -1.3656720034044707, + "angularVelocity": 1.2109444783379562, + "velocityX": -3.527477356413067, + "velocityY": -0.7339160384716592, + "timestamp": 3.881686443768394 + }, + { + "x": 5.697449612094902, + "y": 0.9013098773658532, + "heading": -1.3280729441876722, + "angularVelocity": 0.6874186270114382, + "velocityX": -3.7347091493561853, + "velocityY": -0.4900405134880769, + "timestamp": 3.9363824583065257 + }, + { + "x": 5.481732185562019, + "y": 0.8878298601999052, + "heading": -1.318078319190418, + "angularVelocity": 0.1827304069894859, + "velocityX": -3.9439331796742034, + "velocityY": -0.2464533710504764, + "timestamp": 3.9910784728446576 + }, + { + "x": 5.261756953565252, + "y": 0.8920718938341556, + "heading": -1.3180778684404388, + "angularVelocity": 0.000008241002253313922, + "velocityX": -4.021778073855989, + "velocityY": 0.0775565399064386, + "timestamp": 4.04577448738279 + }, + { + "x": 5.042078221460965, + "y": 0.904252420018932, + "heading": -1.318077498096455, + "angularVelocity": 0.000006770950078101512, + "velocityX": -4.0163572055352335, + "velocityY": 0.22269494930539713, + "timestamp": 4.100470501920922 + }, + { + "x": 4.822399513022668, + "y": 0.9164333730320937, + "heading": -1.3180771277548, + "angularVelocity": 0.0000067709074968390835, + "velocityX": -4.01635677285307, + "velocityY": 0.22270275295231748, + "timestamp": 4.1551665164590545 + }, + { + "x": 4.602720804585333, + "y": 0.9286143260630619, + "heading": -1.318076757413277, + "angularVelocity": 0.000006770905088161991, + "velocityX": -4.016356772835495, + "velocityY": 0.22270275327787067, + "timestamp": 4.209862530997187 + }, + { + "x": 4.383042096147975, + "y": 0.9407952790940776, + "heading": -1.3180763870718855, + "angularVelocity": 0.000006770902679722202, + "velocityX": -4.016356772835924, + "velocityY": 0.2227027532787389, + "timestamp": 4.264558545535319 + }, + { + "x": 4.163363387710593, + "y": 0.95297623212514, + "heading": -1.3180760167306258, + "angularVelocity": 0.00000677090027094694, + "velocityX": -4.016356772836354, + "velocityY": 0.22270275327959346, + "timestamp": 4.3192545600734515 + }, + { + "x": 3.9436846792731868, + "y": 0.9651571851562493, + "heading": -1.3180756463894978, + "angularVelocity": 0.000006770897862681277, + "velocityX": -4.016356772836784, + "velocityY": 0.22270275328044806, + "timestamp": 4.373950574611584 + }, + { + "x": 3.7240059708357576, + "y": 0.9773381381874052, + "heading": -1.3180752760485015, + "angularVelocity": 0.000006770895455737502, + "velocityX": -4.016356772837213, + "velocityY": 0.22270275328130246, + "timestamp": 4.428646589149716 + }, + { + "x": 3.5043272623983053, + "y": 0.9895190912186087, + "heading": -1.318074905707637, + "angularVelocity": 0.000006770893046361098, + "velocityX": -4.016356772837642, + "velocityY": 0.2227027532821716, + "timestamp": 4.4833426036878485 + }, + { + "x": 3.2846485539619055, + "y": 1.001700044269276, + "heading": -1.3180745353669043, + "angularVelocity": 0.000006770890636851538, + "velocityX": -4.016356772818388, + "velocityY": 0.222702753638031, + "timestamp": 4.538038618225981 + }, + { + "x": 3.064969871401373, + "y": 1.0138814639860811, + "heading": -1.3180741650285315, + "angularVelocity": 0.000006770847492232218, + "velocityX": -4.01635629973334, + "velocityY": 0.22271128563330012, + "timestamp": 4.592734632764113 + }, + { + "x": 2.845916361742838, + "y": 1.0344400044578748, + "heading": -1.3180737108157792, + "angularVelocity": 0.000008304311678697556, + "velocityX": -4.0049263462481175, + "velocityY": 0.3758690764838311, + "timestamp": 4.6474306473022455 + }, + { + "x": 2.640815838094564, + "y": 1.0655973128674503, + "heading": -1.289493164429316, + "angularVelocity": 0.5225343496743684, + "velocityX": -3.749825748369397, + "velocityY": 0.5696449489542624, + "timestamp": 4.702126661840378 + }, + { + "x": 2.45163944240355, + "y": 1.1038380268542072, + "heading": -1.2300528516755753, + "angularVelocity": 1.0867393768205997, + "velocityX": -3.4586870229663123, + "velocityY": 0.6991499163087526, + "timestamp": 4.75682267637851 + }, + { + "x": 2.278319287222078, + "y": 1.1496508795842428, + "heading": -1.142402837116851, + "angularVelocity": 1.6024936240577001, + "velocityX": -3.168789474791445, + "velocityY": 0.8375903275017751, + "timestamp": 4.8115186909166425 + }, + { + "x": 2.1205992924858448, + "y": 1.2036932398128166, + "heading": -1.0278733070437767, + "angularVelocity": 2.093928251266424, + "velocityX": -2.8835738045644375, + "velocityY": 0.9880493247071551, + "timestamp": 4.866214705454775 + }, + { + "x": 1.9780450716131908, + "y": 1.2666236472330685, + "heading": -0.8853675420058136, + "angularVelocity": 2.6054140551431404, + "velocityX": -2.606299966760295, + "velocityY": 1.1505483160272978, + "timestamp": 4.920910719992907 + }, + { + "x": 1.8498924673957173, + "y": 1.3391158080858083, + "heading": -0.7099571475748747, + "angularVelocity": 3.2070050425456333, + "velocityX": -2.3429971141339836, + "velocityY": 1.325364589447393, + "timestamp": 4.9756067345310395 + }, + { + "x": 1.7366792266224975, + "y": 1.4213022199550633, + "heading": -0.511849503089355, + "angularVelocity": 3.621975863477321, + "velocityX": -2.0698627080825407, + "velocityY": 1.5026032986728497, + "timestamp": 5.030302749069172 + }, + { + "x": 1.6392937564566123, + "y": 1.512661705013378, + "heading": -0.31945189307685995, + "angularVelocity": 3.517580058385459, + "velocityX": -1.780485671364439, + "velocityY": 1.670313382607105, + "timestamp": 5.084998763607304 + }, + { + "x": 1.5571204513790267, + "y": 1.614239806992095, + "heading": -0.1279423976429246, + "angularVelocity": 3.5013427770029124, + "velocityX": -1.5023636689341893, + "velocityY": 1.857139004303521, + "timestamp": 5.1396947781454365 + }, + { + "x": 1.4895694278857712, + "y": 1.7268360109242187, + "heading": 0.06777744416413656, + "angularVelocity": 3.5783199829050085, + "velocityX": -1.235026428592189, + "velocityY": 2.0585815051227545, + "timestamp": 5.194390792683569 + }, + { + "x": 1.4359342742944703, + "y": 1.8511693868204304, + "heading": 0.274536590508249, + "angularVelocity": 3.7801501277568823, + "velocityX": -0.9806044196128505, + "velocityY": 2.2731706678469514, + "timestamp": 5.249086807221701 + }, + { + "x": 1.3972040384965922, + "y": 1.985797456283131, + "heading": 0.46662822108389085, + "angularVelocity": 3.5119858768086685, + "velocityX": -0.7080997788399548, + "velocityY": 2.461387188802272, + "timestamp": 5.3037828217598335 + }, + { + "x": 1.373390793800354, + "y": 2.129699468612671, + "heading": 0.6304013934483359, + "angularVelocity": 2.9942432505803147, + "velocityX": -0.43537440336967165, + "velocityY": 2.630941459714892, + "timestamp": 5.358478836297966 + }, + { + "x": 1.363097994232707, + "y": 2.277885546279951, + "heading": 0.7388351387639758, + "angularVelocity": 2.0277089931298335, + "velocityX": -0.19247515786758002, + "velocityY": 2.7710768586653205, + "timestamp": 5.411954826039604 + }, + { + "x": 1.3664026020560647, + "y": 2.4333190233418533, + "heading": 0.8009908612218983, + "angularVelocity": 1.1623108381578078, + "velocityX": 0.061796103995895656, + "velocityY": 2.9066030907114606, + "timestamp": 5.465430815781243 + }, + { + "x": 1.3840403171376008, + "y": 2.5953692238583987, + "heading": 0.8219734655425052, + "angularVelocity": 0.39237430521587746, + "velocityX": 0.32982493950555586, + "velocityY": 3.030335694569958, + "timestamp": 5.518906805522882 + }, + { + "x": 1.4168581437621641, + "y": 2.7629163129609116, + "heading": 0.8067832999547334, + "angularVelocity": -0.2840558101151666, + "velocityX": 0.6136927391735564, + "velocityY": 3.133127407496165, + "timestamp": 5.57238279526452 + }, + { + "x": 1.4658290464052255, + "y": 2.9340177695630913, + "heading": 0.7613652665039514, + "angularVelocity": -0.8493163692754846, + "velocityX": 0.915754956189807, + "velocityY": 3.199594012730387, + "timestamp": 5.625858785006159 + }, + { + "x": 1.5318217076610499, + "y": 3.1050959732003816, + "heading": 0.6944449920709036, + "angularVelocity": -1.2514078702678118, + "velocityX": 1.2340615213417758, + "velocityY": 3.199159182725336, + "timestamp": 5.679334774747797 + }, + { + "x": 1.613943962289886, + "y": 3.2693665308083615, + "heading": 0.6243357428898241, + "angularVelocity": -1.3110416379351097, + "velocityX": 1.5356846133301518, + "velocityY": 3.07185633031998, + "timestamp": 5.732810764489436 + }, + { + "x": 1.707205824850905, + "y": 3.420371354182967, + "heading": 0.540461395859667, + "angularVelocity": -1.5684487082031198, + "velocityX": 1.7439950716498998, + "velocityY": 2.823787350251241, + "timestamp": 5.7862867542310745 + }, + { + "x": 1.803332479492569, + "y": 3.554784837261743, + "heading": 0.43066614318580515, + "angularVelocity": -2.0531691550604427, + "velocityX": 1.7975666295487922, + "velocityY": 2.513529599511379, + "timestamp": 5.839762743972713 + }, + { + "x": 1.8963203040621885, + "y": 3.6729531605178494, + "heading": 0.29050245020603344, + "angularVelocity": -2.621058416252808, + "velocityX": 1.738870566377101, + "velocityY": 2.2097454170931385, + "timestamp": 5.893238733714352 + }, + { + "x": 1.9828336996382947, + "y": 3.775961775527485, + "heading": 0.11920450803898805, + "angularVelocity": -3.203268289089085, + "velocityX": 1.6177988662590852, + "velocityY": 1.926259158686087, + "timestamp": 5.94671472345599 + }, + { + "x": 2.060121274343493, + "y": 3.864144859360976, + "heading": -0.05942729762417511, + "angularVelocity": -3.340411398203106, + "velocityX": 1.445276189905063, + "velocityY": 1.6490220051940039, + "timestamp": 6.000190713197629 + }, + { + "x": 2.127082581576336, + "y": 3.938231595081354, + "heading": -0.231565089097053, + "angularVelocity": -3.218973455274698, + "velocityX": 1.2521751828504142, + "velocityY": 1.385420561233492, + "timestamp": 6.0536667029392675 + }, + { + "x": 2.183199632616322, + "y": 3.998876793011361, + "heading": -0.38824009125711667, + "angularVelocity": -2.9298195866409387, + "velocityX": 1.0493877964878584, + "velocityY": 1.1340640579633077, + "timestamp": 6.107142692680906 + }, + { + "x": 2.228229768002549, + "y": 4.046617261125075, + "heading": -0.5226070066615436, + "angularVelocity": -2.512658784878971, + "velocityX": 0.8420626827812412, + "velocityY": 0.892745853688067, + "timestamp": 6.160618682422545 + }, + { + "x": 2.262056803725308, + "y": 4.081907934162313, + "heading": -0.629130589840352, + "angularVelocity": -1.9919889971828624, + "velocityX": 0.632564930283457, + "velocityY": 0.6599349204706556, + "timestamp": 6.214094672164183 + }, + { + "x": 2.2846197824922263, + "y": 4.105137889298369, + "heading": -0.7034068078221513, + "angularVelocity": -1.388963875949814, + "velocityX": 0.4219272775675214, + "velocityY": 0.4343997230960695, + "timestamp": 6.267570661905822 + }, + { + "x": 2.295893669128418, + "y": 4.116629123687744, + "heading": -0.7419473060415813, + "angularVelocity": -0.7207065901095526, + "velocityX": 0.2108214675531846, + "velocityY": 0.21488586644012128, + "timestamp": 6.32104665164746 + }, + { + "x": 2.295893669128418, + "y": 4.116629123687744, + "heading": -0.7419473060415813, + "angularVelocity": 7.691238387715462e-32, + "velocityX": 4.371614381676517e-33, + "velocityY": 2.1909450998623725e-33, + "timestamp": 6.374522641389099 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/4 piece mcnugget.auto b/src/main/deploy/pathplanner/autos/4 piece mcnugget.auto new file mode 100644 index 0000000..5d54ca5 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/4 piece mcnugget.auto @@ -0,0 +1,85 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.55, + "y": 7.0 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "3N Top 1" + } + }, + { + "type": "named", + "data": { + "name": "Note Sniffer" + } + }, + { + "type": "path", + "data": { + "pathName": "3N Top Shoot 1" + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "named", + "data": { + "name": "Note Sniffer2" + } + }, + { + "type": "path", + "data": { + "pathName": "3N Top Shoot 2" + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "named", + "data": { + "name": "Note Sniffer" + } + }, + { + "type": "path", + "data": { + "pathName": "3N Top Shoot 3" + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Troll.auto b/src/main/deploy/pathplanner/autos/Troll.auto new file mode 100644 index 0000000..38134b9 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Troll.auto @@ -0,0 +1,25 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.472737193107605, + "y": 5.55 + }, + "rotation": 0.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Troll.1" + } + } + ] + } + }, + "folder": "Testing", + "choreoAuto": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/2nd Note TOP.path b/src/main/deploy/pathplanner/paths/2nd Note TOP.path new file mode 100644 index 0000000..e4323cc --- /dev/null +++ b/src/main/deploy/pathplanner/paths/2nd Note TOP.path @@ -0,0 +1,86 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 3.8, + "y": 7.0 + }, + "prevControl": null, + "nextControl": { + "x": 3.4685115218933618, + "y": 7.0 + }, + "isLocked": false, + "linkedName": "3 Note Top END" + }, + { + "anchor": { + "x": 1.494174579798092, + "y": 5.5768617000416905 + }, + "prevControl": { + "x": 0.975143016688687, + "y": 5.5768617000416905 + }, + "nextControl": { + "x": 1.8646653920386236, + "y": 5.5768617000416905 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.8, + "y": 5.5768617000416905 + }, + "prevControl": { + "x": 2.5129949850025284, + "y": 5.5768617000416905 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "3 Note Top 2nd END" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "Intake", + "waypointRelativePos": 1.6, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 8.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "3 Note Test", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/3N Top 1.path b/src/main/deploy/pathplanner/paths/3N Top 1.path new file mode 100644 index 0000000..9cbfc49 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/3N Top 1.path @@ -0,0 +1,58 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.55, + "y": 7.0 + }, + "prevControl": null, + "nextControl": { + "x": 1.6500000000000001, + "y": 7.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.3034045117971482, + "y": 7.0 + }, + "prevControl": { + "x": 2.203404511797148, + "y": 7.0 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.15, + "rotationDegrees": 0, + "rotateFast": true + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 8.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "3 Note Real", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/3N Top Shoot 1.path b/src/main/deploy/pathplanner/paths/3N Top Shoot 1.path new file mode 100644 index 0000000..99e1e98 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/3N Top Shoot 1.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 3.5, + "y": 7.0 + }, + "prevControl": null, + "nextControl": { + "x": 2.9141386373747915, + "y": 6.589775457544016 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.689169744135214, + "y": 5.55 + }, + "prevControl": { + "x": 2.4478527662696616, + "y": 6.081235571210264 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 8.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "3 Note Real", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/3N Top Shoot 2.path b/src/main/deploy/pathplanner/paths/3N Top Shoot 2.path new file mode 100644 index 0000000..a377015 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/3N Top Shoot 2.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 3.5, + "y": 5.55 + }, + "prevControl": null, + "nextControl": { + "x": 2.9141386373747915, + "y": 5.139775457544016 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.4746750633643797, + "y": 4.1338974839469875 + }, + "prevControl": { + "x": 2.233358085498825, + "y": 4.665133055157252 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 8.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "3 Note Real", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/3N Top Shoot 3.path b/src/main/deploy/pathplanner/paths/3N Top Shoot 3.path new file mode 100644 index 0000000..1599433 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/3N Top Shoot 3.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.8006421808568094, + "y": 4.1338974839469875 + }, + "prevControl": null, + "nextControl": { + "x": 2.3229040282308606, + "y": 4.25089458254926 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.3089291736778261, + "y": 4.4361399886695265 + }, + "prevControl": { + "x": 1.7866673263037751, + "y": 4.3678916811515345 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 8.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "3 Note Real", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/3rd Note TOP.path b/src/main/deploy/pathplanner/paths/3rd Note TOP.path new file mode 100644 index 0000000..a22c6ae --- /dev/null +++ b/src/main/deploy/pathplanner/paths/3rd Note TOP.path @@ -0,0 +1,147 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 3.8, + "y": 5.5768617000416905 + }, + "prevControl": null, + "nextControl": { + "x": 2.8632110836634554, + "y": 5.5768617000416905 + }, + "isLocked": false, + "linkedName": "3 Note Top 2nd END" + }, + { + "anchor": { + "x": 1.8451658756049114, + "y": 5.5768617000416905 + }, + "prevControl": { + "x": 1.7551658756049113, + "y": 5.5768617000416905 + }, + "nextControl": { + "x": 1.9451658756049115, + "y": 5.5768617000416905 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.8451658756049114, + "y": 4.094898451079563 + }, + "prevControl": { + "x": 1.8451658756049114, + "y": 4.777660033953261 + }, + "nextControl": { + "x": 1.8451658756049114, + "y": 3.9948984510795627 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.75, + "y": 4.094898451079563 + }, + "prevControl": { + "x": 2.6500000000000004, + "y": 4.094898451079563 + }, + "nextControl": { + "x": 2.8499999999999996, + "y": 4.094898451079563 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.328428690111538, + "y": 4.3386424065009646 + }, + "prevControl": { + "x": 2.0609117652914652, + "y": 4.333767527392537 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 3.55, + "rotationDegrees": 0, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [ + { + "name": "Shoot", + "waypointRelativePos": 0.35, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "named", + "data": { + "name": "Target" + } + } + ] + } + } + }, + { + "name": "Intake", + "waypointRelativePos": 2.6, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 8.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -45.0, + "rotateFast": true + }, + "reversed": false, + "folder": "3 Note Test", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Straight Line Top.path b/src/main/deploy/pathplanner/paths/Straight Line Top.path new file mode 100644 index 0000000..3b137b2 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Straight Line Top.path @@ -0,0 +1,58 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.55, + "y": 7.0 + }, + "prevControl": null, + "nextControl": { + "x": 2.320230899131632, + "y": 7.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.8, + "y": 7.0 + }, + "prevControl": { + "x": 2.7755328178240863, + "y": 7.0 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "3 Note Top END" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.3, + "rotationDegrees": 0, + "rotateFast": true + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 8.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "3 Note Test", + "previewStartingState": { + "rotation": 35.0, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ 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 66117b7..f24f068 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -42,8 +42,8 @@ public static class Vision { public static final AprilTagFieldLayout kTagLayout = errorWrapper(); // The standard deviations of our vision estimated poses, which affect correction rate - public static final Matrix kSingleTagStdDevs = VecBuilder.fill(4, 4, 20); - public static final Matrix kMultiTagStdDevs = VecBuilder.fill(2, 2, 15); + public static final Matrix kSingleTagStdDevs = VecBuilder.fill(8, 8, 40); + public static final Matrix kMultiTagStdDevs = VecBuilder.fill(4, 4, 20); public static final Vector stateStdDevs = VecBuilder.fill(1, 1, 1); // Encoder Odometry } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 79ff5fd..6944879 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -119,6 +119,7 @@ public boolean checkRedAlliance() { /** This function is called once each time the robot enters Disabled mode. */ @Override public void disabledInit() { + m_robotContainer.stopAll(); } @Override diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index cfe477b..9313a33 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -90,14 +90,14 @@ public class RobotContainer { */ public RobotContainer() { // Named commands for PathPlanner autos - NamedCommands.registerCommand("Intake", new IntexForAutosByAutos(m_IntexerSubsystem)); + NamedCommands.registerCommand("Intake", new IntexForAutosByAutos(m_IntexerSubsystem, m_Shoota)); + NamedCommands.registerCommand("Shoot", new AimBot(m_Shoota, m_SwerveSubsystem, m_IntexerSubsystem, FiringSolutionsV3.convertToRPM(m_Shoota.getCalculatedVelocity()))); NamedCommands.registerCommand("Idle Speed", new InstantCommand(() -> m_Shoota.setShooterVelocity(Constants.Shooter.idleSpeedRPM))); NamedCommands.registerCommand("Target Speed", new InstantCommand(() -> m_Shoota.setShooterVelocity(FiringSolutionsV3.convertToRPM(m_Shoota.getCalculatedVelocity())))); NamedCommands.registerCommand("Set Shooter Intake", new InstantCommand(() -> m_IntexerSubsystem.setShooterIntake(.9))); NamedCommands.registerCommand("Stop Shooter Intake", new InstantCommand(() -> m_IntexerSubsystem.setShooterIntake(0))); - NamedCommands.registerCommand("Shoot", new FIREEE(m_Shoota, m_IntexerSubsystem)); - // TODO auto lock on command - + NamedCommands.registerCommand("Note Sniffer", new NoteSniffer(m_SwerveSubsystem, m_VisionSubsystem, m_IntexerSubsystem, m_Shoota)); + NamedCommands.registerCommand("Note Sniffer2", new NoteSniffer(m_SwerveSubsystem, m_VisionSubsystem, m_IntexerSubsystem, m_Shoota)); m_SwerveSubsystem.setDefaultCommand( new TeleopSwerve( m_SwerveSubsystem, m_VisionSubsystem, m_Shoota, @@ -175,7 +175,7 @@ private void configureButtonBindings() { // Zero wrist zeroShooter.onTrue(new InstantCommand(() -> m_Shoota.resetWristEncoders(Constants.Shooter.angleOffsetManual))); // Set encoder to zero - autoZeroShooter.onTrue(new ZeroWrist(m_Shoota).andThen(new RizzLevel(m_Shoota, Constants.Shooter.intakeAngleRadians))); + autoZeroShooter.onTrue(new ZeroRizz(m_Shoota).andThen(new RizzLevel(m_Shoota, Constants.Shooter.intakeAngleRadians))); // Wrist shooterToIntake.onTrue(new RizzLevel(m_Shoota, Constants.Shooter.intakeAngleRadians)); // Move wrist to intake position @@ -203,6 +203,12 @@ private void configureButtonBindings() { quasistaticBackwards.whileTrue(m_SwerveSubsystem.sysIdQuasistatic(Direction.kReverse));*/ } + public void stopAll () { + m_Shoota.setShooterVelocity(0); + m_Shoota.manualWristSpeed(0); + m_IntexerSubsystem.setALL(0); + } + /** * Use this to pass the autonomous command to the main {@link Robot} class. * diff --git a/src/main/java/frc/robot/commands/AimBot.java b/src/main/java/frc/robot/commands/AimBot.java new file mode 100644 index 0000000..938e06f --- /dev/null +++ b/src/main/java/frc/robot/commands/AimBot.java @@ -0,0 +1,86 @@ +// 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; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import frc.lib.math.FiringSolutionsV3; +import frc.robot.Constants; +import frc.robot.subsystems.IntexerSubsystem; +import frc.robot.subsystems.ShooterSubsystem; +import frc.robot.subsystems.SwerveSubsystem; + +public class AimBot extends Command { + + private ShooterSubsystem shooter; + private SwerveSubsystem swerveSubsystem; + private IntexerSubsystem intexer; + private PIDController rotationPID = new PIDController(0.65, 0.00001, 0.04); + private double speed; + private Timer timer = new Timer(); + + /** Creates a new AutoShoot. */ + public AimBot(ShooterSubsystem shooterSubsystem, SwerveSubsystem swerve, IntexerSubsystem intexer, double speed) { + this.shooter = shooterSubsystem; + this.speed = speed; + this.intexer = intexer; + this.swerveSubsystem = swerve; + addRequirements(shooterSubsystem, swerve); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + timer.reset(); + timer.start(); + shooter.setShooterVelocity(speed); + + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + Pose2d pose = swerveSubsystem.getPose(); + ChassisSpeeds currentSpeed = swerveSubsystem.getChassisSpeeds(); + + double rotationVal = rotationPID.calculate(swerveSubsystem.getHeading().getRadians(), + FiringSolutionsV3.getAngleToMovingTarget(pose.getX(), pose.getY(), FiringSolutionsV3.speakerTargetX, + FiringSolutionsV3.speakerTargetY, + currentSpeed.vxMetersPerSecond, + currentSpeed.vyMetersPerSecond, + pose.getRotation().getRadians())); + + swerveSubsystem.drive( + new Translation2d(0, 0).times(Constants.Swerve.maxSpeed), + rotationVal * Constants.Swerve.maxAngularVelocity, + true, + false); + + if (shooter.shooterAtSpeed()) { + intexer.setShooterIntake(.9); + } + + shooter.setWristPosition(shooter.getCalculatedAngleToSpeaker()); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + swerveSubsystem.setChassisSpeeds(new ChassisSpeeds(0, 0, 0)); + intexer.setShooterIntake(0); + shooter.manualWristSpeed(0); + + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return !intexer.shooterBreak() && timer.get() > 1.5; + } +} diff --git a/src/main/java/frc/robot/commands/IntexBestHex.java b/src/main/java/frc/robot/commands/IntexBestHex.java index 945a74c..b100fe9 100644 --- a/src/main/java/frc/robot/commands/IntexBestHex.java +++ b/src/main/java/frc/robot/commands/IntexBestHex.java @@ -35,7 +35,7 @@ public void initialize() { public void execute() { if (in) { // Hood logic to run forwards or backwards if (intexer.intakeBreak() && !intexer.shooterBreak()) { // If note is not at shooter yet - intexer.setALL(.4); + intexer.setALL(.35); } else if (!intexer.intakeBreak() && intexer.shooterBreak()) { // Stop note if at shooter controller.setRumble(RumbleType.kBothRumble, 0.75); intexer.setALL(0); diff --git a/src/main/java/frc/robot/commands/IntexForAutosByAutos.java b/src/main/java/frc/robot/commands/IntexForAutosByAutos.java index 43bf8c1..fa82796 100644 --- a/src/main/java/frc/robot/commands/IntexForAutosByAutos.java +++ b/src/main/java/frc/robot/commands/IntexForAutosByAutos.java @@ -5,16 +5,20 @@ package frc.robot.commands; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants; import frc.robot.subsystems.IntexerSubsystem; +import frc.robot.subsystems.ShooterSubsystem; public class IntexForAutosByAutos extends Command { private IntexerSubsystem intexer; + private ShooterSubsystem shooter; /** Creates a new IntexForAutosByAutos. */ - public IntexForAutosByAutos(IntexerSubsystem intexerSub) { + public IntexForAutosByAutos(IntexerSubsystem intexerSub, ShooterSubsystem shooterSubsystem) { this.intexer = intexerSub; - addRequirements(intexerSub); + this.shooter = shooterSubsystem; + addRequirements(intexerSub, shooterSubsystem); } // Called when the command is initially scheduled. @@ -25,24 +29,28 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { + shooter.setWristPosition(Constants.Shooter.intakeAngleRadians); + if (intexer.intakeBreak() && !intexer.shooterBreak()) { // If note is not at shooter yet - intexer.setALL(.4); - } else if (!intexer.intakeBreak() && intexer.shooterBreak()) { // Stop note if at shooter + intexer.setALL(.35); + } else if (intexer.shooterBreak()) { // Stop note if at shooter intexer.setALL(0); } else { // Note is not in robot - intexer.setFrontIntake(.75); + intexer.setFrontIntake(.85); + intexer.setShooterIntake(.35); } } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { + intexer.setALL(0); } // Returns true when the command should end. @Override public boolean isFinished() { - if (!intexer.intakeBreak() && intexer.shooterBreak()) { + if (intexer.shooterBreak()) { return true; } else { return false; diff --git a/src/main/java/frc/robot/commands/NoteSniffer.java b/src/main/java/frc/robot/commands/NoteSniffer.java new file mode 100644 index 0000000..c9d07dc --- /dev/null +++ b/src/main/java/frc/robot/commands/NoteSniffer.java @@ -0,0 +1,99 @@ +// 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; + +import org.photonvision.targeting.PhotonPipelineResult; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants; +import frc.robot.subsystems.IntexerSubsystem; +import frc.robot.subsystems.ShooterSubsystem; +import frc.robot.subsystems.SwerveSubsystem; +import frc.robot.subsystems.VisionSubsystem; + +public class NoteSniffer extends Command { + + private SwerveSubsystem swerveSubsystem; + private VisionSubsystem vision; + private IntexerSubsystem intexer; + private ShooterSubsystem shooter; + private PIDController rotationPID = new PIDController(0.65, 0.00001, 0.04); + private Timer timer = new Timer(); + private boolean noteInside = false; + double translationVal = 0.5; + + /** Creates a new IntakeWithVision. */ + public NoteSniffer(SwerveSubsystem swerve, VisionSubsystem vision, IntexerSubsystem intexer, + ShooterSubsystem shooter) { + this.swerveSubsystem = swerve; + this.vision = vision; + this.intexer = intexer; + this.shooter = shooter; + addRequirements(swerve, intexer); + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + timer.reset(); + timer.start(); + translationVal = .5; + noteInside = false; + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + shooter.setWristPosition(Constants.Shooter.intakeAngleRadians); + + PhotonPipelineResult result = vision.getLatestResultN(); + double rotationVal; + + if (intexer.intakeBreak()) { + noteInside = true; + translationVal = 0; + rotationVal = 0; + } + + if (result.hasTargets() && !noteInside) { // Lock robot towards detected note + double yawToNote = Math.toRadians(result.getBestTarget().getYaw()) + + swerveSubsystem.getGyroYaw().getRadians(); + + SmartDashboard.putNumber("Note Yaw", yawToNote); + + rotationVal = rotationPID.calculate(yawToNote, swerveSubsystem.getGyroYaw().getRadians()); + + intexer.setFrontIntake(.75); + intexer.setShooterIntake(.35); + } else { + rotationVal = 0; + } + + swerveSubsystem.drive( + new Translation2d(translationVal, 0).times(Constants.Swerve.maxSpeed), + rotationVal * Constants.Swerve.maxAngularVelocity, + false, + false); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + intexer.setALL(0); + swerveSubsystem.setChassisSpeeds(new ChassisSpeeds(0, 0, 0)); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return intexer.shooterBreak() || timer.get() > 3; + } +} diff --git a/src/main/java/frc/robot/commands/TeleopSwerve.java b/src/main/java/frc/robot/commands/TeleopSwerve.java index d78ab6b..a7d17d4 100644 --- a/src/main/java/frc/robot/commands/TeleopSwerve.java +++ b/src/main/java/frc/robot/commands/TeleopSwerve.java @@ -86,7 +86,7 @@ public void execute() { FiringSolutionsV3.getAngleToMovingTarget(pose.getX(), pose.getY(), FiringSolutionsV3.speakerTargetX, FiringSolutionsV3.speakerTargetY, currentSpeed.vxMetersPerSecond, currentSpeed.vyMetersPerSecond, - FiringSolutions.getAngleToSpeaker(pose.getX(), pose.getY()))); + pose.getRotation().getRadians())); } else if (shooterOverrideAmp.getAsBoolean()) { // Lock robot angle to amp ChassisSpeeds currentSpeed = swerveSubsystem.getChassisSpeeds(); @@ -95,7 +95,7 @@ public void execute() { FiringSolutionsV3.getAngleToMovingTarget(pose.getX(), pose.getY(), FiringSolutionsV3.ampTargetX, FiringSolutionsV3.ampTargetY, currentSpeed.vxMetersPerSecond, currentSpeed.vyMetersPerSecond, - FiringSolutions.getAngleToSpeaker(pose.getX(), pose.getY()))); + pose.getRotation().getRadians())); } else if (intakeOverride.getAsBoolean() && result.hasTargets()) { // Lock robot towards detected note double yawToNote = Math.toRadians(result.getBestTarget().getYaw()) + swerveSubsystem.getGyroYaw().getRadians(); diff --git a/src/main/java/frc/robot/commands/ZeroWrist.java b/src/main/java/frc/robot/commands/ZeroRizz.java similarity index 94% rename from src/main/java/frc/robot/commands/ZeroWrist.java rename to src/main/java/frc/robot/commands/ZeroRizz.java index 5e70ce4..a063a3e 100644 --- a/src/main/java/frc/robot/commands/ZeroWrist.java +++ b/src/main/java/frc/robot/commands/ZeroRizz.java @@ -9,12 +9,12 @@ import frc.robot.Constants; import frc.robot.subsystems.ShooterSubsystem; -public class ZeroWrist extends Command { +public class ZeroRizz extends Command { ShooterSubsystem shooter; public final Timer timer = new Timer(); /** Creates a new ZeroWrist. */ - public ZeroWrist(ShooterSubsystem shooter) { + public ZeroRizz(ShooterSubsystem shooter) { this.shooter = shooter; addRequirements(shooter); // Use addRequirements() here to declare subsystem dependencies. diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 5a38ac1..3a05b0e 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -176,7 +176,7 @@ public void setWristEncoderPosition(double newPosition) { public boolean shooterAtSpeed() { // Copied from Hudson but made it better // if error less than certain amount start the timer - if (Math.abs(getVelocity() - FiringSolutionsV3.convertToRPM(shooterVelocity)) < 50) { + if (Math.abs(getVelocity() - FiringSolutionsV3.convertToRPM(shooterVelocity)) < 80) { speedTimer.start(); } else { speedTimer.reset(); diff --git a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java index d9304e4..d59bc86 100644 --- a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java @@ -18,6 +18,7 @@ import com.pathplanner.lib.path.PathConstraints; import com.pathplanner.lib.util.HolonomicPathFollowerConfig; import com.pathplanner.lib.util.PIDConstants; +import com.pathplanner.lib.util.PathPlannerLogging; import com.pathplanner.lib.util.ReplanningConfig; import edu.wpi.first.math.Matrix; @@ -137,6 +138,11 @@ public SwerveSubsystem(VisionSubsystem vision) { SmartDashboard.putData(this); SmartDashboard.putData("field", m_field); + PathPlannerLogging.setLogActivePathCallback((poses) -> { + // Do whatever you want with the poses here + m_field.getObject("field").setPoses(poses); + }); + this.vision = vision; } From f2ed1d2ede418b5ae32f8d785613535002f06e91 Mon Sep 17 00:00:00 2001 From: Andrew-K961 Date: Sun, 3 Mar 2024 14:33:01 -0600 Subject: [PATCH 08/49] the two robot system and its consequences... --- src/main/java/frc/robot/Robot.java | 9 ++++-- src/main/java/frc/robot/commands/AimBot.java | 1 - .../java/frc/robot/commands/NoteSniffer.java | 19 +++++++++---- .../java/frc/robot/commands/TeleopSwerve.java | 1 - .../frc/robot/subsystems/SwerveSubsystem.java | 28 +++++++++---------- 5 files changed, 34 insertions(+), 24 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 6944879..e87cbdc 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -22,7 +22,6 @@ import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; -import frc.lib.math.FiringSolutions; import frc.lib.math.FiringSolutionsV3; /** @@ -120,6 +119,10 @@ public boolean checkRedAlliance() { @Override public void disabledInit() { m_robotContainer.stopAll(); + + if (m_autonomousCommand != null){ + m_autonomousCommand.cancel(); + } } @Override @@ -136,7 +139,7 @@ public void autonomousInit() { m_autonomousCommand = m_robotContainer.getAutonomousCommand(); - FiringSolutions.setAlliance(redAlliance); + FiringSolutionsV3.setAlliance(redAlliance); // schedule the autonomous command (example) if (m_autonomousCommand != null) { @@ -155,7 +158,7 @@ public void autonomousPeriodic() { public void teleopInit() { redAlliance = checkRedAlliance(); - FiringSolutions.setAlliance(redAlliance); + FiringSolutionsV3.setAlliance(redAlliance); // This makes sure that the autonomous stops running when // teleop starts running. If you want the autonomous to // continue until interrupted by another command, remove diff --git a/src/main/java/frc/robot/commands/AimBot.java b/src/main/java/frc/robot/commands/AimBot.java index 938e06f..21a8777 100644 --- a/src/main/java/frc/robot/commands/AimBot.java +++ b/src/main/java/frc/robot/commands/AimBot.java @@ -75,7 +75,6 @@ public void end(boolean interrupted) { swerveSubsystem.setChassisSpeeds(new ChassisSpeeds(0, 0, 0)); intexer.setShooterIntake(0); shooter.manualWristSpeed(0); - } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/commands/NoteSniffer.java b/src/main/java/frc/robot/commands/NoteSniffer.java index c9d07dc..2f2d869 100644 --- a/src/main/java/frc/robot/commands/NoteSniffer.java +++ b/src/main/java/frc/robot/commands/NoteSniffer.java @@ -77,11 +77,20 @@ public void execute() { rotationVal = 0; } - swerveSubsystem.drive( - new Translation2d(translationVal, 0).times(Constants.Swerve.maxSpeed), - rotationVal * Constants.Swerve.maxAngularVelocity, - false, - false); + if (shooter.getDistanceToSpeaker() < 2.5){ + swerveSubsystem.drive( + new Translation2d(translationVal, 0).times(Constants.Swerve.maxSpeed), + rotationVal * Constants.Swerve.maxAngularVelocity, + false, + false); + } else { + intexer.setALL(-.5); + swerveSubsystem.drive( + new Translation2d(-0.2, 0).times(Constants.Swerve.maxSpeed), + 0 * Constants.Swerve.maxAngularVelocity, + false, + false); + } } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/commands/TeleopSwerve.java b/src/main/java/frc/robot/commands/TeleopSwerve.java index a7d17d4..baeebb2 100644 --- a/src/main/java/frc/robot/commands/TeleopSwerve.java +++ b/src/main/java/frc/robot/commands/TeleopSwerve.java @@ -1,6 +1,5 @@ package frc.robot.commands; -import frc.lib.math.FiringSolutions; import frc.lib.math.FiringSolutionsV3; import frc.robot.Constants; import frc.robot.Robot; diff --git a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java index d59bc86..2824978 100644 --- a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java @@ -60,7 +60,7 @@ public class SwerveSubsystem extends SubsystemBase { // Vars private final VisionSubsystem vision; private final SwerveDrivePoseEstimator swerveOdomEstimator; - SwerveModuleState[] swerveModuleStates; + private SwerveModuleState[] swerveModuleStates; // Characterization stuff private final MutableMeasure m_appliedVoltage = mutable(Volts.of(0)); @@ -71,7 +71,7 @@ public class SwerveSubsystem extends SubsystemBase { new SysIdRoutine.Config(), new SysIdRoutine.Mechanism( (Measure volts) -> { - voltage(volts.in(Units.Volts)); + voltageDrive(volts.in(Units.Volts)); }, this::sysidroutine, this)); @@ -111,10 +111,10 @@ public SwerveSubsystem(VisionSubsystem vision) { this::setChassisSpeeds, // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds new HolonomicPathFollowerConfig( new PIDConstants(5.0, 0.0, 0.0), // Translation PID constants - new PIDConstants(5.0, 0.0, 0.0), // Rotation PID constants + new PIDConstants(8.0, 0.0, 0.0), // Rotation PID constants TODO tune 4.5, // Max module speed, in m/s - 0.46, // Drive base radius in meters. Distance from robot center to furthest module. - new ReplanningConfig() // Default path replanning config. See the API for the options here + 0.37268, // Drive base radius in meters. Distance from robot center to furthest module. + new ReplanningConfig(true, true) ), checkRedAlliance(), this // Reference to this subsystem to set requirements @@ -139,7 +139,6 @@ public SwerveSubsystem(VisionSubsystem vision) { SmartDashboard.putData("field", m_field); PathPlannerLogging.setLogActivePathCallback((poses) -> { - // Do whatever you want with the poses here m_field.getObject("field").setPoses(poses); }); @@ -259,7 +258,7 @@ public void addVisionMeasurement(Pose2d visionMeasurement, double timestampSecon swerveOdomEstimator.addVisionMeasurement(visionMeasurement, timestampSeconds, stdDevs); } - public void voltage(double Voltage) { + public void voltageDrive(double Voltage) { for (SwerveModule mod : mSwerveMods) { mod.voltageDrive(Voltage); } @@ -269,14 +268,7 @@ public void voltage(double Voltage) { public void periodic() { SwerveModulePosition[] modulePositions = getModulePositions(); - for (SwerveModule mod : mSwerveMods) { - SmartDashboard.putNumber("Mod " + mod.moduleNumber + " CANcoder", mod.getCANcoder().getDegrees()); - SmartDashboard.putNumber("Mod " + mod.moduleNumber + " Angle", modulePositions[mod.moduleNumber].angle.getDegrees()); - SmartDashboard.putNumber("Mod " + mod.moduleNumber + " Velocity", mod.getState().speedMetersPerSecond); - } - // Correct pose estimate with multiple vision measurements - Optional OptionalEstimatedPoseFront = vision.getEstimatedPoseFront(); if (OptionalEstimatedPoseFront.isPresent()) { @@ -299,6 +291,7 @@ public void periodic() { swerveOdomEstimator.addVisionMeasurement(estimatedPose2.estimatedPose.toPose2d(), estimatedPose2.timestampSeconds); } + // Logging swerveOdomEstimator.update(getGyroYaw(), modulePositions); m_field.setRobotPose(getPose()); @@ -306,6 +299,12 @@ public void periodic() { posePublisher.set(getPose()); swervePublisher.set(swerveModuleStates); + for (SwerveModule mod : mSwerveMods) { + SmartDashboard.putNumber("Mod " + mod.moduleNumber + " CANcoder", mod.getCANcoder().getDegrees()); + SmartDashboard.putNumber("Mod " + mod.moduleNumber + " Angle", modulePositions[mod.moduleNumber].angle.getDegrees()); + SmartDashboard.putNumber("Mod " + mod.moduleNumber + " Velocity", mod.getState().speedMetersPerSecond); + } + SmartDashboard.putString("Obodom", getPose().toString()); SmartDashboard.putNumber("Gyro", getGyroYaw().getDegrees()); SmartDashboard.putNumber("Heading", getHeading().getDegrees()); @@ -337,6 +336,7 @@ public Command sysIdDynamic(SysIdRoutine.Direction direction) { return m_sysIdRoutine.dynamic(direction); } + // Pathfinding Commands public Command pathToSource() { if (!Robot.getAlliance()) { return AutoBuilder.pathfindToPose(new Pose2d(1.21, 0.96, Rotation2d.fromDegrees(58.79)), From 5118747f944605e5ae8d16c5328cd98d1eeb0524 Mon Sep 17 00:00:00 2001 From: Camille-Jones <459cmjo2@students.olatheschools.com> Date: Mon, 4 Mar 2024 19:54:39 -0600 Subject: [PATCH 09/49] The robot is now gaining consciousness --- .pathplanner/settings.json | 7 +- .../autos/2Mid2Bottom-5ScoredAuto.auto | 25 -- src/main/deploy/pathplanner/autos/3N-0M.auto | 73 +++ .../deploy/pathplanner/autos/3N-1M-Left.auto | 73 +++ .../deploy/pathplanner/autos/3N-1M-Right.auto | 73 +++ .../deploy/pathplanner/autos/4N-1M-left.auto | 97 ++++ .../autos/Amp&Speak-3Scored4ptAuto.auto | 25 -- .../autos/Midfield-2ScoredAuto.auto | 25 -- .../autos/ShootFromMid-0ScoredAuto.auto | 25 -- .../deploy/pathplanner/autos/TEST AUTO.auto | 2 +- .../autos/noMidfield-4ScoredAuto.auto | 25 -- .../deploy/pathplanner/paths/2Mid-3S5PT.path | 270 ----------- .../paths/2Mid2Bottom-5ScoredAuto.path | 420 ----------------- .../paths/Amp&Speak-3Scored4pt.path | 244 ---------- .../paths/Copy of Midfield-2Scored.path | 0 .../pathplanner/paths/Midfield-2Scored.path | 161 ------- .../paths/ShootFromMid-0Scored.path | 424 ------------------ src/main/deploy/pathplanner/paths/Spin.path | 65 +++ .../deploy/pathplanner/paths/TEST PATH.path | 8 +- .../pathplanner/paths/backCloseLeft.path | 49 ++ .../pathplanner/paths/backCloseMiddle.path | 49 ++ .../pathplanner/paths/backCloseRight.path | 49 ++ .../pathplanner/paths/backMidAmpLeft.path | 65 +++ .../pathplanner/paths/backMidAmpRight.path | 65 +++ .../pathplanner/paths/backMidSourceLeft.path | 65 +++ .../pathplanner/paths/backMidSourceRight.path | 65 +++ .../pathplanner/paths/noMidfield-4Scored.path | 347 -------------- .../deploy/pathplanner/paths/toCloseLeft.path | 49 ++ .../pathplanner/paths/toCloseMiddle.path | 49 ++ .../pathplanner/paths/toCloseRight.path | 49 ++ .../pathplanner/paths/toMidAmpLeft.path | 65 +++ .../pathplanner/paths/toMidAmpRight.path | 65 +++ .../pathplanner/paths/toMidSourceLeft.path | 65 +++ .../pathplanner/paths/toMidSourceRight.path | 65 +++ .../paths/toRightCloseFromRight.path | 49 ++ 35 files changed, 1253 insertions(+), 1999 deletions(-) delete mode 100644 src/main/deploy/pathplanner/autos/2Mid2Bottom-5ScoredAuto.auto create mode 100644 src/main/deploy/pathplanner/autos/3N-0M.auto create mode 100644 src/main/deploy/pathplanner/autos/3N-1M-Left.auto create mode 100644 src/main/deploy/pathplanner/autos/3N-1M-Right.auto create mode 100644 src/main/deploy/pathplanner/autos/4N-1M-left.auto delete mode 100644 src/main/deploy/pathplanner/autos/Amp&Speak-3Scored4ptAuto.auto delete mode 100644 src/main/deploy/pathplanner/autos/Midfield-2ScoredAuto.auto delete mode 100644 src/main/deploy/pathplanner/autos/ShootFromMid-0ScoredAuto.auto delete mode 100644 src/main/deploy/pathplanner/autos/noMidfield-4ScoredAuto.auto delete mode 100644 src/main/deploy/pathplanner/paths/2Mid-3S5PT.path delete mode 100644 src/main/deploy/pathplanner/paths/2Mid2Bottom-5ScoredAuto.path delete mode 100644 src/main/deploy/pathplanner/paths/Amp&Speak-3Scored4pt.path create mode 100644 src/main/deploy/pathplanner/paths/Copy of Midfield-2Scored.path delete mode 100644 src/main/deploy/pathplanner/paths/ShootFromMid-0Scored.path create mode 100644 src/main/deploy/pathplanner/paths/Spin.path create mode 100644 src/main/deploy/pathplanner/paths/backCloseLeft.path create mode 100644 src/main/deploy/pathplanner/paths/backCloseMiddle.path create mode 100644 src/main/deploy/pathplanner/paths/backCloseRight.path create mode 100644 src/main/deploy/pathplanner/paths/backMidAmpLeft.path create mode 100644 src/main/deploy/pathplanner/paths/backMidAmpRight.path create mode 100644 src/main/deploy/pathplanner/paths/backMidSourceLeft.path create mode 100644 src/main/deploy/pathplanner/paths/backMidSourceRight.path delete mode 100644 src/main/deploy/pathplanner/paths/noMidfield-4Scored.path create mode 100644 src/main/deploy/pathplanner/paths/toCloseLeft.path create mode 100644 src/main/deploy/pathplanner/paths/toCloseMiddle.path create mode 100644 src/main/deploy/pathplanner/paths/toCloseRight.path create mode 100644 src/main/deploy/pathplanner/paths/toMidAmpLeft.path create mode 100644 src/main/deploy/pathplanner/paths/toMidAmpRight.path create mode 100644 src/main/deploy/pathplanner/paths/toMidSourceLeft.path create mode 100644 src/main/deploy/pathplanner/paths/toMidSourceRight.path create mode 100644 src/main/deploy/pathplanner/paths/toRightCloseFromRight.path diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index 23d9acb..e6e5585 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -1,9 +1,10 @@ { - "robotWidth": 0.838, - "robotLength": 0.838, + "robotWidth": 0.85, + "robotLength": 0.85, "holonomicMode": true, "pathFolders": [ - "New Folder" + "Close paths", + "Midfield Paths" ], "autoFolders": [], "defaultMaxVel": 3.0, diff --git a/src/main/deploy/pathplanner/autos/2Mid2Bottom-5ScoredAuto.auto b/src/main/deploy/pathplanner/autos/2Mid2Bottom-5ScoredAuto.auto deleted file mode 100644 index da27670..0000000 --- a/src/main/deploy/pathplanner/autos/2Mid2Bottom-5ScoredAuto.auto +++ /dev/null @@ -1,25 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 0.8799398121361578, - "y": 4.085148692862707 - }, - "rotation": 0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "2Mid2Bottom-5ScoredAuto" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/3N-0M.auto b/src/main/deploy/pathplanner/autos/3N-0M.auto new file mode 100644 index 0000000..e42d2a2 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/3N-0M.auto @@ -0,0 +1,73 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.89943932856987, + "y": 3.782906188140168 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "someShooterCode" + } + }, + { + "type": "path", + "data": { + "pathName": "toRightCloseFromRight" + } + }, + { + "type": "named", + "data": { + "name": "someIntakeCode" + } + }, + { + "type": "path", + "data": { + "pathName": "backCloseRight" + } + }, + { + "type": "named", + "data": { + "name": "someShooterCode" + } + }, + { + "type": "path", + "data": { + "pathName": "toCloseMiddle" + } + }, + { + "type": "named", + "data": { + "name": "someIntakeCode" + } + }, + { + "type": "path", + "data": { + "pathName": "backCloseMiddle" + } + }, + { + "type": "named", + "data": { + "name": "someShooterCode" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/3N-1M-Left.auto b/src/main/deploy/pathplanner/autos/3N-1M-Left.auto new file mode 100644 index 0000000..2053f31 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/3N-1M-Left.auto @@ -0,0 +1,73 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.4259262722800992, + "y": 6.415340906691315 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "someShooterCode" + } + }, + { + "type": "path", + "data": { + "pathName": "toMidAmpLeft" + } + }, + { + "type": "named", + "data": { + "name": "someIntakeCode" + } + }, + { + "type": "path", + "data": { + "pathName": "backMidAmpLeft" + } + }, + { + "type": "named", + "data": { + "name": "someShooterCode" + } + }, + { + "type": "path", + "data": { + "pathName": "toCloseLeft" + } + }, + { + "type": "named", + "data": { + "name": "someIntakeCode" + } + }, + { + "type": "path", + "data": { + "pathName": "backCloseLeft" + } + }, + { + "type": "named", + "data": { + "name": "someShooterCode" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/3N-1M-Right.auto b/src/main/deploy/pathplanner/autos/3N-1M-Right.auto new file mode 100644 index 0000000..4661d9a --- /dev/null +++ b/src/main/deploy/pathplanner/autos/3N-1M-Right.auto @@ -0,0 +1,73 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.8896895703530137, + "y": 3.685408605971607 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "someShooterCode" + } + }, + { + "type": "path", + "data": { + "pathName": "toMidSourceRight" + } + }, + { + "type": "named", + "data": { + "name": "someIntakeCode" + } + }, + { + "type": "path", + "data": { + "pathName": "backMidSourceRight" + } + }, + { + "type": "named", + "data": { + "name": "someShooterCode" + } + }, + { + "type": "path", + "data": { + "pathName": "toCloseRight" + } + }, + { + "type": "named", + "data": { + "name": "someIntakeCode" + } + }, + { + "type": "path", + "data": { + "pathName": "backCloseRight" + } + }, + { + "type": "named", + "data": { + "name": "someShooterCode" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/4N-1M-left.auto b/src/main/deploy/pathplanner/autos/4N-1M-left.auto new file mode 100644 index 0000000..9732d7d --- /dev/null +++ b/src/main/deploy/pathplanner/autos/4N-1M-left.auto @@ -0,0 +1,97 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.2991794154609702, + "y": 6.971077125052112 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "someShooterCode" + } + }, + { + "type": "path", + "data": { + "pathName": "toMidAmpLeft" + } + }, + { + "type": "named", + "data": { + "name": "someIntakeCode" + } + }, + { + "type": "path", + "data": { + "pathName": "backMidAmpLeft" + } + }, + { + "type": "named", + "data": { + "name": "someShooterCode" + } + }, + { + "type": "path", + "data": { + "pathName": "toCloseLeft" + } + }, + { + "type": "named", + "data": { + "name": "someIntakeCode" + } + }, + { + "type": "path", + "data": { + "pathName": "backCloseLeft" + } + }, + { + "type": "named", + "data": { + "name": "someShooterCode" + } + }, + { + "type": "path", + "data": { + "pathName": "toCloseMiddle" + } + }, + { + "type": "named", + "data": { + "name": "someIntakeCode" + } + }, + { + "type": "path", + "data": { + "pathName": "backCloseMiddle" + } + }, + { + "type": "named", + "data": { + "name": "someShooterCode" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Amp&Speak-3Scored4ptAuto.auto b/src/main/deploy/pathplanner/autos/Amp&Speak-3Scored4ptAuto.auto deleted file mode 100644 index e1ba890..0000000 --- a/src/main/deploy/pathplanner/autos/Amp&Speak-3Scored4ptAuto.auto +++ /dev/null @@ -1,25 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 0.9146333962183594, - "y": 7.016492665463964 - }, - "rotation": 0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Amp&Speak-3Scored4pt" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Midfield-2ScoredAuto.auto b/src/main/deploy/pathplanner/autos/Midfield-2ScoredAuto.auto deleted file mode 100644 index 4264cd7..0000000 --- a/src/main/deploy/pathplanner/autos/Midfield-2ScoredAuto.auto +++ /dev/null @@ -1,25 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 0.8561626240061597, - "y": 3.8941534293325004 - }, - "rotation": 0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Midfield-2Scored" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/ShootFromMid-0ScoredAuto.auto b/src/main/deploy/pathplanner/autos/ShootFromMid-0ScoredAuto.auto deleted file mode 100644 index 6e5a110..0000000 --- a/src/main/deploy/pathplanner/autos/ShootFromMid-0ScoredAuto.auto +++ /dev/null @@ -1,25 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 2, - "y": 2 - }, - "rotation": 0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "ShootFromMid-0Scored" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/TEST AUTO.auto b/src/main/deploy/pathplanner/autos/TEST AUTO.auto index 0c99fe6..613794d 100644 --- a/src/main/deploy/pathplanner/autos/TEST AUTO.auto +++ b/src/main/deploy/pathplanner/autos/TEST AUTO.auto @@ -26,7 +26,7 @@ { "type": "path", "data": { - "pathName": "Midfield-2Scored" + "pathName": null } } ] diff --git a/src/main/deploy/pathplanner/autos/noMidfield-4ScoredAuto.auto b/src/main/deploy/pathplanner/autos/noMidfield-4ScoredAuto.auto deleted file mode 100644 index 81f1cf4..0000000 --- a/src/main/deploy/pathplanner/autos/noMidfield-4ScoredAuto.auto +++ /dev/null @@ -1,25 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 1.1529330422081288, - "y": 4.37764143936839 - }, - "rotation": 0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "noMidfield-4Scored" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/2Mid-3S5PT.path b/src/main/deploy/pathplanner/paths/2Mid-3S5PT.path deleted file mode 100644 index 3f5f26c..0000000 --- a/src/main/deploy/pathplanner/paths/2Mid-3S5PT.path +++ /dev/null @@ -1,270 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 2.0, - "y": 7.0 - }, - "prevControl": null, - "nextControl": { - "x": 2.2009903848867527, - "y": 7.016492665463964 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.8033891338437944, - "y": 7.64797700535572 - }, - "prevControl": { - "x": 1.2537638750491173, - "y": 7.68305946868304 - }, - "nextControl": { - "x": 2.9013999414215146, - "y": 7.577891209127356 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 8.02467929722184, - "y": 7.449176379834242 - }, - "prevControl": { - "x": 7.521830656196924, - "y": 7.987107484186478 - }, - "nextControl": { - "x": 8.482312218391295, - "y": 6.959615580443663 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.5110352727827958, - "y": 5.589805823486292 - }, - "prevControl": { - "x": 1.3893249505063066, - "y": 5.197628118373163 - }, - "nextControl": { - "x": 1.6162826627647557, - "y": 5.928936302317049 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.194993512494147, - "y": 6.080960310068768 - }, - "prevControl": { - "x": 1.3305254625739038, - "y": 5.8164199490702995 - }, - "nextControl": { - "x": 5.059461562414395, - "y": 6.345500671067237 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 8.02467929722184, - "y": 5.753523985680451 - }, - "prevControl": { - "x": 7.268031870735332, - "y": 5.964681407025525 - }, - "nextControl": { - "x": 8.52752793824676, - "y": 5.613194132371171 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.803089543501024, - "y": 6.525338178881486 - }, - "prevControl": { - "x": 5.237085232005224, - "y": 6.501218985343953 - }, - "nextControl": { - "x": 2.3690938549968252, - "y": 6.5494573724190195 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.522729427225236, - "y": 5.578111669043852 - }, - "prevControl": { - "x": 3.159911049166827, - "y": 5.601499977928731 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 2.0, - "rotationDegrees": 0, - "rotateFast": false - }, - { - "waypointRelativePos": 3.0, - "rotationDegrees": -178.5679038158354, - "rotateFast": false - }, - { - "waypointRelativePos": 5.0, - "rotationDegrees": 0, - "rotateFast": false - }, - { - "waypointRelativePos": 1, - "rotationDegrees": 90.51155586658705, - "rotateFast": false - } - ], - "constraintZones": [], - "eventMarkers": [ - { - "name": "shootInAmp", - "waypointRelativePos": 0, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 2.0 - } - }, - { - "type": "named", - "data": { - "name": "someShooterCode" - } - } - ] - } - } - }, - { - "name": "Intake1", - "waypointRelativePos": 2.0500000000000003, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "someIntakeCode" - } - } - ] - } - } - }, - { - "name": "Shooter1", - "waypointRelativePos": 3.0, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 2.0 - } - }, - { - "type": "named", - "data": { - "name": "someShooterCode" - } - } - ] - } - } - }, - { - "name": "Intake2", - "waypointRelativePos": 5.0, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 2.0 - } - }, - { - "type": "named", - "data": { - "name": "someIntakeCode" - } - } - ] - } - } - }, - { - "name": "Shooter2", - "waypointRelativePos": 7.0, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "someShooterCode" - } - } - ] - } - } - } - ], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": -178.49256424122484, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": null, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/2Mid2Bottom-5ScoredAuto.path b/src/main/deploy/pathplanner/paths/2Mid2Bottom-5ScoredAuto.path deleted file mode 100644 index 4251ab3..0000000 --- a/src/main/deploy/pathplanner/paths/2Mid2Bottom-5ScoredAuto.path +++ /dev/null @@ -1,420 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 0.8604402957024457, - "y": 4.124147725730131 - }, - "prevControl": null, - "nextControl": { - "x": 0.8116915046181651, - "y": 4.348392164717821 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.581922403749797, - "y": 5.5768617000416905 - }, - "prevControl": { - "x": 1.572914856767576, - "y": 4.162676823833019 - }, - "nextControl": { - "x": 1.5916721619666534, - "y": 7.107573740088098 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 7.997263310441111, - "y": 7.47806455232863 - }, - "prevControl": { - "x": 6.126599668254271, - "y": 7.436678188563434 - }, - "nextControl": { - "x": 10.200708667450582, - "y": 7.52681334341291 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.6014219201835092, - "y": 5.567111941824834 - }, - "prevControl": { - "x": 1.6334691550540026, - "y": 7.1267440388555 - }, - "nextControl": { - "x": 1.5721726455329406, - "y": 4.143647242163843 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 8.016762826874823, - "y": 5.8108558972462365 - }, - "prevControl": { - "x": 7.926713984812243, - "y": 7.184100738700596 - }, - "nextControl": { - "x": 8.055761859742246, - "y": 5.216120646018015 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.581922403749797, - "y": 5.5768617000416905 - }, - "prevControl": { - "x": 1.6404209530509337, - "y": 7.12707325652181 - }, - "nextControl": { - "x": 1.559165235018994, - "y": 4.973796728675411 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.625146532953399, - "y": 7.029575674353249 - }, - "prevControl": { - "x": 1.9719127324240393, - "y": 6.961327366835255 - }, - "nextControl": { - "x": 3.8200408628333618, - "y": 7.1544153804601125 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.5916721619666523, - "y": 5.567111941824834 - }, - "prevControl": { - "x": 1.2980437292660003, - "y": 7.035254105328088 - }, - "nextControl": { - "x": 1.650170711267789, - "y": 5.2746191953191515 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.7128943569051045, - "y": 5.537862667174266 - }, - "prevControl": { - "x": 2.750171560065728, - "y": 5.247100482521403 - }, - "nextControl": { - "x": 2.664145565820824, - "y": 5.918103237631654 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.6014219201835083, - "y": 5.5768617000416905 - }, - "prevControl": { - "x": 1.5624228873160841, - "y": 5.986351545149648 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 1, - "rotationDegrees": -177.68627750217576, - "rotateFast": false - }, - { - "waypointRelativePos": 2, - "rotationDegrees": 0, - "rotateFast": false - }, - { - "waypointRelativePos": 3, - "rotationDegrees": -177.25191181994626, - "rotateFast": false - }, - { - "waypointRelativePos": 4, - "rotationDegrees": 0, - "rotateFast": false - }, - { - "waypointRelativePos": 5, - "rotationDegrees": -177.08299271337228, - "rotateFast": false - }, - { - "waypointRelativePos": 6, - "rotationDegrees": 0, - "rotateFast": false - }, - { - "waypointRelativePos": 7, - "rotationDegrees": -176.2088343126348, - "rotateFast": false - }, - { - "waypointRelativePos": 8, - "rotationDegrees": 0, - "rotateFast": false - } - ], - "constraintZones": [], - "eventMarkers": [ - { - "name": "Shooter1", - "waypointRelativePos": 1.0, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 2.0 - } - }, - { - "type": "named", - "data": { - "name": "someShooterCode" - } - } - ] - } - } - }, - { - "name": "Shooter2", - "waypointRelativePos": 1.0, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 2.0 - } - }, - { - "type": "named", - "data": { - "name": "someShooterCode" - } - } - ] - } - } - }, - { - "name": "Shooter3", - "waypointRelativePos": 1.0, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 2.0 - } - }, - { - "type": "named", - "data": { - "name": "someShooterCode" - } - } - ] - } - } - }, - { - "name": "Shooter4", - "waypointRelativePos": 1.0, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 2.0 - } - }, - { - "type": "named", - "data": { - "name": "someShooterCode" - } - } - ] - } - } - }, - { - "name": "Shooter5", - "waypointRelativePos": 1.0, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "someShooterCode" - } - } - ] - } - } - }, - { - "name": "Intake1", - "waypointRelativePos": 2.1999999999999997, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 2.0 - } - }, - { - "type": "named", - "data": { - "name": "someIntakeCode" - } - } - ] - } - } - }, - { - "name": "Intake2", - "waypointRelativePos": 4.0, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 2.0 - } - }, - { - "type": "named", - "data": { - "name": "someIntakeCode" - } - } - ] - } - } - }, - { - "name": "Intake3", - "waypointRelativePos": 6.0, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 2.0 - } - }, - { - "type": "named", - "data": { - "name": "someIntakeCode" - } - } - ] - } - } - }, - { - "name": "Intake4", - "waypointRelativePos": 7.9, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 2.0 - } - }, - { - "type": "named", - "data": { - "name": "someIntakeCode" - } - } - ] - } - } - } - ], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": -177.6784694101673, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": null, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Amp&Speak-3Scored4pt.path b/src/main/deploy/pathplanner/paths/Amp&Speak-3Scored4pt.path deleted file mode 100644 index 77c5f85..0000000 --- a/src/main/deploy/pathplanner/paths/Amp&Speak-3Scored4pt.path +++ /dev/null @@ -1,244 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 0.9029392417759192, - "y": 7.0398809743488435 - }, - "prevControl": null, - "nextControl": { - "x": 0.9029392417759192, - "y": 6.770915422172725 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.8384715971711143, - "y": 7.601200387585961 - }, - "prevControl": { - "x": 1.581200199437436, - "y": 7.835083476434761 - }, - "nextControl": { - "x": 2.1688747139562614, - "y": 7.300833917781282 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.69214487146923, - "y": 6.981410202136645 - }, - "prevControl": { - "x": 2.951179854235866, - "y": 6.7481519756307975 - }, - "nextControl": { - "x": 2.433109888702594, - "y": 7.214668428642491 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.8384715971711143, - "y": 7.57781207870108 - }, - "prevControl": { - "x": 2.1458545827884508, - "y": 7.5884114919982295 - }, - "nextControl": { - "x": 1.4993411183403567, - "y": 7.5661179242586405 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.6453682536994707, - "y": 5.566417514601411 - }, - "prevControl": { - "x": 3.1482168947243934, - "y": 5.554723360158971 - }, - "nextControl": { - "x": 2.023983429260253, - "y": 5.58086832447209 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.4642586550130365, - "y": 5.5430292057165325 - }, - "prevControl": { - "x": 1.6747534349769553, - "y": 5.659970750140931 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 1, - "rotationDegrees": 90.0, - "rotateFast": false - }, - { - "waypointRelativePos": 4.0, - "rotationDegrees": -0.8303154862581124, - "rotateFast": false - }, - { - "waypointRelativePos": 3.0, - "rotationDegrees": 90.0, - "rotateFast": false - }, - { - "waypointRelativePos": 2, - "rotationDegrees": 0, - "rotateFast": false - } - ], - "constraintZones": [], - "eventMarkers": [ - { - "name": "Intake1", - "waypointRelativePos": 1.7000000000000002, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 2.0 - } - }, - { - "type": "named", - "data": { - "name": "someIntakeCode" - } - } - ] - } - } - }, - { - "name": "AmpShooter", - "waypointRelativePos": 1.0, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "someShooterCode" - } - }, - { - "type": "wait", - "data": { - "waitTime": 2.0 - } - } - ] - } - } - }, - { - "name": "Intake2", - "waypointRelativePos": 4.0, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 2.0 - } - }, - { - "type": "named", - "data": { - "name": "someIntakeCode" - } - } - ] - } - } - }, - { - "name": "Shooter3", - "waypointRelativePos": 5.0, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "someShooterCode" - } - } - ] - } - } - }, - { - "name": "AmpShooter2", - "waypointRelativePos": 3.0, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 2.0 - } - }, - { - "type": "named", - "data": { - "name": "someShooterCode" - } - } - ] - } - } - } - ], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": -179.85967042829577, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": null, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Copy of Midfield-2Scored.path b/src/main/deploy/pathplanner/paths/Copy of Midfield-2Scored.path new file mode 100644 index 0000000..e69de29 diff --git a/src/main/deploy/pathplanner/paths/Midfield-2Scored.path b/src/main/deploy/pathplanner/paths/Midfield-2Scored.path index e877228..e69de29 100644 --- a/src/main/deploy/pathplanner/paths/Midfield-2Scored.path +++ b/src/main/deploy/pathplanner/paths/Midfield-2Scored.path @@ -1,161 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 0.8444684695637196, - "y": 3.8941534293325004 - }, - "prevControl": null, - "nextControl": { - "x": 0.853988963664091, - "y": 4.484424063555472 - }, - "isLocked": false, - "linkedName": "test end" - }, - { - "anchor": { - "x": 1.4759528094554764, - "y": 5.566417514601411 - }, - "prevControl": { - "x": 1.4374305843675834, - "y": 3.8200766439502716 - }, - "nextControl": { - "x": 1.5110352727827963, - "y": 7.156822518773243 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 8.065511617959103, - "y": 7.497564068762342 - }, - "prevControl": { - "x": 8.077026272654008, - "y": 8.038752839422866 - }, - "nextControl": { - "x": 8.053817463516664, - "y": 6.947938809967665 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.4642586550130365, - "y": 5.578111669043852 - }, - "prevControl": { - "x": 1.1251281761822776, - "y": 5.355922734637494 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "test start" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 2, - "rotationDegrees": 0, - "rotateFast": false - }, - { - "waypointRelativePos": 0, - "rotationDegrees": 0, - "rotateFast": false - }, - { - "waypointRelativePos": 1, - "rotationDegrees": 176.6725957582734, - "rotateFast": false - } - ], - "constraintZones": [], - "eventMarkers": [ - { - "name": "Shooter1", - "waypointRelativePos": 1.0, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 2.0 - } - }, - { - "type": "named", - "data": { - "name": "someShooterCode" - } - } - ] - } - } - }, - { - "name": "Intake1", - "waypointRelativePos": 2.0, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 2.0 - } - }, - { - "type": "named", - "data": { - "name": "someIntakeCode" - } - } - ] - } - } - }, - { - "name": "Shooter2", - "waypointRelativePos": 3.0, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "someShooterCode" - } - } - ] - } - } - } - ], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": -176.90594194108263, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": null, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ShootFromMid-0Scored.path b/src/main/deploy/pathplanner/paths/ShootFromMid-0Scored.path deleted file mode 100644 index 1a9b429..0000000 --- a/src/main/deploy/pathplanner/paths/ShootFromMid-0Scored.path +++ /dev/null @@ -1,424 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 0.8506905374855894, - "y": 3.919402803176153 - }, - "prevControl": null, - "nextControl": { - "x": 1.8506905374855886, - "y": 3.419402803176153 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 4.497100110589771, - "y": 5.898603721197941 - }, - "prevControl": { - "x": 3.836878882783895, - "y": 5.074678129017778 - }, - "nextControl": { - "x": 5.157321338395647, - "y": 6.722529313378104 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 6.554299094346408, - "y": 5.3428675028371435 - }, - "prevControl": { - "x": 6.135456667323529, - "y": 6.693634329985923 - }, - "nextControl": { - "x": 6.9442894230206536, - "y": 4.085148692862707 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 7.919265244706262, - "y": 2.4374395542140253 - }, - "prevControl": { - "x": 7.900821066252662, - "y": 2.852433569420034 - }, - "nextControl": { - "x": 7.938764761139973, - "y": 1.9987004344555004 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 6.993038214104933, - "y": 6.249595017004761 - }, - "prevControl": { - "x": 6.963788939454365, - "y": 7.12707325652181 - }, - "nextControl": { - "x": 7.002661982519497, - "y": 5.960881964567859 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 7.948514519356831, - "y": 4.153397000380699 - }, - "prevControl": { - "x": 7.996656500512615, - "y": 5.094322925745358 - }, - "nextControl": { - "x": 7.900372538201046, - "y": 3.2124710750160403 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 6.97353869767122, - "y": 6.23009550057105 - }, - "prevControl": { - "x": 7.242497958399245, - "y": 6.443155398989768 - }, - "nextControl": { - "x": 6.704579436943195, - "y": 6.017035602152331 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 7.948514519356831, - "y": 5.771856864378813 - }, - "prevControl": { - "x": 7.921077396101727, - "y": 6.7321561783073935 - }, - "nextControl": { - "x": 7.958264277573687, - "y": 5.430615326788848 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 6.983288455888077, - "y": 6.23009550057105 - }, - "prevControl": { - "x": 6.97353869767122, - "y": 6.5810867963778685 - }, - "nextControl": { - "x": 7.002180878707415, - "y": 5.549968279074879 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 7.938764761139975, - "y": 7.419566003027493 - }, - "prevControl": { - "x": 7.987365757243036, - "y": 8.2333001214527 - }, - "nextControl": { - "x": 7.890163765036912, - "y": 6.605831884602283 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 6.983288455888077, - "y": 6.23009550057105 - }, - "prevControl": { - "x": 8.445752188416485, - "y": 7.214821080473516 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 1, - "rotationDegrees": 14.620873988631796, - "rotateFast": false - }, - { - "waypointRelativePos": 2, - "rotationDegrees": 0, - "rotateFast": false - }, - { - "waypointRelativePos": 3, - "rotationDegrees": 0, - "rotateFast": false - }, - { - "waypointRelativePos": 4, - "rotationDegrees": -175.09093786134454, - "rotateFast": false - }, - { - "waypointRelativePos": 5, - "rotationDegrees": 0, - "rotateFast": false - }, - { - "waypointRelativePos": 6, - "rotationDegrees": -178.8876003837022, - "rotateFast": false - }, - { - "waypointRelativePos": 7, - "rotationDegrees": 0, - "rotateFast": false - }, - { - "waypointRelativePos": 8, - "rotationDegrees": 180.0, - "rotateFast": false - }, - { - "waypointRelativePos": 9, - "rotationDegrees": 0, - "rotateFast": false - } - ], - "constraintZones": [], - "eventMarkers": [ - { - "name": "Shooter1", - "waypointRelativePos": 4.0, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "someShooterCode" - } - }, - { - "type": "wait", - "data": { - "waitTime": 2.0 - } - } - ] - } - } - }, - { - "name": "Shooter2", - "waypointRelativePos": 5.949999999999999, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "someShooterCode" - } - }, - { - "type": "wait", - "data": { - "waitTime": 2.0 - } - } - ] - } - } - }, - { - "name": "Shooter3", - "waypointRelativePos": 7.9, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "someShooterCode" - } - }, - { - "type": "wait", - "data": { - "waitTime": 2.0 - } - } - ] - } - } - }, - { - "name": "Shooter4", - "waypointRelativePos": 10.0, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "someShooterCode" - } - }, - { - "type": "wait", - "data": { - "waitTime": 2.0 - } - } - ] - } - } - }, - { - "name": "Intake1", - "waypointRelativePos": 0, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "someIntakeCode" - } - }, - { - "type": "wait", - "data": { - "waitTime": 2.0 - } - } - ] - } - } - }, - { - "name": "Inkate2", - "waypointRelativePos": 0, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "someIntakeCode" - } - }, - { - "type": "wait", - "data": { - "waitTime": 2.0 - } - } - ] - } - } - }, - { - "name": "Intake3", - "waypointRelativePos": 0, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "someIntakeCode" - } - }, - { - "type": "wait", - "data": { - "waitTime": 2.0 - } - } - ] - } - } - }, - { - "name": "Intake4", - "waypointRelativePos": 0, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "someIntakeCode" - } - }, - { - "type": "wait", - "data": { - "waitTime": 2.0 - } - } - ] - } - } - } - ], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": -179.3097228021349, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": null, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Spin.path b/src/main/deploy/pathplanner/paths/Spin.path new file mode 100644 index 0000000..d9c9acd --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Spin.path @@ -0,0 +1,65 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.9579378778710067, + "y": 7.088074223654385 + }, + "prevControl": null, + "nextControl": { + "x": 1.9579378778710068, + "y": 6.588074223654385 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.647571580178791, + "y": 6.395841390257602 + }, + "prevControl": { + "x": 6.82178789696324, + "y": 5.607668794059819 + }, + "nextControl": { + "x": 4.935839230348296, + "y": 6.873579542883553 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.43, + "y": 5.54 + }, + "prevControl": { + "x": 0.6207700680009443, + "y": 6.485726547035042 + }, + "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/TEST PATH.path b/src/main/deploy/pathplanner/paths/TEST PATH.path index 6bd0810..3401985 100644 --- a/src/main/deploy/pathplanner/paths/TEST PATH.path +++ b/src/main/deploy/pathplanner/paths/TEST PATH.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 0.8444684695637196, - "y": 3.8941534293325004 + "x": 8.293644849397962, + "y": 4.1046482092964185 }, "prevControl": { - "x": 0.8444684695637195, - "y": 5.414844061907055 + "x": 8.293644849397962, + "y": 5.625338841870973 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/backCloseLeft.path b/src/main/deploy/pathplanner/paths/backCloseLeft.path new file mode 100644 index 0000000..29be339 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/backCloseLeft.path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.22, + "y": 6.79 + }, + "prevControl": null, + "nextControl": { + "x": 2.3034045117971482, + "y": 6.668834620329574 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.41, + "y": 5.57 + }, + "prevControl": { + "x": 1.8354161173880559, + "y": 5.791356380812524 + }, + "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": "Close paths", + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/backCloseMiddle.path b/src/main/deploy/pathplanner/paths/backCloseMiddle.path new file mode 100644 index 0000000..6de2e50 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/backCloseMiddle.path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.11, + "y": 5.55 + }, + "prevControl": null, + "nextControl": { + "x": 2.369670227701502, + "y": 5.4398646014394165 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.41, + "y": 5.57 + }, + "prevControl": { + "x": 1.757418051653207, + "y": 5.684109040427108 + }, + "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": "Close paths", + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/backCloseRight.path b/src/main/deploy/pathplanner/paths/backCloseRight.path new file mode 100644 index 0000000..3ab8b3a --- /dev/null +++ b/src/main/deploy/pathplanner/paths/backCloseRight.path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.08, + "y": 4.27 + }, + "prevControl": null, + "nextControl": { + "x": 2.2031736126655166, + "y": 3.838122580283165 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.41, + "y": 5.57 + }, + "prevControl": { + "x": 1.5721726455329406, + "y": 5.830355413679948 + }, + "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": "Close paths", + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/backMidAmpLeft.path b/src/main/deploy/pathplanner/paths/backMidAmpLeft.path new file mode 100644 index 0000000..c972575 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/backMidAmpLeft.path @@ -0,0 +1,65 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 7.3, + "y": 7.35 + }, + "prevControl": null, + "nextControl": { + "x": 6.759044016900386, + "y": 6.873579542883553 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.84809140639659, + "y": 6.990576641485824 + }, + "prevControl": { + "x": 5.4428266576248125, + "y": 7.146572772955523 + }, + "nextControl": { + "x": 3.893767367396956, + "y": 6.740262139453132 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.41, + "y": 5.57 + }, + "prevControl": { + "x": 1.6732434718551152, + "y": 5.862492746505683 + }, + "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": "Midfield Paths", + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/backMidAmpRight.path b/src/main/deploy/pathplanner/paths/backMidAmpRight.path new file mode 100644 index 0000000..00d969c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/backMidAmpRight.path @@ -0,0 +1,65 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 7.28, + "y": 5.68 + }, + "prevControl": null, + "nextControl": { + "x": 6.788293291550955, + "y": 5.966852028715935 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.0430865707337125, + "y": 6.639585345679005 + }, + "prevControl": { + "x": 6.436490622294017, + "y": 6.397872397959361 + }, + "nextControl": { + "x": 4.087610265481815, + "y": 6.805331235365558 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.41, + "y": 5.57 + }, + "prevControl": { + "x": 2.1181591056768823, + "y": 6.044850094450783 + }, + "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": "Midfield Paths", + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/backMidSourceLeft.path b/src/main/deploy/pathplanner/paths/backMidSourceLeft.path new file mode 100644 index 0000000..ed09117 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/backMidSourceLeft.path @@ -0,0 +1,65 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 7.17, + "y": 2.58 + }, + "prevControl": null, + "nextControl": { + "x": 7.32452999347804, + "y": 1.998700434455501 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.3953774320850316, + "y": 2.300942939178041 + }, + "prevControl": { + "x": 4.763522936223294, + "y": 1.9429235549175612 + }, + "nextControl": { + "x": 2.3521533028814297, + "y": 2.5739361692500116 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.41, + "y": 5.57 + }, + "prevControl": { + "x": 1.8159166009543433, + "y": 4.640884911223504 + }, + "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": "Midfield Paths", + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/backMidSourceRight.path b/src/main/deploy/pathplanner/paths/backMidSourceRight.path new file mode 100644 index 0000000..9295085 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/backMidSourceRight.path @@ -0,0 +1,65 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 7.08, + "y": 0.84 + }, + "prevControl": null, + "nextControl": { + "x": 6.710295225816106, + "y": 0.8287294484327685 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.868890488374794, + "y": 2.515437619949907 + }, + "prevControl": { + "x": 3.933276408891291, + "y": 1.5842649119057797 + }, + "nextControl": { + "x": 2.556898225435399, + "y": 2.7883824933930974 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.41, + "y": 5.57 + }, + "prevControl": { + "x": 1.5039243380149394, + "y": 5.030875239898781 + }, + "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": "Midfield Paths", + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/noMidfield-4Scored.path b/src/main/deploy/pathplanner/paths/noMidfield-4Scored.path deleted file mode 100644 index c4346dd..0000000 --- a/src/main/deploy/pathplanner/paths/noMidfield-4Scored.path +++ /dev/null @@ -1,347 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 0.77430354290908, - "y": 7.086657592118603 - }, - "prevControl": null, - "nextControl": { - "x": 0.7255547518247994, - "y": 7.398649855057999 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.5136740962318043, - "y": 5.5086133925236975 - }, - "prevControl": { - "x": 1.495849290866785, - "y": 4.094512166898846 - }, - "nextControl": { - "x": 1.5429233708823726, - "y": 7.829055848135449 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.6446460493871116, - "y": 7.010076157919538 - }, - "prevControl": { - "x": 2.5769499777172586, - "y": 7.490033699758601 - }, - "nextControl": { - "x": 2.753112109549637, - "y": 6.2410639785650135 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.52342385444866, - "y": 5.5086133925236975 - }, - "prevControl": { - "x": 1.4763294431622167, - "y": 6.30921838439323 - }, - "nextControl": { - "x": 1.581922403749797, - "y": 4.514138054404375 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.6251465329533996, - "y": 5.547612425391122 - }, - "prevControl": { - "x": 2.6251465329533996, - "y": 5.089373789198885 - }, - "nextControl": { - "x": 2.6251465329533996, - "y": 6.064441581550969 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.5136740962318043, - "y": 5.52811290895741 - }, - "prevControl": { - "x": 1.5469864305999512, - "y": 6.394269860914725 - }, - "nextControl": { - "x": 1.481987382027023, - "y": 4.704223850633142 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.6251465329533996, - "y": 4.114397967513275 - }, - "prevControl": { - "x": 2.591215328081158, - "y": 5.348049980999785 - }, - "nextControl": { - "x": 2.6540911276596924, - "y": 3.062046373356342 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.494174579798092, - "y": 5.547612425391122 - }, - "prevControl": { - "x": 1.8549156338217678, - "y": 5.976601786932791 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 1.05, - "rotationDegrees": 0, - "rotateFast": false - }, - { - "waypointRelativePos": 1, - "rotationDegrees": 180.0, - "rotateFast": false - }, - { - "waypointRelativePos": 2, - "rotationDegrees": 0, - "rotateFast": false - }, - { - "waypointRelativePos": 3, - "rotationDegrees": 178.56790381583536, - "rotateFast": false - }, - { - "waypointRelativePos": 4, - "rotationDegrees": 0, - "rotateFast": false - }, - { - "waypointRelativePos": 5, - "rotationDegrees": 180.0, - "rotateFast": false - }, - { - "waypointRelativePos": 6, - "rotationDegrees": 0, - "rotateFast": false - }, - { - "waypointRelativePos": 5, - "rotationDegrees": 0, - "rotateFast": false - }, - { - "waypointRelativePos": 0, - "rotationDegrees": 0.584630520705183, - "rotateFast": false - } - ], - "constraintZones": [], - "eventMarkers": [ - { - "name": "Shooter1", - "waypointRelativePos": 0.0, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 2.0 - } - }, - { - "type": "named", - "data": { - "name": "someShooterCode" - } - } - ] - } - } - }, - { - "name": "Intake1", - "waypointRelativePos": 0, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "someIntakeCode" - } - }, - { - "type": "wait", - "data": { - "waitTime": 5.0 - } - } - ] - } - } - }, - { - "name": "Shooter2", - "waypointRelativePos": 0, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 2.0 - } - }, - { - "type": "named", - "data": { - "name": "someShooterCode" - } - } - ] - } - } - }, - { - "name": "Intake2", - "waypointRelativePos": 0, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 2.0 - } - }, - { - "type": "named", - "data": { - "name": "someIntakeCode" - } - } - ] - } - } - }, - { - "name": "Shooter3", - "waypointRelativePos": 0, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 2.0 - } - }, - { - "type": "named", - "data": { - "name": "someShooterCode" - } - } - ] - } - } - }, - { - "name": "Intake3", - "waypointRelativePos": 0, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 2.0 - } - }, - { - "type": "named", - "data": { - "name": "someIntakeCode" - } - } - ] - } - } - }, - { - "name": "Shooter4", - "waypointRelativePos": 0, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "someIntakeCode" - } - } - ] - } - } - } - ], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": -178.76359239714387, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": null, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/toCloseLeft.path b/src/main/deploy/pathplanner/paths/toCloseLeft.path new file mode 100644 index 0000000..ee0e30c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/toCloseLeft.path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.41, + "y": 5.57 + }, + "prevControl": null, + "nextControl": { + "x": 2.127908863898996, + "y": 6.366592115612341 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.215656687850701, + "y": 6.785831718937153 + }, + "prevControl": { + "x": 1.728168777007896, + "y": 6.161847193058363 + }, + "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": "Close paths", + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/toCloseMiddle.path b/src/main/deploy/pathplanner/paths/toCloseMiddle.path new file mode 100644 index 0000000..b565cc1 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/toCloseMiddle.path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.41, + "y": 5.57 + }, + "prevControl": null, + "nextControl": { + "x": 1.6696702277015019, + "y": 5.459864601439417 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.1084093474600265, + "y": 5.547612425391122 + }, + "prevControl": { + "x": 1.8549156338217678, + "y": 5.693858798643963 + }, + "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": "Close paths", + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/toCloseRight.path b/src/main/deploy/pathplanner/paths/toCloseRight.path new file mode 100644 index 0000000..717f20b --- /dev/null +++ b/src/main/deploy/pathplanner/paths/toCloseRight.path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.41, + "y": 5.57 + }, + "prevControl": null, + "nextControl": { + "x": 1.5331736126655164, + "y": 5.138122580283166 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.0791600728094584, + "y": 4.270394098982973 + }, + "prevControl": { + "x": 1.6014219201835092, + "y": 5.352617261054 + }, + "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": "Close paths", + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/toMidAmpLeft.path b/src/main/deploy/pathplanner/paths/toMidAmpLeft.path new file mode 100644 index 0000000..5eab28e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/toMidAmpLeft.path @@ -0,0 +1,65 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.9774373943047189, + "y": 7.068574707220673 + }, + "prevControl": null, + "nextControl": { + "x": 1.9774373943047188, + "y": 6.568574707220673 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.935839230348296, + "y": 6.23009550057105 + }, + "prevControl": { + "x": 3.594198443848425, + "y": 5.7828819050710925 + }, + "nextControl": { + "x": 5.871816019166481, + "y": 6.5420877635104455 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.295280718827471, + "y": 7.3513176955095005 + }, + "prevControl": { + "x": 6.817542566201522, + "y": 7.419566003027494 + }, + "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": "Midfield Paths", + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/toMidAmpRight.path b/src/main/deploy/pathplanner/paths/toMidAmpRight.path new file mode 100644 index 0000000..051fbf5 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/toMidAmpRight.path @@ -0,0 +1,65 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.9676876360878626, + "y": 7.010076157919538 + }, + "prevControl": null, + "nextControl": { + "x": 1.9676876360878635, + "y": 6.510076157919538 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.965088504998865, + "y": 6.171596951269913 + }, + "prevControl": { + "x": 3.5818378347909796, + "y": 5.877288298034192 + }, + "nextControl": { + "x": 5.423327141191101, + "y": 6.269094533438474 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.27578120239376, + "y": 5.684109040427108 + }, + "prevControl": { + "x": 6.096060458154171, + "y": 6.035100336233927 + }, + "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": "Midfield Paths", + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/toMidSourceLeft.path b/src/main/deploy/pathplanner/paths/toMidSourceLeft.path new file mode 100644 index 0000000..01370f5 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/toMidSourceLeft.path @@ -0,0 +1,65 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.9091890867867262, + "y": 3.704908122405319 + }, + "prevControl": null, + "nextControl": { + "x": 1.9091890867867252, + "y": 3.204908122405319 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.39960252842121, + "y": 1.9499516433712205 + }, + "prevControl": { + "x": 3.1082305507614296, + "y": 2.526456990540765 + }, + "nextControl": { + "x": 5.491575448709092, + "y": 1.462463732528416 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.168533862008342, + "y": 2.583685927466867 + }, + "prevControl": { + "x": 6.486050786828415, + "y": 1.6477091386486817 + }, + "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": "Midfield Paths", + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/toMidSourceRight.path b/src/main/deploy/pathplanner/paths/toMidSourceRight.path new file mode 100644 index 0000000..950b0b1 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/toMidSourceRight.path @@ -0,0 +1,65 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.9091890867867262, + "y": 3.704908122405319 + }, + "prevControl": null, + "nextControl": { + "x": 1.9091890867867252, + "y": 3.204908122405319 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.39960252842121, + "y": 1.9499516433712205 + }, + "prevControl": { + "x": 3.1082305507614296, + "y": 2.526456990540765 + }, + "nextControl": { + "x": 5.491575448709092, + "y": 1.462463732528416 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.080786038056638, + "y": 0.8384792066496263 + }, + "prevControl": { + "x": 5.589073030877654, + "y": 1.7062076879498176 + }, + "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": "Midfield Paths", + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/toRightCloseFromRight.path b/src/main/deploy/pathplanner/paths/toRightCloseFromRight.path new file mode 100644 index 0000000..cd29fa5 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/toRightCloseFromRight.path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.93, + "y": 3.782906188140168 + }, + "prevControl": null, + "nextControl": { + "x": 1.5637342840956467, + "y": 3.8414047374413043 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.08, + "y": 4.27 + }, + "prevControl": { + "x": 1.4267661994706409, + "y": 4.182252176048294 + }, + "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": "Close paths", + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file From 8c3bad4a3681ca124750bf8b42f4adb477b41cb8 Mon Sep 17 00:00:00 2001 From: Andrew-K961 Date: Mon, 4 Mar 2024 19:56:21 -0600 Subject: [PATCH 10/49] war has changed --- .pathplanner/settings.json | 4 ++-- src/main/deploy/pathplanner/autos/4 piece mcnugget.auto | 6 ++++++ src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/commands/AimBot.java | 2 +- src/main/java/frc/robot/commands/NoteSniffer.java | 9 ++++----- src/main/java/frc/robot/subsystems/SwerveSubsystem.java | 2 +- 6 files changed, 15 insertions(+), 10 deletions(-) diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index e5b0f33..9b1a9ba 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -1,6 +1,6 @@ { - "robotWidth": 0.6604, - "robotLength": 0.6604, + "robotWidth": 0.85, + "robotLength": 0.85, "holonomicMode": true, "pathFolders": [ "3 Note Test", diff --git a/src/main/deploy/pathplanner/autos/4 piece mcnugget.auto b/src/main/deploy/pathplanner/autos/4 piece mcnugget.auto index 5d54ca5..0515d66 100644 --- a/src/main/deploy/pathplanner/autos/4 piece mcnugget.auto +++ b/src/main/deploy/pathplanner/autos/4 piece mcnugget.auto @@ -76,6 +76,12 @@ "data": { "name": "Shoot" } + }, + { + "type": "named", + "data": { + "name": "Idle Speed" + } } ] } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index f24f068..49aeea2 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -180,7 +180,7 @@ public static final class Shooter { } public static final class Elevator { //TODO tune - public static final double maxHeightMeters = 0.81; + public static final double maxHeightMeters = 0.78; public static final double minHeightMeters = 0.015; } diff --git a/src/main/java/frc/robot/commands/AimBot.java b/src/main/java/frc/robot/commands/AimBot.java index 21a8777..80390f6 100644 --- a/src/main/java/frc/robot/commands/AimBot.java +++ b/src/main/java/frc/robot/commands/AimBot.java @@ -62,7 +62,7 @@ public void execute() { true, false); - if (shooter.shooterAtSpeed()) { + if (shooter.shooterAtSpeed() && rotationPID.getPositionError() <= .035) { intexer.setShooterIntake(.9); } diff --git a/src/main/java/frc/robot/commands/NoteSniffer.java b/src/main/java/frc/robot/commands/NoteSniffer.java index 2f2d869..5cf0254 100644 --- a/src/main/java/frc/robot/commands/NoteSniffer.java +++ b/src/main/java/frc/robot/commands/NoteSniffer.java @@ -37,7 +37,6 @@ public NoteSniffer(SwerveSubsystem swerve, VisionSubsystem vision, IntexerSubsys this.intexer = intexer; this.shooter = shooter; addRequirements(swerve, intexer); - // Use addRequirements() here to declare subsystem dependencies. } // Called when the command is initially scheduled. @@ -71,26 +70,26 @@ public void execute() { rotationVal = rotationPID.calculate(yawToNote, swerveSubsystem.getGyroYaw().getRadians()); - intexer.setFrontIntake(.75); + intexer.setFrontIntake(.8); intexer.setShooterIntake(.35); } else { rotationVal = 0; } - if (shooter.getDistanceToSpeaker() < 2.5){ + //if (shooter.getDistanceToSpeaker() < 2.5){ swerveSubsystem.drive( new Translation2d(translationVal, 0).times(Constants.Swerve.maxSpeed), rotationVal * Constants.Swerve.maxAngularVelocity, false, false); - } else { + /* } else { intexer.setALL(-.5); swerveSubsystem.drive( new Translation2d(-0.2, 0).times(Constants.Swerve.maxSpeed), 0 * Constants.Swerve.maxAngularVelocity, false, false); - } + }*/ } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java index 2824978..2ad1605 100644 --- a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java @@ -111,7 +111,7 @@ public SwerveSubsystem(VisionSubsystem vision) { this::setChassisSpeeds, // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds new HolonomicPathFollowerConfig( new PIDConstants(5.0, 0.0, 0.0), // Translation PID constants - new PIDConstants(8.0, 0.0, 0.0), // Rotation PID constants TODO tune + new PIDConstants(6.0, 0.0, 0.0), // Rotation PID constants TODO tune 4.5, // Max module speed, in m/s 0.37268, // Drive base radius in meters. Distance from robot center to furthest module. new ReplanningConfig(true, true) From d8887d71e125670dd0336d63ac4365607f7d94be Mon Sep 17 00:00:00 2001 From: Camille-Jones <459cmjo2@students.olatheschools.com> Date: Tue, 5 Mar 2024 19:10:35 -0600 Subject: [PATCH 11/49] robot go WEEEEEE --- .pathplanner/settings.json | 7 +++---- src/main/deploy/pathplanner/autos/3N-0M.auto | 2 +- .../deploy/pathplanner/autos/3N-1M-Left.auto | 10 +++++----- .../{3N-1M-Right.auto => 3N-2M-Right.auto} | 18 +++++++++--------- .../deploy/pathplanner/autos/4N-1M-left.auto | 14 +++++++------- .../deploy/pathplanner/paths/2nd Note TOP.path | 2 +- .../deploy/pathplanner/paths/3N Top 1.path | 2 +- .../pathplanner/paths/3N Top Shoot 1.path | 2 +- .../pathplanner/paths/3N Top Shoot 2.path | 2 +- .../pathplanner/paths/3N Top Shoot 3.path | 2 +- .../deploy/pathplanner/paths/3rd Note TOP.path | 2 +- .../pathplanner/paths/Straight Line Top.path | 2 +- .../pathplanner/paths/backCloseLeft.path | 10 +++++----- .../pathplanner/paths/backCloseMiddle.path | 14 +++++++------- .../pathplanner/paths/backCloseRight.path | 10 +++++----- .../pathplanner/paths/backMidAmpLeft.path | 8 ++++---- .../pathplanner/paths/backMidAmpRight.path | 8 ++++---- .../pathplanner/paths/backMidSourceLeft.path | 8 ++++---- .../pathplanner/paths/backMidSourceRight.path | 8 ++++---- .../deploy/pathplanner/paths/toCloseLeft.path | 14 +++++++------- .../pathplanner/paths/toCloseMiddle.path | 14 +++++++------- .../deploy/pathplanner/paths/toCloseRight.path | 12 ++++++------ .../deploy/pathplanner/paths/toMidAmpLeft.path | 16 ++++++++-------- .../pathplanner/paths/toMidAmpRight.path | 16 ++++++++-------- .../pathplanner/paths/toMidSourceLeft.path | 8 ++++---- .../pathplanner/paths/toMidSourceRight.path | 8 ++++---- .../paths/toRightCloseFromRight.path | 14 +++++++------- 27 files changed, 116 insertions(+), 117 deletions(-) rename src/main/deploy/pathplanner/autos/{3N-1M-Right.auto => 3N-2M-Right.auto} (75%) diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index 843172b..f6baf12 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -3,10 +3,9 @@ "robotLength": 0.85, "holonomicMode": true, "pathFolders": [ - "3 Note Test", - "3 Note Real", - "Close paths", - "Midfield Paths" + "McNugget paths", + "Midfield Paths", + "Close" ], "autoFolders": [ "Testing" diff --git a/src/main/deploy/pathplanner/autos/3N-0M.auto b/src/main/deploy/pathplanner/autos/3N-0M.auto index e42d2a2..f226f3d 100644 --- a/src/main/deploy/pathplanner/autos/3N-0M.auto +++ b/src/main/deploy/pathplanner/autos/3N-0M.auto @@ -5,7 +5,7 @@ "x": 0.89943932856987, "y": 3.782906188140168 }, - "rotation": 0 + "rotation": 13.495733280795742 }, "command": { "type": "sequential", diff --git a/src/main/deploy/pathplanner/autos/3N-1M-Left.auto b/src/main/deploy/pathplanner/autos/3N-1M-Left.auto index 2053f31..03ed260 100644 --- a/src/main/deploy/pathplanner/autos/3N-1M-Left.auto +++ b/src/main/deploy/pathplanner/autos/3N-1M-Left.auto @@ -14,7 +14,7 @@ { "type": "named", "data": { - "name": "someShooterCode" + "name": "Shoot" } }, { @@ -26,7 +26,7 @@ { "type": "named", "data": { - "name": "someIntakeCode" + "name": "Note Sniffer" } }, { @@ -38,7 +38,7 @@ { "type": "named", "data": { - "name": "someShooterCode" + "name": "Shoot" } }, { @@ -50,7 +50,7 @@ { "type": "named", "data": { - "name": "someIntakeCode" + "name": "Note Sniffer2" } }, { @@ -62,7 +62,7 @@ { "type": "named", "data": { - "name": "someShooterCode" + "name": "Shoot" } } ] diff --git a/src/main/deploy/pathplanner/autos/3N-1M-Right.auto b/src/main/deploy/pathplanner/autos/3N-2M-Right.auto similarity index 75% rename from src/main/deploy/pathplanner/autos/3N-1M-Right.auto rename to src/main/deploy/pathplanner/autos/3N-2M-Right.auto index 4661d9a..506c0f1 100644 --- a/src/main/deploy/pathplanner/autos/3N-1M-Right.auto +++ b/src/main/deploy/pathplanner/autos/3N-2M-Right.auto @@ -2,8 +2,8 @@ "version": 1.0, "startingPose": { "position": { - "x": 0.8896895703530137, - "y": 3.685408605971607 + "x": 0.9091890867867262, + "y": 3.695158364188463 }, "rotation": 0 }, @@ -14,7 +14,7 @@ { "type": "named", "data": { - "name": "someShooterCode" + "name": "Shoot" } }, { @@ -26,7 +26,7 @@ { "type": "named", "data": { - "name": "someIntakeCode" + "name": "Note Sniffer" } }, { @@ -38,31 +38,31 @@ { "type": "named", "data": { - "name": "someShooterCode" + "name": "Shoot" } }, { "type": "path", "data": { - "pathName": "toCloseRight" + "pathName": "toMidSourceLeft" } }, { "type": "named", "data": { - "name": "someIntakeCode" + "name": "Note Sniffer2" } }, { "type": "path", "data": { - "pathName": "backCloseRight" + "pathName": "backMidSourceLeft" } }, { "type": "named", "data": { - "name": "someShooterCode" + "name": "Shoot" } } ] diff --git a/src/main/deploy/pathplanner/autos/4N-1M-left.auto b/src/main/deploy/pathplanner/autos/4N-1M-left.auto index 9732d7d..1302db3 100644 --- a/src/main/deploy/pathplanner/autos/4N-1M-left.auto +++ b/src/main/deploy/pathplanner/autos/4N-1M-left.auto @@ -14,7 +14,7 @@ { "type": "named", "data": { - "name": "someShooterCode" + "name": "Shoot" } }, { @@ -26,7 +26,7 @@ { "type": "named", "data": { - "name": "someIntakeCode" + "name": "Note Sniffer" } }, { @@ -38,7 +38,7 @@ { "type": "named", "data": { - "name": "someShooterCode" + "name": "Shoot" } }, { @@ -50,7 +50,7 @@ { "type": "named", "data": { - "name": "someIntakeCode" + "name": "Note Sniffer2" } }, { @@ -62,7 +62,7 @@ { "type": "named", "data": { - "name": "someShooterCode" + "name": "Shoot" } }, { @@ -74,7 +74,7 @@ { "type": "named", "data": { - "name": "someIntakeCode" + "name": "Note Sniffer" } }, { @@ -86,7 +86,7 @@ { "type": "named", "data": { - "name": "someShooterCode" + "name": "Shoot" } } ] diff --git a/src/main/deploy/pathplanner/paths/2nd Note TOP.path b/src/main/deploy/pathplanner/paths/2nd Note TOP.path index e4323cc..50a2328 100644 --- a/src/main/deploy/pathplanner/paths/2nd Note TOP.path +++ b/src/main/deploy/pathplanner/paths/2nd Note TOP.path @@ -77,7 +77,7 @@ "rotateFast": false }, "reversed": false, - "folder": "3 Note Test", + "folder": "McNugget paths", "previewStartingState": { "rotation": 0, "velocity": 0 diff --git a/src/main/deploy/pathplanner/paths/3N Top 1.path b/src/main/deploy/pathplanner/paths/3N Top 1.path index 9cbfc49..5ad8e01 100644 --- a/src/main/deploy/pathplanner/paths/3N Top 1.path +++ b/src/main/deploy/pathplanner/paths/3N Top 1.path @@ -49,7 +49,7 @@ "rotateFast": false }, "reversed": false, - "folder": "3 Note Real", + "folder": "McNugget paths", "previewStartingState": { "rotation": 0, "velocity": 0 diff --git a/src/main/deploy/pathplanner/paths/3N Top Shoot 1.path b/src/main/deploy/pathplanner/paths/3N Top Shoot 1.path index 99e1e98..589b09f 100644 --- a/src/main/deploy/pathplanner/paths/3N Top Shoot 1.path +++ b/src/main/deploy/pathplanner/paths/3N Top Shoot 1.path @@ -43,7 +43,7 @@ "rotateFast": false }, "reversed": false, - "folder": "3 Note Real", + "folder": "McNugget paths", "previewStartingState": { "rotation": 0, "velocity": 0 diff --git a/src/main/deploy/pathplanner/paths/3N Top Shoot 2.path b/src/main/deploy/pathplanner/paths/3N Top Shoot 2.path index a377015..30067c6 100644 --- a/src/main/deploy/pathplanner/paths/3N Top Shoot 2.path +++ b/src/main/deploy/pathplanner/paths/3N Top Shoot 2.path @@ -43,7 +43,7 @@ "rotateFast": false }, "reversed": false, - "folder": "3 Note Real", + "folder": "McNugget paths", "previewStartingState": { "rotation": 0, "velocity": 0 diff --git a/src/main/deploy/pathplanner/paths/3N Top Shoot 3.path b/src/main/deploy/pathplanner/paths/3N Top Shoot 3.path index 1599433..06ba0c7 100644 --- a/src/main/deploy/pathplanner/paths/3N Top Shoot 3.path +++ b/src/main/deploy/pathplanner/paths/3N Top Shoot 3.path @@ -43,7 +43,7 @@ "rotateFast": false }, "reversed": false, - "folder": "3 Note Real", + "folder": "McNugget paths", "previewStartingState": { "rotation": 0, "velocity": 0 diff --git a/src/main/deploy/pathplanner/paths/3rd Note TOP.path b/src/main/deploy/pathplanner/paths/3rd Note TOP.path index a22c6ae..6217468 100644 --- a/src/main/deploy/pathplanner/paths/3rd Note TOP.path +++ b/src/main/deploy/pathplanner/paths/3rd Note TOP.path @@ -138,7 +138,7 @@ "rotateFast": true }, "reversed": false, - "folder": "3 Note Test", + "folder": "McNugget paths", "previewStartingState": { "rotation": 0, "velocity": 0 diff --git a/src/main/deploy/pathplanner/paths/Straight Line Top.path b/src/main/deploy/pathplanner/paths/Straight Line Top.path index 3b137b2..9dfe5fb 100644 --- a/src/main/deploy/pathplanner/paths/Straight Line Top.path +++ b/src/main/deploy/pathplanner/paths/Straight Line Top.path @@ -49,7 +49,7 @@ "rotateFast": false }, "reversed": false, - "folder": "3 Note Test", + "folder": "McNugget paths", "previewStartingState": { "rotation": 35.0, "velocity": 0 diff --git a/src/main/deploy/pathplanner/paths/backCloseLeft.path b/src/main/deploy/pathplanner/paths/backCloseLeft.path index 29be339..272c58b 100644 --- a/src/main/deploy/pathplanner/paths/backCloseLeft.path +++ b/src/main/deploy/pathplanner/paths/backCloseLeft.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.22, - "y": 6.79 + "x": 1.9231639413397603, + "y": 6.854080026449839 }, "prevControl": null, "nextControl": { - "x": 2.3034045117971482, - "y": 6.668834620329574 + "x": 2.006568453136908, + "y": 6.732914646779413 }, "isLocked": false, "linkedName": null @@ -43,7 +43,7 @@ "rotateFast": false }, "reversed": false, - "folder": "Close paths", + "folder": "Close", "previewStartingState": null, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/backCloseMiddle.path b/src/main/deploy/pathplanner/paths/backCloseMiddle.path index 6de2e50..92599d7 100644 --- a/src/main/deploy/pathplanner/paths/backCloseMiddle.path +++ b/src/main/deploy/pathplanner/paths/backCloseMiddle.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.11, - "y": 5.55 + "x": 1.8159166009543433, + "y": 5.547612425391122 }, "prevControl": null, "nextControl": { - "x": 2.369670227701502, - "y": 5.4398646014394165 + "x": 1.6014219201835092, + "y": 5.5768617000416905 }, "isLocked": false, "linkedName": null @@ -20,8 +20,8 @@ "y": 5.57 }, "prevControl": { - "x": 1.757418051653207, - "y": 5.684109040427108 + "x": 1.69891950235207, + "y": 5.606110974692258 }, "nextControl": null, "isLocked": false, @@ -43,7 +43,7 @@ "rotateFast": false }, "reversed": false, - "folder": "Close paths", + "folder": "Close", "previewStartingState": null, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/backCloseRight.path b/src/main/deploy/pathplanner/paths/backCloseRight.path index 3ab8b3a..02b1354 100644 --- a/src/main/deploy/pathplanner/paths/backCloseRight.path +++ b/src/main/deploy/pathplanner/paths/backCloseRight.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.08, - "y": 4.27 + "x": 1.5916721619666532, + "y": 4.172896516814411 }, "prevControl": null, "nextControl": { - "x": 2.2031736126655166, - "y": 3.838122580283165 + "x": 1.71484577463217, + "y": 3.7410190970975767 }, "isLocked": false, "linkedName": null @@ -43,7 +43,7 @@ "rotateFast": false }, "reversed": false, - "folder": "Close paths", + "folder": "Close", "previewStartingState": null, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/backMidAmpLeft.path b/src/main/deploy/pathplanner/paths/backMidAmpLeft.path index c972575..9fbc008 100644 --- a/src/main/deploy/pathplanner/paths/backMidAmpLeft.path +++ b/src/main/deploy/pathplanner/paths/backMidAmpLeft.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 7.3, - "y": 7.35 + "x": 6.554299094346408, + "y": 7.361067453726357 }, "prevControl": null, "nextControl": { - "x": 6.759044016900386, - "y": 6.873579542883553 + "x": 6.013343111246795, + "y": 6.88464699660991 }, "isLocked": false, "linkedName": null diff --git a/src/main/deploy/pathplanner/paths/backMidAmpRight.path b/src/main/deploy/pathplanner/paths/backMidAmpRight.path index 00d969c..5c18520 100644 --- a/src/main/deploy/pathplanner/paths/backMidAmpRight.path +++ b/src/main/deploy/pathplanner/paths/backMidAmpRight.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 7.28, - "y": 5.68 + "x": 6.4763010286115605, + "y": 6.10334864375192 }, "prevControl": null, "nextControl": { - "x": 6.788293291550955, - "y": 5.966852028715935 + "x": 5.984594320162516, + "y": 6.390200672467855 }, "isLocked": false, "linkedName": null diff --git a/src/main/deploy/pathplanner/paths/backMidSourceLeft.path b/src/main/deploy/pathplanner/paths/backMidSourceLeft.path index ed09117..fd26538 100644 --- a/src/main/deploy/pathplanner/paths/backMidSourceLeft.path +++ b/src/main/deploy/pathplanner/paths/backMidSourceLeft.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 7.17, - "y": 2.58 + "x": 6.7200449840329615, + "y": 2.3204424556117518 }, "prevControl": null, "nextControl": { - "x": 7.32452999347804, - "y": 1.998700434455501 + "x": 6.396836824885053, + "y": 1.953637570838087 }, "isLocked": false, "linkedName": null diff --git a/src/main/deploy/pathplanner/paths/backMidSourceRight.path b/src/main/deploy/pathplanner/paths/backMidSourceRight.path index 9295085..e1309ba 100644 --- a/src/main/deploy/pathplanner/paths/backMidSourceRight.path +++ b/src/main/deploy/pathplanner/paths/backMidSourceRight.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 7.08, - "y": 0.84 + "x": 6.417802479310422, + "y": 0.8092299319990566 }, "prevControl": null, "nextControl": { - "x": 6.710295225816106, - "y": 0.8287294484327685 + "x": 6.048097705126527, + "y": 0.7979593804318251 }, "isLocked": false, "linkedName": null diff --git a/src/main/deploy/pathplanner/paths/toCloseLeft.path b/src/main/deploy/pathplanner/paths/toCloseLeft.path index ee0e30c..2863e03 100644 --- a/src/main/deploy/pathplanner/paths/toCloseLeft.path +++ b/src/main/deploy/pathplanner/paths/toCloseLeft.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 2.127908863898996, - "y": 6.366592115612341 + "x": 1.6404209530509337, + "y": 6.522588247076731 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.215656687850701, - "y": 6.785831718937153 + "x": 1.7086692605689264, + "y": 6.912578575750977 }, "prevControl": { - "x": 1.728168777007896, - "y": 6.161847193058363 + "x": 1.581922403749797, + "y": 6.288594049872185 }, "nextControl": null, "isLocked": false, @@ -43,7 +43,7 @@ "rotateFast": false }, "reversed": false, - "folder": "Close paths", + "folder": "Close", "previewStartingState": null, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/toCloseMiddle.path b/src/main/deploy/pathplanner/paths/toCloseMiddle.path index b565cc1..42ac9c3 100644 --- a/src/main/deploy/pathplanner/paths/toCloseMiddle.path +++ b/src/main/deploy/pathplanner/paths/toCloseMiddle.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 1.6696702277015019, - "y": 5.459864601439417 + "x": 1.4161765140632432, + "y": 5.5768617000416905 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.1084093474600265, - "y": 5.547612425391122 + "x": 1.7379185352194946, + "y": 5.557362183607978 }, "prevControl": { - "x": 1.8549156338217678, - "y": 5.693858798643963 + "x": 1.7281687770026384, + "y": 5.567111941824834 }, "nextControl": null, "isLocked": false, @@ -43,7 +43,7 @@ "rotateFast": false }, "reversed": false, - "folder": "Close paths", + "folder": "Close", "previewStartingState": null, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/toCloseRight.path b/src/main/deploy/pathplanner/paths/toCloseRight.path index 717f20b..e6da5fa 100644 --- a/src/main/deploy/pathplanner/paths/toCloseRight.path +++ b/src/main/deploy/pathplanner/paths/toCloseRight.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 1.5331736126655164, - "y": 5.138122580283166 + "x": 1.4454257887138118, + "y": 5.225870404234871 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.0791600728094584, + "x": 1.6306711948340775, "y": 4.270394098982973 }, "prevControl": { - "x": 1.6014219201835092, - "y": 5.352617261054 + "x": 1.4844248215812361, + "y": 5.17712161315059 }, "nextControl": null, "isLocked": false, @@ -43,7 +43,7 @@ "rotateFast": false }, "reversed": false, - "folder": "Close paths", + "folder": "Close", "previewStartingState": null, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/toMidAmpLeft.path b/src/main/deploy/pathplanner/paths/toMidAmpLeft.path index 5eab28e..b5184f6 100644 --- a/src/main/deploy/pathplanner/paths/toMidAmpLeft.path +++ b/src/main/deploy/pathplanner/paths/toMidAmpLeft.path @@ -20,24 +20,24 @@ "y": 6.23009550057105 }, "prevControl": { - "x": 3.594198443848425, - "y": 5.7828819050710925 + "x": 3.555302250423032, + "y": 5.923309505032103 }, "nextControl": { - "x": 5.871816019166481, - "y": 6.5420877635104455 + "x": 5.901065293817049, + "y": 6.4445901813418836 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.295280718827471, - "y": 7.3513176955095005 + "x": 6.544549336129553, + "y": 7.380566970160069 }, "prevControl": { - "x": 6.817542566201522, - "y": 7.419566003027494 + "x": 6.066811183503603, + "y": 7.4488152776780625 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/toMidAmpRight.path b/src/main/deploy/pathplanner/paths/toMidAmpRight.path index 051fbf5..75d6640 100644 --- a/src/main/deploy/pathplanner/paths/toMidAmpRight.path +++ b/src/main/deploy/pathplanner/paths/toMidAmpRight.path @@ -20,24 +20,24 @@ "y": 6.171596951269913 }, "prevControl": { - "x": 3.5818378347909796, - "y": 5.877288298034192 + "x": 3.5845515250736004, + "y": 6.478382946808862 }, "nextControl": { - "x": 5.423327141191101, - "y": 6.269094533438474 + "x": 5.491575448709092, + "y": 6.054599852667639 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.27578120239376, - "y": 5.684109040427108 + "x": 6.525049819695841, + "y": 5.742607589728245 }, "prevControl": { - "x": 6.096060458154171, - "y": 6.035100336233927 + "x": 5.345329075456252, + "y": 6.093598885535064 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/toMidSourceLeft.path b/src/main/deploy/pathplanner/paths/toMidSourceLeft.path index 01370f5..96f26bb 100644 --- a/src/main/deploy/pathplanner/paths/toMidSourceLeft.path +++ b/src/main/deploy/pathplanner/paths/toMidSourceLeft.path @@ -32,12 +32,12 @@ }, { "anchor": { - "x": 7.168533862008342, - "y": 2.583685927466867 + "x": 6.671296192948681, + "y": 2.0961980166240624 }, "prevControl": { - "x": 6.486050786828415, - "y": 1.6477091386486817 + "x": 5.988813117768754, + "y": 1.1602212278058772 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/toMidSourceRight.path b/src/main/deploy/pathplanner/paths/toMidSourceRight.path index 950b0b1..12e14b5 100644 --- a/src/main/deploy/pathplanner/paths/toMidSourceRight.path +++ b/src/main/deploy/pathplanner/paths/toMidSourceRight.path @@ -32,12 +32,12 @@ }, { "anchor": { - "x": 7.080786038056638, - "y": 0.8384792066496263 + "x": 6.398302962876711, + "y": 0.8872279977339068 }, "prevControl": { - "x": 5.589073030877654, - "y": 1.7062076879498176 + "x": 4.9065899556977275, + "y": 1.754956479034098 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/toRightCloseFromRight.path b/src/main/deploy/pathplanner/paths/toRightCloseFromRight.path index cd29fa5..10faf60 100644 --- a/src/main/deploy/pathplanner/paths/toRightCloseFromRight.path +++ b/src/main/deploy/pathplanner/paths/toRightCloseFromRight.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 1.5637342840956467, - "y": 3.8414047374413043 + "x": 1.52342385444866, + "y": 4.007150627127858 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.08, - "y": 4.27 + "x": 1.689169744135214, + "y": 4.114397967513275 }, "prevControl": { - "x": 1.4267661994706409, - "y": 4.182252176048294 + "x": 1.1529330422081288, + "y": 3.8999032867424406 }, "nextControl": null, "isLocked": false, @@ -43,7 +43,7 @@ "rotateFast": false }, "reversed": false, - "folder": "Close paths", + "folder": "Close", "previewStartingState": null, "useDefaultConstraints": false } \ No newline at end of file From fad9c1f35bdc77edf3ceeeab780669896420f953 Mon Sep 17 00:00:00 2001 From: Mr-A768 Date: Wed, 6 Mar 2024 01:23:33 -0600 Subject: [PATCH 12/49] elevator polish and illegal wrist shenanigans (banned in 49 states) --- src/main/java/frc/robot/RobotContainer.java | 168 +++++++++++----- .../frc/robot/commands/ElevationManual.java | 10 +- .../java/frc/robot/commands/ElevatorSet.java | 1 - src/main/java/frc/robot/commands/FIREEE.java | 2 +- .../robot/commands/IntakeThroughShooter.java | 4 +- .../commands/IntakeThroughShooterPart2.java | 8 +- .../java/frc/robot/commands/ManRizzt.java | 35 +++- .../java/frc/robot/commands/MissileLock.java | 2 +- .../java/frc/robot/commands/RizzLevel.java | 4 +- .../java/frc/robot/commands/TeleopSwerve.java | 2 +- .../java/frc/robot/commands/ZeroWrist.java | 2 +- .../robot/subsystems/ElevatorSubsystem.java | 29 ++- .../robot/subsystems/ShooterSubsystem.java | 190 +++++++++++------- 13 files changed, 293 insertions(+), 164 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index cfe477b..9504354 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -42,44 +42,85 @@ public class RobotContainer { private final int leftTrigger = XboxController.Axis.kLeftTrigger.value; private final int rightTrigger = XboxController.Axis.kRightTrigger.value; - /* Driver Buttons */ - private final JoystickButton resetOdom = new JoystickButton(driver, XboxController.Button.kStart.value); + /* DRIVER BUTTONS */ + /** Driver B */ private final JoystickButton robotCentric = new JoystickButton(driver, XboxController.Button.kB.value); + /** Driver A */ private final JoystickButton Shoot = new JoystickButton(driver, XboxController.Button.kA.value); + /** Driver Y */ + private final JoystickButton forceShoot = new JoystickButton(driver, XboxController.Button.kY.value); + /** Driver X */ + private final JoystickButton driverX = new JoystickButton(driver, XboxController.Button.kX.value); + /** Driver RB */ private final JoystickButton intex = new JoystickButton(driver, XboxController.Button.kRightBumper.value); + /** Driver LB */ private final JoystickButton outex = new JoystickButton(driver, XboxController.Button.kLeftBumper.value); - private final JoystickButton xButtonDriver = new JoystickButton(driver, XboxController.Button.kX.value); - private final JoystickButton forceShoot = new JoystickButton(driver, XboxController.Button.kY.value); + /** Driver LT */ private final Trigger targetAmp = new Trigger(() -> driver.getRawAxis(leftTrigger) > .5); + /** Driver RT */ private final Trigger targetSpeaker = new Trigger(() -> driver.getRawAxis(rightTrigger) > .5); + /** Driver Up */ + private final Trigger driverUp = new Trigger(() -> driver.getPOV() == 0); + /** Driver Down */ + private final Trigger driverDown = new Trigger(() -> driver.getPOV() == 180); + /** Driver Right */ + private final Trigger driverLeft = new Trigger(() -> driver.getPOV() == 90); + /** Driver Left */ + private final Trigger driverRight = new Trigger(() -> driver.getPOV() == 270); + /** Driver Start */ + private final JoystickButton resetOdom = new JoystickButton(driver, XboxController.Button.kStart.value); - /* Mech Buttons */ - private final JoystickButton zeroShooter = new JoystickButton(mech, XboxController.Button.kBack.value); - private final JoystickButton primeShooterSpeedSpeaker = new JoystickButton(mech, XboxController.Button.kRightBumper.value); - private final JoystickButton primeShooterSpeedAmp = new JoystickButton(mech, XboxController.Button.kLeftBumper.value); - private final JoystickButton shooterTo45 = new JoystickButton(mech, XboxController.Button.kB.value); + /* MECH BUTTONS */ + /** Mech B */ + private final JoystickButton shooterToAntiDefense = new JoystickButton(mech, XboxController.Button.kB.value); + /** Mech A */ private final JoystickButton shooterToIntake = new JoystickButton(mech, XboxController.Button.kA.value); - private final Trigger resetR = new Trigger(() -> mech.getPOV() == 90); - private final Trigger elevatorDown = new Trigger(() -> mech.getPOV() == 180); + /** Mech Y */ + private final JoystickButton shooterToAmp = new JoystickButton(mech, XboxController.Button.kY.value); + /** Mech X */ + private final JoystickButton shooterToSubwoofer = new JoystickButton(mech, XboxController.Button.kX.value); + /** Mech RB */ + private final JoystickButton primeShooterSpeedSpeaker = new JoystickButton(mech, + XboxController.Button.kRightBumper.value); + /** Mech LB */ + private final JoystickButton primeShooterSpeedAmp = new JoystickButton(mech, + XboxController.Button.kLeftBumper.value); + /** Mech RT */ + private final Trigger mechRT = new Trigger(() -> mech.getRawAxis(rightTrigger) > .5); + /** Mech LT */ + private final Trigger mechLT = new Trigger(() -> mech.getRawAxis(leftTrigger) > .5); + /** Mech Up */ private final Trigger elevatorUp = new Trigger(() -> mech.getPOV() == 0); + /** Mech Down */ + private final Trigger elevatorDown = new Trigger(() -> mech.getPOV() == 180); + /** Mech Right */ + private final Trigger resetR = new Trigger(() -> mech.getPOV() == 90); + /** Mech Left */ private final Trigger intakeThroughShooter = new Trigger(() -> mech.getPOV() == 270); + /** Mech Start */ private final JoystickButton autoZeroShooter = new JoystickButton(mech, XboxController.Button.kStart.value); - private final JoystickButton shooterToAmp = new JoystickButton(mech, XboxController.Button.kY.value); - private final JoystickButton xButtonMech = new JoystickButton(mech, XboxController.Button.kX.value); + /** Mech Back */ + private final JoystickButton zeroShooter = new JoystickButton(mech, XboxController.Button.kBack.value); + /** Mech RS */ private final JoystickButton rightStick = new JoystickButton(mech, XboxController.Button.kRightStick.value); - /* - private final Trigger dynamicForward = new Trigger(() -> mech.getPOV() == 90); - private final Trigger dynamicBackward = new Trigger(() -> mech.getPOV() == 270); - private final Trigger quasistaticForward = new Trigger(() -> mech.getPOV() == 0); - private final Trigger quasistaticBackwards = new Trigger(() -> mech.getPOV() == 180); - */ + /* + * private final Trigger dynamicForward = new Trigger(() -> mech.getPOV() == + * 90); + * private final Trigger dynamicBackward = new Trigger(() -> mech.getPOV() == + * 270); + * private final Trigger quasistaticForward = new Trigger(() -> mech.getPOV() == + * 0); + * private final Trigger quasistaticBackwards = new Trigger(() -> mech.getPOV() + * == 180); + */ /* Subsystems */ private final VisionSubsystem m_VisionSubsystem = new VisionSubsystem(); private final SwerveSubsystem m_SwerveSubsystem = new SwerveSubsystem(m_VisionSubsystem); public final ShooterSubsystem m_Shoota = new ShooterSubsystem(m_SwerveSubsystem); - //private final LEDSubsystem m_LEDSubsystem = new LEDSubsystem(m_VisionSubsystem); + // private final LEDSubsystem m_LEDSubsystem = new + // LEDSubsystem(m_VisionSubsystem); private final ElevatorSubsystem m_ElevatorSubsystem = new ElevatorSubsystem(); private final IntexerSubsystem m_IntexerSubsystem = new IntexerSubsystem(); @@ -91,10 +132,14 @@ public class RobotContainer { public RobotContainer() { // Named commands for PathPlanner autos NamedCommands.registerCommand("Intake", new IntexForAutosByAutos(m_IntexerSubsystem)); - NamedCommands.registerCommand("Idle Speed", new InstantCommand(() -> m_Shoota.setShooterVelocity(Constants.Shooter.idleSpeedRPM))); - NamedCommands.registerCommand("Target Speed", new InstantCommand(() -> m_Shoota.setShooterVelocity(FiringSolutionsV3.convertToRPM(m_Shoota.getCalculatedVelocity())))); - NamedCommands.registerCommand("Set Shooter Intake", new InstantCommand(() -> m_IntexerSubsystem.setShooterIntake(.9))); - NamedCommands.registerCommand("Stop Shooter Intake", new InstantCommand(() -> m_IntexerSubsystem.setShooterIntake(0))); + NamedCommands.registerCommand("Idle Speed", + new InstantCommand(() -> m_Shoota.setShooterVelocity(Constants.Shooter.idleSpeedRPM))); + NamedCommands.registerCommand("Target Speed", new InstantCommand( + () -> m_Shoota.setShooterVelocity(FiringSolutionsV3.convertToRPM(m_Shoota.getCalculatedVelocity())))); + NamedCommands.registerCommand("Set Shooter Intake", + new InstantCommand(() -> m_IntexerSubsystem.setShooterIntake(.9))); + NamedCommands.registerCommand("Stop Shooter Intake", + new InstantCommand(() -> m_IntexerSubsystem.setShooterIntake(0))); NamedCommands.registerCommand("Shoot", new FIREEE(m_Shoota, m_IntexerSubsystem)); // TODO auto lock on command @@ -114,7 +159,8 @@ public RobotContainer() { m_ElevatorSubsystem, () -> -mech.getRawAxis(leftVerticalAxis))); - m_Shoota.setDefaultCommand(new ManRizzt(m_Shoota, () -> -mech.getRawAxis(rightVerticalAxis), () -> shooterTo45.getAsBoolean())); + m_Shoota.setDefaultCommand(new ManRizzt(m_Shoota, () -> -mech.getRawAxis(rightVerticalAxis), + () -> shooterToAntiDefense.getAsBoolean())); // m_LEDSubsystem.setAllianceColor(); @@ -142,7 +188,7 @@ private void configureButtonBindings() { // Lock on to speaker targetSpeaker.whileTrue(new MissileLock(m_Shoota, "speaker")); targetAmp.whileTrue(new MissileLock(m_Shoota, "amp")); - + // Shooter targetSpeaker.or(targetAmp).and(Shoot).whileTrue(new FIREEE(m_Shoota, m_IntexerSubsystem)); // Main fire @@ -156,51 +202,65 @@ private void configureButtonBindings() { // Shooter intake forceShoot.whileTrue(new InstantCommand(() -> m_IntexerSubsystem.setShooterIntake(.9))) - .onFalse(new InstantCommand(() -> m_IntexerSubsystem.setShooterIntake(0))); - + .onFalse(new InstantCommand(() -> m_IntexerSubsystem.setShooterIntake(0))); + /* MECH BUTTONS */ - + // Prime for Speaker primeShooterSpeedSpeaker - .whileTrue(new InstantCommand(() -> m_Shoota.setShooterVelocity(FiringSolutionsV3.convertToRPM(m_Shoota.getCalculatedVelocity())))) - .onFalse(new InstantCommand(() -> m_Shoota.setShooterVelocity(Constants.Shooter.idleSpeedRPM))); - + .whileTrue(new InstantCommand(() -> m_Shoota + .setShooterVelocity(FiringSolutionsV3.convertToRPM(m_Shoota.getCalculatedVelocity())))) + .onFalse(new InstantCommand(() -> m_Shoota.setShooterVelocity(Constants.Shooter.idleSpeedRPM))); + // Prime for Amp primeShooterSpeedAmp.whileTrue(new InstantCommand(() -> m_Shoota.setShooterVelocity(3417.8))) - .onFalse(new InstantCommand(() -> m_Shoota.setShooterVelocity(Constants.Shooter.idleSpeedRPM))); - + .onFalse(new InstantCommand(() -> m_Shoota.setShooterVelocity(Constants.Shooter.idleSpeedRPM))); + // Elevator elevatorDown.onTrue(new ElevatorSet(m_ElevatorSubsystem, Constants.Elevator.minHeightMeters)); elevatorUp.onTrue(new ElevatorSet(m_ElevatorSubsystem, Constants.Elevator.maxHeightMeters)); - + // Zero wrist - zeroShooter.onTrue(new InstantCommand(() -> m_Shoota.resetWristEncoders(Constants.Shooter.angleOffsetManual))); // Set encoder to zero - autoZeroShooter.onTrue(new ZeroWrist(m_Shoota).andThen(new RizzLevel(m_Shoota, Constants.Shooter.intakeAngleRadians))); + zeroShooter.onTrue(new InstantCommand(() -> m_Shoota.resetWristEncoders(Constants.Shooter.angleOffsetManual))); // Set + // encoder + // to + // zero + autoZeroShooter + .onTrue(new ZeroWrist(m_Shoota).andThen(new RizzLevel(m_Shoota, Constants.Shooter.intakeAngleRadians))); // Wrist - shooterToIntake.onTrue(new RizzLevel(m_Shoota, Constants.Shooter.intakeAngleRadians)); // Move wrist to intake position - + shooterToIntake.onTrue(new RizzLevel(m_Shoota, Constants.Shooter.intakeAngleRadians)); // Move wrist to intake + // position + // Amp Preset - shooterToAmp.onTrue(new RizzLevel(m_Shoota, -0.48)).onTrue(new ElevatorSet(m_ElevatorSubsystem, Constants.Elevator.maxHeightMeters)); - - //xButton.whileTrue(new InstantCommand(() -> m_Shoota.SetOffsetVelocity(2000))) - //.onFalse(new InstantCommand(() -> m_Shoota.SetShooterVelocity(Constants.Shooter.idleSpeedRPM))); - + shooterToAmp.onTrue(new RizzLevel(m_Shoota, -0.48)) + .onTrue(new ElevatorSet(m_ElevatorSubsystem, Constants.Elevator.maxHeightMeters)); + + // xButton.whileTrue(new InstantCommand(() -> m_Shoota.SetOffsetVelocity(2000))) + // .onFalse(new InstantCommand(() -> + // m_Shoota.SetShooterVelocity(Constants.Shooter.idleSpeedRPM))); + // Reset R - resetR.onTrue(new InstantCommand(() -> FiringSolutionsV3.resetAllR())); // Reset the R calculation incase it gets off - + resetR.onTrue(new InstantCommand(() -> FiringSolutionsV3.resetAllR())); // Reset the R calculation incase it + // gets off + // Kill Shooter - rightStick.onTrue(new InstantCommand(()-> m_Shoota.setShooterVelocity(0))); + rightStick.onTrue(new InstantCommand(() -> m_Shoota.setShooterVelocity(0))); - //Intake Through Shooter + // Intake Through Shooter intakeThroughShooter.whileTrue(new IntakeThroughShooter(m_Shoota, m_IntexerSubsystem, mech)) - .onFalse(new IntakeThroughShooterPart2(m_Shoota, m_IntexerSubsystem, mech)); + .onFalse(new IntakeThroughShooterPart2(m_Shoota, m_IntexerSubsystem, mech)); - // Characterization tests - /*dynamicForward.whileTrue(m_SwerveSubsystem.sysIdDynamic(Direction.kForward)); - dynamicBackward.whileTrue(m_SwerveSubsystem.sysIdDynamic(Direction.kReverse)); - quasistaticForward.whileTrue(m_SwerveSubsystem.sysIdQuasistatic(Direction.kForward)); - quasistaticBackwards.whileTrue(m_SwerveSubsystem.sysIdQuasistatic(Direction.kReverse));*/ + // Characterization tests + /* + * dynamicForward.whileTrue(m_SwerveSubsystem.sysIdDynamic(Direction.kForward)); + * dynamicBackward.whileTrue(m_SwerveSubsystem.sysIdDynamic(Direction.kReverse)) + * ; + * quasistaticForward.whileTrue(m_SwerveSubsystem.sysIdQuasistatic(Direction. + * kForward)); + * quasistaticBackwards.whileTrue(m_SwerveSubsystem.sysIdQuasistatic(Direction. + * kReverse)); + */ } /** diff --git a/src/main/java/frc/robot/commands/ElevationManual.java b/src/main/java/frc/robot/commands/ElevationManual.java index 4ae7d4c..376bf3e 100644 --- a/src/main/java/frc/robot/commands/ElevationManual.java +++ b/src/main/java/frc/robot/commands/ElevationManual.java @@ -16,6 +16,7 @@ public class ElevationManual extends Command { DoubleSupplier axis; Boolean locked = false; double lockedValue = 0.0; + double lastElevatorSetpoint = 0.0; public ElevationManual(ElevatorSubsystem elevate, DoubleSupplier control) { m_elevatorSubsystem = elevate; @@ -31,16 +32,19 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { + lastElevatorSetpoint = m_elevatorSubsystem.getSetpoint(); + double value = axis.getAsDouble(); value = Math.pow(value, 3); if (Math.abs(value) > .05) { // Crime zone - m_elevatorSubsystem.setManualOverride(true); m_elevatorSubsystem.ManSpin(value); } else { - m_elevatorSubsystem.setManualOverride(false); - lockedValue = m_elevatorSubsystem.getEncoderValue(); + if (!m_elevatorSubsystem.locked){ + lastElevatorSetpoint = m_elevatorSubsystem.getPosition(); + m_elevatorSubsystem.setPositionWithEncoder(lastElevatorSetpoint); + } } SmartDashboard.putBoolean("locked", locked); diff --git a/src/main/java/frc/robot/commands/ElevatorSet.java b/src/main/java/frc/robot/commands/ElevatorSet.java index bae921b..030d497 100644 --- a/src/main/java/frc/robot/commands/ElevatorSet.java +++ b/src/main/java/frc/robot/commands/ElevatorSet.java @@ -33,7 +33,6 @@ public void execute() { // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - m_elevator.stopHere(); } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/commands/FIREEE.java b/src/main/java/frc/robot/commands/FIREEE.java index 24fe217..0d30b5c 100644 --- a/src/main/java/frc/robot/commands/FIREEE.java +++ b/src/main/java/frc/robot/commands/FIREEE.java @@ -27,7 +27,7 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if (shooter.shooterAtSpeed()){ + if (shooter.isShooterAtSpeed()){ intexer.setShooterIntake(.9); } } diff --git a/src/main/java/frc/robot/commands/IntakeThroughShooter.java b/src/main/java/frc/robot/commands/IntakeThroughShooter.java index 5dab49c..690f329 100644 --- a/src/main/java/frc/robot/commands/IntakeThroughShooter.java +++ b/src/main/java/frc/robot/commands/IntakeThroughShooter.java @@ -36,14 +36,14 @@ public void initialize() { public void execute() { shooter.setShooterVelocity(-1000); intexer.setALL(-.5); - shooter.setWristPosition(.66); + shooter.setWristByAngle(.66); } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { shooter.setShooterVelocity(Constants.Shooter.idleSpeedRPM); - shooter.setWristPosition(Constants.Shooter.intakeAngleRadians); + shooter.setWristByAngle(Constants.Shooter.intakeAngleRadians); intexer.setALL(0); } diff --git a/src/main/java/frc/robot/commands/IntakeThroughShooterPart2.java b/src/main/java/frc/robot/commands/IntakeThroughShooterPart2.java index 0d7f116..ab6043a 100644 --- a/src/main/java/frc/robot/commands/IntakeThroughShooterPart2.java +++ b/src/main/java/frc/robot/commands/IntakeThroughShooterPart2.java @@ -6,6 +6,7 @@ import edu.wpi.first.wpilibj.GenericHID.RumbleType; import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants; import frc.robot.subsystems.IntexerSubsystem; @@ -17,6 +18,7 @@ public class IntakeThroughShooterPart2 extends Command { private boolean finishPlease = false; Joystick controller; + public final Timer timer = new Timer(); /** Creates a new IntakeFromShooterPart2. */ public IntakeThroughShooterPart2(ShooterSubsystem shooterSub, IntexerSubsystem intex, Joystick controller) { @@ -30,6 +32,8 @@ public IntakeThroughShooterPart2(ShooterSubsystem shooterSub, IntexerSubsystem i // Called when the command is initially scheduled. @Override public void initialize() { + timer.reset(); + timer.start(); controller.setRumble(RumbleType.kBothRumble, 0); } @@ -38,7 +42,7 @@ public void initialize() { public void execute() { shooter.setShooterVelocity(0); intexer.setALL(.35); - shooter.setWristPosition(Constants.Shooter.intakeAngleRadians); + shooter.setWristByAngle(Constants.Shooter.intakeAngleRadians); } // Called once the command ends or is interrupted. @@ -52,7 +56,7 @@ public void end(boolean interrupted) { // Returns true when the command should end. @Override public boolean isFinished() { - if (!intexer.intakeThroughShooterPart2isReady || intexer.shooterBreak()) { + if (!intexer.intakeThroughShooterPart2isReady || intexer.shooterBreak() || timer.get() > 1.5) { return true; } else { return false; diff --git a/src/main/java/frc/robot/commands/ManRizzt.java b/src/main/java/frc/robot/commands/ManRizzt.java index 2e6d3ea..3705c6c 100644 --- a/src/main/java/frc/robot/commands/ManRizzt.java +++ b/src/main/java/frc/robot/commands/ManRizzt.java @@ -13,17 +13,19 @@ public class ManRizzt extends Command { - ShooterSubsystem shooterSubsystem; + ShooterSubsystem m_shooterSubsystem; private DoubleSupplier speed; BooleanSupplier setAngle; + double lastWristSetpoint = 0.0; + boolean wristIsLocked = false; - public ManRizzt(ShooterSubsystem subsystem, DoubleSupplier speed, BooleanSupplier setAngle) { + public ManRizzt(ShooterSubsystem shooterSubsystem, DoubleSupplier speed, BooleanSupplier setAngle) { // Use addRequirements() here to declare subsystem dependencies. - shooterSubsystem = subsystem; + m_shooterSubsystem = shooterSubsystem; this.setAngle = setAngle; this.speed = speed; SmartDashboard.putNumber("Set Wrist Angle", 0); - addRequirements(shooterSubsystem); + addRequirements(m_shooterSubsystem); } // Called when the command is initially scheduled. @@ -33,13 +35,24 @@ public void initialize() { @Override public void execute() { - double speedValue = speed.getAsDouble() * 0.5; - - speedValue = Math.pow(speed.getAsDouble(), 3); - if (setAngle.getAsBoolean()){ - shooterSubsystem.setWristPosition(SmartDashboard.getNumber("Set Wrist Angle", 0)); + double speedValue = Math.pow(speed.getAsDouble(), 3); + + if (setAngle.getAsBoolean()) { + m_shooterSubsystem.setWristByAngle(SmartDashboard.getNumber("Set Wrist Angle", 0)); } else { - shooterSubsystem.manualWristSpeed(speedValue); + if (Math.abs(speedValue) > .05) { + wristIsLocked = false; + m_shooterSubsystem.setManualWristSpeed(speedValue); + } else { + if (m_shooterSubsystem.isZeroed){ + if (!wristIsLocked){ + m_shooterSubsystem.setWristByAngle(m_shooterSubsystem.getCurrentShooterAngle()); + wristIsLocked = true; + } + } else { + m_shooterSubsystem.setManualWristSpeed(0); + } + } } } @@ -50,6 +63,6 @@ public boolean isFinished() { @Override public void end(boolean interrupted) { - shooterSubsystem.manualWristSpeed(0); + m_shooterSubsystem.setManualWristSpeed(0); } } diff --git a/src/main/java/frc/robot/commands/MissileLock.java b/src/main/java/frc/robot/commands/MissileLock.java index 9839f15..9d22a1f 100644 --- a/src/main/java/frc/robot/commands/MissileLock.java +++ b/src/main/java/frc/robot/commands/MissileLock.java @@ -43,7 +43,7 @@ public void execute() { @Override public void end(boolean interrupted) { shooter.setShooterVelocity(Constants.Shooter.idleSpeedRPM); - shooter.setWristPosition(Constants.Shooter.intakeAngleRadians); + shooter.setWristByAngle(Constants.Shooter.intakeAngleRadians); } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/commands/RizzLevel.java b/src/main/java/frc/robot/commands/RizzLevel.java index 02eead3..3f75603 100644 --- a/src/main/java/frc/robot/commands/RizzLevel.java +++ b/src/main/java/frc/robot/commands/RizzLevel.java @@ -30,7 +30,7 @@ public void initialize() { @Override public void execute() { if (shooter.isZeroed){ - shooter.setWristPosition(angle); + shooter.setWristByAngle(angle); } } @@ -44,7 +44,7 @@ public void end(boolean interrupted) { @Override public boolean isFinished() { if (shooter.isZeroed){ - return shooter.getAngle() >= (angle - Math.toRadians(.5)) && shooter.getAngle() < (angle + Math.toRadians(.5)); + return shooter.getCurrentShooterAngle() >= (angle - Math.toRadians(.5)) && shooter.getCurrentShooterAngle() < (angle + Math.toRadians(.5)); } else { return true; } diff --git a/src/main/java/frc/robot/commands/TeleopSwerve.java b/src/main/java/frc/robot/commands/TeleopSwerve.java index d78ab6b..c49ea22 100644 --- a/src/main/java/frc/robot/commands/TeleopSwerve.java +++ b/src/main/java/frc/robot/commands/TeleopSwerve.java @@ -75,7 +75,7 @@ public void execute() { strafeVal = Math.copySign(Math.pow(strafeVal, 2), strafeVal); if (shooterOverrideSpeaker.getAsBoolean()) { // Lock robot angle to speaker - if (shooterSubsystem.getDistanceToSpeaker() >= 3.5){ + if (shooterSubsystem.getDistanceToSpeakerWhileMoving() >= 3.5){ controller.setRumble(RumbleType.kBothRumble, 0.5); } else { controller.setRumble(RumbleType.kBothRumble, 0); diff --git a/src/main/java/frc/robot/commands/ZeroWrist.java b/src/main/java/frc/robot/commands/ZeroWrist.java index 5e70ce4..0679637 100644 --- a/src/main/java/frc/robot/commands/ZeroWrist.java +++ b/src/main/java/frc/robot/commands/ZeroWrist.java @@ -30,7 +30,7 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - shooter.manualWristSpeed(.45); + shooter.setManualWristSpeed(.45); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index 62563fa..2a6e909 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -8,6 +8,7 @@ import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.Slot1Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.controls.PositionDutyCycle; import com.ctre.phoenix6.hardware.TalonFX; @@ -55,6 +56,10 @@ public ElevatorSubsystem() { m_elevatorLeft.setNeutralMode(NeutralModeValue.Brake); m_elevatorRight.setControl(new Follower(m_elevatorLeft.getDeviceID(), true)); + TalonFXConfiguration elevatorConfigs = new TalonFXConfiguration(); + elevatorConfigs.Slot0.kP = 0.09; + elevatorConfigs.MotorOutput.NeutralMode = NeutralModeValue.Brake; + // PID Slot0Configs encoderConfigSlot0 = new Slot0Configs(); ClosedLoopRampsConfigs closedloop = new ClosedLoopRampsConfigs(); @@ -72,7 +77,6 @@ public ElevatorSubsystem() { encoderConfigSlot1.kD = .0; encoderConfigSlot1.kV = .01; - m_elevatorLeft.getConfigurator().apply(encoderConfigSlot0, 0.050); m_elevatorLeft.getConfigurator().apply(closedloop); @@ -110,15 +114,6 @@ public void periodic() { m_elevatorRight.getSupplyCurrent().getValueAsDouble()); SmartDashboard.putNumber("LaserCan Ambient", measurement.ambient); revolutionCount = m_elevatorLeft.getPosition().getValueAsDouble(); - - if (!manualOverride){ // Flagitious logic - if (!locked){ - stopHere(); - locked = true; - } - } else { - locked = false; - } //FiringSolutionsV3.updateHeight(getHeight()); //TODO: test this } @@ -131,11 +126,13 @@ public double getEncoderValue(){ return revolutionCount; } - public void setToEncoder(double value){ + public void setPositionWithEncoder(double value){ + locked = true; m_elevatorLeft.setControl(lockPosition.withPosition(value)); } public void stopHere(){ + locked = true; m_elevatorLeft.setControl(lockPosition.withPosition(revolutionCount).withSlot(0)); } @@ -146,6 +143,7 @@ public double getHeightEncoder() { /** Set height IN METERS. Will run off LaserCan but will switch to encoder if it fails */ public void setHeight(double height) { + locked = true; setHeight = height; if (!lasercanFailureCheck()) { // Run off LaserCan m_elevatorLeft.set(elevatorPID.calculate(getHeightLaserCan(), height)); @@ -171,6 +169,7 @@ public boolean atHeight() { } public void ManSpin(double percent) { + locked = false; m_elevatorLeft.set(percent); } @@ -204,4 +203,12 @@ public boolean lasercanFailureCheck() { } } + public double getSetpoint(){ + return m_elevatorLeft.getClosedLoopReference().getValue(); + } + + public double getPosition(){ + return m_elevatorLeft.getPosition().getValueAsDouble(); + } + } diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 5a38ac1..927468b 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -53,11 +53,14 @@ public class ShooterSubsystem extends SubsystemBase { private double shooterAngleToSpeaker, shooterAngleToAmp; private Boolean ENCFAIL = false; public boolean isZeroed = false; + public boolean wristIsLocked = false; private double angleOffset = 68.2; // IN RADIANS private final Timer speedTimer = new Timer(); private final int m_WristCurrentMax = 84; private final Interpolations interpolation = new Interpolations(); private double distanceToMovingSpeakerTarget = .94; + public double lastWristAngleSetpoint = 0.0; + public boolean manualOverride = false; private SwerveSubsystem swerveSubsystem; @@ -95,7 +98,7 @@ public ShooterSubsystem(SwerveSubsystem swerve) { m_Wrist.burnFlash(); shootaBot.burnFlash(); shootaTop.burnFlash(); - + m_pidWrist = new PIDController(positionP, positionI, positionD); m_VelocityEncoder.setMeasurementPeriod(20); @@ -118,29 +121,31 @@ public ShooterSubsystem(SwerveSubsystem swerve) { @Override public void periodic() { // This method will be called once per scheduler run -/* - pidSpdP = SmartDashboard.getNumber("Velo P", pidSpdP); - pidSpdI = SmartDashboard.getNumber("Velo I", pidSpdI); - pidSpdD = SmartDashboard.getNumber("Velo D", pidSpdD); -Minor whoopsie if these guys were causing loop overruns - botPID.setP(pidSpdP, 0); - botPID.setI(pidSpdI, 0); - botPID.setD(pidSpdD, 0); - - topPID.setP(pidSpdP, 0); - topPID.setI(pidSpdI, 0); - topPID.setD(pidSpdD, 0); -*/ + /* + * pidSpdP = SmartDashboard.getNumber("Velo P", pidSpdP); + * pidSpdI = SmartDashboard.getNumber("Velo I", pidSpdI); + * pidSpdD = SmartDashboard.getNumber("Velo D", pidSpdD); + * Minor whoopsie if these guys were causing loop overruns + * botPID.setP(pidSpdP, 0); + * botPID.setI(pidSpdI, 0); + * botPID.setD(pidSpdD, 0); + * + * topPID.setP(pidSpdP, 0); + * topPID.setI(pidSpdI, 0); + * topPID.setD(pidSpdD, 0); + */ + shooterVelocity = SmartDashboard.getNumber("set velocity", shooterVelocity); FiringSolutionsV3.slipPercent = SmartDashboard.getNumber("Set Slip Offset", FiringSolutionsV3.slipPercent); FiringSolutionsV3.speakerTargetZ = SmartDashboard.getNumber("Set Target Z", FiringSolutionsV3.speakerTargetZ); - SmartDashboard.putNumber("Top - Bottom error", m_VelocityEncoder.getVelocity() - m_VelocityEncoder2.getVelocity()); - SmartDashboard.putNumber("Current Angle Radians", getAngle()); + SmartDashboard.putNumber("Top - Bottom error", + m_VelocityEncoder.getVelocity() - m_VelocityEncoder2.getVelocity()); + SmartDashboard.putNumber("Current Angle Radians", getCurrentShooterAngle()); SmartDashboard.putNumber("Current Velocity", getVelocity()); - SmartDashboard.putBoolean("shooter at speed", shooterAtSpeed()); - SmartDashboard.putNumber("Current Angle Degrees", Units.radiansToDegrees(getAngle())); + SmartDashboard.putBoolean("shooter at speed", isShooterAtSpeed()); + SmartDashboard.putNumber("Current Angle Degrees", Units.radiansToDegrees(getCurrentShooterAngle())); // check for encoder failure if (m_WristEncoder.isConnected()) { @@ -156,25 +161,53 @@ public void periodic() { SmartDashboard.putNumber("Flywheel Right Current", shootaBot.getOutputCurrent()); SmartDashboard.putNumber("Wrist Current", m_Wrist.getOutputCurrent()); SmartDashboard.putBoolean("is Wrist Stalled", isWristMotorStalled()); + + if (isZeroed && !manualOverride){ + m_Wrist.set(m_pidWrist.calculate(getCurrentShooterAngle(), lastWristAngleSetpoint)); + } } - /** Check if wrist motor is exceeding stall current, used for zeroing */ - public boolean isWristMotorStalled() { - if (m_Wrist.getOutputCurrent() > m_WristCurrentMax) { - return true; + /** in RADIANs units MATTER */ + public double getCurrentShooterAngle() { + if (!ENCFAIL) { + return ((-m_WristEncoder.get() * 2 * Math.PI) / 4) + angleOffset; } else { - return false; + return ((m_PositionEncoder.getPosition() * 2 * Math.PI) / 100); } } - /** Reset wrist encoder to given value */ - public void setWristEncoderPosition(double newPosition) { - m_WristEncoder.setPositionOffset(newPosition); + public double getCalculatedAngleToAmp() { + return shooterAngleToAmp; } - /** Get whether shooter is at target speed */ - public boolean shooterAtSpeed() { // Copied from Hudson but made it better + public double getCalculatedAngleToSpeaker() { + return shooterAngleToSpeaker; + } + + public double getCalculatedVelocity() { + return shooterVelocity; + } + + public double getDistanceTo(double x, double y) { + return FiringSolutionsV3.getDistanceToTarget(swerveSubsystem.getPose().getX(), swerveSubsystem.getPose().getY(), + x, y); + } + + public double getDistanceToSpeakerWhileMoving() { + return distanceToMovingSpeakerTarget; + } + + /** Shooter velocity in RPM */ + public double getVelocity() { + return m_VelocityEncoder.getVelocity(); + } + public double getWristRotations() { + return m_PositionEncoder.getPosition(); + } + + /** Get whether shooter is at target speed */ + public boolean isShooterAtSpeed() { // Copied from Hudson but made it better // if error less than certain amount start the timer if (Math.abs(getVelocity() - FiringSolutionsV3.convertToRPM(shooterVelocity)) < 50) { speedTimer.start(); @@ -190,20 +223,21 @@ public boolean shooterAtSpeed() { // Copied from Hudson but made it better } } - /** Shooter velocity in RPM */ - public double getVelocity() { - return m_VelocityEncoder.getVelocity(); - } - - /** in RADIANs units MATTER */ - public double getAngle() { - if (!ENCFAIL) { - return ((-m_WristEncoder.get() * 2 * Math.PI) / 4) + angleOffset; + /** Check if wrist motor is exceeding stall current, used for zeroing */ + public boolean isWristMotorStalled() { + if (m_Wrist.getOutputCurrent() > m_WristCurrentMax) { + return true; } else { - return ((m_PositionEncoder.getPosition() * 2 * Math.PI) / 100); + return false; } } + public void PointShoot(double PointAngle, double launchVelocity) { + setWristByAngle(PointAngle); + botPID.setReference(launchVelocity, CANSparkMax.ControlType.kVelocity); + topPID.setReference(launchVelocity, CANSparkMax.ControlType.kVelocity); + } + public void resetWristEncoders(double newOffset) { angleOffset = newOffset; m_WristEncoder.reset(); @@ -211,87 +245,95 @@ public void resetWristEncoders(double newOffset) { isZeroed = true; } - /** IN RADIANS */ - public void setWristPosition(double angle) { - if (isZeroed){ - m_Wrist.set(m_pidWrist.calculate(getAngle(), angle)); - } + public void setManualWristSpeed(double speed) { + m_Wrist.set(speed); + } + + public void setManualOverride(boolean value){ + manualOverride = value; } /** In rotations per minute */ - public void setShooterVelocity(double velocity) { + public void setOffsetVelocity(double velocity) { if (velocity == 0) { shootaBot.stopMotor(); shootaTop.stopMotor(); } else { - botPID.setReference(velocity, CANSparkMax.ControlType.kVelocity); - topPID.setReference(velocity, CANSparkMax.ControlType.kVelocity); + botPID.setReference(velocity - SmartDashboard.getNumber("Top Bottom Offset", 400), + CANSparkMax.ControlType.kVelocity); + topPID.setReference(velocity + SmartDashboard.getNumber("Top Bottom Offset", 400), + CANSparkMax.ControlType.kVelocity); } } /** In rotations per minute */ - public void SetOffsetVelocity(double velocity) { + public void setShooterVelocity(double velocity) { if (velocity == 0) { shootaBot.stopMotor(); shootaTop.stopMotor(); } else { - botPID.setReference(velocity - SmartDashboard.getNumber("Top Bottom Offset", 400), CANSparkMax.ControlType.kVelocity); - topPID.setReference(velocity + SmartDashboard.getNumber("Top Bottom Offset", 400), CANSparkMax.ControlType.kVelocity); + botPID.setReference(velocity, CANSparkMax.ControlType.kVelocity); + topPID.setReference(velocity, CANSparkMax.ControlType.kVelocity); } } - public void PointShoot(double PointAngle, double launchVelocity) { - setWristPosition(PointAngle); - botPID.setReference(launchVelocity, CANSparkMax.ControlType.kVelocity); - topPID.setReference(launchVelocity, CANSparkMax.ControlType.kVelocity); - } - - public void manualWristSpeed(double speed) { - m_Wrist.set(speed); - } - - public double getCalculatedVelocity() { - return shooterVelocity; + /** Reset wrist encoder to given value */ + public void setWristEncoderOffset(double newPosition) { + m_WristEncoder.setPositionOffset(newPosition); } - public double getCalculatedAngleToAmp() { - return shooterAngleToAmp; + /** IN RADIANS */ + public void setWristByAngle(double angle) { //TODO: Implement redundancy + if (isZeroed) { + updateWristAngleSetpoint(angle); + } } - public double getCalculatedAngleToSpeaker() { - return shooterAngleToSpeaker; + /** IN ROTATIONS */ + public void setWristByRotations(double newPosition) { + if (isZeroed){ + m_Wrist.set(m_pidWrist.calculate(getWristRotations(), newPosition)); + } } - public double getDistanceToSpeaker () { - return distanceToMovingSpeakerTarget; + public void updateWristAngleSetpoint(double angle) { + if (angle != lastWristAngleSetpoint){ + lastWristAngleSetpoint = angle; + } } public void updateShooterMath() { // Shooter Math Pose2d pose = swerveSubsystem.getPose(); ChassisSpeeds chassisSpeeds = swerveSubsystem.getChassisSpeeds(); - distanceToMovingSpeakerTarget = FiringSolutionsV3.getDistanceToMovingTarget(pose.getX(), pose.getY(), FiringSolutionsV3.speakerTargetX, FiringSolutionsV3.speakerTargetY, + distanceToMovingSpeakerTarget = FiringSolutionsV3.getDistanceToMovingTarget(pose.getX(), pose.getY(), + FiringSolutionsV3.speakerTargetX, FiringSolutionsV3.speakerTargetY, chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond, pose.getRotation().getRadians()); FiringSolutionsV3.updateSpeakerR(distanceToMovingSpeakerTarget); - //shooterAngleToSpeaker = FiringSolutionsV3.getShooterAngleFromSpeakerR(); - shooterAngleToSpeaker = Math.toRadians(interpolation.getShooterAngleFromInterpolation(distanceToMovingSpeakerTarget)); + // shooterAngleToSpeaker = FiringSolutionsV3.getShooterAngleFromSpeakerR(); + shooterAngleToSpeaker = Math + .toRadians(interpolation.getShooterAngleFromInterpolation(distanceToMovingSpeakerTarget)); SmartDashboard.putNumber("Target Velocity RPM", FiringSolutionsV3.convertToRPM(shooterVelocity)); SmartDashboard.putNumber("Calculated Angle Radians", shooterAngleToSpeaker); SmartDashboard.putNumber("Calculated Angle Degrees", Math.toDegrees(shooterAngleToSpeaker)); - SmartDashboard.putNumber("distance", FiringSolutionsV3.getDistanceToTarget(pose.getX(), pose.getY(), FiringSolutionsV3.speakerTargetX, FiringSolutionsV3.speakerTargetY)); + SmartDashboard.putNumber("distance", FiringSolutionsV3.getDistanceToTarget(pose.getX(), pose.getY(), + FiringSolutionsV3.speakerTargetX, FiringSolutionsV3.speakerTargetY)); SmartDashboard.putNumber("R", FiringSolutionsV3.getSpeakerR()); SmartDashboard.putNumber("C", FiringSolutionsV3.C(distanceToMovingSpeakerTarget)); - SmartDashboard.putNumber("quarticA", FiringSolutionsV3.quarticA(distanceToMovingSpeakerTarget, FiringSolutionsV3.speakerTargetZ)); - SmartDashboard.putNumber("quarticC", FiringSolutionsV3.quarticC(distanceToMovingSpeakerTarget, FiringSolutionsV3.speakerTargetZ)); + SmartDashboard.putNumber("quarticA", + FiringSolutionsV3.quarticA(distanceToMovingSpeakerTarget, FiringSolutionsV3.speakerTargetZ)); + SmartDashboard.putNumber("quarticC", + FiringSolutionsV3.quarticC(distanceToMovingSpeakerTarget, FiringSolutionsV3.speakerTargetZ)); SmartDashboard.putNumber("quarticE", FiringSolutionsV3.quarticE(distanceToMovingSpeakerTarget)); SmartDashboard.putNumber("Distance to Moving Target", distanceToMovingSpeakerTarget); SmartDashboard.putNumber("robot vx", chassisSpeeds.vxMetersPerSecond); SmartDashboard.putNumber("robot vy", chassisSpeeds.vyMetersPerSecond); - double distanceToMovingAmpTarget = FiringSolutionsV3.getDistanceToMovingTarget(pose.getX(), pose.getY(), FiringSolutionsV3.ampTargetX, FiringSolutionsV3.ampTargetY, + double distanceToMovingAmpTarget = FiringSolutionsV3.getDistanceToMovingTarget(pose.getX(), pose.getY(), + FiringSolutionsV3.ampTargetX, FiringSolutionsV3.ampTargetY, chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond, pose.getRotation().getRadians()); FiringSolutionsV3.updateAmpR(distanceToMovingAmpTarget); From a7769b5370d7bfca6f736d68d50e2e0c91384940 Mon Sep 17 00:00:00 2001 From: Andrew-K961 Date: Wed, 6 Mar 2024 15:40:15 -0600 Subject: [PATCH 13/49] me praying 4th robot solves all the problems --- .../autos/{4N-1M-left.auto => kachow.auto} | 18 +++++++++--------- .../deploy/pathplanner/paths/toMidAmpLeft.path | 16 ++++++++-------- src/main/java/frc/robot/Constants.java | 3 +++ .../frc/robot/subsystems/SwerveSubsystem.java | 2 +- .../frc/robot/subsystems/VisionSubsystem.java | 11 +++++++---- 5 files changed, 28 insertions(+), 22 deletions(-) rename src/main/deploy/pathplanner/autos/{4N-1M-left.auto => kachow.auto} (82%) diff --git a/src/main/deploy/pathplanner/autos/4N-1M-left.auto b/src/main/deploy/pathplanner/autos/kachow.auto similarity index 82% rename from src/main/deploy/pathplanner/autos/4N-1M-left.auto rename to src/main/deploy/pathplanner/autos/kachow.auto index 9732d7d..aefbd8d 100644 --- a/src/main/deploy/pathplanner/autos/4N-1M-left.auto +++ b/src/main/deploy/pathplanner/autos/kachow.auto @@ -2,8 +2,8 @@ "version": 1.0, "startingPose": { "position": { - "x": 1.2991794154609702, - "y": 6.971077125052112 + "x": 1.45, + "y": 6.45 }, "rotation": 0 }, @@ -14,7 +14,7 @@ { "type": "named", "data": { - "name": "someShooterCode" + "name": "Shoot" } }, { @@ -26,7 +26,7 @@ { "type": "named", "data": { - "name": "someIntakeCode" + "name": "Note Sniffer" } }, { @@ -38,7 +38,7 @@ { "type": "named", "data": { - "name": "someShooterCode" + "name": "Shoot" } }, { @@ -50,7 +50,7 @@ { "type": "named", "data": { - "name": "someIntakeCode" + "name": "Note Sniffer2" } }, { @@ -62,7 +62,7 @@ { "type": "named", "data": { - "name": "someShooterCode" + "name": "Shoot" } }, { @@ -74,7 +74,7 @@ { "type": "named", "data": { - "name": "someIntakeCode" + "name": "Note Sniffer" } }, { @@ -86,7 +86,7 @@ { "type": "named", "data": { - "name": "someShooterCode" + "name": "Shoot" } } ] diff --git a/src/main/deploy/pathplanner/paths/toMidAmpLeft.path b/src/main/deploy/pathplanner/paths/toMidAmpLeft.path index 5eab28e..e40f009 100644 --- a/src/main/deploy/pathplanner/paths/toMidAmpLeft.path +++ b/src/main/deploy/pathplanner/paths/toMidAmpLeft.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 0.9774373943047189, - "y": 7.068574707220673 + "x": 1.45, + "y": 6.5 }, "prevControl": null, "nextControl": { - "x": 1.9774373943047188, - "y": 6.568574707220673 + "x": 2.7323938733388164, + "y": 6.249595017004761 }, "isLocked": false, "linkedName": null @@ -20,12 +20,12 @@ "y": 6.23009550057105 }, "prevControl": { - "x": 3.594198443848425, - "y": 5.7828819050710925 + "x": 3.4538759813861684, + "y": 6.132597918402489 }, "nextControl": { - "x": 5.871816019166481, - "y": 6.5420877635104455 + "x": 5.920317158304561, + "y": 6.294863785305015 }, "isLocked": false, "linkedName": null diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 49aeea2..61bf2ac 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -11,6 +11,7 @@ import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.Vector; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; @@ -45,6 +46,8 @@ public static class Vision { public static final Matrix kSingleTagStdDevs = VecBuilder.fill(8, 8, 40); public static final Matrix kMultiTagStdDevs = VecBuilder.fill(4, 4, 20); public static final Vector stateStdDevs = VecBuilder.fill(1, 1, 1); // Encoder Odometry + + public static final Pose2d startingPose = new Pose2d(1.35, 5.55, new Rotation2d(0)); } private static AprilTagFieldLayout errorWrapper() { diff --git a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java index 2ad1605..51e9e11 100644 --- a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java @@ -125,7 +125,7 @@ public SwerveSubsystem(VisionSubsystem vision) { Constants.Swerve.swerveKinematics, getGyroYaw(), getModulePositions(), - new Pose2d(1.35, 5.55, new Rotation2d(0)), + Constants.Vision.startingPose, Constants.Vision.stateStdDevs, Constants.Vision.kSingleTagStdDevs); diff --git a/src/main/java/frc/robot/subsystems/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/VisionSubsystem.java index 71d1f96..65b8c35 100644 --- a/src/main/java/frc/robot/subsystems/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/VisionSubsystem.java @@ -58,12 +58,15 @@ public VisionSubsystem() { Constants.Vision.kTagLayout.setOrigin(OriginPosition.kBlueAllianceWallRightSide); - photonEstimatorFront = new PhotonPoseEstimator( //TODO Fix multi tag - Constants.Vision.kTagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, aprilTagCameraFront, Constants.Vision.kRobotToCamFront); + photonEstimatorFront = new PhotonPoseEstimator( + Constants.Vision.kTagLayout, PoseStrategy.CLOSEST_TO_LAST_POSE, aprilTagCameraFront, Constants.Vision.kRobotToCamFront); photonEstimatorBack = new PhotonPoseEstimator( - Constants.Vision.kTagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, aprilTagCameraBack, Constants.Vision.kRobotToCamBack); + Constants.Vision.kTagLayout, PoseStrategy.CLOSEST_TO_LAST_POSE, aprilTagCameraBack, Constants.Vision.kRobotToCamBack); + photonEstimatorFront.setLastPose(Constants.Vision.startingPose); + photonEstimatorBack.setLastPose(Constants.Vision.startingPose); + photonEstimatorFront.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); photonEstimatorBack.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); } @@ -134,7 +137,7 @@ public Matrix getEstimationStdDevsFront(Pose2d estimatedPose) { if (numTags > 1) estStdDevs = Constants.Vision.kMultiTagStdDevs; // Increase std devs based on (average) distance - if (numTags == 1 && avgDist > 4) + if (/*numTags == 1 && */avgDist > 4) estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE); else estStdDevs = estStdDevs.times(1 + (avgDist * avgDist / 30)); From 15cc545c633cd78065eeae179f68ed0634491a82 Mon Sep 17 00:00:00 2001 From: Mr-A768 Date: Fri, 8 Mar 2024 15:23:44 -0600 Subject: [PATCH 14/49] comp robot "done" "tuesday" --- .../robot/subsystems/ShooterSubsystem.java | 46 +++++++++++++++++-- 1 file changed, 43 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 0028e64..f3b2c27 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -60,8 +60,12 @@ public class ShooterSubsystem extends SubsystemBase { private double distanceToMovingSpeakerTarget = .94; public double lastWristAngleSetpoint = 0.0; public boolean manualOverride = false; + public double wristAngleUpperBound; + public double wristAngleLowerBound; // Constants + private final double wristAngleMax = 0.0; + private final double wristAngleMin = 0.0; private final Timer speedTimer = new Timer(); private final Interpolations interpolation = new Interpolations(); private final int m_WristCurrentMax = 84; @@ -166,9 +170,19 @@ public void periodic() { SmartDashboard.putNumber("Wrist Current", m_Wrist.getOutputCurrent()); SmartDashboard.putBoolean("is Wrist Stalled", isWristMotorStalled()); - if (isZeroed && !manualOverride){ - m_Wrist.set(m_pidWrist.calculate(getCurrentShooterAngle(), lastWristAngleSetpoint)); + if (isZeroed){ + if (!manualOverride){ + m_Wrist.set(m_pidWrist.calculate(getCurrentShooterAngle(), lastWristAngleSetpoint)); + } + + //Implement whenever build stops throwing + /*if (getCurrentShooterAngle() > wristAngleUpperBound){ + m_Wrist.set(m_pidWrist.calculate(getCurrentShooterAngle(), wristAngleUpperBound)); + } else if (getCurrentShooterAngle() < wristAngleLowerBound){ + m_Wrist.set(m_pidWrist.calculate(getCurrentShooterAngle(), wristAngleLowerBound)); + }*/ } + } /** in RADIANs units MATTER */ @@ -286,6 +300,26 @@ public void setWristEncoderOffset(double newPosition) { m_WristEncoder.setPositionOffset(newPosition); } + public void setWristAngleLowerBound(double wristAngleLowerBound) { + if (wristAngleLowerBound < wristAngleMin){ + wristAngleLowerBound = wristAngleMin; + } else if (wristAngleLowerBound > wristAngleUpperBound){ + wristAngleLowerBound = wristAngleUpperBound; + } else { + this.wristAngleLowerBound = wristAngleLowerBound; + } + } + + public void setWristAngleUpperBound(double wristAngleUpperBound) { + if (wristAngleUpperBound > wristAngleMax){ + wristAngleUpperBound = wristAngleMax; + } else if (wristAngleUpperBound < wristAngleLowerBound){ + wristAngleUpperBound = wristAngleLowerBound; + } else { + this.wristAngleUpperBound = wristAngleUpperBound; + } + } + /** IN RADIANS */ public void setWristByAngle(double angle) { //TODO: Implement redundancy if (isZeroed) { @@ -302,7 +336,13 @@ public void setWristByRotations(double newPosition) { public void updateWristAngleSetpoint(double angle) { if (angle != lastWristAngleSetpoint){ - lastWristAngleSetpoint = angle; + if (lastWristAngleSetpoint < wristAngleLowerBound){ + lastWristAngleSetpoint = wristAngleLowerBound; + } else if (lastWristAngleSetpoint > wristAngleUpperBound){ + lastWristAngleSetpoint = wristAngleUpperBound; + } else { + lastWristAngleSetpoint = angle; + } } } From f7b9db4e4e7e1ade432c7b3a6137f7083c2b9bbb Mon Sep 17 00:00:00 2001 From: Andrew-K961 Date: Fri, 8 Mar 2024 15:56:32 -0600 Subject: [PATCH 15/49] =?UTF-8?q?=C2=A1=C2=A1=C2=A1=20blunder=20alert=20!!?= =?UTF-8?q?!?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .gitignore | 4 + README.md | 7 +- src/main/java/frc/robot/Constants.java | 18 +-- src/main/java/frc/robot/Robot.java | 13 +- src/main/java/frc/robot/RobotContainer.java | 32 ++--- src/main/java/frc/robot/SwerveModule.java | 10 +- .../frc/robot/subsystems/SwerveSubsystem.java | 132 ++++++++++++------ .../frc/robot/subsystems/VisionSubsystem.java | 20 +-- 8 files changed, 150 insertions(+), 86 deletions(-) diff --git a/.gitignore b/.gitignore index 5528d4f..2c29e44 100644 --- a/.gitignore +++ b/.gitignore @@ -176,3 +176,7 @@ logs/ # Folder that has CTRE Phoenix Sim device config storage ctre_sim/ + +.SysId/ +SysId +.SysId \ No newline at end of file diff --git a/README.md b/README.md index 47d0fae..dc3b7d4 100644 --- a/README.md +++ b/README.md @@ -1,3 +1,6 @@ # Team 1710 2024 Robot Code
-Thanks to dirtbikerxz for the [template](https://github.com/dirtbikerxz/BaseTalonFXSwerve).

-Using PhotonVision with a Raspberry Pi 4, SDS Mk4i swerve modules, and PathPlanner autos with trajectory generation through Choreo. +Thanks to dirtbikerxz for the [template](https://github.com/dirtbikerxz/BaseTalonFXSwerve). + +Using PhotonVision with a Raspberry Pi 4, SDS Mk4i swerve modules, and PathPlanner for autos and trajectory generation. + +[![Team 1710 2024 Robot Adonis](https://img.youtube.com/vi/qDYD3rMLS-k/0.jpg)](https://www.youtube.com/watch?v=qDYD3rMLS-k) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 61bf2ac..68ac274 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -113,15 +113,15 @@ public static final class Swerve { public static final double angleKD = chosenModule.angleKD; /* Drive Motor PID Values */ - public static final double driveKP = 0.12; + public static final double driveKP = 0.35919; public static final double driveKI = 0.0; public static final double driveKD = 0.0; public static final double driveKF = 0.0; /* Drive Motor Characterization Values From SYSID */ - public static final double driveKS = 0.080723; - public static final double driveKV = 0.73696; - public static final double driveKA = 0.073127; + public static final double driveKS = 0.095261; + public static final double driveKV = 0.71919; + public static final double driveKA = 0.16421; /* Swerve Profiling Values */ /** Meters per Second */ @@ -182,16 +182,16 @@ public static final class Shooter { public static final double angleOffsetAuto = Units.degreesToRadians(75.5); } - public static final class Elevator { //TODO tune + public static final class Elevator { public static final double maxHeightMeters = 0.78; public static final double minHeightMeters = 0.015; } public static final class Auto { - public static final double kMaxSpeedMetersPerSecond = 4.25; - public static final double kMaxAccelerationMetersPerSecondSquared = 8; - public static final double kMaxAngularSpeedRadiansPerSecond = (Math.PI*3)/2; - public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI*2; + public static final double kMaxSpeedMetersPerSecond = 1.8; + public static final double kMaxAccelerationMetersPerSecondSquared = 3; + public static final double kMaxAngularSpeedRadiansPerSecond = 9.424778; + public static final double kMaxAngularSpeedRadiansPerSecondSquared = 12.56637; public static final double kPXController = 1; public static final double kPYController = 1; diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index e87cbdc..a889959 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -25,13 +25,9 @@ import frc.lib.math.FiringSolutionsV3; /** - * The VM is configured to automatically run this class, and to call the - * functions corresponding to - * each mode, as described in the TimedRobot documentation. If you change the - * name of this class or - * the package after creating this project, you must also update the - * build.gradle file in the - * project. + * The VM is configured to automatically run this class, and to call the functions corresponding to + * each mode, as described in the TimedRobot documentation. If you change the name of this class or + * the package after creating this project, you must also update the build.gradle file in the project. */ public class Robot extends TimedRobot { public static final CTREConfigs ctreConfigs = new CTREConfigs(); @@ -81,6 +77,8 @@ public void robotInit() { // Access PhotonVision dashboard when connected via usb TODO make work // PortForwarder.add(5800, "10.17.10.11", 5800); + //SmartDashboard.putData(PDH); + // idk if this is useful System.gc(); } @@ -99,7 +97,6 @@ public void robotPeriodic() { // and running subsystem periodic() methods. This must be called from the robot's periodic // block in order for anything in the Command-based framework to work. CommandScheduler.getInstance().run(); - //SmartDashboard.putData(PDH); } /** Gets the current alliance, true is red */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9ae9052..680896c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -20,19 +20,17 @@ import frc.robot.subsystems.*; /** - * This class is where the bulk of the robot should be declared. Since - * Command-based is a - * "declarative" paradigm, very little robot logic should actually be handled in - * the {@link Robot} + * This class is where the bulk of the robot should be declared. Since Command-based is a + * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot} * periodic methods (other than the scheduler calls). Instead, the structure of - * the robot (including - * subsystems, commands, and button mappings) should be declared here. + * the robot (including subsystems, commands, and button mappings) should be declared here. */ public class RobotContainer { /* Controllers */ private final Joystick driver = new Joystick(0); private final Joystick mech = new Joystick(1); + private final Joystick FF = new Joystick(2); /* Analog */ private final int leftVerticalAxis = XboxController.Axis.kLeftY.value; @@ -102,16 +100,11 @@ public class RobotContainer { /** Mech RS */ private final JoystickButton rightStick = new JoystickButton(mech, XboxController.Button.kRightStick.value); - /* - * private final Trigger dynamicForward = new Trigger(() -> mech.getPOV() == - * 90); - * private final Trigger dynamicBackward = new Trigger(() -> mech.getPOV() == - * 270); - * private final Trigger quasistaticForward = new Trigger(() -> mech.getPOV() == - * 0); - * private final Trigger quasistaticBackwards = new Trigger(() -> mech.getPOV() - * == 180); - */ + + private final Trigger dynamicForward = new Trigger(() -> FF.getPOV() == 90); + private final Trigger dynamicBackward = new Trigger(() -> FF.getPOV() == 270); + private final Trigger quasistaticForward = new Trigger(() -> FF.getPOV() == 0); + private final Trigger quasistaticBackwards = new Trigger(() -> FF.getPOV() == 180); /* Subsystems */ private final VisionSubsystem m_VisionSubsystem = new VisionSubsystem(); @@ -199,6 +192,9 @@ private void configureButtonBindings() { forceShoot.whileTrue(new InstantCommand(() -> m_IntexerSubsystem.setShooterIntake(.9))) .onFalse(new InstantCommand(() -> m_IntexerSubsystem.setShooterIntake(0))); + driverUp.whileTrue(m_SwerveSubsystem.pathToAmpChain()); + driverDown.whileTrue(m_SwerveSubsystem.pathToSourceChain()); + /* MECH BUTTONS */ // Prime for Speaker @@ -240,10 +236,10 @@ private void configureButtonBindings() { .onFalse(new IntakeThroughShooterPart2(m_Shoota, m_IntexerSubsystem, mech)); // Characterization tests - /*dynamicForward.whileTrue(m_SwerveSubsystem.sysIdDynamic(Direction.kForward)); + dynamicForward.whileTrue(m_SwerveSubsystem.sysIdDynamic(Direction.kForward)); dynamicBackward.whileTrue(m_SwerveSubsystem.sysIdDynamic(Direction.kReverse)); quasistaticForward.whileTrue(m_SwerveSubsystem.sysIdQuasistatic(Direction.kForward)); - quasistaticBackwards.whileTrue(m_SwerveSubsystem.sysIdQuasistatic(Direction.kReverse));*/ + quasistaticBackwards.whileTrue(m_SwerveSubsystem.sysIdQuasistatic(Direction.kReverse)); } public void stopAll () { diff --git a/src/main/java/frc/robot/SwerveModule.java b/src/main/java/frc/robot/SwerveModule.java index ca272d7..2cbbe1a 100644 --- a/src/main/java/frc/robot/SwerveModule.java +++ b/src/main/java/frc/robot/SwerveModule.java @@ -73,6 +73,7 @@ public Rotation2d getCANcoder(){ return Rotation2d.fromRotations(angleEncoder.getAbsolutePosition().getValue()); } + /** Set module angles to absolute position */ public void resetToAbsolute(){ double absolutePosition = getCANcoder().getRotations() - angleOffset.getRotations(); mAngleMotor.setPosition(absolutePosition); @@ -92,15 +93,22 @@ public SwerveModulePosition getPosition(){ ); } + /** + * Drive robot based on provided voltage value + *

+ * Does NOT have optimization, meaning wheels have to be facing same direction + */ public void voltageDrive(double Volts){ mDriveMotor.setControl(driveCharacteriztionControl.withOutput(Volts)); } + /** Get drive motor voltage */ public double getMotorVoltage(){ return mDriveMotor.getMotorVoltage().getValue(); } + /** IN METERS PER SECOND */ public double getMotorVelocity(){ - return mDriveMotor.getVelocity().getValue(); + return Conversions.RPSToMPS(mDriveMotor.getVelocity().getValue(), Constants.Swerve.wheelCircumference); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java index 51e9e11..e67a697 100644 --- a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java @@ -61,6 +61,7 @@ public class SwerveSubsystem extends SubsystemBase { private final VisionSubsystem vision; private final SwerveDrivePoseEstimator swerveOdomEstimator; private SwerveModuleState[] swerveModuleStates; + public static boolean followingPath = false; // Characterization stuff private final MutableMeasure m_appliedVoltage = mutable(Volts.of(0)); @@ -110,12 +111,11 @@ public SwerveSubsystem(VisionSubsystem vision) { this::getChassisSpeeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE this::setChassisSpeeds, // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds new HolonomicPathFollowerConfig( - new PIDConstants(5.0, 0.0, 0.0), // Translation PID constants - new PIDConstants(6.0, 0.0, 0.0), // Rotation PID constants TODO tune + new PIDConstants(5, 0, 0), // Translation PID constants + new PIDConstants(5, 0.0, 0.0), // Rotation PID constants 4.5, // Max module speed, in m/s 0.37268, // Drive base radius in meters. Distance from robot center to furthest module. - new ReplanningConfig(true, true) - ), + new ReplanningConfig(true, true, .5, .25)), checkRedAlliance(), this // Reference to this subsystem to set requirements ); @@ -140,15 +140,18 @@ public SwerveSubsystem(VisionSubsystem vision) { PathPlannerLogging.setLogActivePathCallback((poses) -> { m_field.getObject("field").setPoses(poses); + + if (poses.isEmpty()){ + followingPath = false; + } else { + followingPath = true; + } }); this.vision = vision; } - /** - * Check alliance for the AutoBuilder. Returns true when red. Using a method for - * better readability - */ + /** Check alliance for the AutoBuilder. Returns true when red. Using a method for better readability */ public BooleanSupplier checkRedAlliance() { return () -> Robot.getAlliance(); } @@ -157,24 +160,25 @@ public BooleanSupplier checkRedAlliance() { public void drive(Translation2d translation, double rotation, boolean fieldRelative, boolean isOpenLoop) { double translationX = translation.getX(); double translationY = translation.getY(); + SwerveModuleState[] desiredStates; if (fieldRelative) { - swerveModuleStates = Constants.Swerve.swerveKinematics + desiredStates = Constants.Swerve.swerveKinematics .toSwerveModuleStates(ChassisSpeeds.discretize(ChassisSpeeds.fromFieldRelativeSpeeds( translationX, translationY, rotation, getHeading()), 0.02)); } else { - swerveModuleStates = Constants.Swerve.swerveKinematics.toSwerveModuleStates(new ChassisSpeeds(translationX, + desiredStates = Constants.Swerve.swerveKinematics.toSwerveModuleStates(new ChassisSpeeds(translationX, translationY, rotation)); } - SwerveDriveKinematics.desaturateWheelSpeeds(swerveModuleStates, Constants.Swerve.maxSpeed); + SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Constants.Swerve.maxSpeed); for (SwerveModule mod : mSwerveMods) { - mod.setDesiredState(swerveModuleStates[mod.moduleNumber], isOpenLoop); + mod.setDesiredState(desiredStates[mod.moduleNumber], isOpenLoop); } } @@ -188,22 +192,21 @@ public void setModuleStates(SwerveModuleState[] desiredStates) { } public SwerveModuleState[] getModuleStates() { - /* - SwerveModuleState[] states = new SwerveModuleState[4]; + return swerveModuleStates; + } + + public void updateModuleStates() { for (SwerveModule mod : mSwerveMods) { - states[mod.moduleNumber] = mod.getState(); + swerveModuleStates[mod.moduleNumber] = mod.getState(); } - return states; - */ - return swerveModuleStates; } /** Drive method for Autos */ public void setChassisSpeeds(ChassisSpeeds speed) { ChassisSpeeds targetSpeeds = ChassisSpeeds.discretize(speed, 0.02); - SwerveModuleState[] targetStates = Constants.Swerve.swerveKinematics.toSwerveModuleStates(targetSpeeds); - setModuleStates(targetStates); + SwerveModuleState[] desiredState = Constants.Swerve.swerveKinematics.toSwerveModuleStates(targetSpeeds); + setModuleStates(desiredState); } public ChassisSpeeds getChassisSpeeds() { @@ -266,6 +269,7 @@ public void voltageDrive(double Voltage) { @Override public void periodic() { + updateModuleStates(); SwerveModulePosition[] modulePositions = getModulePositions(); // Correct pose estimate with multiple vision measurements @@ -275,20 +279,24 @@ public void periodic() { final EstimatedRobotPose estimatedPose = OptionalEstimatedPoseFront.get(); swerveOdomEstimator - .setVisionMeasurementStdDevs(vision.getEstimationStdDevsFront(estimatedPose.estimatedPose.toPose2d())); + .setVisionMeasurementStdDevs( + vision.getEstimationStdDevsFront(estimatedPose.estimatedPose.toPose2d())); - swerveOdomEstimator.addVisionMeasurement(estimatedPose.estimatedPose.toPose2d(), estimatedPose.timestampSeconds); + swerveOdomEstimator.addVisionMeasurement(estimatedPose.estimatedPose.toPose2d(), + estimatedPose.timestampSeconds); } - Optional OptionalEstimatedPoseBack = vision.getEstimatedPoseBack(); + Optional OptionalEstimatedPoseBack = vision.getEstimatedPoseBack(); if (OptionalEstimatedPoseBack.isPresent()) { final EstimatedRobotPose estimatedPose2 = OptionalEstimatedPoseBack.get(); swerveOdomEstimator - .setVisionMeasurementStdDevs(vision.getEstimationStdDevsBack(estimatedPose2.estimatedPose.toPose2d())); + .setVisionMeasurementStdDevs( + vision.getEstimationStdDevsBack(estimatedPose2.estimatedPose.toPose2d())); - swerveOdomEstimator.addVisionMeasurement(estimatedPose2.estimatedPose.toPose2d(), estimatedPose2.timestampSeconds); + swerveOdomEstimator.addVisionMeasurement(estimatedPose2.estimatedPose.toPose2d(), + estimatedPose2.timestampSeconds); } // Logging @@ -302,7 +310,7 @@ public void periodic() { for (SwerveModule mod : mSwerveMods) { SmartDashboard.putNumber("Mod " + mod.moduleNumber + " CANcoder", mod.getCANcoder().getDegrees()); SmartDashboard.putNumber("Mod " + mod.moduleNumber + " Angle", modulePositions[mod.moduleNumber].angle.getDegrees()); - SmartDashboard.putNumber("Mod " + mod.moduleNumber + " Velocity", mod.getState().speedMetersPerSecond); + SmartDashboard.putNumber("Mod " + mod.moduleNumber + " Velocity", swerveModuleStates[mod.moduleNumber].speedMetersPerSecond); } SmartDashboard.putString("Obodom", getPose().toString()); @@ -312,20 +320,34 @@ public void periodic() { } public void sysidroutine(SysIdRoutineLog log) { - log.motor("drive-left") + log.motor("drive-BR") .voltage( m_appliedVoltage.mut_replace( - mSwerveMods[3].getMotorVoltage(), Volts)) + mSwerveMods[3].getMotorVoltage() * RobotController.getBatteryVoltage(), Volts)) .linearPosition(m_distance.mut_replace(mSwerveMods[3].getPosition().distanceMeters, Meters)) .linearVelocity( m_velocity.mut_replace(mSwerveMods[3].getMotorVelocity(), MetersPerSecond)); - log.motor("drive-right") + log.motor("drive-FL") .voltage( m_appliedVoltage.mut_replace( mSwerveMods[0].getMotorVoltage() * RobotController.getBatteryVoltage(), Volts)) - .linearPosition(m_distance.mut_replace(mSwerveMods[3].getPosition().distanceMeters, Meters)) + .linearPosition(m_distance.mut_replace(mSwerveMods[0].getPosition().distanceMeters, Meters)) .linearVelocity( m_velocity.mut_replace(mSwerveMods[0].getMotorVelocity(), MetersPerSecond)); + log.motor("drive-FR") + .voltage( + m_appliedVoltage.mut_replace( + mSwerveMods[1].getMotorVoltage() * RobotController.getBatteryVoltage(), Volts)) + .linearPosition(m_distance.mut_replace(mSwerveMods[1].getPosition().distanceMeters, Meters)) + .linearVelocity( + m_velocity.mut_replace(mSwerveMods[1].getMotorVelocity(), MetersPerSecond)); + log.motor("drive-BL") + .voltage( + m_appliedVoltage.mut_replace( + mSwerveMods[2].getMotorVoltage() * RobotController.getBatteryVoltage(), Volts)) + .linearPosition(m_distance.mut_replace(mSwerveMods[2].getPosition().distanceMeters, Meters)) + .linearVelocity( + m_velocity.mut_replace(mSwerveMods[2].getMotorVelocity(), MetersPerSecond)); } public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { @@ -340,50 +362,80 @@ public Command sysIdDynamic(SysIdRoutine.Direction direction) { public Command pathToSource() { if (!Robot.getAlliance()) { return AutoBuilder.pathfindToPose(new Pose2d(1.21, 0.96, Rotation2d.fromDegrees(58.79)), - new PathConstraints(Constants.Auto.kMaxSpeedMetersPerSecond, Constants.Auto.kMaxAccelerationMetersPerSecondSquared, Constants.Auto.kMaxAngularSpeedRadiansPerSecond, Constants.Auto.kMaxAngularSpeedRadiansPerSecondSquared)); + new PathConstraints(Constants.Auto.kMaxSpeedMetersPerSecond, + Constants.Auto.kMaxAccelerationMetersPerSecondSquared, + Constants.Auto.kMaxAngularSpeedRadiansPerSecond, + Constants.Auto.kMaxAngularSpeedRadiansPerSecondSquared)); } else { return AutoBuilder.pathfindToPose(new Pose2d(15.47, 1.50, Rotation2d.fromDegrees(-59.53)), - new PathConstraints(Constants.Auto.kMaxSpeedMetersPerSecond, Constants.Auto.kMaxAccelerationMetersPerSecondSquared, Constants.Auto.kMaxAngularSpeedRadiansPerSecond, Constants.Auto.kMaxAngularSpeedRadiansPerSecondSquared)); + new PathConstraints(Constants.Auto.kMaxSpeedMetersPerSecond, + Constants.Auto.kMaxAccelerationMetersPerSecondSquared, + Constants.Auto.kMaxAngularSpeedRadiansPerSecond, + Constants.Auto.kMaxAngularSpeedRadiansPerSecondSquared)); } } public Command pathToAmp() { if (!Robot.getAlliance()) { return AutoBuilder.pathfindToPose(new Pose2d(1.84, 7.59, Rotation2d.fromDegrees(90.37)), - new PathConstraints(Constants.Auto.kMaxSpeedMetersPerSecond, Constants.Auto.kMaxAccelerationMetersPerSecondSquared, Constants.Auto.kMaxAngularSpeedRadiansPerSecond, Constants.Auto.kMaxAngularSpeedRadiansPerSecondSquared)); + new PathConstraints(Constants.Auto.kMaxSpeedMetersPerSecond, + Constants.Auto.kMaxAccelerationMetersPerSecondSquared, + Constants.Auto.kMaxAngularSpeedRadiansPerSecond, + Constants.Auto.kMaxAngularSpeedRadiansPerSecondSquared)); } else { return AutoBuilder.pathfindToPose(new Pose2d(14.74, 7.52, Rotation2d.fromDegrees(90.37)), - new PathConstraints(Constants.Auto.kMaxSpeedMetersPerSecond, Constants.Auto.kMaxAccelerationMetersPerSecondSquared, Constants.Auto.kMaxAngularSpeedRadiansPerSecond, Constants.Auto.kMaxAngularSpeedRadiansPerSecondSquared)); + new PathConstraints(Constants.Auto.kMaxSpeedMetersPerSecond, + Constants.Auto.kMaxAccelerationMetersPerSecondSquared, + Constants.Auto.kMaxAngularSpeedRadiansPerSecond, + Constants.Auto.kMaxAngularSpeedRadiansPerSecondSquared)); } } public Command pathToMidfieldChain() { if (!Robot.getAlliance()) { return AutoBuilder.pathfindToPose(new Pose2d(6.29, 4.08, Rotation2d.fromDegrees(180)), - new PathConstraints(Constants.Auto.kMaxSpeedMetersPerSecond, Constants.Auto.kMaxAccelerationMetersPerSecondSquared, Constants.Auto.kMaxAngularSpeedRadiansPerSecond, Constants.Auto.kMaxAngularSpeedRadiansPerSecondSquared)); + new PathConstraints(Constants.Auto.kMaxSpeedMetersPerSecond, + Constants.Auto.kMaxAccelerationMetersPerSecondSquared, + Constants.Auto.kMaxAngularSpeedRadiansPerSecond, + Constants.Auto.kMaxAngularSpeedRadiansPerSecondSquared)); } else { return AutoBuilder.pathfindToPose(new Pose2d(10.26, 4.10, Rotation2d.fromDegrees(0)), - new PathConstraints(Constants.Auto.kMaxSpeedMetersPerSecond, Constants.Auto.kMaxAccelerationMetersPerSecondSquared, Constants.Auto.kMaxAngularSpeedRadiansPerSecond, Constants.Auto.kMaxAngularSpeedRadiansPerSecondSquared)); + new PathConstraints(Constants.Auto.kMaxSpeedMetersPerSecond, + Constants.Auto.kMaxAccelerationMetersPerSecondSquared, + Constants.Auto.kMaxAngularSpeedRadiansPerSecond, + Constants.Auto.kMaxAngularSpeedRadiansPerSecondSquared)); } } public Command pathToSourceChain() { if (!Robot.getAlliance()) { return AutoBuilder.pathfindToPose(new Pose2d(4.18, 2.79, Rotation2d.fromDegrees(59.47)), - new PathConstraints(Constants.Auto.kMaxSpeedMetersPerSecond, Constants.Auto.kMaxAccelerationMetersPerSecondSquared, Constants.Auto.kMaxAngularSpeedRadiansPerSecond, Constants.Auto.kMaxAngularSpeedRadiansPerSecondSquared)); + new PathConstraints(Constants.Auto.kMaxSpeedMetersPerSecond, + Constants.Auto.kMaxAccelerationMetersPerSecondSquared, + Constants.Auto.kMaxAngularSpeedRadiansPerSecond, + Constants.Auto.kMaxAngularSpeedRadiansPerSecondSquared)); } else { return AutoBuilder.pathfindToPose(new Pose2d(12.43, 2.90, Rotation2d.fromDegrees(121.26)), - new PathConstraints(Constants.Auto.kMaxSpeedMetersPerSecond, Constants.Auto.kMaxAccelerationMetersPerSecondSquared, Constants.Auto.kMaxAngularSpeedRadiansPerSecond, Constants.Auto.kMaxAngularSpeedRadiansPerSecondSquared)); + new PathConstraints(Constants.Auto.kMaxSpeedMetersPerSecond, + Constants.Auto.kMaxAccelerationMetersPerSecondSquared, + Constants.Auto.kMaxAngularSpeedRadiansPerSecond, + Constants.Auto.kMaxAngularSpeedRadiansPerSecondSquared)); } } public Command pathToAmpChain() { if (!Robot.getAlliance()) { return AutoBuilder.pathfindToPose(new Pose2d(4.20, 5.30, Rotation2d.fromDegrees(-58.21)), - new PathConstraints(Constants.Auto.kMaxSpeedMetersPerSecond, Constants.Auto.kMaxAccelerationMetersPerSecondSquared, Constants.Auto.kMaxAngularSpeedRadiansPerSecond, Constants.Auto.kMaxAngularSpeedRadiansPerSecondSquared)); + new PathConstraints(Constants.Auto.kMaxSpeedMetersPerSecond, + Constants.Auto.kMaxAccelerationMetersPerSecondSquared, + Constants.Auto.kMaxAngularSpeedRadiansPerSecond, + Constants.Auto.kMaxAngularSpeedRadiansPerSecondSquared)); } else { return AutoBuilder.pathfindToPose(new Pose2d(12.50, 5.25, Rotation2d.fromDegrees(-120.96)), - new PathConstraints(Constants.Auto.kMaxSpeedMetersPerSecond, Constants.Auto.kMaxAccelerationMetersPerSecondSquared, Constants.Auto.kMaxAngularSpeedRadiansPerSecond, Constants.Auto.kMaxAngularSpeedRadiansPerSecondSquared)); + new PathConstraints(Constants.Auto.kMaxSpeedMetersPerSecond, + Constants.Auto.kMaxAccelerationMetersPerSecondSquared, + Constants.Auto.kMaxAngularSpeedRadiansPerSecond, + Constants.Auto.kMaxAngularSpeedRadiansPerSecondSquared)); } } diff --git a/src/main/java/frc/robot/subsystems/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/VisionSubsystem.java index 65b8c35..850494f 100644 --- a/src/main/java/frc/robot/subsystems/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/VisionSubsystem.java @@ -67,19 +67,23 @@ public VisionSubsystem() { photonEstimatorFront.setLastPose(Constants.Vision.startingPose); photonEstimatorBack.setLastPose(Constants.Vision.startingPose); - photonEstimatorFront.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); - photonEstimatorBack.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); + // 2024 field quality makes multitag impractical + //photonEstimatorFront.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); + //photonEstimatorBack.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); } - public PhotonPipelineResult getLatestResultATF() { // Get the latest result for the April Tag camera + /** Get the latest result from the front April Tag camera */ + public PhotonPipelineResult getLatestResultATF() { return aprilTagCameraFront.getLatestResult(); } + /** Get the latest result from the back April Tag camera */ public PhotonPipelineResult getLatestResultATB() { return aprilTagCameraBack.getLatestResult(); } - public PhotonPipelineResult getLatestResultN() { // Get the latest result for the Note camera + /** Get the latest result from the Note camera */ + public PhotonPipelineResult getLatestResultN() { return noteCamera.getLatestResult(); } @@ -134,8 +138,8 @@ public Matrix getEstimationStdDevsFront(Pose2d estimatedPose) { return estStdDevs; avgDist /= numTags; // Decrease std devs if multiple targets are visible - if (numTags > 1) - estStdDevs = Constants.Vision.kMultiTagStdDevs; + //if (numTags > 1) + // estStdDevs = Constants.Vision.kMultiTagStdDevs; // Increase std devs based on (average) distance if (/*numTags == 1 && */avgDist > 4) estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE); @@ -161,8 +165,8 @@ public Matrix getEstimationStdDevsBack(Pose2d estimatedPose) { return estStdDevs; avgDist /= numTags; // Decrease std devs if multiple targets are visible - if (numTags > 1) - estStdDevs = Constants.Vision.kMultiTagStdDevs; + //if (numTags > 1) + // estStdDevs = Constants.Vision.kMultiTagStdDevs; // Increase std devs based on (average) distance if (/*numTags == 1 &&*/ avgDist > 4) estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE); From 3d7832fe75c85db5fba2295213c90b34675f8884 Mon Sep 17 00:00:00 2001 From: RavenRain44 <89553123+RavenRain44@users.noreply.github.com> Date: Fri, 8 Mar 2024 16:08:14 -0600 Subject: [PATCH 16/49] Comments and Deleted Redundancy --- .../frc/robot/subsystems/LEDSubsystem.java | 68 +++++-------------- 1 file changed, 18 insertions(+), 50 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/LEDSubsystem.java index 234cee8..ae2dc0c 100644 --- a/src/main/java/frc/robot/subsystems/LEDSubsystem.java +++ b/src/main/java/frc/robot/subsystems/LEDSubsystem.java @@ -17,15 +17,18 @@ public class LEDSubsystem extends SubsystemBase { public DigitalOutput bit3 = new DigitalOutput(4); // Bit 3 (4) public DigitalOutput bit4 = new DigitalOutput(5); // Bit 4 (8) - public DigitalOutput[] bits = {bit1, bit2, bit3, bit4}; + // Output bits to the LEDs + public DigitalOutput[] bits = {bit1, bit2, bit3, bit4}; // Actual Outputs + boolean[] output = new boolean[4]; // Computed Outputs + // Booleans used for Easy input public Boolean hasNote = false; public Boolean chargingOuttake = false; public Boolean atSpeed = false; public Boolean[] inputBooleans = { - false, // Amp -0 Rainbow Pattern #1 - false, // Source -1 Rainbow Pattern #2 + false, // Amp -0 Rainbow Pattern #1 ASK ANDREW ABOUT HOW TO DO + false, // Source -1 Rainbow Pattern #2 ASK ANDREW ABOUT HOW TO DO false, // Climb -2 Rainbow Pattern #3 ASK ANDREW ABOUT HOW TO DO false, // Blank/DC -3 None false, // Note Detected -4 White Solid @@ -39,8 +42,7 @@ public class LEDSubsystem extends SubsystemBase { false // Alliance Color -12 Blue Pulse }; - boolean[] output = new boolean[4]; - + // Use subsystems VisionSubsystem vision; ShooterSubsystem shooter; @@ -61,20 +63,21 @@ public void periodic() { private void set() { // Decimal phase var results = vision.getLatestResultN(); + // Note Detected if (results.hasTargets()) { NoteDetected(true); } else { NoteDetected(false); } + // Driver Station Connected if (DriverStation.isDSAttached()) { inputBooleans[1] = false; } else { inputBooleans[1] = true; } - - + // HasNote for ChargingOuttake and AtSpeed if (hasNote) { // Converts from simple inputs to boolean if (chargingOuttake) { inputBooleans[5] = true; @@ -106,16 +109,16 @@ private void encoder() { // Transition phase for (int i = 0; i < inputBooleans.length; i++) { // Picks the first true sequence based on priority if (inputBooleans[i]) { - trueIndex = i; + trueIndex = i; break; } } - String binaryString = Integer.toBinaryString(trueIndex); - - int length = binaryString.length(); + String binaryString = Integer.toBinaryString(trueIndex); // Cast Number to Binary + int length = binaryString.length(); // Find length of Binary (How many bits) + String finalString; // Final binary string - String finalString; + // Adds the amount of 0's needed for acurate transcription. Ex. 0x01 -> 0x0001 if (length < pin_amount) { int zerosToAdd = pin_amount - length; StringBuilder paddedStringBuilder = new StringBuilder(); @@ -135,44 +138,9 @@ private void encoder() { // Transition phase for (int i = pin_amount - 1; i >= 0; i--) { output[i] = finalString.charAt(i) == '1'; } - - - // if (inputBooleans[0]) { - // output[0] = false; output[1] = false; output[2] = false; output[3] = false; // 0000 - // } else if (inputBooleans[1]) { - // output[0] = true; output[1] = false; output[2] = false; output[3] = false; // 1000 - // } else if (inputBooleans[2]) { - // output[0] = false; output[1] = true; output[2] = false; output[3] = false; // 0100 - // } else if (inputBooleans[3]) { - // output[0] = true; output[1] = true; output[2] = false; output[3] = false; // 1100 - // } else if (inputBooleans[4]) { - // output[0] = false; output[1] = false; output[2] = true; output[3] = false; // 0010 - // } else if (inputBooleans[5]) { - // output[0] = true; output[1] = false; output[2] = true; output[3] = false; // 1010 - // } else if (inputBooleans[6]) { - // output[0] = false; output[1] = true; output[2] = true; output[3] = false; // 0110 - // } else if (inputBooleans[7]) { - // output[0] = true; output[1] = true; output[2] = true; output[3] = false; // 1110 - // } else if (inputBooleans[8]) { - // output[0] = false; output[1] = false; output[2] = false; output[3] = true; // 0001 - // } else if (inputBooleans[9]) { - // output[0] = true; output[1] = false; output[2] = false; output[3] = true; // 1001 - // } else if (inputBooleans[10]) { - // output[0] = false; output[1] = true; output[2] = false; output[3] = true; // 0101 - // } else if (inputBooleans[11]) { - // output[0] = true; output[1] = true; output[2] = false; output[3] = true; // 1101 - // } else if (inputBooleans[12]) { - // output[0] = false; output[1] = false; output[2] = true; output[3] = true; // 0011 - // } else if (inputBooleans[13]) { - // output[0] = true; output[1] = false; output[2] = true; output[3] = true; // 1011 - // } else if (inputBooleans[14]) { - // output[0] = false; output[1] = true; output[2] = true; output[3] = true; // 0111 - // } else if (inputBooleans[15]) { - // output[0] = true; output[1] = true; output[2] = true; output[3] = true; // 1111 - // } - } - - private void send() { // Send Phase - Redundancy is bliss + } + + private void send() { // Send Phase - "Redundancy is bliss" for (int i = 0; i < output.length; i++) { bits[i].set(output[i]); } From bfcc7bf7a6397a2eba2d23397005ef6dac9fee19 Mon Sep 17 00:00:00 2001 From: Camille-Jones <459cmjo2@students.olatheschools.com> Date: Fri, 8 Mar 2024 16:20:16 -0600 Subject: [PATCH 17/49] More autos --- src/main/deploy/pathplanner/autos/3N-0M.auto | 12 +- .../deploy/pathplanner/autos/3N-2M-Right.auto | 6 + .../deploy/pathplanner/autos/4N-1M-left.auto | 4 +- .../deploy/pathplanner/autos/5N-1M-left.auto | 121 ++++++++++++++++++ .../pathplanner/paths/backCloseRight.path | 6 +- .../paths/speaker to new path.path | 49 +++++++ .../pathplanner/paths/toCloseMiddle.path | 8 +- .../pathplanner/paths/toCloseRight.path | 8 +- .../paths/toRightCloseFromRight.path | 12 +- 9 files changed, 201 insertions(+), 25 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/5N-1M-left.auto create mode 100644 src/main/deploy/pathplanner/paths/speaker to new path.path diff --git a/src/main/deploy/pathplanner/autos/3N-0M.auto b/src/main/deploy/pathplanner/autos/3N-0M.auto index f226f3d..439cfa8 100644 --- a/src/main/deploy/pathplanner/autos/3N-0M.auto +++ b/src/main/deploy/pathplanner/autos/3N-0M.auto @@ -5,7 +5,7 @@ "x": 0.89943932856987, "y": 3.782906188140168 }, - "rotation": 13.495733280795742 + "rotation": 1.0416266760099369 }, "command": { "type": "sequential", @@ -14,7 +14,7 @@ { "type": "named", "data": { - "name": "someShooterCode" + "name": "Shoot" } }, { @@ -26,7 +26,7 @@ { "type": "named", "data": { - "name": "someIntakeCode" + "name": "Note Sniffer" } }, { @@ -38,7 +38,7 @@ { "type": "named", "data": { - "name": "someShooterCode" + "name": "Shoot" } }, { @@ -50,7 +50,7 @@ { "type": "named", "data": { - "name": "someIntakeCode" + "name": "Note Sniffer2" } }, { @@ -62,7 +62,7 @@ { "type": "named", "data": { - "name": "someShooterCode" + "name": "Shoot" } } ] diff --git a/src/main/deploy/pathplanner/autos/3N-2M-Right.auto b/src/main/deploy/pathplanner/autos/3N-2M-Right.auto index 506c0f1..4fadef4 100644 --- a/src/main/deploy/pathplanner/autos/3N-2M-Right.auto +++ b/src/main/deploy/pathplanner/autos/3N-2M-Right.auto @@ -41,6 +41,12 @@ "name": "Shoot" } }, + { + "type": "path", + "data": { + "pathName": "speaker to new path" + } + }, { "type": "path", "data": { diff --git a/src/main/deploy/pathplanner/autos/4N-1M-left.auto b/src/main/deploy/pathplanner/autos/4N-1M-left.auto index 1302db3..f79442e 100644 --- a/src/main/deploy/pathplanner/autos/4N-1M-left.auto +++ b/src/main/deploy/pathplanner/autos/4N-1M-left.auto @@ -2,8 +2,8 @@ "version": 1.0, "startingPose": { "position": { - "x": 1.2991794154609702, - "y": 6.971077125052112 + "x": 1.3771774811958186, + "y": 6.50308873064302 }, "rotation": 0 }, diff --git a/src/main/deploy/pathplanner/autos/5N-1M-left.auto b/src/main/deploy/pathplanner/autos/5N-1M-left.auto new file mode 100644 index 0000000..7b57ebe --- /dev/null +++ b/src/main/deploy/pathplanner/autos/5N-1M-left.auto @@ -0,0 +1,121 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.3771774811958186, + "y": 6.50308873064302 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "toMidAmpLeft" + } + }, + { + "type": "named", + "data": { + "name": "Note Sniffer" + } + }, + { + "type": "path", + "data": { + "pathName": "backMidAmpLeft" + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "toCloseLeft" + } + }, + { + "type": "named", + "data": { + "name": "Note Sniffer2" + } + }, + { + "type": "path", + "data": { + "pathName": "backCloseLeft" + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "toCloseMiddle" + } + }, + { + "type": "named", + "data": { + "name": "Note Sniffer" + } + }, + { + "type": "path", + "data": { + "pathName": "backCloseMiddle" + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "toCloseRight" + } + }, + { + "type": "named", + "data": { + "name": "Note Sniffer2" + } + }, + { + "type": "path", + "data": { + "pathName": "backCloseRight" + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/backCloseRight.path b/src/main/deploy/pathplanner/paths/backCloseRight.path index 02b1354..774e527 100644 --- a/src/main/deploy/pathplanner/paths/backCloseRight.path +++ b/src/main/deploy/pathplanner/paths/backCloseRight.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.5916721619666532, + "x": 1.59, "y": 4.172896516814411 }, "prevControl": null, "nextControl": { - "x": 1.71484577463217, - "y": 3.7410190970975767 + "x": 1.5916721619666532, + "y": 4.143647242163843 }, "isLocked": false, "linkedName": null diff --git a/src/main/deploy/pathplanner/paths/speaker to new path.path b/src/main/deploy/pathplanner/paths/speaker to new path.path new file mode 100644 index 0000000..c3638ae --- /dev/null +++ b/src/main/deploy/pathplanner/paths/speaker to new path.path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.41, + "y": 5.57 + }, + "prevControl": null, + "nextControl": { + "x": 1.9914122488577533, + "y": 4.484888779753807 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.91, + "y": 3.7 + }, + "prevControl": { + "x": 1.9816624906408973, + "y": 4.475139021536951 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 8.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/toCloseMiddle.path b/src/main/deploy/pathplanner/paths/toCloseMiddle.path index 42ac9c3..5b59258 100644 --- a/src/main/deploy/pathplanner/paths/toCloseMiddle.path +++ b/src/main/deploy/pathplanner/paths/toCloseMiddle.path @@ -4,23 +4,23 @@ { "anchor": { "x": 1.41, - "y": 5.57 + "y": 5.55 }, "prevControl": null, "nextControl": { "x": 1.4161765140632432, - "y": 5.5768617000416905 + "y": 5.55686170004169 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 1.7379185352194946, + "x": 1.82, "y": 5.557362183607978 }, "prevControl": { - "x": 1.7281687770026384, + "x": 1.8102502417831439, "y": 5.567111941824834 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/paths/toCloseRight.path b/src/main/deploy/pathplanner/paths/toCloseRight.path index e6da5fa..f7bc776 100644 --- a/src/main/deploy/pathplanner/paths/toCloseRight.path +++ b/src/main/deploy/pathplanner/paths/toCloseRight.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 1.6306711948340775, - "y": 4.270394098982973 + "x": 1.59, + "y": 4.17 }, "prevControl": { - "x": 1.4844248215812361, - "y": 5.17712161315059 + "x": 1.4437536267471587, + "y": 5.076727514167617 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/toRightCloseFromRight.path b/src/main/deploy/pathplanner/paths/toRightCloseFromRight.path index 10faf60..a646d79 100644 --- a/src/main/deploy/pathplanner/paths/toRightCloseFromRight.path +++ b/src/main/deploy/pathplanner/paths/toRightCloseFromRight.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 1.52342385444866, - "y": 4.007150627127858 + "x": 1.211431591509265, + "y": 3.8706540120918724 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 1.689169744135214, - "y": 4.114397967513275 + "x": 1.59, + "y": 4.17 }, "prevControl": { - "x": 1.1529330422081288, - "y": 3.8999032867424406 + "x": 1.6014219201835092, + "y": 4.163146758597556 }, "nextControl": null, "isLocked": false, From 8704cc28e5bd3f643e2cc84f550eeb70b2d70c9c Mon Sep 17 00:00:00 2001 From: RavenRain44 <89553123+RavenRain44@users.noreply.github.com> Date: Fri, 8 Mar 2024 16:42:03 -0600 Subject: [PATCH 18/49] Staged conflict resolve --- src/main/java/frc/robot/RobotContainer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7a6d7b8..bc68cd2 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -147,7 +147,7 @@ public RobotContainer() { m_ElevatorSubsystem, () -> -mech.getRawAxis(leftVerticalAxis))); - m_Shoota.setDefaultCommand(new ManRizzt(m_Shoota, () -> -mech.getRawAxis(rightVerticalAxis), + m_Shoota.setDefaultCommand(new ManRizzt(m_Shoota, m_LEDSubsystem, () -> -mech.getRawAxis(rightVerticalAxis), () -> shooterToAntiDefense.getAsBoolean())); // m_LEDSubsystem.setAllianceColor(); From 2ef80310042f4ae26f2dea30a633a0fc6abeadac Mon Sep 17 00:00:00 2001 From: Andrew-K961 Date: Fri, 8 Mar 2024 16:57:08 -0600 Subject: [PATCH 19/49] free my .md --- README.md | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 47d0fae..8ea07ab 100644 --- a/README.md +++ b/README.md @@ -1,3 +1,6 @@ # Team 1710 2024 Robot Code
-Thanks to dirtbikerxz for the [template](https://github.com/dirtbikerxz/BaseTalonFXSwerve).

-Using PhotonVision with a Raspberry Pi 4, SDS Mk4i swerve modules, and PathPlanner autos with trajectory generation through Choreo. +Thanks to dirtbikerxz for the [template](https://github.com/dirtbikerxz/BaseTalonFXSwerve). + +Using PhotonVision with a Raspberry Pi 4, SDS Mk4i swerve modules, and PathPlanner for autos and trajectory generation. + +[![Team 1710 2024 Robot Adonis](https://img.youtube.com/vi/qDYD3rMLS-k/0.jpg)](https://www.youtube.com/watch?v=qDYD3rMLS-k) \ No newline at end of file From 0c1876bc8161737ce969aebb0c6cc857de4f5085 Mon Sep 17 00:00:00 2001 From: Camille-Jones <459cmjo2@students.olatheschools.com> Date: Fri, 8 Mar 2024 17:06:56 -0600 Subject: [PATCH 20/49] stuff --- .gitignore | 4 + README.md | 7 +- .../deploy/pathplanner/autos/3N-2M-Right.auto | 10 +- .../deploy/pathplanner/autos/4N-1M-left.auto | 97 ++++++++ src/main/deploy/pathplanner/autos/kachow.auto | 102 ++++++++ .../pathplanner/paths/backCloseLeft.path | 8 +- .../pathplanner/paths/backCloseRight.path | 10 +- .../pathplanner/paths/backMidAmpLeft.path | 20 +- .../pathplanner/paths/backMidSourceLeft.path | 8 +- .../paths/speaker to new path.path | 49 ---- .../deploy/pathplanner/paths/toCloseLeft.path | 12 +- .../pathplanner/paths/toCloseRight.path | 8 +- .../pathplanner/paths/toMidAmpLeft.path | 32 ++- .../pathplanner/paths/toMidSourceLeft.path | 30 +-- .../pathplanner/paths/toMidSourceRight.path | 28 +-- src/main/java/frc/robot/Constants.java | 21 +- src/main/java/frc/robot/Robot.java | 13 +- src/main/java/frc/robot/RobotContainer.java | 142 +++++++---- src/main/java/frc/robot/SwerveModule.java | 10 +- src/main/java/frc/robot/commands/AimBot.java | 6 +- .../frc/robot/commands/ElevationManual.java | 10 +- .../java/frc/robot/commands/ElevatorSet.java | 1 - src/main/java/frc/robot/commands/FIREEE.java | 2 +- .../robot/commands/IntakeThroughShooter.java | 4 +- .../commands/IntakeThroughShooterPart2.java | 8 +- .../robot/commands/IntexForAutosByAutos.java | 2 +- .../java/frc/robot/commands/ManRizzt.java | 35 ++- .../java/frc/robot/commands/MissileLock.java | 2 +- .../java/frc/robot/commands/NoteSniffer.java | 2 +- .../java/frc/robot/commands/RizzLevel.java | 4 +- .../java/frc/robot/commands/TeleopSwerve.java | 2 +- .../java/frc/robot/commands/ZeroRizz.java | 2 +- .../robot/subsystems/ElevatorSubsystem.java | 30 ++- .../robot/subsystems/ShooterSubsystem.java | 234 ++++++++++++------ .../frc/robot/subsystems/SwerveSubsystem.java | 134 +++++++--- .../frc/robot/subsystems/VisionSubsystem.java | 33 ++- 36 files changed, 746 insertions(+), 376 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/4N-1M-left.auto create mode 100644 src/main/deploy/pathplanner/autos/kachow.auto delete mode 100644 src/main/deploy/pathplanner/paths/speaker to new path.path diff --git a/.gitignore b/.gitignore index 5528d4f..2c29e44 100644 --- a/.gitignore +++ b/.gitignore @@ -176,3 +176,7 @@ logs/ # Folder that has CTRE Phoenix Sim device config storage ctre_sim/ + +.SysId/ +SysId +.SysId \ No newline at end of file diff --git a/README.md b/README.md index 47d0fae..dc3b7d4 100644 --- a/README.md +++ b/README.md @@ -1,3 +1,6 @@ # Team 1710 2024 Robot Code
-Thanks to dirtbikerxz for the [template](https://github.com/dirtbikerxz/BaseTalonFXSwerve).

-Using PhotonVision with a Raspberry Pi 4, SDS Mk4i swerve modules, and PathPlanner autos with trajectory generation through Choreo. +Thanks to dirtbikerxz for the [template](https://github.com/dirtbikerxz/BaseTalonFXSwerve). + +Using PhotonVision with a Raspberry Pi 4, SDS Mk4i swerve modules, and PathPlanner for autos and trajectory generation. + +[![Team 1710 2024 Robot Adonis](https://img.youtube.com/vi/qDYD3rMLS-k/0.jpg)](https://www.youtube.com/watch?v=qDYD3rMLS-k) diff --git a/src/main/deploy/pathplanner/autos/3N-2M-Right.auto b/src/main/deploy/pathplanner/autos/3N-2M-Right.auto index 4fadef4..e2001f4 100644 --- a/src/main/deploy/pathplanner/autos/3N-2M-Right.auto +++ b/src/main/deploy/pathplanner/autos/3N-2M-Right.auto @@ -2,8 +2,8 @@ "version": 1.0, "startingPose": { "position": { - "x": 0.9091890867867262, - "y": 3.695158364188463 + "x": 0.72394368066646, + "y": 3.0614240800928165 }, "rotation": 0 }, @@ -41,12 +41,6 @@ "name": "Shoot" } }, - { - "type": "path", - "data": { - "pathName": "speaker to new path" - } - }, { "type": "path", "data": { diff --git a/src/main/deploy/pathplanner/autos/4N-1M-left.auto b/src/main/deploy/pathplanner/autos/4N-1M-left.auto new file mode 100644 index 0000000..f79442e --- /dev/null +++ b/src/main/deploy/pathplanner/autos/4N-1M-left.auto @@ -0,0 +1,97 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.3771774811958186, + "y": 6.50308873064302 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "toMidAmpLeft" + } + }, + { + "type": "named", + "data": { + "name": "Note Sniffer" + } + }, + { + "type": "path", + "data": { + "pathName": "backMidAmpLeft" + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "toCloseLeft" + } + }, + { + "type": "named", + "data": { + "name": "Note Sniffer2" + } + }, + { + "type": "path", + "data": { + "pathName": "backCloseLeft" + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "toCloseMiddle" + } + }, + { + "type": "named", + "data": { + "name": "Note Sniffer" + } + }, + { + "type": "path", + "data": { + "pathName": "backCloseMiddle" + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/kachow.auto b/src/main/deploy/pathplanner/autos/kachow.auto new file mode 100644 index 0000000..0244726 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/kachow.auto @@ -0,0 +1,102 @@ +{ + "version": 1.0, + "startingPose": { + "position": { +<<<<<<< HEAD:src/main/deploy/pathplanner/autos/4N-1M-left.auto + "x": 1.3771774811958186, + "y": 6.50308873064302 +======= + "x": 1.45, + "y": 6.45 +>>>>>>> f8dcaf8cae43b9b9042643a56208b94ff88cf8f9:src/main/deploy/pathplanner/autos/kachow.auto + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "toMidAmpLeft" + } + }, + { + "type": "named", + "data": { + "name": "Note Sniffer" + } + }, + { + "type": "path", + "data": { + "pathName": "backMidAmpLeft" + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "toCloseLeft" + } + }, + { + "type": "named", + "data": { + "name": "Note Sniffer2" + } + }, + { + "type": "path", + "data": { + "pathName": "backCloseLeft" + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "toCloseMiddle" + } + }, + { + "type": "named", + "data": { + "name": "Note Sniffer" + } + }, + { + "type": "path", + "data": { + "pathName": "backCloseMiddle" + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/backCloseLeft.path b/src/main/deploy/pathplanner/paths/backCloseLeft.path index 272c58b..aa8d9e9 100644 --- a/src/main/deploy/pathplanner/paths/backCloseLeft.path +++ b/src/main/deploy/pathplanner/paths/backCloseLeft.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 2.006568453136908, - "y": 6.732914646779413 + "x": 1.776917568086919, + "y": 6.42509066490817 }, "isLocked": false, "linkedName": null @@ -20,8 +20,8 @@ "y": 5.57 }, "prevControl": { - "x": 1.8354161173880559, - "y": 5.791356380812524 + "x": 1.6014219201835092, + "y": 6.113098401968776 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/backCloseRight.path b/src/main/deploy/pathplanner/paths/backCloseRight.path index 774e527..7212e82 100644 --- a/src/main/deploy/pathplanner/paths/backCloseRight.path +++ b/src/main/deploy/pathplanner/paths/backCloseRight.path @@ -4,12 +4,12 @@ { "anchor": { "x": 1.59, - "y": 4.172896516814411 + "y": 4.17 }, "prevControl": null, "nextControl": { - "x": 1.5916721619666532, - "y": 4.143647242163843 + "x": 1.5136740962318043, + "y": 4.7383824933920655 }, "isLocked": false, "linkedName": null @@ -20,8 +20,8 @@ "y": 5.57 }, "prevControl": { - "x": 1.5721726455329406, - "y": 5.830355413679948 + "x": 1.4844248215812361, + "y": 5.079624030982029 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/backMidAmpLeft.path b/src/main/deploy/pathplanner/paths/backMidAmpLeft.path index 9fbc008..919fc31 100644 --- a/src/main/deploy/pathplanner/paths/backMidAmpLeft.path +++ b/src/main/deploy/pathplanner/paths/backMidAmpLeft.path @@ -8,24 +8,24 @@ }, "prevControl": null, "nextControl": { - "x": 6.013343111246795, - "y": 6.88464699660991 + "x": 5.9108150520339064, + "y": 7.088074223654385 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 4.84809140639659, - "y": 6.990576641485824 + "x": 2.7908924226399536, + "y": 6.308093566305898 }, "prevControl": { - "x": 5.4428266576248125, - "y": 7.146572772955523 + "x": 3.3824207875442123, + "y": 6.475840416054866 }, "nextControl": { - "x": 3.893767367396956, - "y": 6.740262139453132 + "x": 2.137658622110595, + "y": 6.122848160185632 }, "isLocked": false, "linkedName": null @@ -36,8 +36,8 @@ "y": 5.57 }, "prevControl": { - "x": 1.6732434718551152, - "y": 5.862492746505683 + "x": 1.8159166009543433, + "y": 5.723108073294532 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/backMidSourceLeft.path b/src/main/deploy/pathplanner/paths/backMidSourceLeft.path index fd26538..ddecd4e 100644 --- a/src/main/deploy/pathplanner/paths/backMidSourceLeft.path +++ b/src/main/deploy/pathplanner/paths/backMidSourceLeft.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 6.7200449840329615, - "y": 2.3204424556117518 + "x": 6.46, + "y": 2.0961980166240624 }, "prevControl": null, "nextControl": { - "x": 6.396836824885053, - "y": 1.953637570838087 + "x": 6.136791840852092, + "y": 1.7293931318503977 }, "isLocked": false, "linkedName": null diff --git a/src/main/deploy/pathplanner/paths/speaker to new path.path b/src/main/deploy/pathplanner/paths/speaker to new path.path deleted file mode 100644 index c3638ae..0000000 --- a/src/main/deploy/pathplanner/paths/speaker to new path.path +++ /dev/null @@ -1,49 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 1.41, - "y": 5.57 - }, - "prevControl": null, - "nextControl": { - "x": 1.9914122488577533, - "y": 4.484888779753807 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 0.91, - "y": 3.7 - }, - "prevControl": { - "x": 1.9816624906408973, - "y": 4.475139021536951 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 8.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/toCloseLeft.path b/src/main/deploy/pathplanner/paths/toCloseLeft.path index 2863e03..7be358d 100644 --- a/src/main/deploy/pathplanner/paths/toCloseLeft.path +++ b/src/main/deploy/pathplanner/paths/toCloseLeft.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 1.6404209530509337, - "y": 6.522588247076731 + "x": 1.6209214366172213, + "y": 6.064349610884495 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 1.7086692605689264, - "y": 6.912578575750977 + "x": 1.92, + "y": 6.85 }, "prevControl": { - "x": 1.581922403749797, - "y": 6.288594049872185 + "x": 1.7476682934363506, + "y": 6.327593082739611 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/toCloseRight.path b/src/main/deploy/pathplanner/paths/toCloseRight.path index f7bc776..cd6183b 100644 --- a/src/main/deploy/pathplanner/paths/toCloseRight.path +++ b/src/main/deploy/pathplanner/paths/toCloseRight.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 1.4454257887138118, - "y": 5.225870404234871 + "x": 1.4551755469306675, + "y": 5.17712161315059 }, "isLocked": false, "linkedName": null @@ -20,8 +20,8 @@ "y": 4.17 }, "prevControl": { - "x": 1.4437536267471587, - "y": 5.076727514167617 + "x": 1.5136740962318043, + "y": 4.933377657729188 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/toMidAmpLeft.path b/src/main/deploy/pathplanner/paths/toMidAmpLeft.path index b5184f6..1d5e6b6 100644 --- a/src/main/deploy/pathplanner/paths/toMidAmpLeft.path +++ b/src/main/deploy/pathplanner/paths/toMidAmpLeft.path @@ -16,16 +16,32 @@ }, { "anchor": { - "x": 4.935839230348296, - "y": 6.23009550057105 + "x": 3.4538759813861684, + "y": 6.288594049872185 }, "prevControl": { - "x": 3.555302250423032, - "y": 5.923309505032103 + "x": 2.0398800070691956, + "y": 6.313400996790026 }, "nextControl": { - "x": 5.901065293817049, - "y": 6.4445901813418836 + "x": 4.565348418107764, + "y": 6.269094533438474 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.286830526155116, + "y": 6.873579542883553 + }, + "prevControl": { + "x": 4.916339713914584, + "y": 6.698083894980142 + }, + "nextControl": { + "x": 5.8563656726391695, + "y": 7.143359349112842 }, "isLocked": false, "linkedName": null @@ -36,8 +52,8 @@ "y": 7.380566970160069 }, "prevControl": { - "x": 6.066811183503603, - "y": 7.4488152776780625 + "x": 5.871816019166482, + "y": 7.107573740088099 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/toMidSourceLeft.path b/src/main/deploy/pathplanner/paths/toMidSourceLeft.path index 96f26bb..6a01a6b 100644 --- a/src/main/deploy/pathplanner/paths/toMidSourceLeft.path +++ b/src/main/deploy/pathplanner/paths/toMidSourceLeft.path @@ -3,41 +3,25 @@ "waypoints": [ { "anchor": { - "x": 0.9091890867867262, - "y": 3.704908122405319 + "x": 3.6683706621570025, + "y": 2.242444389876904 }, "prevControl": null, "nextControl": { - "x": 1.9091890867867252, - "y": 3.204908122405319 + "x": 4.887090439264016, + "y": 2.0571989837566393 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 4.39960252842121, - "y": 1.9499516433712205 - }, - "prevControl": { - "x": 3.1082305507614296, - "y": 2.526456990540765 - }, - "nextControl": { - "x": 5.491575448709092, - "y": 1.462463732528416 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 6.671296192948681, + "x": 6.46, "y": 2.0961980166240624 }, "prevControl": { - "x": 5.988813117768754, - "y": 1.1602212278058772 + "x": 5.306330042588828, + "y": 1.7354569626003862 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/toMidSourceRight.path b/src/main/deploy/pathplanner/paths/toMidSourceRight.path index 12e14b5..6158840 100644 --- a/src/main/deploy/pathplanner/paths/toMidSourceRight.path +++ b/src/main/deploy/pathplanner/paths/toMidSourceRight.path @@ -3,41 +3,41 @@ "waypoints": [ { "anchor": { - "x": 0.9091890867867262, - "y": 3.704908122405319 + "x": 0.72394368066646, + "y": 3.05167432187596 }, "prevControl": null, "nextControl": { - "x": 1.9091890867867252, - "y": 3.204908122405319 + "x": 0.35423890648256584, + "y": 3.0404037703087283 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 4.39960252842121, - "y": 1.9499516433712205 + "x": 3.288130091699615, + "y": 1.8914530940700844 }, "prevControl": { - "x": 3.1082305507614296, - "y": 2.526456990540765 + "x": 2.52128167551193, + "y": 2.1816119542492105 }, "nextControl": { - "x": 5.491575448709092, - "y": 1.462463732528416 + "x": 3.6488711457232905, + "y": 1.7549564790340981 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 6.398302962876711, - "y": 0.8872279977339068 + "x": 6.42, + "y": 0.81 }, "prevControl": { - "x": 4.9065899556977275, - "y": 1.754956479034098 + "x": 5.8815657773833365, + "y": 0.9067275141676177 }, "nextControl": null, "isLocked": false, diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 49aeea2..68ac274 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -11,6 +11,7 @@ import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.Vector; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; @@ -45,6 +46,8 @@ public static class Vision { public static final Matrix kSingleTagStdDevs = VecBuilder.fill(8, 8, 40); public static final Matrix kMultiTagStdDevs = VecBuilder.fill(4, 4, 20); public static final Vector stateStdDevs = VecBuilder.fill(1, 1, 1); // Encoder Odometry + + public static final Pose2d startingPose = new Pose2d(1.35, 5.55, new Rotation2d(0)); } private static AprilTagFieldLayout errorWrapper() { @@ -110,15 +113,15 @@ public static final class Swerve { public static final double angleKD = chosenModule.angleKD; /* Drive Motor PID Values */ - public static final double driveKP = 0.12; + public static final double driveKP = 0.35919; public static final double driveKI = 0.0; public static final double driveKD = 0.0; public static final double driveKF = 0.0; /* Drive Motor Characterization Values From SYSID */ - public static final double driveKS = 0.080723; - public static final double driveKV = 0.73696; - public static final double driveKA = 0.073127; + public static final double driveKS = 0.095261; + public static final double driveKV = 0.71919; + public static final double driveKA = 0.16421; /* Swerve Profiling Values */ /** Meters per Second */ @@ -179,16 +182,16 @@ public static final class Shooter { public static final double angleOffsetAuto = Units.degreesToRadians(75.5); } - public static final class Elevator { //TODO tune + public static final class Elevator { public static final double maxHeightMeters = 0.78; public static final double minHeightMeters = 0.015; } public static final class Auto { - public static final double kMaxSpeedMetersPerSecond = 4.25; - public static final double kMaxAccelerationMetersPerSecondSquared = 8; - public static final double kMaxAngularSpeedRadiansPerSecond = (Math.PI*3)/2; - public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI*2; + public static final double kMaxSpeedMetersPerSecond = 1.8; + public static final double kMaxAccelerationMetersPerSecondSquared = 3; + public static final double kMaxAngularSpeedRadiansPerSecond = 9.424778; + public static final double kMaxAngularSpeedRadiansPerSecondSquared = 12.56637; public static final double kPXController = 1; public static final double kPYController = 1; diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index e87cbdc..a889959 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -25,13 +25,9 @@ import frc.lib.math.FiringSolutionsV3; /** - * The VM is configured to automatically run this class, and to call the - * functions corresponding to - * each mode, as described in the TimedRobot documentation. If you change the - * name of this class or - * the package after creating this project, you must also update the - * build.gradle file in the - * project. + * The VM is configured to automatically run this class, and to call the functions corresponding to + * each mode, as described in the TimedRobot documentation. If you change the name of this class or + * the package after creating this project, you must also update the build.gradle file in the project. */ public class Robot extends TimedRobot { public static final CTREConfigs ctreConfigs = new CTREConfigs(); @@ -81,6 +77,8 @@ public void robotInit() { // Access PhotonVision dashboard when connected via usb TODO make work // PortForwarder.add(5800, "10.17.10.11", 5800); + //SmartDashboard.putData(PDH); + // idk if this is useful System.gc(); } @@ -99,7 +97,6 @@ public void robotPeriodic() { // and running subsystem periodic() methods. This must be called from the robot's periodic // block in order for anything in the Command-based framework to work. CommandScheduler.getInstance().run(); - //SmartDashboard.putData(PDH); } /** Gets the current alliance, true is red */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9313a33..680896c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -20,19 +20,17 @@ import frc.robot.subsystems.*; /** - * This class is where the bulk of the robot should be declared. Since - * Command-based is a - * "declarative" paradigm, very little robot logic should actually be handled in - * the {@link Robot} + * This class is where the bulk of the robot should be declared. Since Command-based is a + * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot} * periodic methods (other than the scheduler calls). Instead, the structure of - * the robot (including - * subsystems, commands, and button mappings) should be declared here. + * the robot (including subsystems, commands, and button mappings) should be declared here. */ public class RobotContainer { /* Controllers */ private final Joystick driver = new Joystick(0); private final Joystick mech = new Joystick(1); + private final Joystick FF = new Joystick(2); /* Analog */ private final int leftVerticalAxis = XboxController.Axis.kLeftY.value; @@ -42,44 +40,78 @@ public class RobotContainer { private final int leftTrigger = XboxController.Axis.kLeftTrigger.value; private final int rightTrigger = XboxController.Axis.kRightTrigger.value; - /* Driver Buttons */ - private final JoystickButton resetOdom = new JoystickButton(driver, XboxController.Button.kStart.value); + /* DRIVER BUTTONS */ + /** Driver B */ private final JoystickButton robotCentric = new JoystickButton(driver, XboxController.Button.kB.value); + /** Driver A */ private final JoystickButton Shoot = new JoystickButton(driver, XboxController.Button.kA.value); + /** Driver Y */ + private final JoystickButton forceShoot = new JoystickButton(driver, XboxController.Button.kY.value); + /** Driver X */ + private final JoystickButton driverX = new JoystickButton(driver, XboxController.Button.kX.value); + /** Driver RB */ private final JoystickButton intex = new JoystickButton(driver, XboxController.Button.kRightBumper.value); + /** Driver LB */ private final JoystickButton outex = new JoystickButton(driver, XboxController.Button.kLeftBumper.value); - private final JoystickButton xButtonDriver = new JoystickButton(driver, XboxController.Button.kX.value); - private final JoystickButton forceShoot = new JoystickButton(driver, XboxController.Button.kY.value); + /** Driver LT */ private final Trigger targetAmp = new Trigger(() -> driver.getRawAxis(leftTrigger) > .5); + /** Driver RT */ private final Trigger targetSpeaker = new Trigger(() -> driver.getRawAxis(rightTrigger) > .5); + /** Driver Up */ + private final Trigger driverUp = new Trigger(() -> driver.getPOV() == 0); + /** Driver Down */ + private final Trigger driverDown = new Trigger(() -> driver.getPOV() == 180); + /** Driver Right */ + private final Trigger driverLeft = new Trigger(() -> driver.getPOV() == 90); + /** Driver Left */ + private final Trigger driverRight = new Trigger(() -> driver.getPOV() == 270); + /** Driver Start */ + private final JoystickButton resetOdom = new JoystickButton(driver, XboxController.Button.kStart.value); - /* Mech Buttons */ - private final JoystickButton zeroShooter = new JoystickButton(mech, XboxController.Button.kBack.value); + /* MECH BUTTONS */ + /** Mech B */ + private final JoystickButton shooterToAntiDefense = new JoystickButton(mech, XboxController.Button.kB.value); + /** Mech A */ + private final JoystickButton shooterToIntake = new JoystickButton(mech, XboxController.Button.kA.value); + /** Mech Y */ + private final JoystickButton shooterToAmp = new JoystickButton(mech, XboxController.Button.kY.value); + /** Mech X */ + private final JoystickButton shooterToSubwoofer = new JoystickButton(mech, XboxController.Button.kX.value); + /** Mech RB */ private final JoystickButton primeShooterSpeedSpeaker = new JoystickButton(mech, XboxController.Button.kRightBumper.value); + /** Mech LB */ private final JoystickButton primeShooterSpeedAmp = new JoystickButton(mech, XboxController.Button.kLeftBumper.value); - private final JoystickButton shooterTo45 = new JoystickButton(mech, XboxController.Button.kB.value); - private final JoystickButton shooterToIntake = new JoystickButton(mech, XboxController.Button.kA.value); - private final Trigger resetR = new Trigger(() -> mech.getPOV() == 90); - private final Trigger elevatorDown = new Trigger(() -> mech.getPOV() == 180); + /** Mech RT */ + private final Trigger mechRT = new Trigger(() -> mech.getRawAxis(rightTrigger) > .5); + /** Mech LT */ + private final Trigger mechLT = new Trigger(() -> mech.getRawAxis(leftTrigger) > .5); + /** Mech Up */ private final Trigger elevatorUp = new Trigger(() -> mech.getPOV() == 0); + /** Mech Down */ + private final Trigger elevatorDown = new Trigger(() -> mech.getPOV() == 180); + /** Mech Right */ + private final Trigger resetR = new Trigger(() -> mech.getPOV() == 90); + /** Mech Left */ private final Trigger intakeThroughShooter = new Trigger(() -> mech.getPOV() == 270); + /** Mech Start */ private final JoystickButton autoZeroShooter = new JoystickButton(mech, XboxController.Button.kStart.value); - private final JoystickButton shooterToAmp = new JoystickButton(mech, XboxController.Button.kY.value); - private final JoystickButton xButtonMech = new JoystickButton(mech, XboxController.Button.kX.value); + /** Mech Back */ + private final JoystickButton zeroShooter = new JoystickButton(mech, XboxController.Button.kBack.value); + /** Mech RS */ private final JoystickButton rightStick = new JoystickButton(mech, XboxController.Button.kRightStick.value); - /* - private final Trigger dynamicForward = new Trigger(() -> mech.getPOV() == 90); - private final Trigger dynamicBackward = new Trigger(() -> mech.getPOV() == 270); - private final Trigger quasistaticForward = new Trigger(() -> mech.getPOV() == 0); - private final Trigger quasistaticBackwards = new Trigger(() -> mech.getPOV() == 180); - */ + + private final Trigger dynamicForward = new Trigger(() -> FF.getPOV() == 90); + private final Trigger dynamicBackward = new Trigger(() -> FF.getPOV() == 270); + private final Trigger quasistaticForward = new Trigger(() -> FF.getPOV() == 0); + private final Trigger quasistaticBackwards = new Trigger(() -> FF.getPOV() == 180); /* Subsystems */ private final VisionSubsystem m_VisionSubsystem = new VisionSubsystem(); private final SwerveSubsystem m_SwerveSubsystem = new SwerveSubsystem(m_VisionSubsystem); public final ShooterSubsystem m_Shoota = new ShooterSubsystem(m_SwerveSubsystem); - //private final LEDSubsystem m_LEDSubsystem = new LEDSubsystem(m_VisionSubsystem); + // private final LEDSubsystem m_LEDSubsystem = new + // LEDSubsystem(m_VisionSubsystem); private final ElevatorSubsystem m_ElevatorSubsystem = new ElevatorSubsystem(); private final IntexerSubsystem m_IntexerSubsystem = new IntexerSubsystem(); @@ -98,6 +130,7 @@ public RobotContainer() { NamedCommands.registerCommand("Stop Shooter Intake", new InstantCommand(() -> m_IntexerSubsystem.setShooterIntake(0))); NamedCommands.registerCommand("Note Sniffer", new NoteSniffer(m_SwerveSubsystem, m_VisionSubsystem, m_IntexerSubsystem, m_Shoota)); NamedCommands.registerCommand("Note Sniffer2", new NoteSniffer(m_SwerveSubsystem, m_VisionSubsystem, m_IntexerSubsystem, m_Shoota)); + m_SwerveSubsystem.setDefaultCommand( new TeleopSwerve( m_SwerveSubsystem, m_VisionSubsystem, m_Shoota, @@ -114,7 +147,8 @@ public RobotContainer() { m_ElevatorSubsystem, () -> -mech.getRawAxis(leftVerticalAxis))); - m_Shoota.setDefaultCommand(new ManRizzt(m_Shoota, () -> -mech.getRawAxis(rightVerticalAxis), () -> shooterTo45.getAsBoolean())); + m_Shoota.setDefaultCommand(new ManRizzt(m_Shoota, () -> -mech.getRawAxis(rightVerticalAxis), + () -> shooterToAntiDefense.getAsBoolean())); // m_LEDSubsystem.setAllianceColor(); @@ -142,7 +176,7 @@ private void configureButtonBindings() { // Lock on to speaker targetSpeaker.whileTrue(new MissileLock(m_Shoota, "speaker")); targetAmp.whileTrue(new MissileLock(m_Shoota, "amp")); - + // Shooter targetSpeaker.or(targetAmp).and(Shoot).whileTrue(new FIREEE(m_Shoota, m_IntexerSubsystem)); // Main fire @@ -156,57 +190,63 @@ private void configureButtonBindings() { // Shooter intake forceShoot.whileTrue(new InstantCommand(() -> m_IntexerSubsystem.setShooterIntake(.9))) - .onFalse(new InstantCommand(() -> m_IntexerSubsystem.setShooterIntake(0))); - + .onFalse(new InstantCommand(() -> m_IntexerSubsystem.setShooterIntake(0))); + + driverUp.whileTrue(m_SwerveSubsystem.pathToAmpChain()); + driverDown.whileTrue(m_SwerveSubsystem.pathToSourceChain()); + /* MECH BUTTONS */ - + // Prime for Speaker primeShooterSpeedSpeaker - .whileTrue(new InstantCommand(() -> m_Shoota.setShooterVelocity(FiringSolutionsV3.convertToRPM(m_Shoota.getCalculatedVelocity())))) - .onFalse(new InstantCommand(() -> m_Shoota.setShooterVelocity(Constants.Shooter.idleSpeedRPM))); - + .whileTrue(new InstantCommand(() -> m_Shoota.setShooterVelocity(FiringSolutionsV3.convertToRPM(m_Shoota.getCalculatedVelocity())))) + .onFalse(new InstantCommand(() -> m_Shoota.setShooterVelocity(Constants.Shooter.idleSpeedRPM))); + // Prime for Amp primeShooterSpeedAmp.whileTrue(new InstantCommand(() -> m_Shoota.setShooterVelocity(3417.8))) - .onFalse(new InstantCommand(() -> m_Shoota.setShooterVelocity(Constants.Shooter.idleSpeedRPM))); - + .onFalse(new InstantCommand(() -> m_Shoota.setShooterVelocity(Constants.Shooter.idleSpeedRPM))); + // Elevator elevatorDown.onTrue(new ElevatorSet(m_ElevatorSubsystem, Constants.Elevator.minHeightMeters)); elevatorUp.onTrue(new ElevatorSet(m_ElevatorSubsystem, Constants.Elevator.maxHeightMeters)); - + // Zero wrist zeroShooter.onTrue(new InstantCommand(() -> m_Shoota.resetWristEncoders(Constants.Shooter.angleOffsetManual))); // Set encoder to zero autoZeroShooter.onTrue(new ZeroRizz(m_Shoota).andThen(new RizzLevel(m_Shoota, Constants.Shooter.intakeAngleRadians))); // Wrist shooterToIntake.onTrue(new RizzLevel(m_Shoota, Constants.Shooter.intakeAngleRadians)); // Move wrist to intake position - + // Amp Preset - shooterToAmp.onTrue(new RizzLevel(m_Shoota, -0.48)).onTrue(new ElevatorSet(m_ElevatorSubsystem, Constants.Elevator.maxHeightMeters)); - - //xButton.whileTrue(new InstantCommand(() -> m_Shoota.SetOffsetVelocity(2000))) - //.onFalse(new InstantCommand(() -> m_Shoota.SetShooterVelocity(Constants.Shooter.idleSpeedRPM))); - - // Reset R - resetR.onTrue(new InstantCommand(() -> FiringSolutionsV3.resetAllR())); // Reset the R calculation incase it gets off - + shooterToAmp.onTrue(new RizzLevel(m_Shoota, -0.48)) + .onTrue(new ElevatorSet(m_ElevatorSubsystem, Constants.Elevator.maxHeightMeters)); + + // xButton.whileTrue(new InstantCommand(() -> m_Shoota.SetOffsetVelocity(2000))) + // .onFalse(new InstantCommand(() -> + // m_Shoota.SetShooterVelocity(Constants.Shooter.idleSpeedRPM))); + + // Reset the R calculation incase it gets off + resetR.onTrue(new InstantCommand(() -> FiringSolutionsV3.resetAllR())); + // Kill Shooter - rightStick.onTrue(new InstantCommand(()-> m_Shoota.setShooterVelocity(0))); + rightStick.onTrue(new InstantCommand(() -> m_Shoota.setShooterVelocity(0))); - //Intake Through Shooter + // Intake Through Shooter intakeThroughShooter.whileTrue(new IntakeThroughShooter(m_Shoota, m_IntexerSubsystem, mech)) - .onFalse(new IntakeThroughShooterPart2(m_Shoota, m_IntexerSubsystem, mech)); + .onFalse(new IntakeThroughShooterPart2(m_Shoota, m_IntexerSubsystem, mech)); // Characterization tests - /*dynamicForward.whileTrue(m_SwerveSubsystem.sysIdDynamic(Direction.kForward)); + dynamicForward.whileTrue(m_SwerveSubsystem.sysIdDynamic(Direction.kForward)); dynamicBackward.whileTrue(m_SwerveSubsystem.sysIdDynamic(Direction.kReverse)); quasistaticForward.whileTrue(m_SwerveSubsystem.sysIdQuasistatic(Direction.kForward)); - quasistaticBackwards.whileTrue(m_SwerveSubsystem.sysIdQuasistatic(Direction.kReverse));*/ + quasistaticBackwards.whileTrue(m_SwerveSubsystem.sysIdQuasistatic(Direction.kReverse)); } public void stopAll () { m_Shoota.setShooterVelocity(0); - m_Shoota.manualWristSpeed(0); + m_Shoota.setManualWristSpeed(0); m_IntexerSubsystem.setALL(0); + m_ElevatorSubsystem.setPositionWithEncoder(m_ElevatorSubsystem.getPosition()); } /** diff --git a/src/main/java/frc/robot/SwerveModule.java b/src/main/java/frc/robot/SwerveModule.java index ca272d7..2cbbe1a 100644 --- a/src/main/java/frc/robot/SwerveModule.java +++ b/src/main/java/frc/robot/SwerveModule.java @@ -73,6 +73,7 @@ public Rotation2d getCANcoder(){ return Rotation2d.fromRotations(angleEncoder.getAbsolutePosition().getValue()); } + /** Set module angles to absolute position */ public void resetToAbsolute(){ double absolutePosition = getCANcoder().getRotations() - angleOffset.getRotations(); mAngleMotor.setPosition(absolutePosition); @@ -92,15 +93,22 @@ public SwerveModulePosition getPosition(){ ); } + /** + * Drive robot based on provided voltage value + *

+ * Does NOT have optimization, meaning wheels have to be facing same direction + */ public void voltageDrive(double Volts){ mDriveMotor.setControl(driveCharacteriztionControl.withOutput(Volts)); } + /** Get drive motor voltage */ public double getMotorVoltage(){ return mDriveMotor.getMotorVoltage().getValue(); } + /** IN METERS PER SECOND */ public double getMotorVelocity(){ - return mDriveMotor.getVelocity().getValue(); + return Conversions.RPSToMPS(mDriveMotor.getVelocity().getValue(), Constants.Swerve.wheelCircumference); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/AimBot.java b/src/main/java/frc/robot/commands/AimBot.java index 80390f6..a9f759d 100644 --- a/src/main/java/frc/robot/commands/AimBot.java +++ b/src/main/java/frc/robot/commands/AimBot.java @@ -62,11 +62,11 @@ public void execute() { true, false); - if (shooter.shooterAtSpeed() && rotationPID.getPositionError() <= .035) { + if (shooter.isShooterAtSpeed() && rotationPID.getPositionError() <= .035) { intexer.setShooterIntake(.9); } - shooter.setWristPosition(shooter.getCalculatedAngleToSpeaker()); + shooter.setWristByAngle(shooter.getCalculatedAngleToSpeaker()); } // Called once the command ends or is interrupted. @@ -74,7 +74,7 @@ public void execute() { public void end(boolean interrupted) { swerveSubsystem.setChassisSpeeds(new ChassisSpeeds(0, 0, 0)); intexer.setShooterIntake(0); - shooter.manualWristSpeed(0); + shooter.setManualWristSpeed(0); } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/commands/ElevationManual.java b/src/main/java/frc/robot/commands/ElevationManual.java index 4ae7d4c..376bf3e 100644 --- a/src/main/java/frc/robot/commands/ElevationManual.java +++ b/src/main/java/frc/robot/commands/ElevationManual.java @@ -16,6 +16,7 @@ public class ElevationManual extends Command { DoubleSupplier axis; Boolean locked = false; double lockedValue = 0.0; + double lastElevatorSetpoint = 0.0; public ElevationManual(ElevatorSubsystem elevate, DoubleSupplier control) { m_elevatorSubsystem = elevate; @@ -31,16 +32,19 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { + lastElevatorSetpoint = m_elevatorSubsystem.getSetpoint(); + double value = axis.getAsDouble(); value = Math.pow(value, 3); if (Math.abs(value) > .05) { // Crime zone - m_elevatorSubsystem.setManualOverride(true); m_elevatorSubsystem.ManSpin(value); } else { - m_elevatorSubsystem.setManualOverride(false); - lockedValue = m_elevatorSubsystem.getEncoderValue(); + if (!m_elevatorSubsystem.locked){ + lastElevatorSetpoint = m_elevatorSubsystem.getPosition(); + m_elevatorSubsystem.setPositionWithEncoder(lastElevatorSetpoint); + } } SmartDashboard.putBoolean("locked", locked); diff --git a/src/main/java/frc/robot/commands/ElevatorSet.java b/src/main/java/frc/robot/commands/ElevatorSet.java index bae921b..030d497 100644 --- a/src/main/java/frc/robot/commands/ElevatorSet.java +++ b/src/main/java/frc/robot/commands/ElevatorSet.java @@ -33,7 +33,6 @@ public void execute() { // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - m_elevator.stopHere(); } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/commands/FIREEE.java b/src/main/java/frc/robot/commands/FIREEE.java index 24fe217..0d30b5c 100644 --- a/src/main/java/frc/robot/commands/FIREEE.java +++ b/src/main/java/frc/robot/commands/FIREEE.java @@ -27,7 +27,7 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if (shooter.shooterAtSpeed()){ + if (shooter.isShooterAtSpeed()){ intexer.setShooterIntake(.9); } } diff --git a/src/main/java/frc/robot/commands/IntakeThroughShooter.java b/src/main/java/frc/robot/commands/IntakeThroughShooter.java index 5dab49c..690f329 100644 --- a/src/main/java/frc/robot/commands/IntakeThroughShooter.java +++ b/src/main/java/frc/robot/commands/IntakeThroughShooter.java @@ -36,14 +36,14 @@ public void initialize() { public void execute() { shooter.setShooterVelocity(-1000); intexer.setALL(-.5); - shooter.setWristPosition(.66); + shooter.setWristByAngle(.66); } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { shooter.setShooterVelocity(Constants.Shooter.idleSpeedRPM); - shooter.setWristPosition(Constants.Shooter.intakeAngleRadians); + shooter.setWristByAngle(Constants.Shooter.intakeAngleRadians); intexer.setALL(0); } diff --git a/src/main/java/frc/robot/commands/IntakeThroughShooterPart2.java b/src/main/java/frc/robot/commands/IntakeThroughShooterPart2.java index 0d7f116..ab6043a 100644 --- a/src/main/java/frc/robot/commands/IntakeThroughShooterPart2.java +++ b/src/main/java/frc/robot/commands/IntakeThroughShooterPart2.java @@ -6,6 +6,7 @@ import edu.wpi.first.wpilibj.GenericHID.RumbleType; import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants; import frc.robot.subsystems.IntexerSubsystem; @@ -17,6 +18,7 @@ public class IntakeThroughShooterPart2 extends Command { private boolean finishPlease = false; Joystick controller; + public final Timer timer = new Timer(); /** Creates a new IntakeFromShooterPart2. */ public IntakeThroughShooterPart2(ShooterSubsystem shooterSub, IntexerSubsystem intex, Joystick controller) { @@ -30,6 +32,8 @@ public IntakeThroughShooterPart2(ShooterSubsystem shooterSub, IntexerSubsystem i // Called when the command is initially scheduled. @Override public void initialize() { + timer.reset(); + timer.start(); controller.setRumble(RumbleType.kBothRumble, 0); } @@ -38,7 +42,7 @@ public void initialize() { public void execute() { shooter.setShooterVelocity(0); intexer.setALL(.35); - shooter.setWristPosition(Constants.Shooter.intakeAngleRadians); + shooter.setWristByAngle(Constants.Shooter.intakeAngleRadians); } // Called once the command ends or is interrupted. @@ -52,7 +56,7 @@ public void end(boolean interrupted) { // Returns true when the command should end. @Override public boolean isFinished() { - if (!intexer.intakeThroughShooterPart2isReady || intexer.shooterBreak()) { + if (!intexer.intakeThroughShooterPart2isReady || intexer.shooterBreak() || timer.get() > 1.5) { return true; } else { return false; diff --git a/src/main/java/frc/robot/commands/IntexForAutosByAutos.java b/src/main/java/frc/robot/commands/IntexForAutosByAutos.java index fa82796..55b321b 100644 --- a/src/main/java/frc/robot/commands/IntexForAutosByAutos.java +++ b/src/main/java/frc/robot/commands/IntexForAutosByAutos.java @@ -29,7 +29,7 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - shooter.setWristPosition(Constants.Shooter.intakeAngleRadians); + shooter.setWristByAngle(Constants.Shooter.intakeAngleRadians); if (intexer.intakeBreak() && !intexer.shooterBreak()) { // If note is not at shooter yet intexer.setALL(.35); diff --git a/src/main/java/frc/robot/commands/ManRizzt.java b/src/main/java/frc/robot/commands/ManRizzt.java index 2e6d3ea..3705c6c 100644 --- a/src/main/java/frc/robot/commands/ManRizzt.java +++ b/src/main/java/frc/robot/commands/ManRizzt.java @@ -13,17 +13,19 @@ public class ManRizzt extends Command { - ShooterSubsystem shooterSubsystem; + ShooterSubsystem m_shooterSubsystem; private DoubleSupplier speed; BooleanSupplier setAngle; + double lastWristSetpoint = 0.0; + boolean wristIsLocked = false; - public ManRizzt(ShooterSubsystem subsystem, DoubleSupplier speed, BooleanSupplier setAngle) { + public ManRizzt(ShooterSubsystem shooterSubsystem, DoubleSupplier speed, BooleanSupplier setAngle) { // Use addRequirements() here to declare subsystem dependencies. - shooterSubsystem = subsystem; + m_shooterSubsystem = shooterSubsystem; this.setAngle = setAngle; this.speed = speed; SmartDashboard.putNumber("Set Wrist Angle", 0); - addRequirements(shooterSubsystem); + addRequirements(m_shooterSubsystem); } // Called when the command is initially scheduled. @@ -33,13 +35,24 @@ public void initialize() { @Override public void execute() { - double speedValue = speed.getAsDouble() * 0.5; - - speedValue = Math.pow(speed.getAsDouble(), 3); - if (setAngle.getAsBoolean()){ - shooterSubsystem.setWristPosition(SmartDashboard.getNumber("Set Wrist Angle", 0)); + double speedValue = Math.pow(speed.getAsDouble(), 3); + + if (setAngle.getAsBoolean()) { + m_shooterSubsystem.setWristByAngle(SmartDashboard.getNumber("Set Wrist Angle", 0)); } else { - shooterSubsystem.manualWristSpeed(speedValue); + if (Math.abs(speedValue) > .05) { + wristIsLocked = false; + m_shooterSubsystem.setManualWristSpeed(speedValue); + } else { + if (m_shooterSubsystem.isZeroed){ + if (!wristIsLocked){ + m_shooterSubsystem.setWristByAngle(m_shooterSubsystem.getCurrentShooterAngle()); + wristIsLocked = true; + } + } else { + m_shooterSubsystem.setManualWristSpeed(0); + } + } } } @@ -50,6 +63,6 @@ public boolean isFinished() { @Override public void end(boolean interrupted) { - shooterSubsystem.manualWristSpeed(0); + m_shooterSubsystem.setManualWristSpeed(0); } } diff --git a/src/main/java/frc/robot/commands/MissileLock.java b/src/main/java/frc/robot/commands/MissileLock.java index 9839f15..9d22a1f 100644 --- a/src/main/java/frc/robot/commands/MissileLock.java +++ b/src/main/java/frc/robot/commands/MissileLock.java @@ -43,7 +43,7 @@ public void execute() { @Override public void end(boolean interrupted) { shooter.setShooterVelocity(Constants.Shooter.idleSpeedRPM); - shooter.setWristPosition(Constants.Shooter.intakeAngleRadians); + shooter.setWristByAngle(Constants.Shooter.intakeAngleRadians); } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/commands/NoteSniffer.java b/src/main/java/frc/robot/commands/NoteSniffer.java index 5cf0254..594d483 100644 --- a/src/main/java/frc/robot/commands/NoteSniffer.java +++ b/src/main/java/frc/robot/commands/NoteSniffer.java @@ -51,7 +51,7 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - shooter.setWristPosition(Constants.Shooter.intakeAngleRadians); + shooter.setWristByAngle(Constants.Shooter.intakeAngleRadians); PhotonPipelineResult result = vision.getLatestResultN(); double rotationVal; diff --git a/src/main/java/frc/robot/commands/RizzLevel.java b/src/main/java/frc/robot/commands/RizzLevel.java index 02eead3..3f75603 100644 --- a/src/main/java/frc/robot/commands/RizzLevel.java +++ b/src/main/java/frc/robot/commands/RizzLevel.java @@ -30,7 +30,7 @@ public void initialize() { @Override public void execute() { if (shooter.isZeroed){ - shooter.setWristPosition(angle); + shooter.setWristByAngle(angle); } } @@ -44,7 +44,7 @@ public void end(boolean interrupted) { @Override public boolean isFinished() { if (shooter.isZeroed){ - return shooter.getAngle() >= (angle - Math.toRadians(.5)) && shooter.getAngle() < (angle + Math.toRadians(.5)); + return shooter.getCurrentShooterAngle() >= (angle - Math.toRadians(.5)) && shooter.getCurrentShooterAngle() < (angle + Math.toRadians(.5)); } else { return true; } diff --git a/src/main/java/frc/robot/commands/TeleopSwerve.java b/src/main/java/frc/robot/commands/TeleopSwerve.java index baeebb2..cb19906 100644 --- a/src/main/java/frc/robot/commands/TeleopSwerve.java +++ b/src/main/java/frc/robot/commands/TeleopSwerve.java @@ -74,7 +74,7 @@ public void execute() { strafeVal = Math.copySign(Math.pow(strafeVal, 2), strafeVal); if (shooterOverrideSpeaker.getAsBoolean()) { // Lock robot angle to speaker - if (shooterSubsystem.getDistanceToSpeaker() >= 3.5){ + if (shooterSubsystem.getDistanceToSpeakerWhileMoving() >= 3.5){ controller.setRumble(RumbleType.kBothRumble, 0.5); } else { controller.setRumble(RumbleType.kBothRumble, 0); diff --git a/src/main/java/frc/robot/commands/ZeroRizz.java b/src/main/java/frc/robot/commands/ZeroRizz.java index a063a3e..ff3d41c 100644 --- a/src/main/java/frc/robot/commands/ZeroRizz.java +++ b/src/main/java/frc/robot/commands/ZeroRizz.java @@ -30,7 +30,7 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - shooter.manualWristSpeed(.45); + shooter.setManualWristSpeed(.45); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index 62563fa..dc8fab7 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -5,9 +5,9 @@ package frc.robot.subsystems; import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs; -import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.Slot1Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.controls.PositionDutyCycle; import com.ctre.phoenix6.hardware.TalonFX; @@ -55,6 +55,10 @@ public ElevatorSubsystem() { m_elevatorLeft.setNeutralMode(NeutralModeValue.Brake); m_elevatorRight.setControl(new Follower(m_elevatorLeft.getDeviceID(), true)); + TalonFXConfiguration elevatorConfigs = new TalonFXConfiguration(); + elevatorConfigs.Slot0.kP = 0.09; + elevatorConfigs.MotorOutput.NeutralMode = NeutralModeValue.Brake; + // PID Slot0Configs encoderConfigSlot0 = new Slot0Configs(); ClosedLoopRampsConfigs closedloop = new ClosedLoopRampsConfigs(); @@ -72,7 +76,6 @@ public ElevatorSubsystem() { encoderConfigSlot1.kD = .0; encoderConfigSlot1.kV = .01; - m_elevatorLeft.getConfigurator().apply(encoderConfigSlot0, 0.050); m_elevatorLeft.getConfigurator().apply(closedloop); @@ -110,15 +113,6 @@ public void periodic() { m_elevatorRight.getSupplyCurrent().getValueAsDouble()); SmartDashboard.putNumber("LaserCan Ambient", measurement.ambient); revolutionCount = m_elevatorLeft.getPosition().getValueAsDouble(); - - if (!manualOverride){ // Flagitious logic - if (!locked){ - stopHere(); - locked = true; - } - } else { - locked = false; - } //FiringSolutionsV3.updateHeight(getHeight()); //TODO: test this } @@ -131,11 +125,13 @@ public double getEncoderValue(){ return revolutionCount; } - public void setToEncoder(double value){ + public void setPositionWithEncoder(double value){ + locked = true; m_elevatorLeft.setControl(lockPosition.withPosition(value)); } public void stopHere(){ + locked = true; m_elevatorLeft.setControl(lockPosition.withPosition(revolutionCount).withSlot(0)); } @@ -146,6 +142,7 @@ public double getHeightEncoder() { /** Set height IN METERS. Will run off LaserCan but will switch to encoder if it fails */ public void setHeight(double height) { + locked = true; setHeight = height; if (!lasercanFailureCheck()) { // Run off LaserCan m_elevatorLeft.set(elevatorPID.calculate(getHeightLaserCan(), height)); @@ -171,6 +168,7 @@ public boolean atHeight() { } public void ManSpin(double percent) { + locked = false; m_elevatorLeft.set(percent); } @@ -204,4 +202,12 @@ public boolean lasercanFailureCheck() { } } + public double getSetpoint(){ + return m_elevatorLeft.getClosedLoopReference().getValue(); + } + + public double getPosition(){ + return m_elevatorLeft.getPosition().getValueAsDouble(); + } + } diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 3a05b0e..f3b2c27 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -49,15 +49,26 @@ public class ShooterSubsystem extends SubsystemBase { private double positionD = 0; // Vars + /** In m/s */ private double shooterVelocity = 12.1; private double shooterAngleToSpeaker, shooterAngleToAmp; - private Boolean ENCFAIL = false; + private boolean ENCFAIL = false; public boolean isZeroed = false; - private double angleOffset = 68.2; // IN RADIANS + public boolean wristIsLocked = false; + /** IN RADIANS */ + private double angleOffset = 68.2; + private double distanceToMovingSpeakerTarget = .94; + public double lastWristAngleSetpoint = 0.0; + public boolean manualOverride = false; + public double wristAngleUpperBound; + public double wristAngleLowerBound; + + // Constants + private final double wristAngleMax = 0.0; + private final double wristAngleMin = 0.0; private final Timer speedTimer = new Timer(); - private final int m_WristCurrentMax = 84; private final Interpolations interpolation = new Interpolations(); - private double distanceToMovingSpeakerTarget = .94; + private final int m_WristCurrentMax = 84; private SwerveSubsystem swerveSubsystem; @@ -95,7 +106,7 @@ public ShooterSubsystem(SwerveSubsystem swerve) { m_Wrist.burnFlash(); shootaBot.burnFlash(); shootaTop.burnFlash(); - + m_pidWrist = new PIDController(positionP, positionI, positionD); m_VelocityEncoder.setMeasurementPeriod(20); @@ -118,29 +129,31 @@ public ShooterSubsystem(SwerveSubsystem swerve) { @Override public void periodic() { // This method will be called once per scheduler run -/* - pidSpdP = SmartDashboard.getNumber("Velo P", pidSpdP); - pidSpdI = SmartDashboard.getNumber("Velo I", pidSpdI); - pidSpdD = SmartDashboard.getNumber("Velo D", pidSpdD); -Minor whoopsie if these guys were causing loop overruns - botPID.setP(pidSpdP, 0); - botPID.setI(pidSpdI, 0); - botPID.setD(pidSpdD, 0); - - topPID.setP(pidSpdP, 0); - topPID.setI(pidSpdI, 0); - topPID.setD(pidSpdD, 0); -*/ + /* + * pidSpdP = SmartDashboard.getNumber("Velo P", pidSpdP); + * pidSpdI = SmartDashboard.getNumber("Velo I", pidSpdI); + * pidSpdD = SmartDashboard.getNumber("Velo D", pidSpdD); + * Minor whoopsie if these guys were causing loop overruns + * botPID.setP(pidSpdP, 0); + * botPID.setI(pidSpdI, 0); + * botPID.setD(pidSpdD, 0); + * + * topPID.setP(pidSpdP, 0); + * topPID.setI(pidSpdI, 0); + * topPID.setD(pidSpdD, 0); + */ + shooterVelocity = SmartDashboard.getNumber("set velocity", shooterVelocity); FiringSolutionsV3.slipPercent = SmartDashboard.getNumber("Set Slip Offset", FiringSolutionsV3.slipPercent); FiringSolutionsV3.speakerTargetZ = SmartDashboard.getNumber("Set Target Z", FiringSolutionsV3.speakerTargetZ); - SmartDashboard.putNumber("Top - Bottom error", m_VelocityEncoder.getVelocity() - m_VelocityEncoder2.getVelocity()); - SmartDashboard.putNumber("Current Angle Radians", getAngle()); + SmartDashboard.putNumber("Top - Bottom error", + m_VelocityEncoder.getVelocity() - m_VelocityEncoder2.getVelocity()); + SmartDashboard.putNumber("Current Angle Radians", getCurrentShooterAngle()); SmartDashboard.putNumber("Current Velocity", getVelocity()); - SmartDashboard.putBoolean("shooter at speed", shooterAtSpeed()); - SmartDashboard.putNumber("Current Angle Degrees", Units.radiansToDegrees(getAngle())); + SmartDashboard.putBoolean("shooter at speed", isShooterAtSpeed()); + SmartDashboard.putNumber("Current Angle Degrees", Units.radiansToDegrees(getCurrentShooterAngle())); // check for encoder failure if (m_WristEncoder.isConnected()) { @@ -156,25 +169,63 @@ public void periodic() { SmartDashboard.putNumber("Flywheel Right Current", shootaBot.getOutputCurrent()); SmartDashboard.putNumber("Wrist Current", m_Wrist.getOutputCurrent()); SmartDashboard.putBoolean("is Wrist Stalled", isWristMotorStalled()); + + if (isZeroed){ + if (!manualOverride){ + m_Wrist.set(m_pidWrist.calculate(getCurrentShooterAngle(), lastWristAngleSetpoint)); + } + + //Implement whenever build stops throwing + /*if (getCurrentShooterAngle() > wristAngleUpperBound){ + m_Wrist.set(m_pidWrist.calculate(getCurrentShooterAngle(), wristAngleUpperBound)); + } else if (getCurrentShooterAngle() < wristAngleLowerBound){ + m_Wrist.set(m_pidWrist.calculate(getCurrentShooterAngle(), wristAngleLowerBound)); + }*/ + } + } - /** Check if wrist motor is exceeding stall current, used for zeroing */ - public boolean isWristMotorStalled() { - if (m_Wrist.getOutputCurrent() > m_WristCurrentMax) { - return true; + /** in RADIANs units MATTER */ + public double getCurrentShooterAngle() { + if (!ENCFAIL) { + return ((-m_WristEncoder.get() * 2 * Math.PI) / 4) + angleOffset; } else { - return false; + return ((m_PositionEncoder.getPosition() * 2 * Math.PI) / 100); } } - /** Reset wrist encoder to given value */ - public void setWristEncoderPosition(double newPosition) { - m_WristEncoder.setPositionOffset(newPosition); + public double getCalculatedAngleToAmp() { + return shooterAngleToAmp; } - /** Get whether shooter is at target speed */ - public boolean shooterAtSpeed() { // Copied from Hudson but made it better + public double getCalculatedAngleToSpeaker() { + return shooterAngleToSpeaker; + } + + public double getCalculatedVelocity() { + return shooterVelocity; + } + + public double getDistanceTo(double x, double y) { + return FiringSolutionsV3.getDistanceToTarget(swerveSubsystem.getPose().getX(), swerveSubsystem.getPose().getY(), + x, y); + } + + public double getDistanceToSpeakerWhileMoving() { + return distanceToMovingSpeakerTarget; + } + /** Shooter velocity in RPM */ + public double getVelocity() { + return m_VelocityEncoder.getVelocity(); + } + + public double getWristRotations() { + return m_PositionEncoder.getPosition(); + } + + /** Get whether shooter is at target speed */ + public boolean isShooterAtSpeed() { // Copied from Hudson but made it better // if error less than certain amount start the timer if (Math.abs(getVelocity() - FiringSolutionsV3.convertToRPM(shooterVelocity)) < 80) { speedTimer.start(); @@ -190,20 +241,21 @@ public boolean shooterAtSpeed() { // Copied from Hudson but made it better } } - /** Shooter velocity in RPM */ - public double getVelocity() { - return m_VelocityEncoder.getVelocity(); - } - - /** in RADIANs units MATTER */ - public double getAngle() { - if (!ENCFAIL) { - return ((-m_WristEncoder.get() * 2 * Math.PI) / 4) + angleOffset; + /** Check if wrist motor is exceeding stall current, used for zeroing */ + public boolean isWristMotorStalled() { + if (m_Wrist.getOutputCurrent() > m_WristCurrentMax) { + return true; } else { - return ((m_PositionEncoder.getPosition() * 2 * Math.PI) / 100); + return false; } } + public void PointShoot(double PointAngle, double launchVelocity) { + setWristByAngle(PointAngle); + botPID.setReference(launchVelocity, CANSparkMax.ControlType.kVelocity); + topPID.setReference(launchVelocity, CANSparkMax.ControlType.kVelocity); + } + public void resetWristEncoders(double newOffset) { angleOffset = newOffset; m_WristEncoder.reset(); @@ -211,87 +263,121 @@ public void resetWristEncoders(double newOffset) { isZeroed = true; } - /** IN RADIANS */ - public void setWristPosition(double angle) { - if (isZeroed){ - m_Wrist.set(m_pidWrist.calculate(getAngle(), angle)); - } + public void setManualWristSpeed(double speed) { + m_Wrist.set(speed); + } + + public void setManualOverride(boolean value){ + manualOverride = value; } /** In rotations per minute */ - public void setShooterVelocity(double velocity) { + public void setOffsetVelocity(double velocity) { if (velocity == 0) { shootaBot.stopMotor(); shootaTop.stopMotor(); } else { - botPID.setReference(velocity, CANSparkMax.ControlType.kVelocity); - topPID.setReference(velocity, CANSparkMax.ControlType.kVelocity); + botPID.setReference(velocity - SmartDashboard.getNumber("Top Bottom Offset", 400), + CANSparkMax.ControlType.kVelocity); + topPID.setReference(velocity + SmartDashboard.getNumber("Top Bottom Offset", 400), + CANSparkMax.ControlType.kVelocity); } } /** In rotations per minute */ - public void SetOffsetVelocity(double velocity) { + public void setShooterVelocity(double velocity) { if (velocity == 0) { shootaBot.stopMotor(); shootaTop.stopMotor(); } else { - botPID.setReference(velocity - SmartDashboard.getNumber("Top Bottom Offset", 400), CANSparkMax.ControlType.kVelocity); - topPID.setReference(velocity + SmartDashboard.getNumber("Top Bottom Offset", 400), CANSparkMax.ControlType.kVelocity); + botPID.setReference(velocity, CANSparkMax.ControlType.kVelocity); + topPID.setReference(velocity, CANSparkMax.ControlType.kVelocity); } } - public void PointShoot(double PointAngle, double launchVelocity) { - setWristPosition(PointAngle); - botPID.setReference(launchVelocity, CANSparkMax.ControlType.kVelocity); - topPID.setReference(launchVelocity, CANSparkMax.ControlType.kVelocity); + /** Reset wrist encoder to given value */ + public void setWristEncoderOffset(double newPosition) { + m_WristEncoder.setPositionOffset(newPosition); } - public void manualWristSpeed(double speed) { - m_Wrist.set(speed); + public void setWristAngleLowerBound(double wristAngleLowerBound) { + if (wristAngleLowerBound < wristAngleMin){ + wristAngleLowerBound = wristAngleMin; + } else if (wristAngleLowerBound > wristAngleUpperBound){ + wristAngleLowerBound = wristAngleUpperBound; + } else { + this.wristAngleLowerBound = wristAngleLowerBound; + } } - public double getCalculatedVelocity() { - return shooterVelocity; + public void setWristAngleUpperBound(double wristAngleUpperBound) { + if (wristAngleUpperBound > wristAngleMax){ + wristAngleUpperBound = wristAngleMax; + } else if (wristAngleUpperBound < wristAngleLowerBound){ + wristAngleUpperBound = wristAngleLowerBound; + } else { + this.wristAngleUpperBound = wristAngleUpperBound; + } } - public double getCalculatedAngleToAmp() { - return shooterAngleToAmp; + /** IN RADIANS */ + public void setWristByAngle(double angle) { //TODO: Implement redundancy + if (isZeroed) { + updateWristAngleSetpoint(angle); + } } - public double getCalculatedAngleToSpeaker() { - return shooterAngleToSpeaker; + /** IN ROTATIONS */ + public void setWristByRotations(double newPosition) { + if (isZeroed){ + m_Wrist.set(m_pidWrist.calculate(getWristRotations(), newPosition)); + } } - public double getDistanceToSpeaker () { - return distanceToMovingSpeakerTarget; + public void updateWristAngleSetpoint(double angle) { + if (angle != lastWristAngleSetpoint){ + if (lastWristAngleSetpoint < wristAngleLowerBound){ + lastWristAngleSetpoint = wristAngleLowerBound; + } else if (lastWristAngleSetpoint > wristAngleUpperBound){ + lastWristAngleSetpoint = wristAngleUpperBound; + } else { + lastWristAngleSetpoint = angle; + } + } } public void updateShooterMath() { // Shooter Math Pose2d pose = swerveSubsystem.getPose(); ChassisSpeeds chassisSpeeds = swerveSubsystem.getChassisSpeeds(); - distanceToMovingSpeakerTarget = FiringSolutionsV3.getDistanceToMovingTarget(pose.getX(), pose.getY(), FiringSolutionsV3.speakerTargetX, FiringSolutionsV3.speakerTargetY, + distanceToMovingSpeakerTarget = FiringSolutionsV3.getDistanceToMovingTarget(pose.getX(), pose.getY(), + FiringSolutionsV3.speakerTargetX, FiringSolutionsV3.speakerTargetY, chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond, pose.getRotation().getRadians()); FiringSolutionsV3.updateSpeakerR(distanceToMovingSpeakerTarget); - //shooterAngleToSpeaker = FiringSolutionsV3.getShooterAngleFromSpeakerR(); - shooterAngleToSpeaker = Math.toRadians(interpolation.getShooterAngleFromInterpolation(distanceToMovingSpeakerTarget)); + // shooterAngleToSpeaker = FiringSolutionsV3.getShooterAngleFromSpeakerR(); + shooterAngleToSpeaker = Math + .toRadians(interpolation.getShooterAngleFromInterpolation(distanceToMovingSpeakerTarget)); SmartDashboard.putNumber("Target Velocity RPM", FiringSolutionsV3.convertToRPM(shooterVelocity)); SmartDashboard.putNumber("Calculated Angle Radians", shooterAngleToSpeaker); SmartDashboard.putNumber("Calculated Angle Degrees", Math.toDegrees(shooterAngleToSpeaker)); - SmartDashboard.putNumber("distance", FiringSolutionsV3.getDistanceToTarget(pose.getX(), pose.getY(), FiringSolutionsV3.speakerTargetX, FiringSolutionsV3.speakerTargetY)); + SmartDashboard.putNumber("distance", FiringSolutionsV3.getDistanceToTarget(pose.getX(), pose.getY(), + FiringSolutionsV3.speakerTargetX, FiringSolutionsV3.speakerTargetY)); SmartDashboard.putNumber("R", FiringSolutionsV3.getSpeakerR()); SmartDashboard.putNumber("C", FiringSolutionsV3.C(distanceToMovingSpeakerTarget)); - SmartDashboard.putNumber("quarticA", FiringSolutionsV3.quarticA(distanceToMovingSpeakerTarget, FiringSolutionsV3.speakerTargetZ)); - SmartDashboard.putNumber("quarticC", FiringSolutionsV3.quarticC(distanceToMovingSpeakerTarget, FiringSolutionsV3.speakerTargetZ)); + SmartDashboard.putNumber("quarticA", + FiringSolutionsV3.quarticA(distanceToMovingSpeakerTarget, FiringSolutionsV3.speakerTargetZ)); + SmartDashboard.putNumber("quarticC", + FiringSolutionsV3.quarticC(distanceToMovingSpeakerTarget, FiringSolutionsV3.speakerTargetZ)); SmartDashboard.putNumber("quarticE", FiringSolutionsV3.quarticE(distanceToMovingSpeakerTarget)); SmartDashboard.putNumber("Distance to Moving Target", distanceToMovingSpeakerTarget); SmartDashboard.putNumber("robot vx", chassisSpeeds.vxMetersPerSecond); SmartDashboard.putNumber("robot vy", chassisSpeeds.vyMetersPerSecond); - double distanceToMovingAmpTarget = FiringSolutionsV3.getDistanceToMovingTarget(pose.getX(), pose.getY(), FiringSolutionsV3.ampTargetX, FiringSolutionsV3.ampTargetY, + double distanceToMovingAmpTarget = FiringSolutionsV3.getDistanceToMovingTarget(pose.getX(), pose.getY(), + FiringSolutionsV3.ampTargetX, FiringSolutionsV3.ampTargetY, chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond, pose.getRotation().getRadians()); FiringSolutionsV3.updateAmpR(distanceToMovingAmpTarget); diff --git a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java index 2ad1605..e67a697 100644 --- a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java @@ -61,6 +61,7 @@ public class SwerveSubsystem extends SubsystemBase { private final VisionSubsystem vision; private final SwerveDrivePoseEstimator swerveOdomEstimator; private SwerveModuleState[] swerveModuleStates; + public static boolean followingPath = false; // Characterization stuff private final MutableMeasure m_appliedVoltage = mutable(Volts.of(0)); @@ -110,12 +111,11 @@ public SwerveSubsystem(VisionSubsystem vision) { this::getChassisSpeeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE this::setChassisSpeeds, // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds new HolonomicPathFollowerConfig( - new PIDConstants(5.0, 0.0, 0.0), // Translation PID constants - new PIDConstants(6.0, 0.0, 0.0), // Rotation PID constants TODO tune + new PIDConstants(5, 0, 0), // Translation PID constants + new PIDConstants(5, 0.0, 0.0), // Rotation PID constants 4.5, // Max module speed, in m/s 0.37268, // Drive base radius in meters. Distance from robot center to furthest module. - new ReplanningConfig(true, true) - ), + new ReplanningConfig(true, true, .5, .25)), checkRedAlliance(), this // Reference to this subsystem to set requirements ); @@ -125,7 +125,7 @@ public SwerveSubsystem(VisionSubsystem vision) { Constants.Swerve.swerveKinematics, getGyroYaw(), getModulePositions(), - new Pose2d(1.35, 5.55, new Rotation2d(0)), + Constants.Vision.startingPose, Constants.Vision.stateStdDevs, Constants.Vision.kSingleTagStdDevs); @@ -140,15 +140,18 @@ public SwerveSubsystem(VisionSubsystem vision) { PathPlannerLogging.setLogActivePathCallback((poses) -> { m_field.getObject("field").setPoses(poses); + + if (poses.isEmpty()){ + followingPath = false; + } else { + followingPath = true; + } }); this.vision = vision; } - /** - * Check alliance for the AutoBuilder. Returns true when red. Using a method for - * better readability - */ + /** Check alliance for the AutoBuilder. Returns true when red. Using a method for better readability */ public BooleanSupplier checkRedAlliance() { return () -> Robot.getAlliance(); } @@ -157,24 +160,25 @@ public BooleanSupplier checkRedAlliance() { public void drive(Translation2d translation, double rotation, boolean fieldRelative, boolean isOpenLoop) { double translationX = translation.getX(); double translationY = translation.getY(); + SwerveModuleState[] desiredStates; if (fieldRelative) { - swerveModuleStates = Constants.Swerve.swerveKinematics + desiredStates = Constants.Swerve.swerveKinematics .toSwerveModuleStates(ChassisSpeeds.discretize(ChassisSpeeds.fromFieldRelativeSpeeds( translationX, translationY, rotation, getHeading()), 0.02)); } else { - swerveModuleStates = Constants.Swerve.swerveKinematics.toSwerveModuleStates(new ChassisSpeeds(translationX, + desiredStates = Constants.Swerve.swerveKinematics.toSwerveModuleStates(new ChassisSpeeds(translationX, translationY, rotation)); } - SwerveDriveKinematics.desaturateWheelSpeeds(swerveModuleStates, Constants.Swerve.maxSpeed); + SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Constants.Swerve.maxSpeed); for (SwerveModule mod : mSwerveMods) { - mod.setDesiredState(swerveModuleStates[mod.moduleNumber], isOpenLoop); + mod.setDesiredState(desiredStates[mod.moduleNumber], isOpenLoop); } } @@ -188,22 +192,21 @@ public void setModuleStates(SwerveModuleState[] desiredStates) { } public SwerveModuleState[] getModuleStates() { - /* - SwerveModuleState[] states = new SwerveModuleState[4]; + return swerveModuleStates; + } + + public void updateModuleStates() { for (SwerveModule mod : mSwerveMods) { - states[mod.moduleNumber] = mod.getState(); + swerveModuleStates[mod.moduleNumber] = mod.getState(); } - return states; - */ - return swerveModuleStates; } /** Drive method for Autos */ public void setChassisSpeeds(ChassisSpeeds speed) { ChassisSpeeds targetSpeeds = ChassisSpeeds.discretize(speed, 0.02); - SwerveModuleState[] targetStates = Constants.Swerve.swerveKinematics.toSwerveModuleStates(targetSpeeds); - setModuleStates(targetStates); + SwerveModuleState[] desiredState = Constants.Swerve.swerveKinematics.toSwerveModuleStates(targetSpeeds); + setModuleStates(desiredState); } public ChassisSpeeds getChassisSpeeds() { @@ -266,6 +269,7 @@ public void voltageDrive(double Voltage) { @Override public void periodic() { + updateModuleStates(); SwerveModulePosition[] modulePositions = getModulePositions(); // Correct pose estimate with multiple vision measurements @@ -275,20 +279,24 @@ public void periodic() { final EstimatedRobotPose estimatedPose = OptionalEstimatedPoseFront.get(); swerveOdomEstimator - .setVisionMeasurementStdDevs(vision.getEstimationStdDevsFront(estimatedPose.estimatedPose.toPose2d())); + .setVisionMeasurementStdDevs( + vision.getEstimationStdDevsFront(estimatedPose.estimatedPose.toPose2d())); - swerveOdomEstimator.addVisionMeasurement(estimatedPose.estimatedPose.toPose2d(), estimatedPose.timestampSeconds); + swerveOdomEstimator.addVisionMeasurement(estimatedPose.estimatedPose.toPose2d(), + estimatedPose.timestampSeconds); } - Optional OptionalEstimatedPoseBack = vision.getEstimatedPoseBack(); + Optional OptionalEstimatedPoseBack = vision.getEstimatedPoseBack(); if (OptionalEstimatedPoseBack.isPresent()) { final EstimatedRobotPose estimatedPose2 = OptionalEstimatedPoseBack.get(); swerveOdomEstimator - .setVisionMeasurementStdDevs(vision.getEstimationStdDevsBack(estimatedPose2.estimatedPose.toPose2d())); + .setVisionMeasurementStdDevs( + vision.getEstimationStdDevsBack(estimatedPose2.estimatedPose.toPose2d())); - swerveOdomEstimator.addVisionMeasurement(estimatedPose2.estimatedPose.toPose2d(), estimatedPose2.timestampSeconds); + swerveOdomEstimator.addVisionMeasurement(estimatedPose2.estimatedPose.toPose2d(), + estimatedPose2.timestampSeconds); } // Logging @@ -302,7 +310,7 @@ public void periodic() { for (SwerveModule mod : mSwerveMods) { SmartDashboard.putNumber("Mod " + mod.moduleNumber + " CANcoder", mod.getCANcoder().getDegrees()); SmartDashboard.putNumber("Mod " + mod.moduleNumber + " Angle", modulePositions[mod.moduleNumber].angle.getDegrees()); - SmartDashboard.putNumber("Mod " + mod.moduleNumber + " Velocity", mod.getState().speedMetersPerSecond); + SmartDashboard.putNumber("Mod " + mod.moduleNumber + " Velocity", swerveModuleStates[mod.moduleNumber].speedMetersPerSecond); } SmartDashboard.putString("Obodom", getPose().toString()); @@ -312,20 +320,34 @@ public void periodic() { } public void sysidroutine(SysIdRoutineLog log) { - log.motor("drive-left") + log.motor("drive-BR") .voltage( m_appliedVoltage.mut_replace( - mSwerveMods[3].getMotorVoltage(), Volts)) + mSwerveMods[3].getMotorVoltage() * RobotController.getBatteryVoltage(), Volts)) .linearPosition(m_distance.mut_replace(mSwerveMods[3].getPosition().distanceMeters, Meters)) .linearVelocity( m_velocity.mut_replace(mSwerveMods[3].getMotorVelocity(), MetersPerSecond)); - log.motor("drive-right") + log.motor("drive-FL") .voltage( m_appliedVoltage.mut_replace( mSwerveMods[0].getMotorVoltage() * RobotController.getBatteryVoltage(), Volts)) - .linearPosition(m_distance.mut_replace(mSwerveMods[3].getPosition().distanceMeters, Meters)) + .linearPosition(m_distance.mut_replace(mSwerveMods[0].getPosition().distanceMeters, Meters)) .linearVelocity( m_velocity.mut_replace(mSwerveMods[0].getMotorVelocity(), MetersPerSecond)); + log.motor("drive-FR") + .voltage( + m_appliedVoltage.mut_replace( + mSwerveMods[1].getMotorVoltage() * RobotController.getBatteryVoltage(), Volts)) + .linearPosition(m_distance.mut_replace(mSwerveMods[1].getPosition().distanceMeters, Meters)) + .linearVelocity( + m_velocity.mut_replace(mSwerveMods[1].getMotorVelocity(), MetersPerSecond)); + log.motor("drive-BL") + .voltage( + m_appliedVoltage.mut_replace( + mSwerveMods[2].getMotorVoltage() * RobotController.getBatteryVoltage(), Volts)) + .linearPosition(m_distance.mut_replace(mSwerveMods[2].getPosition().distanceMeters, Meters)) + .linearVelocity( + m_velocity.mut_replace(mSwerveMods[2].getMotorVelocity(), MetersPerSecond)); } public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { @@ -340,50 +362,80 @@ public Command sysIdDynamic(SysIdRoutine.Direction direction) { public Command pathToSource() { if (!Robot.getAlliance()) { return AutoBuilder.pathfindToPose(new Pose2d(1.21, 0.96, Rotation2d.fromDegrees(58.79)), - new PathConstraints(Constants.Auto.kMaxSpeedMetersPerSecond, Constants.Auto.kMaxAccelerationMetersPerSecondSquared, Constants.Auto.kMaxAngularSpeedRadiansPerSecond, Constants.Auto.kMaxAngularSpeedRadiansPerSecondSquared)); + new PathConstraints(Constants.Auto.kMaxSpeedMetersPerSecond, + Constants.Auto.kMaxAccelerationMetersPerSecondSquared, + Constants.Auto.kMaxAngularSpeedRadiansPerSecond, + Constants.Auto.kMaxAngularSpeedRadiansPerSecondSquared)); } else { return AutoBuilder.pathfindToPose(new Pose2d(15.47, 1.50, Rotation2d.fromDegrees(-59.53)), - new PathConstraints(Constants.Auto.kMaxSpeedMetersPerSecond, Constants.Auto.kMaxAccelerationMetersPerSecondSquared, Constants.Auto.kMaxAngularSpeedRadiansPerSecond, Constants.Auto.kMaxAngularSpeedRadiansPerSecondSquared)); + new PathConstraints(Constants.Auto.kMaxSpeedMetersPerSecond, + Constants.Auto.kMaxAccelerationMetersPerSecondSquared, + Constants.Auto.kMaxAngularSpeedRadiansPerSecond, + Constants.Auto.kMaxAngularSpeedRadiansPerSecondSquared)); } } public Command pathToAmp() { if (!Robot.getAlliance()) { return AutoBuilder.pathfindToPose(new Pose2d(1.84, 7.59, Rotation2d.fromDegrees(90.37)), - new PathConstraints(Constants.Auto.kMaxSpeedMetersPerSecond, Constants.Auto.kMaxAccelerationMetersPerSecondSquared, Constants.Auto.kMaxAngularSpeedRadiansPerSecond, Constants.Auto.kMaxAngularSpeedRadiansPerSecondSquared)); + new PathConstraints(Constants.Auto.kMaxSpeedMetersPerSecond, + Constants.Auto.kMaxAccelerationMetersPerSecondSquared, + Constants.Auto.kMaxAngularSpeedRadiansPerSecond, + Constants.Auto.kMaxAngularSpeedRadiansPerSecondSquared)); } else { return AutoBuilder.pathfindToPose(new Pose2d(14.74, 7.52, Rotation2d.fromDegrees(90.37)), - new PathConstraints(Constants.Auto.kMaxSpeedMetersPerSecond, Constants.Auto.kMaxAccelerationMetersPerSecondSquared, Constants.Auto.kMaxAngularSpeedRadiansPerSecond, Constants.Auto.kMaxAngularSpeedRadiansPerSecondSquared)); + new PathConstraints(Constants.Auto.kMaxSpeedMetersPerSecond, + Constants.Auto.kMaxAccelerationMetersPerSecondSquared, + Constants.Auto.kMaxAngularSpeedRadiansPerSecond, + Constants.Auto.kMaxAngularSpeedRadiansPerSecondSquared)); } } public Command pathToMidfieldChain() { if (!Robot.getAlliance()) { return AutoBuilder.pathfindToPose(new Pose2d(6.29, 4.08, Rotation2d.fromDegrees(180)), - new PathConstraints(Constants.Auto.kMaxSpeedMetersPerSecond, Constants.Auto.kMaxAccelerationMetersPerSecondSquared, Constants.Auto.kMaxAngularSpeedRadiansPerSecond, Constants.Auto.kMaxAngularSpeedRadiansPerSecondSquared)); + new PathConstraints(Constants.Auto.kMaxSpeedMetersPerSecond, + Constants.Auto.kMaxAccelerationMetersPerSecondSquared, + Constants.Auto.kMaxAngularSpeedRadiansPerSecond, + Constants.Auto.kMaxAngularSpeedRadiansPerSecondSquared)); } else { return AutoBuilder.pathfindToPose(new Pose2d(10.26, 4.10, Rotation2d.fromDegrees(0)), - new PathConstraints(Constants.Auto.kMaxSpeedMetersPerSecond, Constants.Auto.kMaxAccelerationMetersPerSecondSquared, Constants.Auto.kMaxAngularSpeedRadiansPerSecond, Constants.Auto.kMaxAngularSpeedRadiansPerSecondSquared)); + new PathConstraints(Constants.Auto.kMaxSpeedMetersPerSecond, + Constants.Auto.kMaxAccelerationMetersPerSecondSquared, + Constants.Auto.kMaxAngularSpeedRadiansPerSecond, + Constants.Auto.kMaxAngularSpeedRadiansPerSecondSquared)); } } public Command pathToSourceChain() { if (!Robot.getAlliance()) { return AutoBuilder.pathfindToPose(new Pose2d(4.18, 2.79, Rotation2d.fromDegrees(59.47)), - new PathConstraints(Constants.Auto.kMaxSpeedMetersPerSecond, Constants.Auto.kMaxAccelerationMetersPerSecondSquared, Constants.Auto.kMaxAngularSpeedRadiansPerSecond, Constants.Auto.kMaxAngularSpeedRadiansPerSecondSquared)); + new PathConstraints(Constants.Auto.kMaxSpeedMetersPerSecond, + Constants.Auto.kMaxAccelerationMetersPerSecondSquared, + Constants.Auto.kMaxAngularSpeedRadiansPerSecond, + Constants.Auto.kMaxAngularSpeedRadiansPerSecondSquared)); } else { return AutoBuilder.pathfindToPose(new Pose2d(12.43, 2.90, Rotation2d.fromDegrees(121.26)), - new PathConstraints(Constants.Auto.kMaxSpeedMetersPerSecond, Constants.Auto.kMaxAccelerationMetersPerSecondSquared, Constants.Auto.kMaxAngularSpeedRadiansPerSecond, Constants.Auto.kMaxAngularSpeedRadiansPerSecondSquared)); + new PathConstraints(Constants.Auto.kMaxSpeedMetersPerSecond, + Constants.Auto.kMaxAccelerationMetersPerSecondSquared, + Constants.Auto.kMaxAngularSpeedRadiansPerSecond, + Constants.Auto.kMaxAngularSpeedRadiansPerSecondSquared)); } } public Command pathToAmpChain() { if (!Robot.getAlliance()) { return AutoBuilder.pathfindToPose(new Pose2d(4.20, 5.30, Rotation2d.fromDegrees(-58.21)), - new PathConstraints(Constants.Auto.kMaxSpeedMetersPerSecond, Constants.Auto.kMaxAccelerationMetersPerSecondSquared, Constants.Auto.kMaxAngularSpeedRadiansPerSecond, Constants.Auto.kMaxAngularSpeedRadiansPerSecondSquared)); + new PathConstraints(Constants.Auto.kMaxSpeedMetersPerSecond, + Constants.Auto.kMaxAccelerationMetersPerSecondSquared, + Constants.Auto.kMaxAngularSpeedRadiansPerSecond, + Constants.Auto.kMaxAngularSpeedRadiansPerSecondSquared)); } else { return AutoBuilder.pathfindToPose(new Pose2d(12.50, 5.25, Rotation2d.fromDegrees(-120.96)), - new PathConstraints(Constants.Auto.kMaxSpeedMetersPerSecond, Constants.Auto.kMaxAccelerationMetersPerSecondSquared, Constants.Auto.kMaxAngularSpeedRadiansPerSecond, Constants.Auto.kMaxAngularSpeedRadiansPerSecondSquared)); + new PathConstraints(Constants.Auto.kMaxSpeedMetersPerSecond, + Constants.Auto.kMaxAccelerationMetersPerSecondSquared, + Constants.Auto.kMaxAngularSpeedRadiansPerSecond, + Constants.Auto.kMaxAngularSpeedRadiansPerSecondSquared)); } } diff --git a/src/main/java/frc/robot/subsystems/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/VisionSubsystem.java index 71d1f96..850494f 100644 --- a/src/main/java/frc/robot/subsystems/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/VisionSubsystem.java @@ -58,25 +58,32 @@ public VisionSubsystem() { Constants.Vision.kTagLayout.setOrigin(OriginPosition.kBlueAllianceWallRightSide); - photonEstimatorFront = new PhotonPoseEstimator( //TODO Fix multi tag - Constants.Vision.kTagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, aprilTagCameraFront, Constants.Vision.kRobotToCamFront); + photonEstimatorFront = new PhotonPoseEstimator( + Constants.Vision.kTagLayout, PoseStrategy.CLOSEST_TO_LAST_POSE, aprilTagCameraFront, Constants.Vision.kRobotToCamFront); photonEstimatorBack = new PhotonPoseEstimator( - Constants.Vision.kTagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, aprilTagCameraBack, Constants.Vision.kRobotToCamBack); - - photonEstimatorFront.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); - photonEstimatorBack.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); + Constants.Vision.kTagLayout, PoseStrategy.CLOSEST_TO_LAST_POSE, aprilTagCameraBack, Constants.Vision.kRobotToCamBack); + + photonEstimatorFront.setLastPose(Constants.Vision.startingPose); + photonEstimatorBack.setLastPose(Constants.Vision.startingPose); + + // 2024 field quality makes multitag impractical + //photonEstimatorFront.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); + //photonEstimatorBack.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); } - public PhotonPipelineResult getLatestResultATF() { // Get the latest result for the April Tag camera + /** Get the latest result from the front April Tag camera */ + public PhotonPipelineResult getLatestResultATF() { return aprilTagCameraFront.getLatestResult(); } + /** Get the latest result from the back April Tag camera */ public PhotonPipelineResult getLatestResultATB() { return aprilTagCameraBack.getLatestResult(); } - public PhotonPipelineResult getLatestResultN() { // Get the latest result for the Note camera + /** Get the latest result from the Note camera */ + public PhotonPipelineResult getLatestResultN() { return noteCamera.getLatestResult(); } @@ -131,10 +138,10 @@ public Matrix getEstimationStdDevsFront(Pose2d estimatedPose) { return estStdDevs; avgDist /= numTags; // Decrease std devs if multiple targets are visible - if (numTags > 1) - estStdDevs = Constants.Vision.kMultiTagStdDevs; + //if (numTags > 1) + // estStdDevs = Constants.Vision.kMultiTagStdDevs; // Increase std devs based on (average) distance - if (numTags == 1 && avgDist > 4) + if (/*numTags == 1 && */avgDist > 4) estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE); else estStdDevs = estStdDevs.times(1 + (avgDist * avgDist / 30)); @@ -158,8 +165,8 @@ public Matrix getEstimationStdDevsBack(Pose2d estimatedPose) { return estStdDevs; avgDist /= numTags; // Decrease std devs if multiple targets are visible - if (numTags > 1) - estStdDevs = Constants.Vision.kMultiTagStdDevs; + //if (numTags > 1) + // estStdDevs = Constants.Vision.kMultiTagStdDevs; // Increase std devs based on (average) distance if (/*numTags == 1 &&*/ avgDist > 4) estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE); From 0853e09b6712068aaec66f69711e8c7cf7971fe4 Mon Sep 17 00:00:00 2001 From: RavenRain44 <89553123+RavenRain44@users.noreply.github.com> Date: Fri, 8 Mar 2024 17:21:28 -0600 Subject: [PATCH 21/49] Finished the LED Code --- src/main/java/frc/robot/RobotContainer.java | 3 +- .../robot/commands/ToBreakOrNotToBreak.java | 5 +- .../frc/robot/subsystems/LEDSubsystem.java | 50 ++++++++++++------- 3 files changed, 34 insertions(+), 24 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index bc68cd2..c36586a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -110,10 +110,9 @@ public class RobotContainer { private final VisionSubsystem m_VisionSubsystem = new VisionSubsystem(); private final SwerveSubsystem m_SwerveSubsystem = new SwerveSubsystem(m_VisionSubsystem); public final ShooterSubsystem m_Shoota = new ShooterSubsystem(m_SwerveSubsystem); - private final LEDSubsystem m_LEDSubsystem = new LEDSubsystem(m_VisionSubsystem, m_Shoota); - //private final LEDSubsystem m_LEDSubsystem = new LEDSubsystem(m_VisionSubsystem); private final ElevatorSubsystem m_ElevatorSubsystem = new ElevatorSubsystem(); private final IntexerSubsystem m_IntexerSubsystem = new IntexerSubsystem(); + private final LEDSubsystem m_LEDSubsystem = new LEDSubsystem(m_VisionSubsystem, m_Shoota, m_IntexerSubsystem, m_SwerveSubsystem); private final SendableChooser autoChooser; diff --git a/src/main/java/frc/robot/commands/ToBreakOrNotToBreak.java b/src/main/java/frc/robot/commands/ToBreakOrNotToBreak.java index 7199e56..193bb39 100644 --- a/src/main/java/frc/robot/commands/ToBreakOrNotToBreak.java +++ b/src/main/java/frc/robot/commands/ToBreakOrNotToBreak.java @@ -13,10 +13,9 @@ public class ToBreakOrNotToBreak extends Command { LEDSubsystem ledSubsystem; /** Creates a new ToBreakOrNotToBreak. */ - public ToBreakOrNotToBreak(IntexerSubsystem intexer, LEDSubsystem ledSubsystem) { + public ToBreakOrNotToBreak(IntexerSubsystem intexer) { // Use addRequirements() here to declare subsystem dependencies. this.intexer = intexer; - this.ledSubsystem = ledSubsystem; addRequirements(intexer); } @@ -30,10 +29,8 @@ public void initialize() { public void execute() { if (!intexer.shooterBreak()){ intexer.setShooterIntake(.5); - ledSubsystem.hasNote = false; } else { intexer.setShooterIntake(0); - ledSubsystem.hasNote = true; } } diff --git a/src/main/java/frc/robot/subsystems/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/LEDSubsystem.java index ae2dc0c..0d003e6 100644 --- a/src/main/java/frc/robot/subsystems/LEDSubsystem.java +++ b/src/main/java/frc/robot/subsystems/LEDSubsystem.java @@ -27,17 +27,17 @@ public class LEDSubsystem extends SubsystemBase { public Boolean atSpeed = false; public Boolean[] inputBooleans = { - false, // Amp -0 Rainbow Pattern #1 ASK ANDREW ABOUT HOW TO DO - false, // Source -1 Rainbow Pattern #2 ASK ANDREW ABOUT HOW TO DO - false, // Climb -2 Rainbow Pattern #3 ASK ANDREW ABOUT HOW TO DO + false, // Amp -0 Rainbow Pattern #1 + false, // Source -1 Rainbow Pattern #2 + false, // Climb -2 Rainbow Pattern #3 false, // Blank/DC -3 None false, // Note Detected -4 White Solid false, // Charging -5 Green Pulse (HasNote) false, // At Speed -6 Green BLink (HasNote) false, // Charging -7 Magenta Pulse (NoNote) false, // At Speed -8 Magenta BLink (NoNote) - false, // Note in Intake -9 Orange Blink ASK ANDREW ABOUT HOW TO DO - false, // Note in Shooter -10 Orange Solid ASK ANDREW ABOUT HOW TO DO + false, // Note in Intake -9 Orange Blink + false, // Note in Shooter -10 Orange Solid false, // Alliance Color -11 Red Pulse false // Alliance Color -12 Blue Pulse }; @@ -45,12 +45,16 @@ public class LEDSubsystem extends SubsystemBase { // Use subsystems VisionSubsystem vision; ShooterSubsystem shooter; + IntexerSubsystem intexer; + SwerveSubsystem swerve; /** Creates a new LEDSubsystem. */ - public LEDSubsystem(VisionSubsystem m_VisionSubsystem, ShooterSubsystem m_ShooterSubsystem) { + public LEDSubsystem(VisionSubsystem m_VisionSubsystem, ShooterSubsystem m_ShooterSubsystem, IntexerSubsystem m_IntexerSubsystem, SwerveSubsystem m_SwerveSubsystem) { this.vision = m_VisionSubsystem; this.shooter = m_ShooterSubsystem; + this.intexer = m_IntexerSubsystem; + this.swerve = m_SwerveSubsystem; } @Override @@ -80,24 +84,34 @@ private void set() { // Decimal phase // HasNote for ChargingOuttake and AtSpeed if (hasNote) { // Converts from simple inputs to boolean if (chargingOuttake) { - inputBooleans[5] = true; - inputBooleans[6] = false; + inputBooleans[5] = true; inputBooleans[6] = false; } else if (atSpeed) { - inputBooleans[5] = false; - inputBooleans[6] = true; + inputBooleans[5] = false; inputBooleans[6] = true; } - inputBooleans[7] = false; - inputBooleans[8] = false; + inputBooleans[7] = false; inputBooleans[8] = false; } else { if (chargingOuttake) { - inputBooleans[7] = true; - inputBooleans[8] = false; + inputBooleans[7] = true; inputBooleans[8] = false; } else if (atSpeed) { - inputBooleans[7] = false; - inputBooleans[8] = true; + inputBooleans[7] = false; inputBooleans[8] = true; } - inputBooleans[5] = false; - inputBooleans[6] = false; + inputBooleans[5] = false; inputBooleans[6] = false; + } + + // Check if pathfinding + if (SwerveSubsystem.followingPath) { + inputBooleans[0] = true; + } else { + inputBooleans[0] = false; + } + + // Check beam breaks + if (intexer.intakeBreak()) { + inputBooleans[9] = true; + inputBooleans[10] = false; + } else if (intexer.shooterBreak()) { + inputBooleans[9] = false; + inputBooleans[10] = true; } } From c54d9425734f954422fca2e44dbae3fb37aea339 Mon Sep 17 00:00:00 2001 From: Camille-Jones <459cmjo2@students.olatheschools.com> Date: Fri, 8 Mar 2024 17:33:10 -0600 Subject: [PATCH 22/49] auto385938 --- .../deploy/pathplanner/autos/5N-1M-left.auto | 4 +-- .../pathplanner/paths/backCloseLeft.path | 6 ++-- .../pathplanner/paths/backCloseMiddle.path | 6 ++-- .../pathplanner/paths/backCloseRight.path | 6 ++-- .../pathplanner/paths/backMidAmpLeft.path | 6 ++-- .../pathplanner/paths/backMidAmpRight.path | 6 ++-- .../pathplanner/paths/backMidSourceLeft.path | 6 ++-- .../pathplanner/paths/backMidSourceRight.path | 6 ++-- .../deploy/pathplanner/paths/toCloseLeft.path | 6 ++-- .../pathplanner/paths/toCloseMiddle.path | 6 ++-- .../pathplanner/paths/toCloseRight.path | 6 ++-- .../pathplanner/paths/toMidAmpLeft.path | 34 +++++++++---------- .../pathplanner/paths/toMidAmpRight.path | 6 ++-- .../pathplanner/paths/toMidSourceLeft.path | 6 ++-- .../pathplanner/paths/toMidSourceRight.path | 6 ++-- .../paths/toRightCloseFromRight.path | 6 ++-- 16 files changed, 61 insertions(+), 61 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/5N-1M-left.auto b/src/main/deploy/pathplanner/autos/5N-1M-left.auto index 7b57ebe..3c7d412 100644 --- a/src/main/deploy/pathplanner/autos/5N-1M-left.auto +++ b/src/main/deploy/pathplanner/autos/5N-1M-left.auto @@ -2,8 +2,8 @@ "version": 1.0, "startingPose": { "position": { - "x": 1.3771774811958186, - "y": 6.50308873064302 + "x": 1.3479282065452507, + "y": 6.620085829245293 }, "rotation": 0 }, diff --git a/src/main/deploy/pathplanner/paths/backCloseLeft.path b/src/main/deploy/pathplanner/paths/backCloseLeft.path index aa8d9e9..bbebd94 100644 --- a/src/main/deploy/pathplanner/paths/backCloseLeft.path +++ b/src/main/deploy/pathplanner/paths/backCloseLeft.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.0, + "maxAcceleration": 8.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, @@ -45,5 +45,5 @@ "reversed": false, "folder": "Close", "previewStartingState": null, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/backCloseMiddle.path b/src/main/deploy/pathplanner/paths/backCloseMiddle.path index 92599d7..0e9cc01 100644 --- a/src/main/deploy/pathplanner/paths/backCloseMiddle.path +++ b/src/main/deploy/pathplanner/paths/backCloseMiddle.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.0, + "maxAcceleration": 8.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, @@ -45,5 +45,5 @@ "reversed": false, "folder": "Close", "previewStartingState": null, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/backCloseRight.path b/src/main/deploy/pathplanner/paths/backCloseRight.path index 7212e82..3216dff 100644 --- a/src/main/deploy/pathplanner/paths/backCloseRight.path +++ b/src/main/deploy/pathplanner/paths/backCloseRight.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.0, + "maxAcceleration": 8.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, @@ -45,5 +45,5 @@ "reversed": false, "folder": "Close", "previewStartingState": null, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/backMidAmpLeft.path b/src/main/deploy/pathplanner/paths/backMidAmpLeft.path index 919fc31..2927175 100644 --- a/src/main/deploy/pathplanner/paths/backMidAmpLeft.path +++ b/src/main/deploy/pathplanner/paths/backMidAmpLeft.path @@ -48,8 +48,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.0, + "maxAcceleration": 8.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, @@ -61,5 +61,5 @@ "reversed": false, "folder": "Midfield Paths", "previewStartingState": null, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/backMidAmpRight.path b/src/main/deploy/pathplanner/paths/backMidAmpRight.path index 5c18520..d0d5918 100644 --- a/src/main/deploy/pathplanner/paths/backMidAmpRight.path +++ b/src/main/deploy/pathplanner/paths/backMidAmpRight.path @@ -48,8 +48,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.0, + "maxAcceleration": 8.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, @@ -61,5 +61,5 @@ "reversed": false, "folder": "Midfield Paths", "previewStartingState": null, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/backMidSourceLeft.path b/src/main/deploy/pathplanner/paths/backMidSourceLeft.path index ddecd4e..01e80ad 100644 --- a/src/main/deploy/pathplanner/paths/backMidSourceLeft.path +++ b/src/main/deploy/pathplanner/paths/backMidSourceLeft.path @@ -48,8 +48,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.0, + "maxAcceleration": 8.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, @@ -61,5 +61,5 @@ "reversed": false, "folder": "Midfield Paths", "previewStartingState": null, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/backMidSourceRight.path b/src/main/deploy/pathplanner/paths/backMidSourceRight.path index e1309ba..6f01df6 100644 --- a/src/main/deploy/pathplanner/paths/backMidSourceRight.path +++ b/src/main/deploy/pathplanner/paths/backMidSourceRight.path @@ -48,8 +48,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.0, + "maxAcceleration": 8.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, @@ -61,5 +61,5 @@ "reversed": false, "folder": "Midfield Paths", "previewStartingState": null, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/toCloseLeft.path b/src/main/deploy/pathplanner/paths/toCloseLeft.path index 7be358d..8b49246 100644 --- a/src/main/deploy/pathplanner/paths/toCloseLeft.path +++ b/src/main/deploy/pathplanner/paths/toCloseLeft.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.0, + "maxAcceleration": 8.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, @@ -45,5 +45,5 @@ "reversed": false, "folder": "Close", "previewStartingState": null, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/toCloseMiddle.path b/src/main/deploy/pathplanner/paths/toCloseMiddle.path index 5b59258..bdaf095 100644 --- a/src/main/deploy/pathplanner/paths/toCloseMiddle.path +++ b/src/main/deploy/pathplanner/paths/toCloseMiddle.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.0, + "maxAcceleration": 8.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, @@ -45,5 +45,5 @@ "reversed": false, "folder": "Close", "previewStartingState": null, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/toCloseRight.path b/src/main/deploy/pathplanner/paths/toCloseRight.path index cd6183b..7866d27 100644 --- a/src/main/deploy/pathplanner/paths/toCloseRight.path +++ b/src/main/deploy/pathplanner/paths/toCloseRight.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.0, + "maxAcceleration": 8.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, @@ -45,5 +45,5 @@ "reversed": false, "folder": "Close", "previewStartingState": null, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/toMidAmpLeft.path b/src/main/deploy/pathplanner/paths/toMidAmpLeft.path index 1d5e6b6..c88ccbc 100644 --- a/src/main/deploy/pathplanner/paths/toMidAmpLeft.path +++ b/src/main/deploy/pathplanner/paths/toMidAmpLeft.path @@ -8,40 +8,40 @@ }, "prevControl": null, "nextControl": { - "x": 1.9774373943047188, - "y": 6.568574707220673 + "x": 1.1334335257744161, + "y": 6.7858317189318464 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.4538759813861684, - "y": 6.288594049872185 + "x": 2.4983996761342704, + "y": 6.308093566305898 }, "prevControl": { - "x": 2.0398800070691956, - "y": 6.313400996790026 + "x": 1.0868292314408643, + "y": 6.394516246593248 }, "nextControl": { - "x": 4.565348418107764, - "y": 6.269094533438474 + "x": 2.976137828760219, + "y": 6.2788442916553295 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.286830526155116, - "y": 6.873579542883553 + "x": 4.087610265481815, + "y": 6.4445901813418836 }, "prevControl": { - "x": 4.916339713914584, - "y": 6.698083894980142 + "x": 3.7073696950244264, + "y": 6.3470925991733225 }, "nextControl": { - "x": 5.8563656726391695, - "y": 7.143359349112842 + "x": 4.698061762695483, + "y": 6.601116206268465 }, "isLocked": false, "linkedName": null @@ -64,8 +64,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.0, + "maxAcceleration": 8.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, @@ -77,5 +77,5 @@ "reversed": false, "folder": "Midfield Paths", "previewStartingState": null, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/toMidAmpRight.path b/src/main/deploy/pathplanner/paths/toMidAmpRight.path index 75d6640..70a9a58 100644 --- a/src/main/deploy/pathplanner/paths/toMidAmpRight.path +++ b/src/main/deploy/pathplanner/paths/toMidAmpRight.path @@ -48,8 +48,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.0, + "maxAcceleration": 8.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, @@ -61,5 +61,5 @@ "reversed": false, "folder": "Midfield Paths", "previewStartingState": null, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/toMidSourceLeft.path b/src/main/deploy/pathplanner/paths/toMidSourceLeft.path index 6a01a6b..f9eed3c 100644 --- a/src/main/deploy/pathplanner/paths/toMidSourceLeft.path +++ b/src/main/deploy/pathplanner/paths/toMidSourceLeft.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.0, + "maxAcceleration": 8.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, @@ -45,5 +45,5 @@ "reversed": false, "folder": "Midfield Paths", "previewStartingState": null, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/toMidSourceRight.path b/src/main/deploy/pathplanner/paths/toMidSourceRight.path index 6158840..83a6a49 100644 --- a/src/main/deploy/pathplanner/paths/toMidSourceRight.path +++ b/src/main/deploy/pathplanner/paths/toMidSourceRight.path @@ -48,8 +48,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.0, + "maxAcceleration": 8.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, @@ -61,5 +61,5 @@ "reversed": false, "folder": "Midfield Paths", "previewStartingState": null, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/toRightCloseFromRight.path b/src/main/deploy/pathplanner/paths/toRightCloseFromRight.path index a646d79..04f26cb 100644 --- a/src/main/deploy/pathplanner/paths/toRightCloseFromRight.path +++ b/src/main/deploy/pathplanner/paths/toRightCloseFromRight.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.0, + "maxAcceleration": 8.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, @@ -45,5 +45,5 @@ "reversed": false, "folder": "Close", "previewStartingState": null, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file From 9f339ba8d7a8eac93ab32f59a678a1f64d53e60f Mon Sep 17 00:00:00 2001 From: Chief Technology Officer Date: Fri, 8 Mar 2024 17:39:01 -0600 Subject: [PATCH 23/49] I will never forgive Linus Torvalds --- README.md | 2 -- 1 file changed, 2 deletions(-) diff --git a/README.md b/README.md index 815ecb1..dc3b7d4 100644 --- a/README.md +++ b/README.md @@ -4,5 +4,3 @@ Thanks to dirtbikerxz for the [template](https://github.com/dirtbikerxz/BaseTalo Using PhotonVision with a Raspberry Pi 4, SDS Mk4i swerve modules, and PathPlanner for autos and trajectory generation. [![Team 1710 2024 Robot Adonis](https://img.youtube.com/vi/qDYD3rMLS-k/0.jpg)](https://www.youtube.com/watch?v=qDYD3rMLS-k) - -[![Team 1710 2024 Robot Adonis](https://img.youtube.com/vi/qDYD3rMLS-k/0.jpg)](https://www.youtube.com/watch?v=qDYD3rMLS-k) \ No newline at end of file From b59e2a361a7e383b65dfb2caddcd4b71beb6edcf Mon Sep 17 00:00:00 2001 From: Mr-A768 Date: Sat, 9 Mar 2024 00:53:46 -0600 Subject: [PATCH 24/49] robot aquired but time expired --- src/main/java/frc/robot/Constants.java | 7 +- src/main/java/frc/robot/RobotContainer.java | 71 ++++++++++++------- .../frc/robot/commands/ElevationManual.java | 2 +- .../commands/IntakeThroughShooterPart2.java | 6 +- .../java/frc/robot/commands/IntexBestHex.java | 2 +- .../robot/subsystems/ElevatorSubsystem.java | 2 +- .../robot/subsystems/ShooterSubsystem.java | 30 ++++---- 7 files changed, 75 insertions(+), 45 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 61bf2ac..fc26a01 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -22,6 +22,7 @@ import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.Unit; import edu.wpi.first.wpilibj.DataLogManager; import frc.lib.util.COTSTalonFXSwerveConstants; import frc.lib.util.SwerveModuleConstants; @@ -61,7 +62,7 @@ private static AprilTagFieldLayout errorWrapper() { } public static final class Swerve { - public static final String canivore = "uno"; + public static final String canivore = "rex"; public static final int pigeonID = 13; public static final COTSTalonFXSwerveConstants chosenModule = @@ -178,7 +179,9 @@ public static final class Mod3 { public static final class Shooter { public static final double intakeAngleRadians = 0.56; public static final double idleSpeedRPM = 1300; - public static final double angleOffsetManual = Units.degreesToRadians(68.2); + public static final double wristAngleMax = Units.degreesToRadians(62.7); + public static final double wristAngleMin = Units.degreesToRadians(-28.8); + public static final double angleOffsetManual = Units.degreesToRadians(62.7); public static final double angleOffsetAuto = Units.degreesToRadians(75.5); } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9ae9052..1d69b08 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -80,9 +80,11 @@ public class RobotContainer { /** Mech X */ private final JoystickButton shooterToSubwoofer = new JoystickButton(mech, XboxController.Button.kX.value); /** Mech RB */ - private final JoystickButton primeShooterSpeedSpeaker = new JoystickButton(mech, XboxController.Button.kRightBumper.value); + private final JoystickButton primeShooterSpeedSpeaker = new JoystickButton(mech, + XboxController.Button.kRightBumper.value); /** Mech LB */ - private final JoystickButton primeShooterSpeedAmp = new JoystickButton(mech, XboxController.Button.kLeftBumper.value); + private final JoystickButton primeShooterSpeedAmp = new JoystickButton(mech, + XboxController.Button.kLeftBumper.value); /** Mech RT */ private final Trigger mechRT = new Trigger(() -> mech.getRawAxis(rightTrigger) > .5); /** Mech LT */ @@ -130,13 +132,20 @@ public class RobotContainer { public RobotContainer() { // Named commands for PathPlanner autos NamedCommands.registerCommand("Intake", new IntexForAutosByAutos(m_IntexerSubsystem, m_Shoota)); - NamedCommands.registerCommand("Shoot", new AimBot(m_Shoota, m_SwerveSubsystem, m_IntexerSubsystem, FiringSolutionsV3.convertToRPM(m_Shoota.getCalculatedVelocity()))); - NamedCommands.registerCommand("Idle Speed", new InstantCommand(() -> m_Shoota.setShooterVelocity(Constants.Shooter.idleSpeedRPM))); - NamedCommands.registerCommand("Target Speed", new InstantCommand(() -> m_Shoota.setShooterVelocity(FiringSolutionsV3.convertToRPM(m_Shoota.getCalculatedVelocity())))); - NamedCommands.registerCommand("Set Shooter Intake", new InstantCommand(() -> m_IntexerSubsystem.setShooterIntake(.9))); - NamedCommands.registerCommand("Stop Shooter Intake", new InstantCommand(() -> m_IntexerSubsystem.setShooterIntake(0))); - NamedCommands.registerCommand("Note Sniffer", new NoteSniffer(m_SwerveSubsystem, m_VisionSubsystem, m_IntexerSubsystem, m_Shoota)); - NamedCommands.registerCommand("Note Sniffer2", new NoteSniffer(m_SwerveSubsystem, m_VisionSubsystem, m_IntexerSubsystem, m_Shoota)); + NamedCommands.registerCommand("Shoot", new AimBot(m_Shoota, m_SwerveSubsystem, m_IntexerSubsystem, + FiringSolutionsV3.convertToRPM(m_Shoota.getCalculatedVelocity()))); + NamedCommands.registerCommand("Idle Speed", + new InstantCommand(() -> m_Shoota.setShooterVelocity(Constants.Shooter.idleSpeedRPM))); + NamedCommands.registerCommand("Target Speed", new InstantCommand(() -> m_Shoota + .setShooterVelocity(FiringSolutionsV3.convertToRPM(m_Shoota.getCalculatedVelocity())))); + NamedCommands.registerCommand("Set Shooter Intake", + new InstantCommand(() -> m_IntexerSubsystem.setShooterIntake(.9))); + NamedCommands.registerCommand("Stop Shooter Intake", + new InstantCommand(() -> m_IntexerSubsystem.setShooterIntake(0))); + NamedCommands.registerCommand("Note Sniffer", + new NoteSniffer(m_SwerveSubsystem, m_VisionSubsystem, m_IntexerSubsystem, m_Shoota)); + NamedCommands.registerCommand("Note Sniffer2", + new NoteSniffer(m_SwerveSubsystem, m_VisionSubsystem, m_IntexerSubsystem, m_Shoota)); m_SwerveSubsystem.setDefaultCommand( new TeleopSwerve( @@ -189,7 +198,8 @@ private void configureButtonBindings() { // Reset Odometry resetOdom.onTrue(new InstantCommand(() -> m_SwerveSubsystem.zeroHeading()).alongWith( - new InstantCommand(() -> m_SwerveSubsystem.setPose(new Pose2d(1.35, 5.55, new Rotation2d(0)))))); + new InstantCommand(() -> m_SwerveSubsystem + .setPose(new Pose2d(1.35, 5.55, new Rotation2d(0)))))); // Intexer intex.whileTrue(new IntexBestHex(m_IntexerSubsystem, true, driver)); @@ -203,23 +213,31 @@ private void configureButtonBindings() { // Prime for Speaker primeShooterSpeedSpeaker - .whileTrue(new InstantCommand(() -> m_Shoota.setShooterVelocity(FiringSolutionsV3.convertToRPM(m_Shoota.getCalculatedVelocity())))) - .onFalse(new InstantCommand(() -> m_Shoota.setShooterVelocity(Constants.Shooter.idleSpeedRPM))); + .whileTrue(new InstantCommand(() -> m_Shoota.setShooterVelocity( + FiringSolutionsV3.convertToRPM(m_Shoota.getCalculatedVelocity())))) + .onFalse(new InstantCommand( + () -> m_Shoota.setShooterVelocity(Constants.Shooter.idleSpeedRPM))); // Prime for Amp primeShooterSpeedAmp.whileTrue(new InstantCommand(() -> m_Shoota.setShooterVelocity(3417.8))) - .onFalse(new InstantCommand(() -> m_Shoota.setShooterVelocity(Constants.Shooter.idleSpeedRPM))); + .onFalse(new InstantCommand( + () -> m_Shoota.setShooterVelocity(Constants.Shooter.idleSpeedRPM))); // Elevator - elevatorDown.onTrue(new ElevatorSet(m_ElevatorSubsystem, Constants.Elevator.minHeightMeters)); - elevatorUp.onTrue(new ElevatorSet(m_ElevatorSubsystem, Constants.Elevator.maxHeightMeters)); + elevatorDown.onTrue(new ElevatorSet(m_ElevatorSubsystem, Constants.Elevator.minHeightMeters) + .alongWith(new RizzLevel(m_Shoota, Constants.Shooter.intakeAngleRadians))); + elevatorUp.onTrue(new ElevatorSet(m_ElevatorSubsystem, Constants.Elevator.maxHeightMeters) + .alongWith(new RizzLevel(m_Shoota, 0.0))); // Zero wrist - zeroShooter.onTrue(new InstantCommand(() -> m_Shoota.resetWristEncoders(Constants.Shooter.angleOffsetManual))); // Set encoder to zero - autoZeroShooter.onTrue(new ZeroRizz(m_Shoota).andThen(new RizzLevel(m_Shoota, Constants.Shooter.intakeAngleRadians))); + zeroShooter.onTrue(new InstantCommand( + () -> m_Shoota.resetWristEncoders(Constants.Shooter.angleOffsetManual))); // Set encoder to zero + autoZeroShooter.onTrue(new ZeroRizz(m_Shoota) + .andThen(new RizzLevel(m_Shoota, Constants.Shooter.intakeAngleRadians))); // Wrist - shooterToIntake.onTrue(new RizzLevel(m_Shoota, Constants.Shooter.intakeAngleRadians)); // Move wrist to intake position + shooterToIntake.onTrue(new RizzLevel(m_Shoota, Constants.Shooter.intakeAngleRadians)); // Move wrist to intake + // position // Amp Preset shooterToAmp.onTrue(new RizzLevel(m_Shoota, -0.48)) @@ -239,14 +257,19 @@ private void configureButtonBindings() { intakeThroughShooter.whileTrue(new IntakeThroughShooter(m_Shoota, m_IntexerSubsystem, mech)) .onFalse(new IntakeThroughShooterPart2(m_Shoota, m_IntexerSubsystem, mech)); - // Characterization tests - /*dynamicForward.whileTrue(m_SwerveSubsystem.sysIdDynamic(Direction.kForward)); - dynamicBackward.whileTrue(m_SwerveSubsystem.sysIdDynamic(Direction.kReverse)); - quasistaticForward.whileTrue(m_SwerveSubsystem.sysIdQuasistatic(Direction.kForward)); - quasistaticBackwards.whileTrue(m_SwerveSubsystem.sysIdQuasistatic(Direction.kReverse));*/ + // Characterization tests + /* + * dynamicForward.whileTrue(m_SwerveSubsystem.sysIdDynamic(Direction.kForward)); + * dynamicBackward.whileTrue(m_SwerveSubsystem.sysIdDynamic(Direction.kReverse)) + * ; + * quasistaticForward.whileTrue(m_SwerveSubsystem.sysIdQuasistatic(Direction. + * kForward)); + * quasistaticBackwards.whileTrue(m_SwerveSubsystem.sysIdQuasistatic(Direction. + * kReverse)); + */ } - public void stopAll () { + public void stopAll() { m_Shoota.setShooterVelocity(0); m_Shoota.setManualWristSpeed(0); m_IntexerSubsystem.setALL(0); diff --git a/src/main/java/frc/robot/commands/ElevationManual.java b/src/main/java/frc/robot/commands/ElevationManual.java index 376bf3e..859ec3e 100644 --- a/src/main/java/frc/robot/commands/ElevationManual.java +++ b/src/main/java/frc/robot/commands/ElevationManual.java @@ -34,7 +34,7 @@ public void initialize() { public void execute() { lastElevatorSetpoint = m_elevatorSubsystem.getSetpoint(); - double value = axis.getAsDouble(); + double value = -axis.getAsDouble(); value = Math.pow(value, 3); diff --git a/src/main/java/frc/robot/commands/IntakeThroughShooterPart2.java b/src/main/java/frc/robot/commands/IntakeThroughShooterPart2.java index ab6043a..83e54f8 100644 --- a/src/main/java/frc/robot/commands/IntakeThroughShooterPart2.java +++ b/src/main/java/frc/robot/commands/IntakeThroughShooterPart2.java @@ -35,14 +35,14 @@ public void initialize() { timer.reset(); timer.start(); controller.setRumble(RumbleType.kBothRumble, 0); + shooter.setWristByAngle(Constants.Shooter.intakeAngleRadians); } - + // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { shooter.setShooterVelocity(0); - intexer.setALL(.35); - shooter.setWristByAngle(Constants.Shooter.intakeAngleRadians); + intexer.setALL(.3); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/commands/IntexBestHex.java b/src/main/java/frc/robot/commands/IntexBestHex.java index b100fe9..c0b2579 100644 --- a/src/main/java/frc/robot/commands/IntexBestHex.java +++ b/src/main/java/frc/robot/commands/IntexBestHex.java @@ -35,7 +35,7 @@ public void initialize() { public void execute() { if (in) { // Hood logic to run forwards or backwards if (intexer.intakeBreak() && !intexer.shooterBreak()) { // If note is not at shooter yet - intexer.setALL(.35); + intexer.setALL(.3); } else if (!intexer.intakeBreak() && intexer.shooterBreak()) { // Stop note if at shooter controller.setRumble(RumbleType.kBothRumble, 0.75); intexer.setALL(0); diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index dc8fab7..a4a8ed0 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -111,7 +111,7 @@ public void periodic() { SmartDashboard.putNumber("Elevator Left Supply Current", m_elevatorLeft.getSupplyCurrent().getValueAsDouble()); SmartDashboard.putNumber("Elevator Right Supply Current", m_elevatorRight.getSupplyCurrent().getValueAsDouble()); - SmartDashboard.putNumber("LaserCan Ambient", measurement.ambient); + //SmartDashboard.putNumber("LaserCan Ambient", measurement.ambient); revolutionCount = m_elevatorLeft.getPosition().getValueAsDouble(); //FiringSolutionsV3.updateHeight(getHeight()); //TODO: test this diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index f3b2c27..dc21516 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -7,6 +7,8 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.lib.math.FiringSolutionsV3; import frc.lib.math.Interpolations; +import frc.robot.Constants; + import com.revrobotics.CANSparkBase; import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkPIDController; @@ -60,12 +62,10 @@ public class ShooterSubsystem extends SubsystemBase { private double distanceToMovingSpeakerTarget = .94; public double lastWristAngleSetpoint = 0.0; public boolean manualOverride = false; - public double wristAngleUpperBound; - public double wristAngleLowerBound; + public double wristAngleUpperBound = Constants.Shooter.wristAngleMax; + public double wristAngleLowerBound = Constants.Shooter.wristAngleMin; // Constants - private final double wristAngleMax = 0.0; - private final double wristAngleMin = 0.0; private final Timer speedTimer = new Timer(); private final Interpolations interpolation = new Interpolations(); private final int m_WristCurrentMax = 84; @@ -81,13 +81,17 @@ public ShooterSubsystem(SwerveSubsystem swerve) { m_PositionEncoder = m_Wrist.getEncoder(); m_WristEncoder = new DutyCycleEncoder(0); + // Wrist Encoder Reset + m_WristEncoder.reset(); + m_PositionEncoder.setPosition(0); + // Spark Max Setup shootaTop.restoreFactoryDefaults(); shootaBot.restoreFactoryDefaults(); m_Wrist.restoreFactoryDefaults(); m_Wrist.setIdleMode(IdleMode.kBrake); - m_Wrist.setInverted(true); + m_Wrist.setInverted(false); shootaBot.setInverted(false); @@ -188,7 +192,7 @@ public void periodic() { /** in RADIANs units MATTER */ public double getCurrentShooterAngle() { if (!ENCFAIL) { - return ((-m_WristEncoder.get() * 2 * Math.PI) / 4) + angleOffset; + return ((m_WristEncoder.get() * 2 * Math.PI) / 4) + angleOffset; } else { return ((m_PositionEncoder.getPosition() * 2 * Math.PI) / 100); } @@ -301,8 +305,8 @@ public void setWristEncoderOffset(double newPosition) { } public void setWristAngleLowerBound(double wristAngleLowerBound) { - if (wristAngleLowerBound < wristAngleMin){ - wristAngleLowerBound = wristAngleMin; + if (wristAngleLowerBound < Constants.Shooter.wristAngleMin){ + wristAngleLowerBound = Constants.Shooter.wristAngleMin; } else if (wristAngleLowerBound > wristAngleUpperBound){ wristAngleLowerBound = wristAngleUpperBound; } else { @@ -311,8 +315,8 @@ public void setWristAngleLowerBound(double wristAngleLowerBound) { } public void setWristAngleUpperBound(double wristAngleUpperBound) { - if (wristAngleUpperBound > wristAngleMax){ - wristAngleUpperBound = wristAngleMax; + if (wristAngleUpperBound > Constants.Shooter.wristAngleMax){ + wristAngleUpperBound = Constants.Shooter.wristAngleMax; } else if (wristAngleUpperBound < wristAngleLowerBound){ wristAngleUpperBound = wristAngleLowerBound; } else { @@ -321,7 +325,7 @@ public void setWristAngleUpperBound(double wristAngleUpperBound) { } /** IN RADIANS */ - public void setWristByAngle(double angle) { //TODO: Implement redundancy + public void setWristByAngle(double angle) { if (isZeroed) { updateWristAngleSetpoint(angle); } @@ -336,9 +340,9 @@ public void setWristByRotations(double newPosition) { public void updateWristAngleSetpoint(double angle) { if (angle != lastWristAngleSetpoint){ - if (lastWristAngleSetpoint < wristAngleLowerBound){ + if (angle < wristAngleLowerBound){ lastWristAngleSetpoint = wristAngleLowerBound; - } else if (lastWristAngleSetpoint > wristAngleUpperBound){ + } else if (angle > wristAngleUpperBound){ lastWristAngleSetpoint = wristAngleUpperBound; } else { lastWristAngleSetpoint = angle; From 805b49834e247dbe9f355c46c9b2ed947ac28758 Mon Sep 17 00:00:00 2001 From: Mr-A768 Date: Sat, 9 Mar 2024 00:58:07 -0600 Subject: [PATCH 25/49] merge conflict jumpscare --- src/main/java/frc/robot/RobotContainer.java | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d6ea4bc..e40b46b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -254,8 +254,7 @@ private void configureButtonBindings() { .onFalse(new IntakeThroughShooterPart2(m_Shoota, m_IntexerSubsystem, mech)); // Characterization tests - /* - * dynamicForward.whileTrue(m_SwerveSubsystem.sysIdDynamic(Direction.kForward)); + /* dynamicForward.whileTrue(m_SwerveSubsystem.sysIdDynamic(Direction.kForward)); * dynamicBackward.whileTrue(m_SwerveSubsystem.sysIdDynamic(Direction.kReverse)); * quasistaticForward.whileTrue(m_SwerveSubsystem.sysIdQuasistatic(Direction.kForward)); * quasistaticBackwards.whileTrue(m_SwerveSubsystem.sysIdQuasistatic(Direction.kReverse)); From 9effd67b62d9f17008826ba50b318bb594b3d201 Mon Sep 17 00:00:00 2001 From: Andrew-K961 Date: Sat, 9 Mar 2024 12:57:57 -0600 Subject: [PATCH 26/49] new robot new blunders --- src/main/deploy/pathplanner/autos/kachow.auto | 5 --- src/main/java/frc/robot/Constants.java | 10 ++--- src/main/java/frc/robot/Robot.java | 4 +- .../frc/robot/commands/ElevationManual.java | 2 +- .../java/frc/robot/commands/NoteSniffer.java | 2 +- .../robot/subsystems/ElevatorSubsystem.java | 43 ++++++------------- .../robot/subsystems/ShooterSubsystem.java | 4 +- 7 files changed, 24 insertions(+), 46 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/kachow.auto b/src/main/deploy/pathplanner/autos/kachow.auto index 0244726..aefbd8d 100644 --- a/src/main/deploy/pathplanner/autos/kachow.auto +++ b/src/main/deploy/pathplanner/autos/kachow.auto @@ -2,13 +2,8 @@ "version": 1.0, "startingPose": { "position": { -<<<<<<< HEAD:src/main/deploy/pathplanner/autos/4N-1M-left.auto - "x": 1.3771774811958186, - "y": 6.50308873064302 -======= "x": 1.45, "y": 6.45 ->>>>>>> f8dcaf8cae43b9b9042643a56208b94ff88cf8f9:src/main/deploy/pathplanner/autos/kachow.auto }, "rotation": 0 }, diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 68ac274..c9aa74e 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -30,14 +30,14 @@ public final class Constants { public static final double stickDeadband = 0.07; public static class Vision { - public static final String kAprilTagCameraFront = "ChristiansThirdEye"; - public static final String kAprilTagCameraBack = "ChristiansFourthEye"; + public static final String kAprilTagCameraFront = "ChristiansFourthEye"; + public static final String kAprilTagCameraBack = "ChristiansThirdEye"; public static final String kNoteCamera = "OnionRing"; public static final Transform3d kRobotToCamFront = - new Transform3d(new Translation3d(Units.inchesToMeters(12), Units.inchesToMeters(1.25), Units.inchesToMeters(13.5)), new Rotation3d(0, 0, 0)); + new Transform3d(new Translation3d(Units.inchesToMeters(12.5), Units.inchesToMeters(-1.25), Units.inchesToMeters(14.75)), new Rotation3d(0, Units.degreesToRadians(15), 0)); public static final Transform3d kRobotToCamBack = - new Transform3d(new Translation3d(Units.inchesToMeters(-8), 0, Units.inchesToMeters(12)), new Rotation3d(0, 0, Math.PI)); + new Transform3d(new Translation3d(Units.inchesToMeters(-6.5), Units.inchesToMeters(11.5), Units.inchesToMeters(15)), new Rotation3d(0, Units.degreesToRadians(15), Math.PI)); // The layout of the AprilTags on the field public static final AprilTagFieldLayout kTagLayout = errorWrapper(); @@ -61,7 +61,7 @@ private static AprilTagFieldLayout errorWrapper() { } public static final class Swerve { - public static final String canivore = "uno"; + public static final String canivore = "rex"; public static final int pigeonID = 13; public static final COTSTalonFXSwerveConstants chosenModule = diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a889959..879379a 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -38,7 +38,7 @@ public class Robot extends TimedRobot { private static boolean redAlliance; - //PowerDistribution PDH = new PowerDistribution(1, ModuleType.kRev); + PowerDistribution PDH = new PowerDistribution(1, ModuleType.kRev); /** * This function is run when the robot is first started up and should be used for any @@ -77,7 +77,7 @@ public void robotInit() { // Access PhotonVision dashboard when connected via usb TODO make work // PortForwarder.add(5800, "10.17.10.11", 5800); - //SmartDashboard.putData(PDH); + SmartDashboard.putData(PDH); // idk if this is useful System.gc(); diff --git a/src/main/java/frc/robot/commands/ElevationManual.java b/src/main/java/frc/robot/commands/ElevationManual.java index 376bf3e..40de12a 100644 --- a/src/main/java/frc/robot/commands/ElevationManual.java +++ b/src/main/java/frc/robot/commands/ElevationManual.java @@ -38,7 +38,7 @@ public void execute() { value = Math.pow(value, 3); - if (Math.abs(value) > .05) { // Crime zone + if (Math.abs(value) > .1) { // Crime zone m_elevatorSubsystem.ManSpin(value); } else { if (!m_elevatorSubsystem.locked){ diff --git a/src/main/java/frc/robot/commands/NoteSniffer.java b/src/main/java/frc/robot/commands/NoteSniffer.java index 594d483..1d0972a 100644 --- a/src/main/java/frc/robot/commands/NoteSniffer.java +++ b/src/main/java/frc/robot/commands/NoteSniffer.java @@ -27,7 +27,7 @@ public class NoteSniffer extends Command { private PIDController rotationPID = new PIDController(0.65, 0.00001, 0.04); private Timer timer = new Timer(); private boolean noteInside = false; - double translationVal = 0.5; + private double translationVal = 0.5; /** Creates a new IntakeWithVision. */ public NoteSniffer(SwerveSubsystem swerve, VisionSubsystem vision, IntexerSubsystem intexer, diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index dc8fab7..d5db165 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -4,14 +4,12 @@ package frc.robot.subsystems; -import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs; -import com.ctre.phoenix6.configs.Slot0Configs; -import com.ctre.phoenix6.configs.Slot1Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.controls.PositionDutyCycle; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.GravityTypeValue; +import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import au.grapplerobotics.ConfigurationFailedException; @@ -21,6 +19,7 @@ import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; public class ElevatorSubsystem extends SubsystemBase { @@ -33,51 +32,35 @@ public class ElevatorSubsystem extends SubsystemBase { private final PositionDutyCycle m_requestPosition = new PositionDutyCycle(0); private final PositionDutyCycle lockPosition = new PositionDutyCycle(0); private final PIDController elevatorPID = new PIDController(0, 0, 0); - private final Slot1Configs encoderConfigSlot1 = new Slot1Configs(); // Constants IN METERS private final double spoolCircumference = 0.0508; private final double gearRatio = 17.33; - private final double maxHeight = .8; // Vars private double revolutionCount; private double currentHeight; private double setHeight; private boolean laser; - LaserCan.Measurement measurement; + private LaserCan.Measurement measurement; public boolean manualOverride = false; public boolean locked = false; public ElevatorSubsystem() { // Falcon setup - m_elevatorLeft.setNeutralMode(NeutralModeValue.Brake); - m_elevatorRight.setControl(new Follower(m_elevatorLeft.getDeviceID(), true)); - TalonFXConfiguration elevatorConfigs = new TalonFXConfiguration(); + elevatorConfigs.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; elevatorConfigs.Slot0.kP = 0.09; + elevatorConfigs.Slot1.kP = 1; + elevatorConfigs.Slot0.GravityType = GravityTypeValue.Elevator_Static; elevatorConfigs.MotorOutput.NeutralMode = NeutralModeValue.Brake; + elevatorConfigs.ClosedLoopRamps.DutyCycleClosedLoopRampPeriod = 0.5; + elevatorConfigs.ClosedLoopRamps.TorqueClosedLoopRampPeriod = 0.5; + elevatorConfigs.ClosedLoopRamps.VoltageClosedLoopRampPeriod = 0.5; - // PID - Slot0Configs encoderConfigSlot0 = new Slot0Configs(); - ClosedLoopRampsConfigs closedloop = new ClosedLoopRampsConfigs(); - - closedloop.withDutyCycleClosedLoopRampPeriod(.5).withTorqueClosedLoopRampPeriod(0.5); - encoderConfigSlot0.kP = .09; - encoderConfigSlot0.kI = .0; - encoderConfigSlot0.kD = .0; - encoderConfigSlot0.kV = .0; - encoderConfigSlot0.kG = .0; - encoderConfigSlot0.GravityType = GravityTypeValue.Elevator_Static; - - encoderConfigSlot1.kP = 1; - encoderConfigSlot1.kI = .0; - encoderConfigSlot1.kD = .0; - encoderConfigSlot1.kV = .01; - - m_elevatorLeft.getConfigurator().apply(encoderConfigSlot0, 0.050); - m_elevatorLeft.getConfigurator().apply(closedloop); + m_elevatorLeft.getConfigurator().apply(elevatorConfigs); + m_elevatorRight.setControl(new Follower(m_elevatorLeft.getDeviceID(), true)); // laser can pid shenanigans elevatorPID.setP(2.5); @@ -111,7 +94,7 @@ public void periodic() { SmartDashboard.putNumber("Elevator Left Supply Current", m_elevatorLeft.getSupplyCurrent().getValueAsDouble()); SmartDashboard.putNumber("Elevator Right Supply Current", m_elevatorRight.getSupplyCurrent().getValueAsDouble()); - SmartDashboard.putNumber("LaserCan Ambient", measurement.ambient); + SmartDashboard.putNumber("LaserCan Ambient", measurement != null ? measurement.ambient : 0); revolutionCount = m_elevatorLeft.getPosition().getValueAsDouble(); //FiringSolutionsV3.updateHeight(getHeight()); //TODO: test this @@ -148,7 +131,7 @@ public void setHeight(double height) { m_elevatorLeft.set(elevatorPID.calculate(getHeightLaserCan(), height)); } else { // Run off encoder double rot = (height / (spoolCircumference * Math.PI)) * gearRatio; - if (getHeightEncoder() < maxHeight) { + if (getHeightEncoder() < Constants.Elevator.maxHeightMeters) { m_elevatorLeft.setControl(m_requestPosition.withPosition(rot).withSlot(1)); } } diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index f3b2c27..1e619db 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -87,7 +87,7 @@ public ShooterSubsystem(SwerveSubsystem swerve) { m_Wrist.restoreFactoryDefaults(); m_Wrist.setIdleMode(IdleMode.kBrake); - m_Wrist.setInverted(true); + m_Wrist.setInverted(false); shootaBot.setInverted(false); @@ -188,7 +188,7 @@ public void periodic() { /** in RADIANs units MATTER */ public double getCurrentShooterAngle() { if (!ENCFAIL) { - return ((-m_WristEncoder.get() * 2 * Math.PI) / 4) + angleOffset; + return ((m_WristEncoder.get() * 2 * Math.PI) / 4) + angleOffset; } else { return ((m_PositionEncoder.getPosition() * 2 * Math.PI) / 100); } From ffd41d0cd7aafdcf3caeaf0e7c99d97a96d067b8 Mon Sep 17 00:00:00 2001 From: RavenRain44 <89553123+RavenRain44@users.noreply.github.com> Date: Sat, 9 Mar 2024 13:17:22 -0600 Subject: [PATCH 27/49] Changed DIO ports --- src/main/java/frc/robot/subsystems/LEDSubsystem.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/LEDSubsystem.java index 0d003e6..44c3973 100644 --- a/src/main/java/frc/robot/subsystems/LEDSubsystem.java +++ b/src/main/java/frc/robot/subsystems/LEDSubsystem.java @@ -12,10 +12,10 @@ import frc.robot.Robot; public class LEDSubsystem extends SubsystemBase { - public DigitalOutput bit1 = new DigitalOutput(2); // Bit 1 (1) - public DigitalOutput bit2 = new DigitalOutput(3); // Bit 2 (2) - public DigitalOutput bit3 = new DigitalOutput(4); // Bit 3 (4) - public DigitalOutput bit4 = new DigitalOutput(5); // Bit 4 (8) + public DigitalOutput bit1 = new DigitalOutput(3); // Bit 1 (1) + public DigitalOutput bit2 = new DigitalOutput(4); // Bit 2 (2) + public DigitalOutput bit3 = new DigitalOutput(5); // Bit 3 (4) + public DigitalOutput bit4 = new DigitalOutput(6); // Bit 4 (8) // Output bits to the LEDs public DigitalOutput[] bits = {bit1, bit2, bit3, bit4}; // Actual Outputs From 61fe73ae658362aa3a7a7fd88998308c15d7ee2f Mon Sep 17 00:00:00 2001 From: Mr-A768 Date: Sat, 9 Mar 2024 16:18:14 -0600 Subject: [PATCH 28/49] AAAAAAAAAAAAAAAAA --- src/main/deploy/pathplanner/autos/kachow.auto | 5 ---- .../java/frc/robot/commands/ManRizzt.java | 1 + .../java/frc/robot/commands/RizzLevel.java | 12 +++------ .../robot/subsystems/ShooterSubsystem.java | 26 +++++++++++-------- 4 files changed, 19 insertions(+), 25 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/kachow.auto b/src/main/deploy/pathplanner/autos/kachow.auto index 0244726..aefbd8d 100644 --- a/src/main/deploy/pathplanner/autos/kachow.auto +++ b/src/main/deploy/pathplanner/autos/kachow.auto @@ -2,13 +2,8 @@ "version": 1.0, "startingPose": { "position": { -<<<<<<< HEAD:src/main/deploy/pathplanner/autos/4N-1M-left.auto - "x": 1.3771774811958186, - "y": 6.50308873064302 -======= "x": 1.45, "y": 6.45 ->>>>>>> f8dcaf8cae43b9b9042643a56208b94ff88cf8f9:src/main/deploy/pathplanner/autos/kachow.auto }, "rotation": 0 }, diff --git a/src/main/java/frc/robot/commands/ManRizzt.java b/src/main/java/frc/robot/commands/ManRizzt.java index 3705c6c..9e3632c 100644 --- a/src/main/java/frc/robot/commands/ManRizzt.java +++ b/src/main/java/frc/robot/commands/ManRizzt.java @@ -46,6 +46,7 @@ public void execute() { } else { if (m_shooterSubsystem.isZeroed){ if (!wristIsLocked){ + m_shooterSubsystem.setManualOverride(false); m_shooterSubsystem.setWristByAngle(m_shooterSubsystem.getCurrentShooterAngle()); wristIsLocked = true; } diff --git a/src/main/java/frc/robot/commands/RizzLevel.java b/src/main/java/frc/robot/commands/RizzLevel.java index 3f75603..9813c2f 100644 --- a/src/main/java/frc/robot/commands/RizzLevel.java +++ b/src/main/java/frc/robot/commands/RizzLevel.java @@ -23,15 +23,13 @@ public RizzLevel(ShooterSubsystem noteAccelerator, double angleInRADIANS) { // Called when the command is initially scheduled. @Override public void initialize() { - + shooter.setManualOverride(false); + shooter.setWristByAngle(angle); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if (shooter.isZeroed){ - shooter.setWristByAngle(angle); - } } // Called once the command ends or is interrupted. @@ -43,10 +41,6 @@ public void end(boolean interrupted) { // Returns true when the command should end. @Override public boolean isFinished() { - if (shooter.isZeroed){ - return shooter.getCurrentShooterAngle() >= (angle - Math.toRadians(.5)) && shooter.getCurrentShooterAngle() < (angle + Math.toRadians(.5)); - } else { - return true; - } + return true; } } diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index dc21516..e25c198 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -58,14 +58,16 @@ public class ShooterSubsystem extends SubsystemBase { public boolean isZeroed = false; public boolean wristIsLocked = false; /** IN RADIANS */ - private double angleOffset = 68.2; + private double angleOffset = Constants.Shooter.angleOffsetManual; private double distanceToMovingSpeakerTarget = .94; public double lastWristAngleSetpoint = 0.0; public boolean manualOverride = false; - public double wristAngleUpperBound = Constants.Shooter.wristAngleMax; - public double wristAngleLowerBound = Constants.Shooter.wristAngleMin; + public double wristAngleUpperBound; + public double wristAngleLowerBound; // Constants + private final double wristAngleMax = 0.0; + private final double wristAngleMin = 0.0; private final Timer speedTimer = new Timer(); private final Interpolations interpolation = new Interpolations(); private final int m_WristCurrentMax = 84; @@ -268,6 +270,7 @@ public void resetWristEncoders(double newOffset) { } public void setManualWristSpeed(double speed) { + manualOverride = true; m_Wrist.set(speed); } @@ -305,8 +308,8 @@ public void setWristEncoderOffset(double newPosition) { } public void setWristAngleLowerBound(double wristAngleLowerBound) { - if (wristAngleLowerBound < Constants.Shooter.wristAngleMin){ - wristAngleLowerBound = Constants.Shooter.wristAngleMin; + if (wristAngleLowerBound < wristAngleMin){ + wristAngleLowerBound = wristAngleMin; } else if (wristAngleLowerBound > wristAngleUpperBound){ wristAngleLowerBound = wristAngleUpperBound; } else { @@ -315,8 +318,8 @@ public void setWristAngleLowerBound(double wristAngleLowerBound) { } public void setWristAngleUpperBound(double wristAngleUpperBound) { - if (wristAngleUpperBound > Constants.Shooter.wristAngleMax){ - wristAngleUpperBound = Constants.Shooter.wristAngleMax; + if (wristAngleUpperBound > wristAngleMax){ + wristAngleUpperBound = wristAngleMax; } else if (wristAngleUpperBound < wristAngleLowerBound){ wristAngleUpperBound = wristAngleLowerBound; } else { @@ -340,13 +343,14 @@ public void setWristByRotations(double newPosition) { public void updateWristAngleSetpoint(double angle) { if (angle != lastWristAngleSetpoint){ - if (angle < wristAngleLowerBound){ + /*if (lastWristAngleSetpoint < wristAngleLowerBound){ lastWristAngleSetpoint = wristAngleLowerBound; - } else if (angle > wristAngleUpperBound){ + } else if (lastWristAngleSetpoint > wristAngleUpperBound){ lastWristAngleSetpoint = wristAngleUpperBound; } else { - lastWristAngleSetpoint = angle; - } + + } */ + lastWristAngleSetpoint = angle; } } From bf30d35d007c03f40f0d6c3f994a365a15d79049 Mon Sep 17 00:00:00 2001 From: Andrew-K961 Date: Sun, 10 Mar 2024 12:16:15 -0500 Subject: [PATCH 29/49] chat is it joever --- src/main/java/frc/robot/Constants.java | 70 ++++++++++-------- src/main/java/frc/robot/Robot.java | 10 ++- src/main/java/frc/robot/RobotContainer.java | 71 ++++++++++--------- src/main/java/frc/robot/commands/AimBot.java | 2 +- .../frc/robot/commands/ElevationManual.java | 10 +-- .../robot/commands/IntakeThroughShooter.java | 2 +- .../commands/IntakeThroughShooterPart2.java | 2 +- .../java/frc/robot/commands/IntexBestHex.java | 2 +- .../robot/commands/IntexForAutosByAutos.java | 6 +- .../java/frc/robot/commands/ManRizzt.java | 7 +- .../java/frc/robot/commands/NoteSniffer.java | 6 +- .../java/frc/robot/commands/RizzLevel.java | 1 - .../java/frc/robot/commands/TeleopSwerve.java | 4 +- .../robot/subsystems/ElevatorSubsystem.java | 22 +++--- .../robot/subsystems/IntexerSubsystem.java | 17 +++-- .../robot/subsystems/ShooterSubsystem.java | 64 ++++++++++------- .../frc/robot/subsystems/SwerveSubsystem.java | 51 +++---------- .../frc/robot/subsystems/VisionSubsystem.java | 10 +-- vendordeps/PathplannerLib.json | 6 +- 19 files changed, 189 insertions(+), 174 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 65d0f4d..c1629a5 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -5,6 +5,7 @@ import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.signals.SensorDirectionValue; +import com.pathplanner.lib.path.PathConstraints; import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; @@ -34,13 +35,16 @@ public static class Vision { public static final String kAprilTagCameraBack = "ChristiansThirdEye"; public static final String kNoteCamera = "OnionRing"; - public static final Transform3d kRobotToCamFront = - new Transform3d(new Translation3d(Units.inchesToMeters(12.5), Units.inchesToMeters(-1.25), Units.inchesToMeters(14.75)), new Rotation3d(0, Units.degreesToRadians(15), 0)); - public static final Transform3d kRobotToCamBack = - new Transform3d(new Translation3d(Units.inchesToMeters(-6.5), Units.inchesToMeters(11.5), Units.inchesToMeters(15)), new Rotation3d(0, Units.degreesToRadians(15), Math.PI)); + public static final Transform3d kRobotToCamFront = new Transform3d( + new Translation3d(Units.inchesToMeters(12.5), Units.inchesToMeters(-1.25), Units.inchesToMeters(14.75)), + new Rotation3d(0, Units.degreesToRadians(15), 0)); + + public static final Transform3d kRobotToCamBack = new Transform3d( + new Translation3d(Units.inchesToMeters(-7), Units.inchesToMeters(11), Units.inchesToMeters(15)), + new Rotation3d(0, Units.degreesToRadians(14), Math.PI)); // The layout of the AprilTags on the field - public static final AprilTagFieldLayout kTagLayout = errorWrapper(); + public static final AprilTagFieldLayout kTagLayout = getFieldLayout(); // The standard deviations of our vision estimated poses, which affect correction rate public static final Matrix kSingleTagStdDevs = VecBuilder.fill(8, 8, 40); @@ -50,7 +54,7 @@ public static class Vision { public static final Pose2d startingPose = new Pose2d(1.35, 5.55, new Rotation2d(0)); } - private static AprilTagFieldLayout errorWrapper() { + private static AprilTagFieldLayout getFieldLayout() { try { AprilTagFieldLayout attemptedKTagLayout = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField(); return attemptedKTagLayout; @@ -64,8 +68,8 @@ public static final class Swerve { public static final String canivore = "rex"; public static final int pigeonID = 13; - public static final COTSTalonFXSwerveConstants chosenModule = - COTSTalonFXSwerveConstants.SDS.MK4i.Falcon500(COTSTalonFXSwerveConstants.SDS.MK4i.driveRatios.L2); + public static final COTSTalonFXSwerveConstants chosenModule = COTSTalonFXSwerveConstants.SDS.MK4i + .Falcon500(COTSTalonFXSwerveConstants.SDS.MK4i.driveRatios.L2); /* Drivetrain Constants */ public static final double trackWidth = Units.inchesToMeters(20.75); @@ -74,11 +78,11 @@ public static final class Swerve { /* Swerve Kinematics * No need to ever change this unless you are not doing a traditional rectangular/square 4 module swerve */ - public static final SwerveDriveKinematics swerveKinematics = new SwerveDriveKinematics( - new Translation2d(wheelBase / 2.0, trackWidth / 2.0), - new Translation2d(wheelBase / 2.0, -trackWidth / 2.0), - new Translation2d(-wheelBase / 2.0, trackWidth / 2.0), - new Translation2d(-wheelBase / 2.0, -trackWidth / 2.0)); + public static final SwerveDriveKinematics swerveKinematics = new SwerveDriveKinematics( + new Translation2d(wheelBase / 2.0, trackWidth / 2.0), + new Translation2d(wheelBase / 2.0, -trackWidth / 2.0), + new Translation2d(-wheelBase / 2.0, trackWidth / 2.0), + new Translation2d(-wheelBase / 2.0, -trackWidth / 2.0)); /* Module Gear Ratios */ public static final double driveGearRatio = chosenModule.driveGearRatio; @@ -134,50 +138,51 @@ public static final class Swerve { public static final NeutralModeValue driveNeutralMode = NeutralModeValue.Brake; /* Module Specific Constants */ - /* Front Left Module - Module 0 */ + /** Front Left Module - Module 0 */ public static final class Mod0 { public static final int driveMotorID = 1; public static final int angleMotorID = 3; public static final int canCoderID = 2; public static final Rotation2d angleOffset = Rotation2d.fromDegrees(112.5); - public static final SwerveModuleConstants constants = - new SwerveModuleConstants(driveMotorID, angleMotorID, canCoderID, angleOffset); + public static final SwerveModuleConstants constants = new SwerveModuleConstants(driveMotorID, angleMotorID, + canCoderID, angleOffset); } - /* Front Right Module - Module 1 */ + /** Front Right Module - Module 1 */ public static final class Mod1 { public static final int driveMotorID = 4; public static final int angleMotorID = 6; public static final int canCoderID = 5; public static final Rotation2d angleOffset = Rotation2d.fromDegrees(-79.4); - public static final SwerveModuleConstants constants = - new SwerveModuleConstants(driveMotorID, angleMotorID, canCoderID, angleOffset); + public static final SwerveModuleConstants constants = new SwerveModuleConstants(driveMotorID, angleMotorID, + canCoderID, angleOffset); } - - /* Back Left Module - Module 2 */ + + /** Back Left Module - Module 2 */ public static final class Mod2 { public static final int driveMotorID = 7; public static final int angleMotorID = 9; public static final int canCoderID = 8; public static final Rotation2d angleOffset = Rotation2d.fromDegrees(-80.5); - public static final SwerveModuleConstants constants = - new SwerveModuleConstants(driveMotorID, angleMotorID, canCoderID, angleOffset); + public static final SwerveModuleConstants constants = new SwerveModuleConstants(driveMotorID, angleMotorID, + canCoderID, angleOffset); } - /* Back Right Module - Module 3 */ + /** Back Right Module - Module 3 */ public static final class Mod3 { public static final int driveMotorID = 10; public static final int angleMotorID = 12; public static final int canCoderID = 11; public static final Rotation2d angleOffset = Rotation2d.fromDegrees(114); - public static final SwerveModuleConstants constants = - new SwerveModuleConstants(driveMotorID, angleMotorID, canCoderID, angleOffset); + public static final SwerveModuleConstants constants = new SwerveModuleConstants(driveMotorID, angleMotorID, + canCoderID, angleOffset); } } public static final class Shooter { public static final double intakeAngleRadians = 0.56; public static final double idleSpeedRPM = 1300; + public static final double angleOffsetBottom = Units.degreesToRadians(2.3); public static final double wristAngleMax = Units.degreesToRadians(62.7); public static final double wristAngleMin = Units.degreesToRadians(-28.8); public static final double angleOffsetManual = Units.degreesToRadians(62.7); @@ -185,8 +190,8 @@ public static final class Shooter { } public static final class Elevator { - public static final double maxHeightMeters = 0.78; - public static final double minHeightMeters = 0.015; + public static final double maxHeightMeters = 0.6; + public static final double minHeightMeters = -0.01; } public static final class Auto { @@ -199,9 +204,12 @@ public static final class Auto { public static final double kPYController = 1; public static final double kPThetaController = 1; - /* Constraint for the motion profilied robot angle controller */ - public static final TrapezoidProfile.Constraints kThetaControllerConstraints = - new TrapezoidProfile.Constraints( + public static final PathConstraints PathfindingConstraints = new PathConstraints(kMaxSpeedMetersPerSecond, + kMaxAccelerationMetersPerSecondSquared, kMaxAngularSpeedRadiansPerSecond, + kMaxAngularSpeedRadiansPerSecondSquared); + + /** Constraint for the motion profilied robot angle controller */ + public static final TrapezoidProfile.Constraints kThetaControllerConstraints = new TrapezoidProfile.Constraints( kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared); } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 879379a..0675530 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -38,7 +38,7 @@ public class Robot extends TimedRobot { private static boolean redAlliance; - PowerDistribution PDH = new PowerDistribution(1, ModuleType.kRev); + PowerDistribution PDH; /** * This function is run when the robot is first started up and should be used for any @@ -50,6 +50,8 @@ public void robotInit() { // autonomous chooser on the dashboard. m_robotContainer = new RobotContainer(); + PDH = new PowerDistribution(1, ModuleType.kRev); + // Disable LiveWindow since we don't use it LiveWindow.disableAllTelemetry(); LiveWindow.setEnabled(false); @@ -97,6 +99,7 @@ public void robotPeriodic() { // and running subsystem periodic() methods. This must be called from the robot's periodic // block in order for anything in the Command-based framework to work. CommandScheduler.getInstance().run(); + SmartDashboard.putNumber("Match Time", DriverStation.getMatchTime()); } /** Gets the current alliance, true is red */ @@ -108,8 +111,10 @@ public boolean checkRedAlliance() { Optional alliance = DriverStation.getAlliance(); if (alliance.isPresent()) { return alliance.get() == DriverStation.Alliance.Red; + } else { + DataLogManager.log("ERROR: Alliance not found. Defaulting to Blue"); + return false; } - return false; } /** This function is called once each time the robot enters Disabled mode. */ @@ -144,6 +149,7 @@ public void autonomousInit() { } FiringSolutionsV3.resetAllR(); + m_robotContainer.zeroWristEncoders(); } /** This function is called periodically during autonomous. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index fb638f5..af40a9a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -109,9 +109,8 @@ public class RobotContainer { /* Subsystems */ private final VisionSubsystem m_VisionSubsystem = new VisionSubsystem(); private final SwerveSubsystem m_SwerveSubsystem = new SwerveSubsystem(m_VisionSubsystem); - public final ShooterSubsystem m_Shoota = new ShooterSubsystem(m_SwerveSubsystem); - // private final LEDSubsystem m_LEDSubsystem = new - // LEDSubsystem(m_VisionSubsystem); + private final ShooterSubsystem m_ShooterSubsystem = new ShooterSubsystem(m_SwerveSubsystem); + // private final LEDSubsystem m_LEDSubsystem = new LEDSubsystem(m_VisionSubsystem); private final ElevatorSubsystem m_ElevatorSubsystem = new ElevatorSubsystem(); private final IntexerSubsystem m_IntexerSubsystem = new IntexerSubsystem(); @@ -122,18 +121,18 @@ public class RobotContainer { */ public RobotContainer() { // Named commands for PathPlanner autos - NamedCommands.registerCommand("Intake", new IntexForAutosByAutos(m_IntexerSubsystem, m_Shoota)); - NamedCommands.registerCommand("Shoot", new AimBot(m_Shoota, m_SwerveSubsystem, m_IntexerSubsystem, FiringSolutionsV3.convertToRPM(m_Shoota.getCalculatedVelocity()))); - NamedCommands.registerCommand("Idle Speed", new InstantCommand(() -> m_Shoota.setShooterVelocity(Constants.Shooter.idleSpeedRPM))); - NamedCommands.registerCommand("Target Speed", new InstantCommand(() -> m_Shoota.setShooterVelocity(FiringSolutionsV3.convertToRPM(m_Shoota.getCalculatedVelocity())))); + NamedCommands.registerCommand("Intake", new IntexForAutosByAutos(m_IntexerSubsystem, m_ShooterSubsystem)); + NamedCommands.registerCommand("Shoot", new AimBot(m_ShooterSubsystem, m_SwerveSubsystem, m_IntexerSubsystem, FiringSolutionsV3.convertToRPM(m_ShooterSubsystem.getCalculatedVelocity()))); + NamedCommands.registerCommand("Idle Speed", new InstantCommand(() -> m_ShooterSubsystem.setShooterVelocity(Constants.Shooter.idleSpeedRPM))); + NamedCommands.registerCommand("Target Speed", new InstantCommand(() -> m_ShooterSubsystem.setShooterVelocity(FiringSolutionsV3.convertToRPM(m_ShooterSubsystem.getCalculatedVelocity())))); NamedCommands.registerCommand("Set Shooter Intake", new InstantCommand(() -> m_IntexerSubsystem.setShooterIntake(.9))); NamedCommands.registerCommand("Stop Shooter Intake", new InstantCommand(() -> m_IntexerSubsystem.setShooterIntake(0))); - NamedCommands.registerCommand("Note Sniffer", new NoteSniffer(m_SwerveSubsystem, m_VisionSubsystem, m_IntexerSubsystem, m_Shoota)); - NamedCommands.registerCommand("Note Sniffer2", new NoteSniffer(m_SwerveSubsystem, m_VisionSubsystem, m_IntexerSubsystem, m_Shoota)); + NamedCommands.registerCommand("Note Sniffer", new NoteSniffer(m_SwerveSubsystem, m_VisionSubsystem, m_IntexerSubsystem, m_ShooterSubsystem)); + NamedCommands.registerCommand("Note Sniffer2", new NoteSniffer(m_SwerveSubsystem, m_VisionSubsystem, m_IntexerSubsystem, m_ShooterSubsystem)); m_SwerveSubsystem.setDefaultCommand( new TeleopSwerve( - m_SwerveSubsystem, m_VisionSubsystem, m_Shoota, + m_SwerveSubsystem, m_VisionSubsystem, m_ShooterSubsystem, () -> -driver.getRawAxis(leftVerticalAxis), () -> -driver.getRawAxis(leftHorizontalAxis), () -> -driver.getRawAxis(rightHorizontalAxis), @@ -147,7 +146,7 @@ public RobotContainer() { m_ElevatorSubsystem, () -> -mech.getRawAxis(leftVerticalAxis))); - m_Shoota.setDefaultCommand(new ManRizzt(m_Shoota, () -> -mech.getRawAxis(rightVerticalAxis), + m_ShooterSubsystem.setDefaultCommand(new ManRizzt(m_ShooterSubsystem, () -> -mech.getRawAxis(rightVerticalAxis), () -> shooterToAntiDefense.getAsBoolean())); // m_LEDSubsystem.setAllianceColor(); @@ -164,21 +163,19 @@ public RobotContainer() { /** * Use this method to define your button->command mappings. Buttons can be - * created by - * instantiating a {@link GenericHID} or one of its subclasses ({@link + * created by instantiating a {@link GenericHID} or one of its subclasses ({@link * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing - * it to a {@link - * edu.wpi.first.wpilibj2.command.button.JoystickButton}. + * it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}. */ private void configureButtonBindings() { /* DRIVER BUTTONS */ // Lock on to speaker - targetSpeaker.whileTrue(new MissileLock(m_Shoota, "speaker")); - targetAmp.whileTrue(new MissileLock(m_Shoota, "amp")); + targetSpeaker.whileTrue(new MissileLock(m_ShooterSubsystem, "speaker")); + targetAmp.whileTrue(new MissileLock(m_ShooterSubsystem, "amp")); // Shooter - targetSpeaker.or(targetAmp).and(Shoot).whileTrue(new FIREEE(m_Shoota, m_IntexerSubsystem)); // Main fire + targetSpeaker.or(targetAmp).and(Shoot).whileTrue(new FIREEE(m_ShooterSubsystem, m_IntexerSubsystem)); // Main fire // Reset Odometry resetOdom.onTrue(new InstantCommand(() -> m_SwerveSubsystem.zeroHeading()).alongWith( @@ -200,33 +197,33 @@ private void configureButtonBindings() { // Prime for Speaker primeShooterSpeedSpeaker - .whileTrue(new InstantCommand(() -> m_Shoota.setShooterVelocity( - FiringSolutionsV3.convertToRPM(m_Shoota.getCalculatedVelocity())))) + .whileTrue(new InstantCommand(() -> m_ShooterSubsystem.setShooterVelocity( + FiringSolutionsV3.convertToRPM(m_ShooterSubsystem.getCalculatedVelocity())))) .onFalse(new InstantCommand( - () -> m_Shoota.setShooterVelocity(Constants.Shooter.idleSpeedRPM))); + () -> m_ShooterSubsystem.setShooterVelocity(Constants.Shooter.idleSpeedRPM))); // Prime for Amp - primeShooterSpeedAmp.whileTrue(new InstantCommand(() -> m_Shoota.setShooterVelocity(3417.8))) + primeShooterSpeedAmp.whileTrue(new InstantCommand(() -> m_ShooterSubsystem.setShooterVelocity(3417.8))) .onFalse(new InstantCommand( - () -> m_Shoota.setShooterVelocity(Constants.Shooter.idleSpeedRPM))); + () -> m_ShooterSubsystem.setShooterVelocity(Constants.Shooter.idleSpeedRPM))); // Elevator elevatorDown.onTrue(new ElevatorSet(m_ElevatorSubsystem, Constants.Elevator.minHeightMeters) - .alongWith(new RizzLevel(m_Shoota, Constants.Shooter.intakeAngleRadians))); + .alongWith(new RizzLevel(m_ShooterSubsystem, Constants.Shooter.intakeAngleRadians))); elevatorUp.onTrue(new ElevatorSet(m_ElevatorSubsystem, Constants.Elevator.maxHeightMeters) - .alongWith(new RizzLevel(m_Shoota, 0.0))); + .alongWith(new RizzLevel(m_ShooterSubsystem, 0.0))); // Zero wrist zeroShooter.onTrue(new InstantCommand( - () -> m_Shoota.resetWristEncoders(Constants.Shooter.angleOffsetManual))); // Set encoder to zero - autoZeroShooter.onTrue(new ZeroRizz(m_Shoota) - .andThen(new RizzLevel(m_Shoota, Constants.Shooter.intakeAngleRadians))); + () -> m_ShooterSubsystem.resetWristEncoders(Constants.Shooter.angleOffsetManual))); // Set encoder to zero + autoZeroShooter.onTrue(new ZeroRizz(m_ShooterSubsystem) + .andThen(new RizzLevel(m_ShooterSubsystem, Constants.Shooter.intakeAngleRadians))); // Wrist - shooterToIntake.onTrue(new RizzLevel(m_Shoota, Constants.Shooter.intakeAngleRadians)); // Move wrist to intake position + shooterToIntake.onTrue(new RizzLevel(m_ShooterSubsystem, Constants.Shooter.intakeAngleRadians)); // Move wrist to intake position // Amp Preset - shooterToAmp.onTrue(new RizzLevel(m_Shoota, -0.48)) + shooterToAmp.onTrue(new RizzLevel(m_ShooterSubsystem, -0.48)) .onTrue(new ElevatorSet(m_ElevatorSubsystem, Constants.Elevator.maxHeightMeters)); // xButton.whileTrue(new InstantCommand(() -> m_Shoota.SetOffsetVelocity(2000))) @@ -237,11 +234,11 @@ private void configureButtonBindings() { resetR.onTrue(new InstantCommand(() -> FiringSolutionsV3.resetAllR())); // Kill Shooter - rightStick.onTrue(new InstantCommand(() -> m_Shoota.setShooterVelocity(0))); + rightStick.onTrue(new InstantCommand(() -> m_ShooterSubsystem.setShooterVelocity(0))); // Intake Through Shooter - intakeThroughShooter.whileTrue(new IntakeThroughShooter(m_Shoota, m_IntexerSubsystem, mech)) - .onFalse(new IntakeThroughShooterPart2(m_Shoota, m_IntexerSubsystem, mech)); + intakeThroughShooter.whileTrue(new IntakeThroughShooter(m_ShooterSubsystem, m_IntexerSubsystem, mech)) + .onFalse(new IntakeThroughShooterPart2(m_ShooterSubsystem, m_IntexerSubsystem, mech)); // Characterization tests dynamicForward.whileTrue(m_SwerveSubsystem.sysIdDynamic(Direction.kForward)); @@ -251,12 +248,16 @@ private void configureButtonBindings() { } public void stopAll() { - m_Shoota.setShooterVelocity(0); - m_Shoota.setManualWristSpeed(0); + m_ShooterSubsystem.setShooterVelocity(0); + m_ShooterSubsystem.setManualWristSpeed(0); m_IntexerSubsystem.setALL(0); m_ElevatorSubsystem.setPositionWithEncoder(m_ElevatorSubsystem.getPosition()); } + public void zeroWristEncoders(){ + m_ShooterSubsystem.restartWristEncoders(); + } + /** * Use this to pass the autonomous command to the main {@link Robot} class. * diff --git a/src/main/java/frc/robot/commands/AimBot.java b/src/main/java/frc/robot/commands/AimBot.java index a9f759d..4ede3f2 100644 --- a/src/main/java/frc/robot/commands/AimBot.java +++ b/src/main/java/frc/robot/commands/AimBot.java @@ -25,7 +25,7 @@ public class AimBot extends Command { private double speed; private Timer timer = new Timer(); - /** Creates a new AutoShoot. */ + /** Creates a new AimBot. */ public AimBot(ShooterSubsystem shooterSubsystem, SwerveSubsystem swerve, IntexerSubsystem intexer, double speed) { this.shooter = shooterSubsystem; this.speed = speed; diff --git a/src/main/java/frc/robot/commands/ElevationManual.java b/src/main/java/frc/robot/commands/ElevationManual.java index 40de12a..4f6b621 100644 --- a/src/main/java/frc/robot/commands/ElevationManual.java +++ b/src/main/java/frc/robot/commands/ElevationManual.java @@ -6,8 +6,10 @@ import java.util.function.DoubleSupplier; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants; import frc.robot.subsystems.ElevatorSubsystem; public class ElevationManual extends Command { @@ -35,17 +37,17 @@ public void execute() { lastElevatorSetpoint = m_elevatorSubsystem.getSetpoint(); double value = axis.getAsDouble(); - + value = MathUtil.applyDeadband(value, Constants.stickDeadband); value = Math.pow(value, 3); - if (Math.abs(value) > .1) { // Crime zone +// if (Math.abs(value) > .1) { // Crime zone m_elevatorSubsystem.ManSpin(value); - } else { +/* } else { if (!m_elevatorSubsystem.locked){ lastElevatorSetpoint = m_elevatorSubsystem.getPosition(); m_elevatorSubsystem.setPositionWithEncoder(lastElevatorSetpoint); } - } + }*/ SmartDashboard.putBoolean("locked", locked); SmartDashboard.putNumber("locked value", lockedValue); diff --git a/src/main/java/frc/robot/commands/IntakeThroughShooter.java b/src/main/java/frc/robot/commands/IntakeThroughShooter.java index 690f329..b7d0ee4 100644 --- a/src/main/java/frc/robot/commands/IntakeThroughShooter.java +++ b/src/main/java/frc/robot/commands/IntakeThroughShooter.java @@ -29,6 +29,7 @@ public IntakeThroughShooter(ShooterSubsystem shooterSub, IntexerSubsystem intex, // Called when the command is initially scheduled. @Override public void initialize() { + shooter.setWristByAngle(.66); } // Called every time the scheduler runs while the command is scheduled. @@ -36,7 +37,6 @@ public void initialize() { public void execute() { shooter.setShooterVelocity(-1000); intexer.setALL(-.5); - shooter.setWristByAngle(.66); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/commands/IntakeThroughShooterPart2.java b/src/main/java/frc/robot/commands/IntakeThroughShooterPart2.java index 83e54f8..5bf4509 100644 --- a/src/main/java/frc/robot/commands/IntakeThroughShooterPart2.java +++ b/src/main/java/frc/robot/commands/IntakeThroughShooterPart2.java @@ -56,7 +56,7 @@ public void end(boolean interrupted) { // Returns true when the command should end. @Override public boolean isFinished() { - if (!intexer.intakeThroughShooterPart2isReady || intexer.shooterBreak() || timer.get() > 1.5) { + if (/*intexer.intakeThroughShooterPart2isReady ||*/ intexer.shooterBreak() || timer.get() > 1.5) { return true; } else { return false; diff --git a/src/main/java/frc/robot/commands/IntexBestHex.java b/src/main/java/frc/robot/commands/IntexBestHex.java index c0b2579..9472ef8 100644 --- a/src/main/java/frc/robot/commands/IntexBestHex.java +++ b/src/main/java/frc/robot/commands/IntexBestHex.java @@ -40,7 +40,7 @@ public void execute() { controller.setRumble(RumbleType.kBothRumble, 0.75); intexer.setALL(0); } else { // Note is not in robot - intexer.setFrontIntake(.75); + intexer.setFrontIntake(.65); } } else { // Outtake intexer.setALL(-0.5); diff --git a/src/main/java/frc/robot/commands/IntexForAutosByAutos.java b/src/main/java/frc/robot/commands/IntexForAutosByAutos.java index 55b321b..b6fa78e 100644 --- a/src/main/java/frc/robot/commands/IntexForAutosByAutos.java +++ b/src/main/java/frc/robot/commands/IntexForAutosByAutos.java @@ -32,12 +32,12 @@ public void execute() { shooter.setWristByAngle(Constants.Shooter.intakeAngleRadians); if (intexer.intakeBreak() && !intexer.shooterBreak()) { // If note is not at shooter yet - intexer.setALL(.35); + intexer.setALL(.3); } else if (intexer.shooterBreak()) { // Stop note if at shooter intexer.setALL(0); } else { // Note is not in robot - intexer.setFrontIntake(.85); - intexer.setShooterIntake(.35); + intexer.setFrontIntake(.65); + intexer.setShooterIntake(.3); } } diff --git a/src/main/java/frc/robot/commands/ManRizzt.java b/src/main/java/frc/robot/commands/ManRizzt.java index 9e3632c..989a6b6 100644 --- a/src/main/java/frc/robot/commands/ManRizzt.java +++ b/src/main/java/frc/robot/commands/ManRizzt.java @@ -7,8 +7,10 @@ import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants; import frc.robot.subsystems.ShooterSubsystem; public class ManRizzt extends Command { @@ -35,12 +37,13 @@ public void initialize() { @Override public void execute() { - double speedValue = Math.pow(speed.getAsDouble(), 3); + double speedValue = MathUtil.applyDeadband(speed.getAsDouble(), Constants.stickDeadband); + speedValue = Math.pow(speedValue, 3); if (setAngle.getAsBoolean()) { m_shooterSubsystem.setWristByAngle(SmartDashboard.getNumber("Set Wrist Angle", 0)); } else { - if (Math.abs(speedValue) > .05) { + if (Math.abs(speedValue) > .0) { wristIsLocked = false; m_shooterSubsystem.setManualWristSpeed(speedValue); } else { diff --git a/src/main/java/frc/robot/commands/NoteSniffer.java b/src/main/java/frc/robot/commands/NoteSniffer.java index 1d0972a..05fa7aa 100644 --- a/src/main/java/frc/robot/commands/NoteSniffer.java +++ b/src/main/java/frc/robot/commands/NoteSniffer.java @@ -44,7 +44,7 @@ public NoteSniffer(SwerveSubsystem swerve, VisionSubsystem vision, IntexerSubsys public void initialize() { timer.reset(); timer.start(); - translationVal = .5; + translationVal = .25; noteInside = false; } @@ -70,8 +70,8 @@ public void execute() { rotationVal = rotationPID.calculate(yawToNote, swerveSubsystem.getGyroYaw().getRadians()); - intexer.setFrontIntake(.8); - intexer.setShooterIntake(.35); + intexer.setFrontIntake(.65); + intexer.setShooterIntake(.3); } else { rotationVal = 0; } diff --git a/src/main/java/frc/robot/commands/RizzLevel.java b/src/main/java/frc/robot/commands/RizzLevel.java index 9813c2f..564444e 100644 --- a/src/main/java/frc/robot/commands/RizzLevel.java +++ b/src/main/java/frc/robot/commands/RizzLevel.java @@ -23,7 +23,6 @@ public RizzLevel(ShooterSubsystem noteAccelerator, double angleInRADIANS) { // Called when the command is initially scheduled. @Override public void initialize() { - shooter.setManualOverride(false); shooter.setWristByAngle(angle); } diff --git a/src/main/java/frc/robot/commands/TeleopSwerve.java b/src/main/java/frc/robot/commands/TeleopSwerve.java index cb19906..f4d44e2 100644 --- a/src/main/java/frc/robot/commands/TeleopSwerve.java +++ b/src/main/java/frc/robot/commands/TeleopSwerve.java @@ -74,11 +74,12 @@ public void execute() { strafeVal = Math.copySign(Math.pow(strafeVal, 2), strafeVal); if (shooterOverrideSpeaker.getAsBoolean()) { // Lock robot angle to speaker - if (shooterSubsystem.getDistanceToSpeakerWhileMoving() >= 3.5){ + if (shooterSubsystem.getDistanceToSpeakerWhileMoving() >= 3){ controller.setRumble(RumbleType.kBothRumble, 0.5); } else { controller.setRumble(RumbleType.kBothRumble, 0); } + ChassisSpeeds currentSpeed = swerveSubsystem.getChassisSpeeds(); rotationVal = rotationPID.calculate(swerveSubsystem.getHeading().getRadians(), @@ -102,6 +103,7 @@ public void execute() { SmartDashboard.putNumber("Note Yaw", yawToNote); rotationVal = rotationPID.calculate(yawToNote, swerveSubsystem.getGyroYaw().getRadians()); + } else { rotationVal = MathUtil.applyDeadband(rotationSup.getAsDouble(), Constants.stickDeadband); rotationVal = Math.copySign(Math.pow(rotationVal, 2), rotationVal); diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index d5db165..f7184e5 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -24,9 +24,9 @@ public class ElevatorSubsystem extends SubsystemBase { // Devices - public TalonFX m_elevatorLeft = new TalonFX(20); // left leader - public TalonFX m_elevatorRight = new TalonFX(21); - public LaserCan lasercan = new LaserCan(22); + public TalonFX m_elevatorLeft; // left leader + public TalonFX m_elevatorRight; + public LaserCan lasercan; // Falcon stuff private final PositionDutyCycle m_requestPosition = new PositionDutyCycle(0); @@ -48,6 +48,10 @@ public class ElevatorSubsystem extends SubsystemBase { public boolean locked = false; public ElevatorSubsystem() { + m_elevatorLeft = new TalonFX(20); // left leader + m_elevatorRight = new TalonFX(21); + lasercan = new LaserCan(22); + // Falcon setup TalonFXConfiguration elevatorConfigs = new TalonFXConfiguration(); elevatorConfigs.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; @@ -55,15 +59,15 @@ public ElevatorSubsystem() { elevatorConfigs.Slot1.kP = 1; elevatorConfigs.Slot0.GravityType = GravityTypeValue.Elevator_Static; elevatorConfigs.MotorOutput.NeutralMode = NeutralModeValue.Brake; - elevatorConfigs.ClosedLoopRamps.DutyCycleClosedLoopRampPeriod = 0.5; - elevatorConfigs.ClosedLoopRamps.TorqueClosedLoopRampPeriod = 0.5; - elevatorConfigs.ClosedLoopRamps.VoltageClosedLoopRampPeriod = 0.5; + elevatorConfigs.ClosedLoopRamps.DutyCycleClosedLoopRampPeriod = 0.25; + elevatorConfigs.ClosedLoopRamps.TorqueClosedLoopRampPeriod = 0.25; + elevatorConfigs.ClosedLoopRamps.VoltageClosedLoopRampPeriod = 0.25; m_elevatorLeft.getConfigurator().apply(elevatorConfigs); m_elevatorRight.setControl(new Follower(m_elevatorLeft.getDeviceID(), true)); // laser can pid shenanigans - elevatorPID.setP(2.5); + elevatorPID.setP(3); elevatorPID.setI(0); elevatorPID.setD(0); elevatorPID.setTolerance(0.02); @@ -92,8 +96,7 @@ public void periodic() { SmartDashboard.putNumber("LaserCan Meters", getHeightLaserCan()); SmartDashboard.putBoolean("LaserCan failure", lasercanFailureCheck()); SmartDashboard.putNumber("Elevator Left Supply Current", m_elevatorLeft.getSupplyCurrent().getValueAsDouble()); - SmartDashboard.putNumber("Elevator Right Supply Current", - m_elevatorRight.getSupplyCurrent().getValueAsDouble()); + SmartDashboard.putNumber("Elevator Right Supply Current", m_elevatorRight.getSupplyCurrent().getValueAsDouble()); SmartDashboard.putNumber("LaserCan Ambient", measurement != null ? measurement.ambient : 0); revolutionCount = m_elevatorLeft.getPosition().getValueAsDouble(); @@ -129,6 +132,7 @@ public void setHeight(double height) { setHeight = height; if (!lasercanFailureCheck()) { // Run off LaserCan m_elevatorLeft.set(elevatorPID.calculate(getHeightLaserCan(), height)); + m_elevatorLeft.getFault_StatorCurrLimit().getValue(); } else { // Run off encoder double rot = (height / (spoolCircumference * Math.PI)) * gearRatio; if (getHeightEncoder() < Constants.Elevator.maxHeightMeters) { diff --git a/src/main/java/frc/robot/subsystems/IntexerSubsystem.java b/src/main/java/frc/robot/subsystems/IntexerSubsystem.java index 258296a..3641624 100644 --- a/src/main/java/frc/robot/subsystems/IntexerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntexerSubsystem.java @@ -15,17 +15,24 @@ public class IntexerSubsystem extends SubsystemBase { // Devices - private CANSparkBase left = new CANSparkMax(30, MotorType.kBrushless); - private CANSparkBase right = new CANSparkMax(31, MotorType.kBrushless); - private CANSparkBase shooterIntake = new CANSparkMax(10, MotorType.kBrushless); + private CANSparkBase left; + private CANSparkBase right; + private CANSparkBase shooterIntake; public boolean intakeThroughShooterPart2isReady = false; /** Front Intake */ - private DigitalInput breakingBeam = new DigitalInput(2); + private DigitalInput breakingBeam; /** Shooter Intake */ - private DigitalInput beamKamen = new DigitalInput(1); + private DigitalInput beamKamen; public IntexerSubsystem() { + left = new CANSparkMax(30, MotorType.kBrushless); + right = new CANSparkMax(31, MotorType.kBrushless); + shooterIntake = new CANSparkMax(10, MotorType.kBrushless); + + beamKamen = new DigitalInput(1); + breakingBeam = new DigitalInput(2); + left.setIdleMode(IdleMode.kCoast); right.setIdleMode(IdleMode.kCoast); shooterIntake.setIdleMode(IdleMode.kCoast); diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index e25c198..3ca26ba 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -27,9 +27,9 @@ public class ShooterSubsystem extends SubsystemBase { // Devices - private CANSparkBase m_Wrist = new CANSparkMax(13, MotorType.kBrushless); - private CANSparkBase shootaTop = new CANSparkMax(11, MotorType.kBrushless); // leader - private CANSparkBase shootaBot = new CANSparkMax(12, MotorType.kBrushless); + private CANSparkBase m_Wrist; + private CANSparkBase shootaTop; // leader + private CANSparkBase shootaBot; private RelativeEncoder m_VelocityEncoder; private RelativeEncoder m_VelocityEncoder2; @@ -42,11 +42,11 @@ public class ShooterSubsystem extends SubsystemBase { private SparkPIDController topPID; // PID Constants - private double velocityP = 0.00018; + private double velocityP = 0.0001; private double velocityI = 5.2e-7; - private double velocityD = 1.5; + private double velocityD = 0; - private double positionP = 2; + private double positionP = 1; private double positionI = 0; private double positionD = 0; @@ -77,16 +77,16 @@ public class ShooterSubsystem extends SubsystemBase { public ShooterSubsystem(SwerveSubsystem swerve) { swerveSubsystem = swerve; + m_Wrist = new CANSparkMax(13, MotorType.kBrushless); + shootaTop = new CANSparkMax(11, MotorType.kBrushless); // leader + shootaBot = new CANSparkMax(12, MotorType.kBrushless); + // Encoders m_VelocityEncoder = shootaTop.getEncoder(); m_VelocityEncoder2 = shootaBot.getEncoder(); m_PositionEncoder = m_Wrist.getEncoder(); m_WristEncoder = new DutyCycleEncoder(0); - // Wrist Encoder Reset - m_WristEncoder.reset(); - m_PositionEncoder.setPosition(0); - // Spark Max Setup shootaTop.restoreFactoryDefaults(); shootaBot.restoreFactoryDefaults(); @@ -135,20 +135,25 @@ public ShooterSubsystem(SwerveSubsystem swerve) { @Override public void periodic() { // This method will be called once per scheduler run - /* - * pidSpdP = SmartDashboard.getNumber("Velo P", pidSpdP); - * pidSpdI = SmartDashboard.getNumber("Velo I", pidSpdI); - * pidSpdD = SmartDashboard.getNumber("Velo D", pidSpdD); - * Minor whoopsie if these guys were causing loop overruns - * botPID.setP(pidSpdP, 0); - * botPID.setI(pidSpdI, 0); - * botPID.setD(pidSpdD, 0); - * - * topPID.setP(pidSpdP, 0); - * topPID.setI(pidSpdI, 0); - * topPID.setD(pidSpdD, 0); - */ + /* + if (velocityP != SmartDashboard.getNumber("Velo P", velocityP)){ + velocityP = SmartDashboard.getNumber("Velo P", velocityP); + topPID.setP(velocityP, 0); + botPID.setP(velocityP, 0); + } + + if (velocityI != SmartDashboard.getNumber("Velo I", velocityI)){ + velocityI = SmartDashboard.getNumber("Velo I", velocityI); + topPID.setI(velocityI, 0); + botPID.setI(velocityI, 0); + } + if (velocityD != SmartDashboard.getNumber("Velo D", velocityD)){ + velocityD = SmartDashboard.getNumber("Velo D", velocityD); + topPID.setD(velocityD, 0); + botPID.setD(velocityD, 0); + } + */ shooterVelocity = SmartDashboard.getNumber("set velocity", shooterVelocity); FiringSolutionsV3.slipPercent = SmartDashboard.getNumber("Set Slip Offset", FiringSolutionsV3.slipPercent); @@ -269,6 +274,12 @@ public void resetWristEncoders(double newOffset) { isZeroed = true; } + /** Wrist Encoder Reset */ + public void restartWristEncoders(){ + m_WristEncoder.reset(); + m_PositionEncoder.setPosition(0); + } + public void setManualWristSpeed(double speed) { manualOverride = true; m_Wrist.set(speed); @@ -329,7 +340,9 @@ public void setWristAngleUpperBound(double wristAngleUpperBound) { /** IN RADIANS */ public void setWristByAngle(double angle) { + manualOverride = false; if (isZeroed) { + //manualOverride = false; updateWristAngleSetpoint(angle); } } @@ -364,9 +377,8 @@ public void updateShooterMath() { // Shooter Math FiringSolutionsV3.updateSpeakerR(distanceToMovingSpeakerTarget); - // shooterAngleToSpeaker = FiringSolutionsV3.getShooterAngleFromSpeakerR(); - shooterAngleToSpeaker = Math - .toRadians(interpolation.getShooterAngleFromInterpolation(distanceToMovingSpeakerTarget)); + shooterAngleToSpeaker = FiringSolutionsV3.getShooterAngleFromSpeakerR(); + //shooterAngleToSpeaker = Math.toRadians(interpolation.getShooterAngleFromInterpolation(distanceToMovingSpeakerTarget)); SmartDashboard.putNumber("Target Velocity RPM", FiringSolutionsV3.convertToRPM(shooterVelocity)); SmartDashboard.putNumber("Calculated Angle Radians", shooterAngleToSpeaker); diff --git a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java index e67a697..6bbd2f1 100644 --- a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java @@ -15,7 +15,6 @@ import com.ctre.phoenix6.configs.Pigeon2Configuration; import com.ctre.phoenix6.hardware.Pigeon2; import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.path.PathConstraints; import com.pathplanner.lib.util.HolonomicPathFollowerConfig; import com.pathplanner.lib.util.PIDConstants; import com.pathplanner.lib.util.PathPlannerLogging; @@ -362,80 +361,50 @@ public Command sysIdDynamic(SysIdRoutine.Direction direction) { public Command pathToSource() { if (!Robot.getAlliance()) { return AutoBuilder.pathfindToPose(new Pose2d(1.21, 0.96, Rotation2d.fromDegrees(58.79)), - new PathConstraints(Constants.Auto.kMaxSpeedMetersPerSecond, - Constants.Auto.kMaxAccelerationMetersPerSecondSquared, - Constants.Auto.kMaxAngularSpeedRadiansPerSecond, - Constants.Auto.kMaxAngularSpeedRadiansPerSecondSquared)); + Constants.Auto.PathfindingConstraints); } else { return AutoBuilder.pathfindToPose(new Pose2d(15.47, 1.50, Rotation2d.fromDegrees(-59.53)), - new PathConstraints(Constants.Auto.kMaxSpeedMetersPerSecond, - Constants.Auto.kMaxAccelerationMetersPerSecondSquared, - Constants.Auto.kMaxAngularSpeedRadiansPerSecond, - Constants.Auto.kMaxAngularSpeedRadiansPerSecondSquared)); + Constants.Auto.PathfindingConstraints); } } public Command pathToAmp() { if (!Robot.getAlliance()) { return AutoBuilder.pathfindToPose(new Pose2d(1.84, 7.59, Rotation2d.fromDegrees(90.37)), - new PathConstraints(Constants.Auto.kMaxSpeedMetersPerSecond, - Constants.Auto.kMaxAccelerationMetersPerSecondSquared, - Constants.Auto.kMaxAngularSpeedRadiansPerSecond, - Constants.Auto.kMaxAngularSpeedRadiansPerSecondSquared)); + Constants.Auto.PathfindingConstraints); } else { return AutoBuilder.pathfindToPose(new Pose2d(14.74, 7.52, Rotation2d.fromDegrees(90.37)), - new PathConstraints(Constants.Auto.kMaxSpeedMetersPerSecond, - Constants.Auto.kMaxAccelerationMetersPerSecondSquared, - Constants.Auto.kMaxAngularSpeedRadiansPerSecond, - Constants.Auto.kMaxAngularSpeedRadiansPerSecondSquared)); + Constants.Auto.PathfindingConstraints); } } public Command pathToMidfieldChain() { if (!Robot.getAlliance()) { return AutoBuilder.pathfindToPose(new Pose2d(6.29, 4.08, Rotation2d.fromDegrees(180)), - new PathConstraints(Constants.Auto.kMaxSpeedMetersPerSecond, - Constants.Auto.kMaxAccelerationMetersPerSecondSquared, - Constants.Auto.kMaxAngularSpeedRadiansPerSecond, - Constants.Auto.kMaxAngularSpeedRadiansPerSecondSquared)); + Constants.Auto.PathfindingConstraints); } else { return AutoBuilder.pathfindToPose(new Pose2d(10.26, 4.10, Rotation2d.fromDegrees(0)), - new PathConstraints(Constants.Auto.kMaxSpeedMetersPerSecond, - Constants.Auto.kMaxAccelerationMetersPerSecondSquared, - Constants.Auto.kMaxAngularSpeedRadiansPerSecond, - Constants.Auto.kMaxAngularSpeedRadiansPerSecondSquared)); + Constants.Auto.PathfindingConstraints); } } public Command pathToSourceChain() { if (!Robot.getAlliance()) { return AutoBuilder.pathfindToPose(new Pose2d(4.18, 2.79, Rotation2d.fromDegrees(59.47)), - new PathConstraints(Constants.Auto.kMaxSpeedMetersPerSecond, - Constants.Auto.kMaxAccelerationMetersPerSecondSquared, - Constants.Auto.kMaxAngularSpeedRadiansPerSecond, - Constants.Auto.kMaxAngularSpeedRadiansPerSecondSquared)); + Constants.Auto.PathfindingConstraints); } else { return AutoBuilder.pathfindToPose(new Pose2d(12.43, 2.90, Rotation2d.fromDegrees(121.26)), - new PathConstraints(Constants.Auto.kMaxSpeedMetersPerSecond, - Constants.Auto.kMaxAccelerationMetersPerSecondSquared, - Constants.Auto.kMaxAngularSpeedRadiansPerSecond, - Constants.Auto.kMaxAngularSpeedRadiansPerSecondSquared)); + Constants.Auto.PathfindingConstraints); } } public Command pathToAmpChain() { if (!Robot.getAlliance()) { return AutoBuilder.pathfindToPose(new Pose2d(4.20, 5.30, Rotation2d.fromDegrees(-58.21)), - new PathConstraints(Constants.Auto.kMaxSpeedMetersPerSecond, - Constants.Auto.kMaxAccelerationMetersPerSecondSquared, - Constants.Auto.kMaxAngularSpeedRadiansPerSecond, - Constants.Auto.kMaxAngularSpeedRadiansPerSecondSquared)); + Constants.Auto.PathfindingConstraints); } else { return AutoBuilder.pathfindToPose(new Pose2d(12.50, 5.25, Rotation2d.fromDegrees(-120.96)), - new PathConstraints(Constants.Auto.kMaxSpeedMetersPerSecond, - Constants.Auto.kMaxAccelerationMetersPerSecondSquared, - Constants.Auto.kMaxAngularSpeedRadiansPerSecond, - Constants.Auto.kMaxAngularSpeedRadiansPerSecondSquared)); + Constants.Auto.PathfindingConstraints); } } diff --git a/src/main/java/frc/robot/subsystems/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/VisionSubsystem.java index 850494f..f854e74 100644 --- a/src/main/java/frc/robot/subsystems/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/VisionSubsystem.java @@ -45,12 +45,14 @@ public class VisionSubsystem extends SubsystemBase { private final PhotonCamera aprilTagCameraBack; private final PhotonCamera noteCamera; - public final PhotonPoseEstimator photonEstimatorFront; - public final PhotonPoseEstimator photonEstimatorBack; + private final PhotonPoseEstimator photonEstimatorFront; + private final PhotonPoseEstimator photonEstimatorBack; private double lastTimeStampFront = 0; private double lastEstTimestampBack = 0; + private final double maxAcceptableRange = 4; + public VisionSubsystem() { aprilTagCameraFront = new PhotonCamera(Constants.Vision.kAprilTagCameraFront); aprilTagCameraBack = new PhotonCamera(Constants.Vision.kAprilTagCameraBack); @@ -141,7 +143,7 @@ public Matrix getEstimationStdDevsFront(Pose2d estimatedPose) { //if (numTags > 1) // estStdDevs = Constants.Vision.kMultiTagStdDevs; // Increase std devs based on (average) distance - if (/*numTags == 1 && */avgDist > 4) + if (/*numTags == 1 && */avgDist > maxAcceptableRange) estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE); else estStdDevs = estStdDevs.times(1 + (avgDist * avgDist / 30)); @@ -168,7 +170,7 @@ public Matrix getEstimationStdDevsBack(Pose2d estimatedPose) { //if (numTags > 1) // estStdDevs = Constants.Vision.kMultiTagStdDevs; // Increase std devs based on (average) distance - if (/*numTags == 1 &&*/ avgDist > 4) + if (/*numTags == 1 &&*/ avgDist > maxAcceptableRange) estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE); else estStdDevs = estStdDevs.times(1 + (avgDist * avgDist / 30)); diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json index 787450f..b849e92 100644 --- a/vendordeps/PathplannerLib.json +++ b/vendordeps/PathplannerLib.json @@ -1,7 +1,7 @@ { "fileName": "PathplannerLib.json", "name": "PathplannerLib", - "version": "2024.2.4", + "version": "2024.2.5", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2024", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2024.2.4" + "version": "2024.2.5" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2024.2.4", + "version": "2024.2.5", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, From 7c85d402c77f9dd8a576207f4224e47067027782 Mon Sep 17 00:00:00 2001 From: Andrew-K961 Date: Sun, 10 Mar 2024 16:47:53 -0500 Subject: [PATCH 30/49] me when the garbage colllection is trash --- build.gradle | 7 +++++++ src/main/java/frc/robot/Robot.java | 10 ++++----- src/main/java/frc/robot/RobotContainer.java | 8 +------ .../frc/robot/subsystems/SwerveSubsystem.java | 21 ++++++++++++------- 4 files changed, 27 insertions(+), 19 deletions(-) diff --git a/build.gradle b/build.gradle index abaef8a..432fe5f 100644 --- a/build.gradle +++ b/build.gradle @@ -27,6 +27,13 @@ deploy { // getTargetTypeClass is a shortcut to get the class type using a string frcJava(getArtifactTypeClass('FRCJavaArtifact')) { + // Switch GC to 2023's G1 + //gcType = 'G1' + //jvmArgs.add("-XX:+UseG1GC") + //jvmArgs.add("-XX:MaxGCPauseMillis=1") + //jvmArgs.add("-XX:GCTimeRatio=1") + //jvmArgs.remove('-XX:+AlwaysPreTouch') + // Use VisualVM for code profiling //jvmArgs.add("-Dcom.sun.management.jmxremote=true") //jvmArgs.add("-Dcom.sun.management.jmxremote.port=1099") diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 0675530..1f88c6d 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -70,18 +70,18 @@ public void robotInit() { URCL.start(); // Log data from all CTRE devices - SignalLogger.setPath("/media/sda1/logs/"); - SignalLogger.start(); + //SignalLogger.setPath("/media/sda1/logs/"); + //SignalLogger.start(); // Output command scheduler to dashboard SmartDashboard.putData(CommandScheduler.getInstance()); // Access PhotonVision dashboard when connected via usb TODO make work - // PortForwarder.add(5800, "10.17.10.11", 5800); + PortForwarder.add(5800, "10.17.10.11", 5800); - SmartDashboard.putData(PDH); + SmartDashboard.putData(PDH); - // idk if this is useful + // idk if this is useful System.gc(); } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index af40a9a..ef91651 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -3,8 +3,6 @@ import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.XboxController; @@ -180,7 +178,7 @@ private void configureButtonBindings() { // Reset Odometry resetOdom.onTrue(new InstantCommand(() -> m_SwerveSubsystem.zeroHeading()).alongWith( new InstantCommand(() -> m_SwerveSubsystem - .setPose(new Pose2d(1.35, 5.55, new Rotation2d(0)))))); + .setPose(Constants.Vision.startingPose)))); // Intexer intex.whileTrue(new IntexBestHex(m_IntexerSubsystem, true, driver)); @@ -226,10 +224,6 @@ private void configureButtonBindings() { shooterToAmp.onTrue(new RizzLevel(m_ShooterSubsystem, -0.48)) .onTrue(new ElevatorSet(m_ElevatorSubsystem, Constants.Elevator.maxHeightMeters)); - // xButton.whileTrue(new InstantCommand(() -> m_Shoota.SetOffsetVelocity(2000))) - // .onFalse(new InstantCommand(() -> - // m_Shoota.SetShooterVelocity(Constants.Shooter.idleSpeedRPM))); - // Reset the R calculation incase it gets off resetR.onTrue(new InstantCommand(() -> FiringSolutionsV3.resetAllR())); diff --git a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java index 6bbd2f1..d580f0c 100644 --- a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java @@ -42,7 +42,10 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import static edu.wpi.first.units.Units.Meters; @@ -140,7 +143,7 @@ public SwerveSubsystem(VisionSubsystem vision) { PathPlannerLogging.setLogActivePathCallback((poses) -> { m_field.getObject("field").setPoses(poses); - if (poses.isEmpty()){ + if (poses.isEmpty()) { followingPath = false; } else { followingPath = true; @@ -308,8 +311,10 @@ public void periodic() { for (SwerveModule mod : mSwerveMods) { SmartDashboard.putNumber("Mod " + mod.moduleNumber + " CANcoder", mod.getCANcoder().getDegrees()); - SmartDashboard.putNumber("Mod " + mod.moduleNumber + " Angle", modulePositions[mod.moduleNumber].angle.getDegrees()); - SmartDashboard.putNumber("Mod " + mod.moduleNumber + " Velocity", swerveModuleStates[mod.moduleNumber].speedMetersPerSecond); + SmartDashboard.putNumber("Mod " + mod.moduleNumber + " Angle", + modulePositions[mod.moduleNumber].angle.getDegrees()); + SmartDashboard.putNumber("Mod " + mod.moduleNumber + " Velocity", + swerveModuleStates[mod.moduleNumber].speedMetersPerSecond); } SmartDashboard.putString("Obodom", getPose().toString()); @@ -322,14 +327,14 @@ public void sysidroutine(SysIdRoutineLog log) { log.motor("drive-BR") .voltage( m_appliedVoltage.mut_replace( - mSwerveMods[3].getMotorVoltage() * RobotController.getBatteryVoltage(), Volts)) + mSwerveMods[3].getMotorVoltage(), Volts)) .linearPosition(m_distance.mut_replace(mSwerveMods[3].getPosition().distanceMeters, Meters)) .linearVelocity( m_velocity.mut_replace(mSwerveMods[3].getMotorVelocity(), MetersPerSecond)); log.motor("drive-FL") .voltage( m_appliedVoltage.mut_replace( - mSwerveMods[0].getMotorVoltage() * RobotController.getBatteryVoltage(), Volts)) + mSwerveMods[0].getMotorVoltage(), Volts)) .linearPosition(m_distance.mut_replace(mSwerveMods[0].getPosition().distanceMeters, Meters)) .linearVelocity( m_velocity.mut_replace(mSwerveMods[0].getMotorVelocity(), MetersPerSecond)); @@ -350,11 +355,13 @@ public void sysidroutine(SysIdRoutineLog log) { } public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { - return m_sysIdRoutine.quasistatic(direction); + return new SequentialCommandGroup(new InstantCommand(this::resetModulesToAbsolute, this), new WaitCommand(0.5), + m_sysIdRoutine.quasistatic(direction)); } public Command sysIdDynamic(SysIdRoutine.Direction direction) { - return m_sysIdRoutine.dynamic(direction); + return new SequentialCommandGroup(new InstantCommand(this::resetModulesToAbsolute, this), new WaitCommand(0.5), + m_sysIdRoutine.dynamic(direction)); } // Pathfinding Commands From 59c08f28f9f34f01f7542a83f4d74e6883e36870 Mon Sep 17 00:00:00 2001 From: Andrew-K961 Date: Sun, 10 Mar 2024 17:30:12 -0500 Subject: [PATCH 31/49] im dumb --- .../java/frc/robot/subsystems/SwerveSubsystem.java | 11 ++++------- 1 file changed, 4 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java index d580f0c..87af697 100644 --- a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java @@ -37,7 +37,6 @@ import edu.wpi.first.units.Units; import edu.wpi.first.units.Velocity; import edu.wpi.first.units.Voltage; -import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; @@ -311,10 +310,8 @@ public void periodic() { for (SwerveModule mod : mSwerveMods) { SmartDashboard.putNumber("Mod " + mod.moduleNumber + " CANcoder", mod.getCANcoder().getDegrees()); - SmartDashboard.putNumber("Mod " + mod.moduleNumber + " Angle", - modulePositions[mod.moduleNumber].angle.getDegrees()); - SmartDashboard.putNumber("Mod " + mod.moduleNumber + " Velocity", - swerveModuleStates[mod.moduleNumber].speedMetersPerSecond); + SmartDashboard.putNumber("Mod " + mod.moduleNumber + " Angle", modulePositions[mod.moduleNumber].angle.getDegrees()); + SmartDashboard.putNumber("Mod " + mod.moduleNumber + " Velocity", swerveModuleStates[mod.moduleNumber].speedMetersPerSecond); } SmartDashboard.putString("Obodom", getPose().toString()); @@ -341,14 +338,14 @@ public void sysidroutine(SysIdRoutineLog log) { log.motor("drive-FR") .voltage( m_appliedVoltage.mut_replace( - mSwerveMods[1].getMotorVoltage() * RobotController.getBatteryVoltage(), Volts)) + mSwerveMods[1].getMotorVoltage(), Volts)) .linearPosition(m_distance.mut_replace(mSwerveMods[1].getPosition().distanceMeters, Meters)) .linearVelocity( m_velocity.mut_replace(mSwerveMods[1].getMotorVelocity(), MetersPerSecond)); log.motor("drive-BL") .voltage( m_appliedVoltage.mut_replace( - mSwerveMods[2].getMotorVoltage() * RobotController.getBatteryVoltage(), Volts)) + mSwerveMods[2].getMotorVoltage(), Volts)) .linearPosition(m_distance.mut_replace(mSwerveMods[2].getPosition().distanceMeters, Meters)) .linearVelocity( m_velocity.mut_replace(mSwerveMods[2].getMotorVelocity(), MetersPerSecond)); From 221b4ca671f828b8220cd4324b2833fe1275c9ef Mon Sep 17 00:00:00 2001 From: Mr-A768 Date: Sun, 10 Mar 2024 22:24:05 -0500 Subject: [PATCH 32/49] =?UTF-8?q?we=20shootin=20notes=20outside=20of=20the?= =?UTF-8?q?=20field=20with=20this=20one=20=F0=9F=94=A5=F0=9F=94=A5?= =?UTF-8?q?=F0=9F=94=A5?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../java/frc/lib/math/FiringSolutionsV3.java | 33 ++++++----- .../java/frc/robot/commands/MissileLock.java | 4 +- .../robot/subsystems/ShooterSubsystem.java | 59 +++++++++++-------- 3 files changed, 56 insertions(+), 40 deletions(-) diff --git a/src/main/java/frc/lib/math/FiringSolutionsV3.java b/src/main/java/frc/lib/math/FiringSolutionsV3.java index 5da7ae6..9241da3 100644 --- a/src/main/java/frc/lib/math/FiringSolutionsV3.java +++ b/src/main/java/frc/lib/math/FiringSolutionsV3.java @@ -6,29 +6,34 @@ public class FiringSolutionsV3 { + // Robot Values private static final double defaultShooterHeight = 0.42672; - private static final double laserCANOffset = 0; private static double shooterHeight = 0.42672; - + private static final double laserCANOffset = 0; + public static final double maxShooterAngle = Math.toRadians(70); + public static double slipPercent = 0.5324; + private static double shooterVelocity = 10.0; private static final double noteFallAccel = 9.8; - private static final double maxShooterAngle = Math.toRadians(70); - private static final double ampTargetXBlue = 1.84; - private static final double ampTargetXRed = 14.7; - - public static final double speakerTargetY = 5.55; - public static final double ampTargetY = 8.0; - public static final double ampTargetZ = .125; + // Amp Values + private static final double ampTargetXRed = 15.91; + private static final double ampTargetXBlue = 16.54 - ampTargetXRed; + public static double ampTargetX; + public static final double ampTargetY = 7; + public static final double ampTargetZ = .125; + + // Speaker Values public static double speakerTargetXOffset = .24; - public static double speakerTargetZ = 1.92; - public static double slipPercent = 0.5324; - private static double speakerTargetXBlue = 0.0 + speakerTargetXOffset; private static double speakerTargetXRed = 16.54 - speakerTargetXOffset; public static double speakerTargetX; - public static double ampTargetX; - private static double shooterVelocity = 10.0; + public static final double speakerTargetY = 5.55; + public static double speakerTargetZ = 1.92; + + //R Values private static double speakerR, ampR, customR = 1.0; + public static double maxRangeWithR = 11.25; + private FiringSolutionsV3() { } diff --git a/src/main/java/frc/robot/commands/MissileLock.java b/src/main/java/frc/robot/commands/MissileLock.java index 9d22a1f..6a19f6f 100644 --- a/src/main/java/frc/robot/commands/MissileLock.java +++ b/src/main/java/frc/robot/commands/MissileLock.java @@ -31,8 +31,8 @@ public void initialize() { @Override public void execute() { if (target == "amp") { - shooter.PointShoot(shooter.getCalculatedAngleToAmp(), - FiringSolutionsV3.convertToRPM(shooter.getCalculatedVelocity())); + shooter.PointShoot(Math.toRadians(60), + FiringSolutionsV3.convertToRPM(10)); } else { shooter.PointShoot(shooter.getCalculatedAngleToSpeaker(), FiringSolutionsV3.convertToRPM(shooter.getCalculatedVelocity())); diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index e25c198..1d609f5 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -65,6 +65,8 @@ public class ShooterSubsystem extends SubsystemBase { public double wristAngleUpperBound; public double wristAngleLowerBound; + public boolean outOfAmpRange = false; + // Constants private final double wristAngleMax = 0.0; private final double wristAngleMin = 0.0; @@ -176,17 +178,21 @@ public void periodic() { SmartDashboard.putNumber("Wrist Current", m_Wrist.getOutputCurrent()); SmartDashboard.putBoolean("is Wrist Stalled", isWristMotorStalled()); - if (isZeroed){ - if (!manualOverride){ + if (isZeroed) { + if (!manualOverride) { m_Wrist.set(m_pidWrist.calculate(getCurrentShooterAngle(), lastWristAngleSetpoint)); } - //Implement whenever build stops throwing - /*if (getCurrentShooterAngle() > wristAngleUpperBound){ - m_Wrist.set(m_pidWrist.calculate(getCurrentShooterAngle(), wristAngleUpperBound)); - } else if (getCurrentShooterAngle() < wristAngleLowerBound){ - m_Wrist.set(m_pidWrist.calculate(getCurrentShooterAngle(), wristAngleLowerBound)); - }*/ + // Implement whenever build stops throwing + /* + * if (getCurrentShooterAngle() > wristAngleUpperBound){ + * m_Wrist.set(m_pidWrist.calculate(getCurrentShooterAngle(), + * wristAngleUpperBound)); + * } else if (getCurrentShooterAngle() < wristAngleLowerBound){ + * m_Wrist.set(m_pidWrist.calculate(getCurrentShooterAngle(), + * wristAngleLowerBound)); + * } + */ } } @@ -274,7 +280,7 @@ public void setManualWristSpeed(double speed) { m_Wrist.set(speed); } - public void setManualOverride(boolean value){ + public void setManualOverride(boolean value) { manualOverride = value; } @@ -308,9 +314,9 @@ public void setWristEncoderOffset(double newPosition) { } public void setWristAngleLowerBound(double wristAngleLowerBound) { - if (wristAngleLowerBound < wristAngleMin){ + if (wristAngleLowerBound < wristAngleMin) { wristAngleLowerBound = wristAngleMin; - } else if (wristAngleLowerBound > wristAngleUpperBound){ + } else if (wristAngleLowerBound > wristAngleUpperBound) { wristAngleLowerBound = wristAngleUpperBound; } else { this.wristAngleLowerBound = wristAngleLowerBound; @@ -318,9 +324,9 @@ public void setWristAngleLowerBound(double wristAngleLowerBound) { } public void setWristAngleUpperBound(double wristAngleUpperBound) { - if (wristAngleUpperBound > wristAngleMax){ + if (wristAngleUpperBound > wristAngleMax) { wristAngleUpperBound = wristAngleMax; - } else if (wristAngleUpperBound < wristAngleLowerBound){ + } else if (wristAngleUpperBound < wristAngleLowerBound) { wristAngleUpperBound = wristAngleLowerBound; } else { this.wristAngleUpperBound = wristAngleUpperBound; @@ -336,25 +342,19 @@ public void setWristByAngle(double angle) { /** IN ROTATIONS */ public void setWristByRotations(double newPosition) { - if (isZeroed){ + if (isZeroed) { m_Wrist.set(m_pidWrist.calculate(getWristRotations(), newPosition)); } } public void updateWristAngleSetpoint(double angle) { - if (angle != lastWristAngleSetpoint){ - /*if (lastWristAngleSetpoint < wristAngleLowerBound){ - lastWristAngleSetpoint = wristAngleLowerBound; - } else if (lastWristAngleSetpoint > wristAngleUpperBound){ - lastWristAngleSetpoint = wristAngleUpperBound; - } else { - - } */ + if (angle != lastWristAngleSetpoint) { lastWristAngleSetpoint = angle; } } public void updateShooterMath() { // Shooter Math + Pose2d pose = swerveSubsystem.getPose(); ChassisSpeeds chassisSpeeds = swerveSubsystem.getChassisSpeeds(); @@ -365,8 +365,19 @@ public void updateShooterMath() { // Shooter Math FiringSolutionsV3.updateSpeakerR(distanceToMovingSpeakerTarget); // shooterAngleToSpeaker = FiringSolutionsV3.getShooterAngleFromSpeakerR(); - shooterAngleToSpeaker = Math - .toRadians(interpolation.getShooterAngleFromInterpolation(distanceToMovingSpeakerTarget)); + shooterAngleToSpeaker = Math.toRadians(interpolation.getShooterAngleFromInterpolation(distanceToMovingSpeakerTarget)); + + if (FiringSolutionsV3.getDistanceToMovingTarget(pose.getX(), pose.getY(), + FiringSolutionsV3.ampTargetX, FiringSolutionsV3.ampTargetY, + chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond, + pose.getRotation().getRadians()) > FiringSolutionsV3.maxRangeWithR){ + outOfAmpRange = true; + } else { + if (outOfAmpRange){ + outOfAmpRange = false; + FiringSolutionsV3.resetAmpR(); + } + } SmartDashboard.putNumber("Target Velocity RPM", FiringSolutionsV3.convertToRPM(shooterVelocity)); SmartDashboard.putNumber("Calculated Angle Radians", shooterAngleToSpeaker); From ddb9a95f38c49b06b4e3e674917c8926fbc74219 Mon Sep 17 00:00:00 2001 From: Mr-A768 Date: Mon, 11 Mar 2024 01:52:46 -0500 Subject: [PATCH 33/49] no really these will probably get us penalized --- .../java/frc/lib/math/FiringSolutionsV3.java | 10 +++++- src/main/java/frc/robot/RobotContainer.java | 30 ++++++++++++---- src/main/java/frc/robot/commands/AimBot.java | 2 +- .../java/frc/robot/commands/ManRizzt.java | 7 ++-- .../java/frc/robot/commands/MissileLock.java | 8 +++-- .../java/frc/robot/commands/TeleopSwerve.java | 16 +++++---- .../java/frc/robot/commands/ZeroRizz.java | 2 +- .../robot/subsystems/ElevatorSubsystem.java | 4 +++ .../robot/subsystems/ShooterSubsystem.java | 36 ++++++++++--------- 9 files changed, 76 insertions(+), 39 deletions(-) diff --git a/src/main/java/frc/lib/math/FiringSolutionsV3.java b/src/main/java/frc/lib/math/FiringSolutionsV3.java index 9241da3..aaf2ffc 100644 --- a/src/main/java/frc/lib/math/FiringSolutionsV3.java +++ b/src/main/java/frc/lib/math/FiringSolutionsV3.java @@ -15,12 +15,18 @@ public class FiringSolutionsV3 { private static double shooterVelocity = 10.0; private static final double noteFallAccel = 9.8; - // Amp Values + // Amp Values (used for targeting from afar) private static final double ampTargetXRed = 15.91; private static final double ampTargetXBlue = 16.54 - ampTargetXRed; public static double ampTargetX; public static final double ampTargetY = 7; public static final double ampTargetZ = .125; + + // True Amp Values + private static final double trueAmpXBlue = 1.8415; + private static final double trueAmpXRed = 16.54 - trueAmpXBlue; + public static double trueAmpX; + public static final double trueAmpY = 8.2; // Speaker Values public static double speakerTargetXOffset = .24; @@ -42,9 +48,11 @@ public static void setAlliance(boolean redAlliance) { if (redAlliance) { speakerTargetX = speakerTargetXRed; ampTargetX = ampTargetXRed; + trueAmpX = trueAmpXRed; } else { speakerTargetX = speakerTargetXBlue; ampTargetX = ampTargetXBlue; + trueAmpX = trueAmpXBlue; } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ef91651..4a44438 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -96,7 +96,9 @@ public class RobotContainer { /** Mech Back */ private final JoystickButton zeroShooter = new JoystickButton(mech, XboxController.Button.kBack.value); /** Mech RS */ - private final JoystickButton rightStick = new JoystickButton(mech, XboxController.Button.kRightStick.value); + private final JoystickButton mechRightStick = new JoystickButton(mech, XboxController.Button.kRightStick.value); + /** Mech LS */ + private final JoystickButton mechLeftStick = new JoystickButton(mech, XboxController.Button.kLeftStick.value); private final Trigger dynamicForward = new Trigger(() -> FF.getPOV() == 90); @@ -188,9 +190,16 @@ private void configureButtonBindings() { forceShoot.whileTrue(new InstantCommand(() -> m_IntexerSubsystem.setShooterIntake(.9))) .onFalse(new InstantCommand(() -> m_IntexerSubsystem.setShooterIntake(0))); + // Move to Amp driverUp.whileTrue(m_SwerveSubsystem.pathToAmpChain()); + + // Move to Source driverDown.whileTrue(m_SwerveSubsystem.pathToSourceChain()); + // Intake from Source + driverX.whileTrue(new IntakeThroughShooter(m_ShooterSubsystem, m_IntexerSubsystem, driver)) + .onFalse(new IntakeThroughShooterPart2(m_ShooterSubsystem, m_IntexerSubsystem, driver)); + /* MECH BUTTONS */ // Prime for Speaker @@ -227,13 +236,20 @@ private void configureButtonBindings() { // Reset the R calculation incase it gets off resetR.onTrue(new InstantCommand(() -> FiringSolutionsV3.resetAllR())); - // Kill Shooter - rightStick.onTrue(new InstantCommand(() -> m_ShooterSubsystem.setShooterVelocity(0))); - // Intake Through Shooter intakeThroughShooter.whileTrue(new IntakeThroughShooter(m_ShooterSubsystem, m_IntexerSubsystem, mech)) - .onFalse(new IntakeThroughShooterPart2(m_ShooterSubsystem, m_IntexerSubsystem, mech)); - + .onFalse(new IntakeThroughShooterPart2(m_ShooterSubsystem, m_IntexerSubsystem, mech)); + + // Kill Flywheels + mechRightStick.onTrue(new InstantCommand(() -> m_ShooterSubsystem.setShooterVelocity(0))); + + // Kill Wrist + mechLT.and(mechRightStick).onTrue(new InstantCommand(() -> m_ShooterSubsystem.setWristSpeedManual(0))); + + // Kill Elevator + mechLT.and(mechLeftStick).onTrue(new InstantCommand(() -> m_ElevatorSubsystem.setElevatorSpeedManual(0))); + + /* THIRD CONTROLLER */ // Characterization tests dynamicForward.whileTrue(m_SwerveSubsystem.sysIdDynamic(Direction.kForward)); dynamicBackward.whileTrue(m_SwerveSubsystem.sysIdDynamic(Direction.kReverse)); @@ -243,7 +259,7 @@ private void configureButtonBindings() { public void stopAll() { m_ShooterSubsystem.setShooterVelocity(0); - m_ShooterSubsystem.setManualWristSpeed(0); + m_ShooterSubsystem.setWristSpeedManual(0); m_IntexerSubsystem.setALL(0); m_ElevatorSubsystem.setPositionWithEncoder(m_ElevatorSubsystem.getPosition()); } diff --git a/src/main/java/frc/robot/commands/AimBot.java b/src/main/java/frc/robot/commands/AimBot.java index 4ede3f2..8ee51c0 100644 --- a/src/main/java/frc/robot/commands/AimBot.java +++ b/src/main/java/frc/robot/commands/AimBot.java @@ -74,7 +74,7 @@ public void execute() { public void end(boolean interrupted) { swerveSubsystem.setChassisSpeeds(new ChassisSpeeds(0, 0, 0)); intexer.setShooterIntake(0); - shooter.setManualWristSpeed(0); + shooter.setWristSpeedManual(0); } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/commands/ManRizzt.java b/src/main/java/frc/robot/commands/ManRizzt.java index 989a6b6..c1e1526 100644 --- a/src/main/java/frc/robot/commands/ManRizzt.java +++ b/src/main/java/frc/robot/commands/ManRizzt.java @@ -45,16 +45,15 @@ public void execute() { } else { if (Math.abs(speedValue) > .0) { wristIsLocked = false; - m_shooterSubsystem.setManualWristSpeed(speedValue); + m_shooterSubsystem.setWristSpeedManual(speedValue); } else { if (m_shooterSubsystem.isZeroed){ if (!wristIsLocked){ - m_shooterSubsystem.setManualOverride(false); m_shooterSubsystem.setWristByAngle(m_shooterSubsystem.getCurrentShooterAngle()); wristIsLocked = true; } } else { - m_shooterSubsystem.setManualWristSpeed(0); + m_shooterSubsystem.setWristSpeedManual(0); } } } @@ -67,6 +66,6 @@ public boolean isFinished() { @Override public void end(boolean interrupted) { - m_shooterSubsystem.setManualWristSpeed(0); + m_shooterSubsystem.setWristSpeedManual(0); } } diff --git a/src/main/java/frc/robot/commands/MissileLock.java b/src/main/java/frc/robot/commands/MissileLock.java index 6a19f6f..5450017 100644 --- a/src/main/java/frc/robot/commands/MissileLock.java +++ b/src/main/java/frc/robot/commands/MissileLock.java @@ -31,8 +31,12 @@ public void initialize() { @Override public void execute() { if (target == "amp") { - shooter.PointShoot(Math.toRadians(60), - FiringSolutionsV3.convertToRPM(10)); + if (shooter.getDistanceTo(FiringSolutionsV3.trueAmpX, FiringSolutionsV3.trueAmpY) > 4) { + shooter.PointShoot(Math.toRadians(60), + FiringSolutionsV3.convertToRPM(10)); + } else { + shooter.setShooterVelocity(FiringSolutionsV3.convertToRPM(12)); + } } else { shooter.PointShoot(shooter.getCalculatedAngleToSpeaker(), FiringSolutionsV3.convertToRPM(shooter.getCalculatedVelocity())); diff --git a/src/main/java/frc/robot/commands/TeleopSwerve.java b/src/main/java/frc/robot/commands/TeleopSwerve.java index f4d44e2..4927d83 100644 --- a/src/main/java/frc/robot/commands/TeleopSwerve.java +++ b/src/main/java/frc/robot/commands/TeleopSwerve.java @@ -90,12 +90,16 @@ public void execute() { } else if (shooterOverrideAmp.getAsBoolean()) { // Lock robot angle to amp ChassisSpeeds currentSpeed = swerveSubsystem.getChassisSpeeds(); - - rotationVal = rotationPID.calculate(swerveSubsystem.getHeading().getRadians(), - FiringSolutionsV3.getAngleToMovingTarget(pose.getX(), pose.getY(), FiringSolutionsV3.ampTargetX, FiringSolutionsV3.ampTargetY, - currentSpeed.vxMetersPerSecond, - currentSpeed.vyMetersPerSecond, - pose.getRotation().getRadians())); + if (FiringSolutionsV3.getDistanceToTarget(swerveSubsystem.getPose().getX(), swerveSubsystem.getPose().getY(), + FiringSolutionsV3.trueAmpX, FiringSolutionsV3.trueAmpY) > 4) { + rotationVal = rotationPID.calculate(swerveSubsystem.getHeading().getRadians(), + FiringSolutionsV3.getAngleToMovingTarget(pose.getX(), pose.getY(), FiringSolutionsV3.ampTargetX, FiringSolutionsV3.ampTargetY, + currentSpeed.vxMetersPerSecond, + currentSpeed.vyMetersPerSecond, + pose.getRotation().getRadians())); + } else { + rotationVal = rotationPID.calculate(swerveSubsystem.getHeading().getRadians(),Math.toRadians(90)); + } } else if (intakeOverride.getAsBoolean() && result.hasTargets()) { // Lock robot towards detected note double yawToNote = Math.toRadians(result.getBestTarget().getYaw()) + swerveSubsystem.getGyroYaw().getRadians(); diff --git a/src/main/java/frc/robot/commands/ZeroRizz.java b/src/main/java/frc/robot/commands/ZeroRizz.java index ff3d41c..ff01532 100644 --- a/src/main/java/frc/robot/commands/ZeroRizz.java +++ b/src/main/java/frc/robot/commands/ZeroRizz.java @@ -30,7 +30,7 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - shooter.setManualWristSpeed(.45); + shooter.setWristSpeedManual(.45); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index f7184e5..5865f53 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -103,6 +103,10 @@ public void periodic() { //FiringSolutionsV3.updateHeight(getHeight()); //TODO: test this } + public void setElevatorSpeedManual(double value){ + m_elevatorLeft.set(value); + } + public void setManualOverride(boolean value){ manualOverride = value; } diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 99da1d2..27b7209 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -143,13 +143,13 @@ public void periodic() { topPID.setP(velocityP, 0); botPID.setP(velocityP, 0); } - + if (velocityI != SmartDashboard.getNumber("Velo I", velocityI)){ velocityI = SmartDashboard.getNumber("Velo I", velocityI); topPID.setI(velocityI, 0); botPID.setI(velocityI, 0); } - + if (velocityD != SmartDashboard.getNumber("Velo D", velocityD)){ velocityD = SmartDashboard.getNumber("Velo D", velocityD); topPID.setD(velocityD, 0); @@ -273,6 +273,7 @@ public void PointShoot(double PointAngle, double launchVelocity) { topPID.setReference(launchVelocity, CANSparkMax.ControlType.kVelocity); } + public void resetWristEncoders(double newOffset) { angleOffset = newOffset; m_WristEncoder.reset(); @@ -281,16 +282,11 @@ public void resetWristEncoders(double newOffset) { } /** Wrist Encoder Reset */ - public void restartWristEncoders(){ + public void restartWristEncoders() { m_WristEncoder.reset(); m_PositionEncoder.setPosition(0); } - public void setManualWristSpeed(double speed) { - manualOverride = true; - m_Wrist.set(speed); - } - public void setManualOverride(boolean value) { manualOverride = value; } @@ -360,6 +356,11 @@ public void setWristByRotations(double newPosition) { } } + public void setWristSpeedManual(double speed) { + manualOverride = true; + m_Wrist.set(speed); + } + public void updateWristAngleSetpoint(double angle) { if (angle != lastWristAngleSetpoint) { lastWristAngleSetpoint = angle; @@ -378,19 +379,20 @@ public void updateShooterMath() { // Shooter Math FiringSolutionsV3.updateSpeakerR(distanceToMovingSpeakerTarget); // shooterAngleToSpeaker = FiringSolutionsV3.getShooterAngleFromSpeakerR(); - shooterAngleToSpeaker = Math.toRadians(interpolation.getShooterAngleFromInterpolation(distanceToMovingSpeakerTarget)); + shooterAngleToSpeaker = Math + .toRadians(interpolation.getShooterAngleFromInterpolation(distanceToMovingSpeakerTarget)); if (FiringSolutionsV3.getDistanceToMovingTarget(pose.getX(), pose.getY(), FiringSolutionsV3.ampTargetX, FiringSolutionsV3.ampTargetY, chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond, - pose.getRotation().getRadians()) > FiringSolutionsV3.maxRangeWithR){ - outOfAmpRange = true; - } else { - if (outOfAmpRange){ - outOfAmpRange = false; - FiringSolutionsV3.resetAmpR(); - } - } + pose.getRotation().getRadians()) > FiringSolutionsV3.maxRangeWithR) { + outOfAmpRange = true; + } else { + if (outOfAmpRange) { + outOfAmpRange = false; + FiringSolutionsV3.resetAmpR(); + } + } SmartDashboard.putNumber("Target Velocity RPM", FiringSolutionsV3.convertToRPM(shooterVelocity)); SmartDashboard.putNumber("Calculated Angle Radians", shooterAngleToSpeaker); From 7c2cf389f037a13d5e85eec0b61c5cc227a20930 Mon Sep 17 00:00:00 2001 From: Andrew-K961 Date: Mon, 11 Mar 2024 23:16:16 -0500 Subject: [PATCH 34/49] Never Be Game Over --- .pathplanner/settings.json | 4 +- build.gradle | 12 +-- .../pathplanner/autos/4 piece mcnugget.auto | 6 ++ .../pathplanner/paths/2nd Note TOP.path | 4 +- .../pathplanner/paths/3N Grab Note 3.path | 52 +++++++++++++ .../deploy/pathplanner/paths/3N Top 1.path | 4 +- .../pathplanner/paths/3N Top Shoot 1.path | 20 ++--- .../pathplanner/paths/3N Top Shoot 2.path | 22 +++--- .../pathplanner/paths/3N Top Shoot 3.path | 22 +++--- .../pathplanner/paths/3rd Note TOP.path | 4 +- .../pathplanner/paths/Straight Line Top.path | 6 +- .../java/frc/lib/math/Interpolations.java | 74 ++++++++++++++----- src/main/java/frc/robot/Constants.java | 28 ++++--- src/main/java/frc/robot/Robot.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 28 ++++--- src/main/java/frc/robot/commands/AimBot.java | 7 +- .../robot/commands/IntakeThroughShooter.java | 2 +- .../commands/IntakeThroughShooterPart2.java | 2 +- .../java/frc/robot/commands/IntexBestHex.java | 7 +- .../robot/commands/IntexForAutosByAutos.java | 6 +- .../java/frc/robot/commands/ManRizzt.java | 2 +- .../java/frc/robot/commands/MissileLock.java | 10 ++- .../java/frc/robot/commands/NoteSniffer.java | 8 +- .../java/frc/robot/commands/TeleopSwerve.java | 18 +++-- .../robot/subsystems/ElevatorSubsystem.java | 1 + .../robot/subsystems/ShooterSubsystem.java | 14 +++- .../frc/robot/subsystems/SwerveSubsystem.java | 2 +- .../frc/robot/subsystems/VisionSubsystem.java | 6 +- 28 files changed, 247 insertions(+), 126 deletions(-) create mode 100644 src/main/deploy/pathplanner/paths/3N Grab Note 3.path diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index f6baf12..e287b0f 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -10,8 +10,8 @@ "autoFolders": [ "Testing" ], - "defaultMaxVel": 4.0, - "defaultMaxAccel": 8.0, + "defaultMaxVel": 2.0, + "defaultMaxAccel": 3.0, "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, "maxModuleSpeed": 4.5 diff --git a/build.gradle b/build.gradle index 432fe5f..539e22f 100644 --- a/build.gradle +++ b/build.gradle @@ -35,12 +35,12 @@ deploy { //jvmArgs.remove('-XX:+AlwaysPreTouch') // Use VisualVM for code profiling - //jvmArgs.add("-Dcom.sun.management.jmxremote=true") - //jvmArgs.add("-Dcom.sun.management.jmxremote.port=1099") - //jvmArgs.add("-Dcom.sun.management.jmxremote.local.only=false") - //jvmArgs.add("-Dcom.sun.management.jmxremote.ssl=false") - //jvmArgs.add("-Dcom.sun.management.jmxremote.authenticate=false") - //jvmArgs.add("-Djava.rmi.server.hostname=172.22.11.2") + jvmArgs.add("-Dcom.sun.management.jmxremote=true") + jvmArgs.add("-Dcom.sun.management.jmxremote.port=1099") + jvmArgs.add("-Dcom.sun.management.jmxremote.local.only=false") + jvmArgs.add("-Dcom.sun.management.jmxremote.ssl=false") + jvmArgs.add("-Dcom.sun.management.jmxremote.authenticate=false") + jvmArgs.add("-Djava.rmi.server.hostname=10.17.10.2") } // Static files artifact diff --git a/src/main/deploy/pathplanner/autos/4 piece mcnugget.auto b/src/main/deploy/pathplanner/autos/4 piece mcnugget.auto index 0515d66..607c514 100644 --- a/src/main/deploy/pathplanner/autos/4 piece mcnugget.auto +++ b/src/main/deploy/pathplanner/autos/4 piece mcnugget.auto @@ -59,6 +59,12 @@ "name": "Shoot" } }, + { + "type": "path", + "data": { + "pathName": "3N Grab Note 3" + } + }, { "type": "named", "data": { diff --git a/src/main/deploy/pathplanner/paths/2nd Note TOP.path b/src/main/deploy/pathplanner/paths/2nd Note TOP.path index 50a2328..dd95373 100644 --- a/src/main/deploy/pathplanner/paths/2nd Note TOP.path +++ b/src/main/deploy/pathplanner/paths/2nd Note TOP.path @@ -66,8 +66,8 @@ } ], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 8.0, + "maxVelocity": 2.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/3N Grab Note 3.path b/src/main/deploy/pathplanner/paths/3N Grab Note 3.path new file mode 100644 index 0000000..fb53f75 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/3N Grab Note 3.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.9251138929831317, + "y": 4.585369909386533 + }, + "prevControl": null, + "nextControl": { + "x": 1.9173088951256527, + "y": 4.251704312807356 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.9251138929831317, + "y": 4.131723545348486 + }, + "prevControl": { + "x": 1.9290137962698273, + "y": 4.525613777309474 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": true + }, + "reversed": false, + "folder": "McNugget paths", + "previewStartingState": { + "rotation": -20.0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/3N Top 1.path b/src/main/deploy/pathplanner/paths/3N Top 1.path index 5ad8e01..bfdfcd7 100644 --- a/src/main/deploy/pathplanner/paths/3N Top 1.path +++ b/src/main/deploy/pathplanner/paths/3N Top 1.path @@ -38,8 +38,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 8.0, + "maxVelocity": 2.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/3N Top Shoot 1.path b/src/main/deploy/pathplanner/paths/3N Top Shoot 1.path index 589b09f..bd6490b 100644 --- a/src/main/deploy/pathplanner/paths/3N Top Shoot 1.path +++ b/src/main/deploy/pathplanner/paths/3N Top Shoot 1.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 3.5, - "y": 7.0 + "x": 2.8981397630253705, + "y": 6.980826883268969 }, "prevControl": null, "nextControl": { - "x": 2.9141386373747915, - "y": 6.589775457544016 + "x": 2.4009020939657093, + "y": 6.4075411001178315 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 1.689169744135214, - "y": 5.55 + "x": 2.055760653089004, + "y": 5.565161990181464 }, "prevControl": { - "x": 2.4478527662696616, - "y": 6.081235571210264 + "x": 2.2683053822164663, + "y": 6.2437451620746485 }, "nextControl": null, "isLocked": false, @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 8.0, + "maxVelocity": 2.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/3N Top Shoot 2.path b/src/main/deploy/pathplanner/paths/3N Top Shoot 2.path index 30067c6..5cb0994 100644 --- a/src/main/deploy/pathplanner/paths/3N Top Shoot 2.path +++ b/src/main/deploy/pathplanner/paths/3N Top Shoot 2.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 3.5, - "y": 5.55 + "x": 2.755077047615666, + "y": 5.505607227681089 }, "prevControl": null, "nextControl": { - "x": 2.9141386373747915, - "y": 5.139775457544016 + "x": 2.299589943374272, + "y": 5.098637580035078 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 1.4746750633643797, - "y": 4.1338974839469875 + "x": 1.8966611698070783, + "y": 4.6478800976884465 }, "prevControl": { - "x": 2.233358085498825, - "y": 4.665133055157252 + "x": 2.203112957932501, + "y": 4.995742093990707 }, "nextControl": null, "isLocked": false, @@ -32,14 +32,14 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 8.0, + "maxVelocity": 2.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, "goalEndState": { "velocity": 0, - "rotation": 0, + "rotation": -20.0, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/3N Top Shoot 3.path b/src/main/deploy/pathplanner/paths/3N Top Shoot 3.path index 06ba0c7..7b4eefb 100644 --- a/src/main/deploy/pathplanner/paths/3N Top Shoot 3.path +++ b/src/main/deploy/pathplanner/paths/3N Top Shoot 3.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 2.8006421808568094, - "y": 4.1338974839469875 + "x": 2.6022294159701613, + "y": 4.1159435424278135 }, "prevControl": null, "nextControl": { - "x": 2.3229040282308606, - "y": 4.25089458254926 + "x": 2.3610924813659686, + "y": 4.356024795515781 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 1.3089291736778261, - "y": 4.4361399886695265 + "x": 2.0196231716467223, + "y": 4.912666546975921 }, "prevControl": { - "x": 1.7866673263037751, - "y": 4.3678916811515345 + "x": 2.1833413338408816, + "y": 4.674105796350148 }, "nextControl": null, "isLocked": false, @@ -32,14 +32,14 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 8.0, + "maxVelocity": 2.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, "goalEndState": { "velocity": 0, - "rotation": 0, + "rotation": -12.0, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/3rd Note TOP.path b/src/main/deploy/pathplanner/paths/3rd Note TOP.path index 6217468..ba204bc 100644 --- a/src/main/deploy/pathplanner/paths/3rd Note TOP.path +++ b/src/main/deploy/pathplanner/paths/3rd Note TOP.path @@ -127,8 +127,8 @@ } ], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 8.0, + "maxVelocity": 2.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Straight Line Top.path b/src/main/deploy/pathplanner/paths/Straight Line Top.path index 9dfe5fb..1205986 100644 --- a/src/main/deploy/pathplanner/paths/Straight Line Top.path +++ b/src/main/deploy/pathplanner/paths/Straight Line Top.path @@ -38,8 +38,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 8.0, + "maxVelocity": 2.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, @@ -54,5 +54,5 @@ "rotation": 35.0, "velocity": 0 }, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/java/frc/lib/math/Interpolations.java b/src/main/java/frc/lib/math/Interpolations.java index ae8047c..16eba82 100644 --- a/src/main/java/frc/lib/math/Interpolations.java +++ b/src/main/java/frc/lib/math/Interpolations.java @@ -8,24 +8,58 @@ public class Interpolations { - public InterpolatingDoubleTreeMap shooterSpeeds = new InterpolatingDoubleTreeMap(); - - public Interpolations() { - shooterSpeeds.put(1.25, 53.0); - shooterSpeeds.put(1.5, 48.0); - shooterSpeeds.put(1.75, 44.0); - shooterSpeeds.put(2.0, 41.0); - shooterSpeeds.put(2.25, 39.0); - shooterSpeeds.put(2.5, 37.5); - shooterSpeeds.put(2.75, 35.5); - shooterSpeeds.put(3.0, 34.0); - shooterSpeeds.put(3.25, 33.0); - shooterSpeeds.put(3.5, 32.0); - shooterSpeeds.put(4.0, 30.75); - shooterSpeeds.put(4.5, 29.75); - } - - public double getShooterAngleFromInterpolation(double distanceToTarget){ - return shooterSpeeds.get(distanceToTarget); - } + public InterpolatingDoubleTreeMap shooterSpeeds = new InterpolatingDoubleTreeMap(); + public InterpolatingDoubleTreeMap bozoDefenseBotDriversHonestReactionWhenElevatorGoUp_speeds = new InterpolatingDoubleTreeMap(); + + private double offset = 1; + + public Interpolations() { + /* + shooterSpeeds.put(1.25, 53.0); + shooterSpeeds.put(1.5, 48.0); + shooterSpeeds.put(1.75, 44.0); + shooterSpeeds.put(2.0, 41.0); + shooterSpeeds.put(2.25, 39.0); + shooterSpeeds.put(2.5, 37.5); + shooterSpeeds.put(2.75, 35.5); + shooterSpeeds.put(3.0, 34.0); + shooterSpeeds.put(3.25, 33.0); + shooterSpeeds.put(3.5, 32.0); + shooterSpeeds.put(4.0, 30.75); + shooterSpeeds.put(4.5, 29.75); +*/ + bozoDefenseBotDriversHonestReactionWhenElevatorGoUp_speeds.put(1.25, 44.0); + bozoDefenseBotDriversHonestReactionWhenElevatorGoUp_speeds.put(1.5, 39.5); + bozoDefenseBotDriversHonestReactionWhenElevatorGoUp_speeds.put(1.75, 36.0); + bozoDefenseBotDriversHonestReactionWhenElevatorGoUp_speeds.put(2.0, 32.25); + bozoDefenseBotDriversHonestReactionWhenElevatorGoUp_speeds.put(2.25, 30.0); + bozoDefenseBotDriversHonestReactionWhenElevatorGoUp_speeds.put(2.5, 28.0); + bozoDefenseBotDriversHonestReactionWhenElevatorGoUp_speeds.put(2.75, 26.5); + bozoDefenseBotDriversHonestReactionWhenElevatorGoUp_speeds.put(3.0, 24.5); + bozoDefenseBotDriversHonestReactionWhenElevatorGoUp_speeds.put(3.25, 22.5); + bozoDefenseBotDriversHonestReactionWhenElevatorGoUp_speeds.put(3.5, 21.25); + bozoDefenseBotDriversHonestReactionWhenElevatorGoUp_speeds.put(3.75, 20.0); + + shooterSpeeds.put(1.25, 56.0 + offset); + shooterSpeeds.put(1.35, 53.0 + offset); + shooterSpeeds.put(1.5, 50.0 + offset); + shooterSpeeds.put(1.75, 46.0 + offset); + shooterSpeeds.put(2.0, 42.0 + offset); + shooterSpeeds.put(2.25, 38.5 + offset); + shooterSpeeds.put(2.5, 35.5 + offset); + shooterSpeeds.put(2.75, 33.75 + offset); + shooterSpeeds.put(3.0, 32.25 + offset); + shooterSpeeds.put(3.25, 31.0 + offset); + shooterSpeeds.put(3.5, 30.0 + offset); + shooterSpeeds.put(4.0, 28.25 + offset); + + } + + public double getShooterAngleFromInterpolation(double distanceToTarget) { + return shooterSpeeds.get(distanceToTarget); + } + + public double getShooterAngleFromInterpolationElevatorUp(double distanceToTarget) { + return bozoDefenseBotDriversHonestReactionWhenElevatorGoUp_speeds.get(distanceToTarget); + } } \ 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 c1629a5..4a0f5b6 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -36,12 +36,12 @@ public static class Vision { public static final String kNoteCamera = "OnionRing"; public static final Transform3d kRobotToCamFront = new Transform3d( - new Translation3d(Units.inchesToMeters(12.5), Units.inchesToMeters(-1.25), Units.inchesToMeters(14.75)), + new Translation3d(Units.inchesToMeters(11.5), Units.inchesToMeters(-1.25), Units.inchesToMeters(14.75)), new Rotation3d(0, Units.degreesToRadians(15), 0)); public static final Transform3d kRobotToCamBack = new Transform3d( - new Translation3d(Units.inchesToMeters(-7), Units.inchesToMeters(11), Units.inchesToMeters(15)), - new Rotation3d(0, Units.degreesToRadians(14), Math.PI)); + new Translation3d(Units.inchesToMeters(-7), Units.inchesToMeters(11), Units.inchesToMeters(14.5)), + new Rotation3d(0, Units.degreesToRadians(15), Math.PI)); // The layout of the AprilTags on the field public static final AprilTagFieldLayout kTagLayout = getFieldLayout(); @@ -49,7 +49,7 @@ public static class Vision { // The standard deviations of our vision estimated poses, which affect correction rate public static final Matrix kSingleTagStdDevs = VecBuilder.fill(8, 8, 40); public static final Matrix kMultiTagStdDevs = VecBuilder.fill(4, 4, 20); - public static final Vector stateStdDevs = VecBuilder.fill(1, 1, 1); // Encoder Odometry + public static final Vector stateStdDevs = VecBuilder.fill(.5, .5, .5); // Encoder Odometry public static final Pose2d startingPose = new Pose2d(1.35, 5.55, new Rotation2d(0)); } @@ -117,15 +117,15 @@ public static final class Swerve { public static final double angleKD = chosenModule.angleKD; /* Drive Motor PID Values */ - public static final double driveKP = 0.35919; + public static final double driveKP = 0.36; public static final double driveKI = 0.0; public static final double driveKD = 0.0; public static final double driveKF = 0.0; /* Drive Motor Characterization Values From SYSID */ - public static final double driveKS = 0.095261; - public static final double driveKV = 0.71919; - public static final double driveKA = 0.16421; + public static final double driveKS = 0.072013; + public static final double driveKV = 2.3106; + public static final double driveKA = 0.27696; /* Swerve Profiling Values */ /** Meters per Second */ @@ -179,10 +179,16 @@ public static final class Mod3 { } } + public static final class Intake { + public static final double noteOutsideSpeed = 0.5; + public static final double noteInsideSpeed = 0.3; + public static final double outakeSpeed = -0.5; + } + public static final class Shooter { public static final double intakeAngleRadians = 0.56; public static final double idleSpeedRPM = 1300; - public static final double angleOffsetBottom = Units.degreesToRadians(2.3); + public static final double angleOffsetBottom = Units.degreesToRadians(-1); public static final double wristAngleMax = Units.degreesToRadians(62.7); public static final double wristAngleMin = Units.degreesToRadians(-28.8); public static final double angleOffsetManual = Units.degreesToRadians(62.7); @@ -190,8 +196,10 @@ public static final class Shooter { } public static final class Elevator { - public static final double maxHeightMeters = 0.6; + public static final double maxHeightMeters = 0.61; public static final double minHeightMeters = -0.01; + public static final double ampHeight = 0.513; + public static final double antiBozoSmileToasterAhhGoonBotShooterHeight = .5; } public static final class Auto { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 1f88c6d..31f7502 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -82,7 +82,7 @@ public void robotInit() { SmartDashboard.putData(PDH); // idk if this is useful - System.gc(); + //System.gc(); } /** diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4a44438..63abec5 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -109,10 +109,10 @@ public class RobotContainer { /* Subsystems */ private final VisionSubsystem m_VisionSubsystem = new VisionSubsystem(); private final SwerveSubsystem m_SwerveSubsystem = new SwerveSubsystem(m_VisionSubsystem); - private final ShooterSubsystem m_ShooterSubsystem = new ShooterSubsystem(m_SwerveSubsystem); // private final LEDSubsystem m_LEDSubsystem = new LEDSubsystem(m_VisionSubsystem); private final ElevatorSubsystem m_ElevatorSubsystem = new ElevatorSubsystem(); private final IntexerSubsystem m_IntexerSubsystem = new IntexerSubsystem(); + private final ShooterSubsystem m_ShooterSubsystem = new ShooterSubsystem(m_SwerveSubsystem, m_ElevatorSubsystem); private final SendableChooser autoChooser; @@ -171,12 +171,13 @@ private void configureButtonBindings() { /* DRIVER BUTTONS */ // Lock on to speaker - targetSpeaker.whileTrue(new MissileLock(m_ShooterSubsystem, "speaker")); - targetAmp.whileTrue(new MissileLock(m_ShooterSubsystem, "amp")); + targetSpeaker.whileTrue(new MissileLock(m_ShooterSubsystem, "speaker")) + .onFalse(new InstantCommand(() -> m_ShooterSubsystem.setShooterVelocity(Constants.Shooter.idleSpeedRPM))); + targetAmp.whileTrue(new MissileLock(m_ShooterSubsystem, "amp")) + .onFalse(new InstantCommand(() -> m_ShooterSubsystem.setShooterVelocity(Constants.Shooter.idleSpeedRPM))); // Shooter targetSpeaker.or(targetAmp).and(Shoot).whileTrue(new FIREEE(m_ShooterSubsystem, m_IntexerSubsystem)); // Main fire - // Reset Odometry resetOdom.onTrue(new InstantCommand(() -> m_SwerveSubsystem.zeroHeading()).alongWith( new InstantCommand(() -> m_SwerveSubsystem @@ -221,17 +222,21 @@ private void configureButtonBindings() { .alongWith(new RizzLevel(m_ShooterSubsystem, 0.0))); // Zero wrist - zeroShooter.onTrue(new InstantCommand( + mechLT.negate().and(zeroShooter).onTrue(new InstantCommand( () -> m_ShooterSubsystem.resetWristEncoders(Constants.Shooter.angleOffsetManual))); // Set encoder to zero - autoZeroShooter.onTrue(new ZeroRizz(m_ShooterSubsystem) - .andThen(new RizzLevel(m_ShooterSubsystem, Constants.Shooter.intakeAngleRadians))); + //autoZeroShooter.onTrue(new ZeroRizz(m_ShooterSubsystem) + // .andThen(new RizzLevel(m_ShooterSubsystem, Constants.Shooter.intakeAngleRadians))); // Wrist shooterToIntake.onTrue(new RizzLevel(m_ShooterSubsystem, Constants.Shooter.intakeAngleRadians)); // Move wrist to intake position // Amp Preset shooterToAmp.onTrue(new RizzLevel(m_ShooterSubsystem, -0.48)) - .onTrue(new ElevatorSet(m_ElevatorSubsystem, Constants.Elevator.maxHeightMeters)); + .onTrue(new ElevatorSet(m_ElevatorSubsystem, Constants.Elevator.ampHeight)); + + // Anti-Defense Preset + shooterToAntiDefense + .onTrue(new ElevatorSet(m_ElevatorSubsystem, Constants.Elevator.antiBozoSmileToasterAhhGoonBotShooterHeight)); // Reset the R calculation incase it gets off resetR.onTrue(new InstantCommand(() -> FiringSolutionsV3.resetAllR())); @@ -241,7 +246,7 @@ private void configureButtonBindings() { .onFalse(new IntakeThroughShooterPart2(m_ShooterSubsystem, m_IntexerSubsystem, mech)); // Kill Flywheels - mechRightStick.onTrue(new InstantCommand(() -> m_ShooterSubsystem.setShooterVelocity(0))); + mechLT.negate().and(mechRightStick).onTrue(new InstantCommand(() -> m_ShooterSubsystem.setShooterVelocity(0))); // Kill Wrist mechLT.and(mechRightStick).onTrue(new InstantCommand(() -> m_ShooterSubsystem.setWristSpeedManual(0))); @@ -249,6 +254,9 @@ private void configureButtonBindings() { // Kill Elevator mechLT.and(mechLeftStick).onTrue(new InstantCommand(() -> m_ElevatorSubsystem.setElevatorSpeedManual(0))); + mechLT.and(zeroShooter).onTrue(new InstantCommand( + () -> m_ShooterSubsystem.resetWristEncoders(Constants.Shooter.angleOffsetBottom))); + /* THIRD CONTROLLER */ // Characterization tests dynamicForward.whileTrue(m_SwerveSubsystem.sysIdDynamic(Direction.kForward)); @@ -265,7 +273,7 @@ public void stopAll() { } public void zeroWristEncoders(){ - m_ShooterSubsystem.restartWristEncoders(); + m_ShooterSubsystem.resetWristEncoders(Constants.Shooter.angleOffsetBottom); } /** diff --git a/src/main/java/frc/robot/commands/AimBot.java b/src/main/java/frc/robot/commands/AimBot.java index 8ee51c0..8f045d1 100644 --- a/src/main/java/frc/robot/commands/AimBot.java +++ b/src/main/java/frc/robot/commands/AimBot.java @@ -37,10 +37,7 @@ public AimBot(ShooterSubsystem shooterSubsystem, SwerveSubsystem swerve, Intexer // Called when the command is initially scheduled. @Override public void initialize() { - timer.reset(); - timer.start(); shooter.setShooterVelocity(speed); - } // Called every time the scheduler runs while the command is scheduled. @@ -63,6 +60,8 @@ public void execute() { false); if (shooter.isShooterAtSpeed() && rotationPID.getPositionError() <= .035) { + timer.reset(); + timer.start(); intexer.setShooterIntake(.9); } @@ -80,6 +79,6 @@ public void end(boolean interrupted) { // Returns true when the command should end. @Override public boolean isFinished() { - return !intexer.shooterBreak() && timer.get() > 1.5; + return !intexer.shooterBreak() && timer.get() > .2; } } diff --git a/src/main/java/frc/robot/commands/IntakeThroughShooter.java b/src/main/java/frc/robot/commands/IntakeThroughShooter.java index b7d0ee4..837460a 100644 --- a/src/main/java/frc/robot/commands/IntakeThroughShooter.java +++ b/src/main/java/frc/robot/commands/IntakeThroughShooter.java @@ -36,7 +36,7 @@ public void initialize() { @Override public void execute() { shooter.setShooterVelocity(-1000); - intexer.setALL(-.5); + intexer.setALL(Constants.Intake.outakeSpeed); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/commands/IntakeThroughShooterPart2.java b/src/main/java/frc/robot/commands/IntakeThroughShooterPart2.java index 5bf4509..6cc00ce 100644 --- a/src/main/java/frc/robot/commands/IntakeThroughShooterPart2.java +++ b/src/main/java/frc/robot/commands/IntakeThroughShooterPart2.java @@ -42,7 +42,7 @@ public void initialize() { @Override public void execute() { shooter.setShooterVelocity(0); - intexer.setALL(.3); + intexer.setALL(Constants.Intake.noteInsideSpeed); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/commands/IntexBestHex.java b/src/main/java/frc/robot/commands/IntexBestHex.java index 9472ef8..bdab8c5 100644 --- a/src/main/java/frc/robot/commands/IntexBestHex.java +++ b/src/main/java/frc/robot/commands/IntexBestHex.java @@ -8,6 +8,7 @@ import edu.wpi.first.wpilibj.GenericHID.RumbleType; import edu.wpi.first.wpilibj2.command.Command; import frc.lib.math.FiringSolutionsV3; +import frc.robot.Constants; import frc.robot.subsystems.IntexerSubsystem; public class IntexBestHex extends Command { @@ -35,15 +36,15 @@ public void initialize() { public void execute() { if (in) { // Hood logic to run forwards or backwards if (intexer.intakeBreak() && !intexer.shooterBreak()) { // If note is not at shooter yet - intexer.setALL(.3); + intexer.setALL(Constants.Intake.noteInsideSpeed); } else if (!intexer.intakeBreak() && intexer.shooterBreak()) { // Stop note if at shooter controller.setRumble(RumbleType.kBothRumble, 0.75); intexer.setALL(0); } else { // Note is not in robot - intexer.setFrontIntake(.65); + intexer.setFrontIntake(Constants.Intake.noteOutsideSpeed); } } else { // Outtake - intexer.setALL(-0.5); + intexer.setALL(Constants.Intake.outakeSpeed); } } diff --git a/src/main/java/frc/robot/commands/IntexForAutosByAutos.java b/src/main/java/frc/robot/commands/IntexForAutosByAutos.java index b6fa78e..a7544b1 100644 --- a/src/main/java/frc/robot/commands/IntexForAutosByAutos.java +++ b/src/main/java/frc/robot/commands/IntexForAutosByAutos.java @@ -32,12 +32,12 @@ public void execute() { shooter.setWristByAngle(Constants.Shooter.intakeAngleRadians); if (intexer.intakeBreak() && !intexer.shooterBreak()) { // If note is not at shooter yet - intexer.setALL(.3); + intexer.setALL(Constants.Intake.noteInsideSpeed); } else if (intexer.shooterBreak()) { // Stop note if at shooter intexer.setALL(0); } else { // Note is not in robot - intexer.setFrontIntake(.65); - intexer.setShooterIntake(.3); + intexer.setFrontIntake(Constants.Intake.noteOutsideSpeed); + intexer.setShooterIntake(Constants.Intake.noteInsideSpeed); } } diff --git a/src/main/java/frc/robot/commands/ManRizzt.java b/src/main/java/frc/robot/commands/ManRizzt.java index c1e1526..b5fda3c 100644 --- a/src/main/java/frc/robot/commands/ManRizzt.java +++ b/src/main/java/frc/robot/commands/ManRizzt.java @@ -66,6 +66,6 @@ public boolean isFinished() { @Override public void end(boolean interrupted) { - m_shooterSubsystem.setWristSpeedManual(0); + //m_shooterSubsystem.setWristSpeedManual(0); } } diff --git a/src/main/java/frc/robot/commands/MissileLock.java b/src/main/java/frc/robot/commands/MissileLock.java index 5450017..ba5dabf 100644 --- a/src/main/java/frc/robot/commands/MissileLock.java +++ b/src/main/java/frc/robot/commands/MissileLock.java @@ -32,8 +32,8 @@ public void initialize() { public void execute() { if (target == "amp") { if (shooter.getDistanceTo(FiringSolutionsV3.trueAmpX, FiringSolutionsV3.trueAmpY) > 4) { - shooter.PointShoot(Math.toRadians(60), - FiringSolutionsV3.convertToRPM(10)); + shooter.PointShoot(Math.toRadians(55), + FiringSolutionsV3.convertToRPM(12)); } else { shooter.setShooterVelocity(FiringSolutionsV3.convertToRPM(12)); } @@ -46,8 +46,10 @@ public void execute() { // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - shooter.setShooterVelocity(Constants.Shooter.idleSpeedRPM); - shooter.setWristByAngle(Constants.Shooter.intakeAngleRadians); + //shooter.setShooterVelocity(Constants.Shooter.idleSpeedRPM); + if (target != "amp" || shooter.getDistanceTo(FiringSolutionsV3.trueAmpX, FiringSolutionsV3.trueAmpY) > 4){ + shooter.setWristByAngle(Constants.Shooter.intakeAngleRadians); + } } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/commands/NoteSniffer.java b/src/main/java/frc/robot/commands/NoteSniffer.java index 05fa7aa..3833630 100644 --- a/src/main/java/frc/robot/commands/NoteSniffer.java +++ b/src/main/java/frc/robot/commands/NoteSniffer.java @@ -27,7 +27,7 @@ public class NoteSniffer extends Command { private PIDController rotationPID = new PIDController(0.65, 0.00001, 0.04); private Timer timer = new Timer(); private boolean noteInside = false; - private double translationVal = 0.5; + private double translationVal; /** Creates a new IntakeWithVision. */ public NoteSniffer(SwerveSubsystem swerve, VisionSubsystem vision, IntexerSubsystem intexer, @@ -44,7 +44,7 @@ public NoteSniffer(SwerveSubsystem swerve, VisionSubsystem vision, IntexerSubsys public void initialize() { timer.reset(); timer.start(); - translationVal = .25; + translationVal = .35; noteInside = false; } @@ -70,8 +70,8 @@ public void execute() { rotationVal = rotationPID.calculate(yawToNote, swerveSubsystem.getGyroYaw().getRadians()); - intexer.setFrontIntake(.65); - intexer.setShooterIntake(.3); + intexer.setFrontIntake(Constants.Intake.noteOutsideSpeed); + intexer.setShooterIntake(Constants.Intake.noteInsideSpeed); } else { rotationVal = 0; } diff --git a/src/main/java/frc/robot/commands/TeleopSwerve.java b/src/main/java/frc/robot/commands/TeleopSwerve.java index 4927d83..8f74711 100644 --- a/src/main/java/frc/robot/commands/TeleopSwerve.java +++ b/src/main/java/frc/robot/commands/TeleopSwerve.java @@ -62,6 +62,7 @@ public TeleopSwerve(SwerveSubsystem swerve, VisionSubsystem vision, ShooterSubsy public void execute() { Pose2d pose = swerveSubsystem.getPose(); boolean robotCentric = robotCentricSup.getAsBoolean(); + boolean openLoop = true; PhotonPipelineResult result = vision.getLatestResultN(); /* Get Values, Deadband */ @@ -74,7 +75,7 @@ public void execute() { strafeVal = Math.copySign(Math.pow(strafeVal, 2), strafeVal); if (shooterOverrideSpeaker.getAsBoolean()) { // Lock robot angle to speaker - if (shooterSubsystem.getDistanceToSpeakerWhileMoving() >= 3){ + if (shooterSubsystem.getDistanceToSpeakerWhileMoving() >= 3.5){ controller.setRumble(RumbleType.kBothRumble, 0.5); } else { controller.setRumble(RumbleType.kBothRumble, 0); @@ -82,23 +83,25 @@ public void execute() { ChassisSpeeds currentSpeed = swerveSubsystem.getChassisSpeeds(); - rotationVal = rotationPID.calculate(swerveSubsystem.getHeading().getRadians(), + rotationVal = rotationPID.calculate(pose.getRotation().getRadians(), FiringSolutionsV3.getAngleToMovingTarget(pose.getX(), pose.getY(), FiringSolutionsV3.speakerTargetX, FiringSolutionsV3.speakerTargetY, currentSpeed.vxMetersPerSecond, currentSpeed.vyMetersPerSecond, pose.getRotation().getRadians())); + openLoop = false; } else if (shooterOverrideAmp.getAsBoolean()) { // Lock robot angle to amp ChassisSpeeds currentSpeed = swerveSubsystem.getChassisSpeeds(); - if (FiringSolutionsV3.getDistanceToTarget(swerveSubsystem.getPose().getX(), swerveSubsystem.getPose().getY(), - FiringSolutionsV3.trueAmpX, FiringSolutionsV3.trueAmpY) > 4) { - rotationVal = rotationPID.calculate(swerveSubsystem.getHeading().getRadians(), + + if (FiringSolutionsV3.getDistanceToTarget(pose.getX(), pose.getY(), FiringSolutionsV3.trueAmpX, FiringSolutionsV3.trueAmpY) > 4) { + + rotationVal = rotationPID.calculate(pose.getRotation().getRadians(), FiringSolutionsV3.getAngleToMovingTarget(pose.getX(), pose.getY(), FiringSolutionsV3.ampTargetX, FiringSolutionsV3.ampTargetY, currentSpeed.vxMetersPerSecond, currentSpeed.vyMetersPerSecond, pose.getRotation().getRadians())); } else { - rotationVal = rotationPID.calculate(swerveSubsystem.getHeading().getRadians(),Math.toRadians(90)); + rotationVal = rotationPID.calculate(pose.getRotation().getRadians(), Math.toRadians(-90)); } } else if (intakeOverride.getAsBoolean() && result.hasTargets()) { // Lock robot towards detected note @@ -106,6 +109,7 @@ public void execute() { SmartDashboard.putNumber("Note Yaw", yawToNote); + //openLoop = false; rotationVal = rotationPID.calculate(yawToNote, swerveSubsystem.getGyroYaw().getRadians()); } else { @@ -124,6 +128,6 @@ public void execute() { new Translation2d(translationVal, strafeVal).times(Constants.Swerve.maxSpeed), rotationVal * Constants.Swerve.maxAngularVelocity, !robotCentric, - true); + openLoop); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index 5865f53..a966639 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -64,6 +64,7 @@ public ElevatorSubsystem() { elevatorConfigs.ClosedLoopRamps.VoltageClosedLoopRampPeriod = 0.25; m_elevatorLeft.getConfigurator().apply(elevatorConfigs); + m_elevatorRight.getConfigurator().apply(elevatorConfigs); m_elevatorRight.setControl(new Follower(m_elevatorLeft.getDeviceID(), true)); // laser can pid shenanigans diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 27b7209..fecd64a 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -75,9 +75,11 @@ public class ShooterSubsystem extends SubsystemBase { private final int m_WristCurrentMax = 84; private SwerveSubsystem swerveSubsystem; + private ElevatorSubsystem elevatorSubsystem; - public ShooterSubsystem(SwerveSubsystem swerve) { + public ShooterSubsystem(SwerveSubsystem swerve, ElevatorSubsystem elevator) { swerveSubsystem = swerve; + elevatorSubsystem = elevator; m_Wrist = new CANSparkMax(13, MotorType.kBrushless); shootaTop = new CANSparkMax(11, MotorType.kBrushless); // leader @@ -121,7 +123,7 @@ public ShooterSubsystem(SwerveSubsystem swerve) { SmartDashboard.putNumber("set velocity", shooterVelocity); - SmartDashboard.putData(m_pidWrist); + SmartDashboard.putData("Wrist PID", m_pidWrist); SmartDashboard.putData(this); SmartDashboard.putNumber("Set Slip Offset", FiringSolutionsV3.slipPercent); @@ -379,8 +381,12 @@ public void updateShooterMath() { // Shooter Math FiringSolutionsV3.updateSpeakerR(distanceToMovingSpeakerTarget); // shooterAngleToSpeaker = FiringSolutionsV3.getShooterAngleFromSpeakerR(); - shooterAngleToSpeaker = Math - .toRadians(interpolation.getShooterAngleFromInterpolation(distanceToMovingSpeakerTarget)); + + if (elevatorSubsystem.getHeight() > 0.3){ + shooterAngleToSpeaker = Math.toRadians(interpolation.getShooterAngleFromInterpolationElevatorUp(distanceToMovingSpeakerTarget)); + } else { + shooterAngleToSpeaker = Math.toRadians(interpolation.getShooterAngleFromInterpolation(distanceToMovingSpeakerTarget)); + } if (FiringSolutionsV3.getDistanceToMovingTarget(pose.getX(), pose.getY(), FiringSolutionsV3.ampTargetX, FiringSolutionsV3.ampTargetY, diff --git a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java index 87af697..befbce4 100644 --- a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java @@ -314,7 +314,7 @@ public void periodic() { SmartDashboard.putNumber("Mod " + mod.moduleNumber + " Velocity", swerveModuleStates[mod.moduleNumber].speedMetersPerSecond); } - SmartDashboard.putString("Obodom", getPose().toString()); + //SmartDashboard.putString("Obodom", getPose().toString()); SmartDashboard.putNumber("Gyro", getGyroYaw().getDegrees()); SmartDashboard.putNumber("Heading", getHeading().getDegrees()); diff --git a/src/main/java/frc/robot/subsystems/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/VisionSubsystem.java index f854e74..5f27517 100644 --- a/src/main/java/frc/robot/subsystems/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/VisionSubsystem.java @@ -51,7 +51,7 @@ public class VisionSubsystem extends SubsystemBase { private double lastTimeStampFront = 0; private double lastEstTimestampBack = 0; - private final double maxAcceptableRange = 4; + private final double maxAcceptableRange = 2.75; public VisionSubsystem() { aprilTagCameraFront = new PhotonCamera(Constants.Vision.kAprilTagCameraFront); @@ -61,10 +61,10 @@ public VisionSubsystem() { Constants.Vision.kTagLayout.setOrigin(OriginPosition.kBlueAllianceWallRightSide); photonEstimatorFront = new PhotonPoseEstimator( - Constants.Vision.kTagLayout, PoseStrategy.CLOSEST_TO_LAST_POSE, aprilTagCameraFront, Constants.Vision.kRobotToCamFront); + Constants.Vision.kTagLayout, PoseStrategy.LOWEST_AMBIGUITY, aprilTagCameraFront, Constants.Vision.kRobotToCamFront); photonEstimatorBack = new PhotonPoseEstimator( - Constants.Vision.kTagLayout, PoseStrategy.CLOSEST_TO_LAST_POSE, aprilTagCameraBack, Constants.Vision.kRobotToCamBack); + Constants.Vision.kTagLayout, PoseStrategy.LOWEST_AMBIGUITY, aprilTagCameraBack, Constants.Vision.kRobotToCamBack); photonEstimatorFront.setLastPose(Constants.Vision.startingPose); photonEstimatorBack.setLastPose(Constants.Vision.startingPose); From d107878ed80e46b8763273a05e8ae5f7f35af1e4 Mon Sep 17 00:00:00 2001 From: Mr-A768 Date: Tue, 12 Mar 2024 00:52:41 -0500 Subject: [PATCH 35/49] we stay winning --- src/main/java/frc/robot/RobotContainer.java | 34 +++++----- .../robot/commands/IntakeThroughShooter.java | 1 - .../commands/IntakeThroughShooterPart2.java | 2 +- .../robot/commands/ResetNoteInShooter.java | 58 +++++++++++++++++ .../commands/ResetNoteInShooterPart2.java | 62 +++++++++++++++++++ .../robot/subsystems/IntexerSubsystem.java | 6 ++ .../robot/subsystems/ShooterSubsystem.java | 7 +++ 7 files changed, 152 insertions(+), 18 deletions(-) create mode 100644 src/main/java/frc/robot/commands/ResetNoteInShooter.java create mode 100644 src/main/java/frc/robot/commands/ResetNoteInShooterPart2.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 63abec5..245551c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -78,7 +78,7 @@ public class RobotContainer { /** Mech RB */ private final JoystickButton primeShooterSpeedSpeaker = new JoystickButton(mech, XboxController.Button.kRightBumper.value); /** Mech LB */ - private final JoystickButton primeShooterSpeedAmp = new JoystickButton(mech, XboxController.Button.kLeftBumper.value); + private final JoystickButton resetNoteInShooter = new JoystickButton(mech, XboxController.Button.kLeftBumper.value); /** Mech RT */ private final Trigger mechRT = new Trigger(() -> mech.getRawAxis(rightTrigger) > .5); /** Mech LT */ @@ -90,7 +90,7 @@ public class RobotContainer { /** Mech Right */ private final Trigger resetR = new Trigger(() -> mech.getPOV() == 90); /** Mech Left */ - private final Trigger intakeThroughShooter = new Trigger(() -> mech.getPOV() == 270); + private final Trigger mechLeft = new Trigger(() -> mech.getPOV() == 270); /** Mech Start */ private final JoystickButton autoZeroShooter = new JoystickButton(mech, XboxController.Button.kStart.value); /** Mech Back */ @@ -147,7 +147,7 @@ public RobotContainer() { () -> -mech.getRawAxis(leftVerticalAxis))); m_ShooterSubsystem.setDefaultCommand(new ManRizzt(m_ShooterSubsystem, () -> -mech.getRawAxis(rightVerticalAxis), - () -> shooterToAntiDefense.getAsBoolean())); + () -> shooterToSubwoofer.getAsBoolean())); // m_LEDSubsystem.setAllianceColor(); @@ -203,18 +203,13 @@ private void configureButtonBindings() { /* MECH BUTTONS */ - // Prime for Speaker + // Prime Shooter primeShooterSpeedSpeaker .whileTrue(new InstantCommand(() -> m_ShooterSubsystem.setShooterVelocity( FiringSolutionsV3.convertToRPM(m_ShooterSubsystem.getCalculatedVelocity())))) .onFalse(new InstantCommand( () -> m_ShooterSubsystem.setShooterVelocity(Constants.Shooter.idleSpeedRPM))); - // Prime for Amp - primeShooterSpeedAmp.whileTrue(new InstantCommand(() -> m_ShooterSubsystem.setShooterVelocity(3417.8))) - .onFalse(new InstantCommand( - () -> m_ShooterSubsystem.setShooterVelocity(Constants.Shooter.idleSpeedRPM))); - // Elevator elevatorDown.onTrue(new ElevatorSet(m_ElevatorSubsystem, Constants.Elevator.minHeightMeters) .alongWith(new RizzLevel(m_ShooterSubsystem, Constants.Shooter.intakeAngleRadians))); @@ -227,29 +222,36 @@ private void configureButtonBindings() { //autoZeroShooter.onTrue(new ZeroRizz(m_ShooterSubsystem) // .andThen(new RizzLevel(m_ShooterSubsystem, Constants.Shooter.intakeAngleRadians))); - // Wrist - shooterToIntake.onTrue(new RizzLevel(m_ShooterSubsystem, Constants.Shooter.intakeAngleRadians)); // Move wrist to intake position + // Intake Preset + shooterToIntake.onTrue(new RizzLevel(m_ShooterSubsystem, Constants.Shooter.intakeAngleRadians)) + .onTrue(new ElevatorSet(m_ElevatorSubsystem, Constants.Elevator.minHeightMeters)); // Amp Preset shooterToAmp.onTrue(new RizzLevel(m_ShooterSubsystem, -0.48)) .onTrue(new ElevatorSet(m_ElevatorSubsystem, Constants.Elevator.ampHeight)); + // Subwoofer Preset + shooterToSubwoofer.onTrue(new RizzLevel(m_ShooterSubsystem, Math.toRadians(57))) + .onTrue(new ElevatorSet(m_ElevatorSubsystem, Constants.Elevator.minHeightMeters)); + // Anti-Defense Preset shooterToAntiDefense .onTrue(new ElevatorSet(m_ElevatorSubsystem, Constants.Elevator.antiBozoSmileToasterAhhGoonBotShooterHeight)); - // Reset the R calculation incase it gets off + // Reset the R calculation in case it gets off resetR.onTrue(new InstantCommand(() -> FiringSolutionsV3.resetAllR())); - // Intake Through Shooter - intakeThroughShooter.whileTrue(new IntakeThroughShooter(m_ShooterSubsystem, m_IntexerSubsystem, mech)) - .onFalse(new IntakeThroughShooterPart2(m_ShooterSubsystem, m_IntexerSubsystem, mech)); + // Reset Note in Shooter + resetNoteInShooter.whileTrue(new ResetNoteInShooter(m_ShooterSubsystem, m_IntexerSubsystem, mech)) + .onFalse(new ResetNoteInShooterPart2(m_ShooterSubsystem, m_IntexerSubsystem, mech)); // Kill Flywheels mechLT.negate().and(mechRightStick).onTrue(new InstantCommand(() -> m_ShooterSubsystem.setShooterVelocity(0))); // Kill Wrist - mechLT.and(mechRightStick).onTrue(new InstantCommand(() -> m_ShooterSubsystem.setWristSpeedManual(0))); + mechLT.and(mechRightStick).onTrue(new InstantCommand(() -> m_ShooterSubsystem.setWristSpeedManual(0)) + .alongWith(new InstantCommand(() -> m_ShooterSubsystem.setWristToCoast()))) + .onFalse(new InstantCommand(() -> m_ShooterSubsystem.setWristToBrake())); // Kill Elevator mechLT.and(mechLeftStick).onTrue(new InstantCommand(() -> m_ElevatorSubsystem.setElevatorSpeedManual(0))); diff --git a/src/main/java/frc/robot/commands/IntakeThroughShooter.java b/src/main/java/frc/robot/commands/IntakeThroughShooter.java index 837460a..768e6be 100644 --- a/src/main/java/frc/robot/commands/IntakeThroughShooter.java +++ b/src/main/java/frc/robot/commands/IntakeThroughShooter.java @@ -52,7 +52,6 @@ public void end(boolean interrupted) { public boolean isFinished() { if (intexer.intakeBreak()) { controller.setRumble(RumbleType.kBothRumble, 0.75); - intexer.setIntakeThroughShooterPart2Status(true); return true; } else { return false; diff --git a/src/main/java/frc/robot/commands/IntakeThroughShooterPart2.java b/src/main/java/frc/robot/commands/IntakeThroughShooterPart2.java index 6cc00ce..f289097 100644 --- a/src/main/java/frc/robot/commands/IntakeThroughShooterPart2.java +++ b/src/main/java/frc/robot/commands/IntakeThroughShooterPart2.java @@ -56,7 +56,7 @@ public void end(boolean interrupted) { // Returns true when the command should end. @Override public boolean isFinished() { - if (/*intexer.intakeThroughShooterPart2isReady ||*/ intexer.shooterBreak() || timer.get() > 1.5) { + if (intexer.shooterBreak() || timer.get() > 1.5) { return true; } else { return false; diff --git a/src/main/java/frc/robot/commands/ResetNoteInShooter.java b/src/main/java/frc/robot/commands/ResetNoteInShooter.java new file mode 100644 index 0000000..d937692 --- /dev/null +++ b/src/main/java/frc/robot/commands/ResetNoteInShooter.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; + +import edu.wpi.first.wpilibj.GenericHID.RumbleType; +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants; +import frc.robot.subsystems.IntexerSubsystem; +import frc.robot.subsystems.ShooterSubsystem; + +public class ResetNoteInShooter extends Command { + private ShooterSubsystem shooter; + private IntexerSubsystem intexer; + + Joystick controller; + + /** Creates a new IntakeFromShooter. */ + public ResetNoteInShooter(ShooterSubsystem shooterSub, IntexerSubsystem intex, Joystick controller) { + shooter = shooterSub; + intexer = intex; + this.controller = controller; + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(shooterSub, intex); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + shooter.setWristByAngle(.66); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + intexer.setALL(Constants.Intake.outakeSpeed); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + shooter.setWristByAngle(Constants.Shooter.intakeAngleRadians); + intexer.setALL(0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + if (intexer.intakeBreak()) { + controller.setRumble(RumbleType.kBothRumble, 0.75); + return true; + } else { + return false; + } + } +} diff --git a/src/main/java/frc/robot/commands/ResetNoteInShooterPart2.java b/src/main/java/frc/robot/commands/ResetNoteInShooterPart2.java new file mode 100644 index 0000000..b308b5e --- /dev/null +++ b/src/main/java/frc/robot/commands/ResetNoteInShooterPart2.java @@ -0,0 +1,62 @@ +// 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; + +import edu.wpi.first.wpilibj.GenericHID.RumbleType; +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants; +import frc.robot.subsystems.IntexerSubsystem; +import frc.robot.subsystems.ShooterSubsystem; + +public class ResetNoteInShooterPart2 extends Command { + private ShooterSubsystem shooter; + private IntexerSubsystem intexer; + + Joystick controller; + public final Timer timer = new Timer(); + + /** Creates a new IntakeFromShooterPart2. */ + public ResetNoteInShooterPart2(ShooterSubsystem shooterSub, IntexerSubsystem intex, Joystick controller) { + shooter = shooterSub; + intexer = intex; + this.controller = controller; + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(shooterSub, intex); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + timer.reset(); + timer.start(); + controller.setRumble(RumbleType.kBothRumble, 0); + shooter.setWristByAngle(Constants.Shooter.intakeAngleRadians); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + intexer.setALL(Constants.Intake.noteInsideSpeed); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + intexer.setALL(0); + intexer.setIntakeThroughShooterPart2Status(false); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + if (intexer.shooterBreak() || timer.get() > 1.5) { + return true; + } else { + return false; + } + } +} diff --git a/src/main/java/frc/robot/subsystems/IntexerSubsystem.java b/src/main/java/frc/robot/subsystems/IntexerSubsystem.java index 3641624..d1c9217 100644 --- a/src/main/java/frc/robot/subsystems/IntexerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntexerSubsystem.java @@ -19,6 +19,8 @@ public class IntexerSubsystem extends SubsystemBase { private CANSparkBase right; private CANSparkBase shooterIntake; public boolean intakeThroughShooterPart2isReady = false; + public boolean resetNoteInShooterPart2isReady = false; + /** Front Intake */ private DigitalInput breakingBeam; @@ -87,6 +89,10 @@ public void setIntakeThroughShooterPart2Status(boolean value){ intakeThroughShooterPart2isReady = value; } + public void resetNoteInShooterPart2Status(boolean value){ + resetNoteInShooterPart2isReady = value; + } + @Override public void periodic() { SmartDashboard.putBoolean("Intake Beam Break", intakeBreak()); diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index fecd64a..4fe8217 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -363,6 +363,13 @@ public void setWristSpeedManual(double speed) { m_Wrist.set(speed); } + public void setWristToBrake(){ + m_Wrist.setIdleMode(IdleMode.kBrake); + } + public void setWristToCoast(){ + m_Wrist.setIdleMode(IdleMode.kCoast); + } + public void updateWristAngleSetpoint(double angle) { if (angle != lastWristAngleSetpoint) { lastWristAngleSetpoint = angle; From f348016ef421645236027b678153971e7f366c02 Mon Sep 17 00:00:00 2001 From: Camille-Jones <459cmjo2@students.olatheschools.com> Date: Tue, 12 Mar 2024 08:54:33 -0500 Subject: [PATCH 36/49] stuff --- src/main/deploy/pathplanner/autos/3N-1M-Left.auto | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/3N-1M-Left.auto b/src/main/deploy/pathplanner/autos/3N-1M-Left.auto index 03ed260..bd33ff5 100644 --- a/src/main/deploy/pathplanner/autos/3N-1M-Left.auto +++ b/src/main/deploy/pathplanner/autos/3N-1M-Left.auto @@ -2,8 +2,8 @@ "version": 1.0, "startingPose": { "position": { - "x": 1.4259262722800992, - "y": 6.415340906691315 + "x": 1.4174820372432768, + "y": 6.537032333323926 }, "rotation": 0 }, From 11a14d1d60daf6605d4b7aad0f85298be43f6454 Mon Sep 17 00:00:00 2001 From: Camille-Jones <459cmjo2@students.olatheschools.com> Date: Tue, 12 Mar 2024 09:33:39 -0500 Subject: [PATCH 37/49] straitness --- .../pathplanner/paths/toMidAmpLeft.path | 54 ++++++++++++------- 1 file changed, 35 insertions(+), 19 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/toMidAmpLeft.path b/src/main/deploy/pathplanner/paths/toMidAmpLeft.path index c88ccbc..77d8522 100644 --- a/src/main/deploy/pathplanner/paths/toMidAmpLeft.path +++ b/src/main/deploy/pathplanner/paths/toMidAmpLeft.path @@ -8,40 +8,56 @@ }, "prevControl": null, "nextControl": { - "x": 1.1334335257744161, - "y": 6.7858317189318464 + "x": 1.4525645005705963, + "y": 6.373314171129768 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.4983996761342704, - "y": 6.308093566305898 + "x": 2.62197994481459, + "y": 6.303149244475128 }, "prevControl": { - "x": 1.0868292314408643, - "y": 6.394516246593248 + "x": 2.2009903848867527, + "y": 6.338231707802447 }, "nextControl": { - "x": 2.976137828760219, - "y": 6.2788442916553295 + "x": 3.102052624207869, + "y": 6.263143187859021 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 4.087610265481815, - "y": 6.4445901813418836 + "x": 3.873254470155664, + "y": 6.537032333323926 }, "prevControl": { - "x": 3.7073696950244264, - "y": 6.3470925991733225 + "x": 3.4392388686731197, + "y": 6.418028055498066 }, "nextControl": { - "x": 4.698061762695483, - "y": 6.601116206268465 + "x": 4.598292045586939, + "y": 6.735832958845405 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.323329621018216, + "y": 6.958021893251764 + }, + "prevControl": { + "x": 4.880368383741278, + "y": 6.831969463656056 + }, + "nextControl": { + "x": 5.929464796776777, + "y": 7.130508343843873 }, "isLocked": false, "linkedName": null @@ -52,8 +68,8 @@ "y": 7.380566970160069 }, "prevControl": { - "x": 5.871816019166482, - "y": 7.107573740088099 + "x": 6.118532123104131, + "y": 7.203599136543003 }, "nextControl": null, "isLocked": false, @@ -64,8 +80,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 8.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, @@ -77,5 +93,5 @@ "reversed": false, "folder": "Midfield Paths", "previewStartingState": null, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file From 5fe021018d43ca74991110b851f29c5d243bff0a Mon Sep 17 00:00:00 2001 From: Camille-Jones <459cmjo2@students.olatheschools.com> Date: Tue, 12 Mar 2024 12:15:46 -0500 Subject: [PATCH 38/49] more stuff --- .../deploy/pathplanner/autos/3N-2M-Right.auto | 6 +- .../pathplanner/autos/Copy of 5N-1M-left.auto | 115 ++++++++++++++++++ .../pathplanner/paths/toMidSourceRight.path | 33 ++--- 3 files changed, 128 insertions(+), 26 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/Copy of 5N-1M-left.auto diff --git a/src/main/deploy/pathplanner/autos/3N-2M-Right.auto b/src/main/deploy/pathplanner/autos/3N-2M-Right.auto index e2001f4..def3347 100644 --- a/src/main/deploy/pathplanner/autos/3N-2M-Right.auto +++ b/src/main/deploy/pathplanner/autos/3N-2M-Right.auto @@ -2,10 +2,10 @@ "version": 1.0, "startingPose": { "position": { - "x": 0.72394368066646, - "y": 3.0614240800928165 + "x": 0.7141939224496041, + "y": 4.309393131850397 }, - "rotation": 0 + "rotation": -61.949224233821454 }, "command": { "type": "sequential", diff --git a/src/main/deploy/pathplanner/autos/Copy of 5N-1M-left.auto b/src/main/deploy/pathplanner/autos/Copy of 5N-1M-left.auto new file mode 100644 index 0000000..a95fdbd --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Copy of 5N-1M-left.auto @@ -0,0 +1,115 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.3771774811958186, + "y": 6.50308873064302 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "toMidAmpLeft" + } + }, + { + "type": "named", + "data": { + "name": "Note Sniffer" + } + }, + { + "type": "path", + "data": { + "pathName": "backMidAmpLeft" + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "toCloseLeft" + } + }, + { + "type": "named", + "data": { + "name": "Note Sniffer2" + } + }, + { + "type": "path", + "data": { + "pathName": "backCloseLeft" + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "toCloseMiddle" + } + }, + { + "type": "named", + "data": { + "name": "Note Sniffer" + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "toCloseRight" + } + }, + { + "type": "named", + "data": { + "name": "Note Sniffer2" + } + }, + { + "type": "path", + "data": { + "pathName": "backCloseRight" + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/toMidSourceRight.path b/src/main/deploy/pathplanner/paths/toMidSourceRight.path index 83a6a49..b1db96f 100644 --- a/src/main/deploy/pathplanner/paths/toMidSourceRight.path +++ b/src/main/deploy/pathplanner/paths/toMidSourceRight.path @@ -3,29 +3,13 @@ "waypoints": [ { "anchor": { - "x": 0.72394368066646, - "y": 3.05167432187596 + "x": 0.7141939224496041, + "y": 4.309393131850397 }, "prevControl": null, "nextControl": { - "x": 0.35423890648256584, - "y": 3.0404037703087283 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.288130091699615, - "y": 1.8914530940700844 - }, - "prevControl": { - "x": 2.52128167551193, - "y": 2.1816119542492105 - }, - "nextControl": { - "x": 3.6488711457232905, - "y": 1.7549564790340981 + "x": 0.8116915046181651, + "y": 4.172896516814411 }, "isLocked": false, "linkedName": null @@ -36,8 +20,8 @@ "y": 0.81 }, "prevControl": { - "x": 5.8815657773833365, - "y": 0.9067275141676177 + "x": 5.891315535600193, + "y": 0.9554763052518983 }, "nextControl": null, "isLocked": false, @@ -60,6 +44,9 @@ }, "reversed": false, "folder": "Midfield Paths", - "previewStartingState": null, + "previewStartingState": { + "rotation": -60.321510074864804, + "velocity": 0 + }, "useDefaultConstraints": true } \ No newline at end of file From 5e5cafd028f0a5d9adee2bf13900df84f7047c85 Mon Sep 17 00:00:00 2001 From: RavenRain44 <89553123+RavenRain44@users.noreply.github.com> Date: Tue, 12 Mar 2024 17:35:18 -0500 Subject: [PATCH 39/49] Coding Practices W --- src/main/java/frc/robot/Robot.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 2 - src/main/java/frc/robot/commands/FIREEE.java | 6 - .../frc/robot/subsystems/LEDSubsystem.java | 397 ++++++++++-------- 4 files changed, 222 insertions(+), 185 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a889959..58bc1c4 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -104,7 +104,7 @@ public static boolean getAlliance() { return redAlliance; } - public boolean checkRedAlliance() { + public static boolean checkRedAlliance() { Optional alliance = DriverStation.getAlliance(); if (alliance.isPresent()) { return alliance.get() == DriverStation.Alliance.Red; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c36586a..619370f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -149,8 +149,6 @@ public RobotContainer() { m_Shoota.setDefaultCommand(new ManRizzt(m_Shoota, m_LEDSubsystem, () -> -mech.getRawAxis(rightVerticalAxis), () -> shooterToAntiDefense.getAsBoolean())); - // m_LEDSubsystem.setAllianceColor(); - // Another option that allows you to specify the default auto by its name // autoChooser = AutoBuilder.buildAutoChooser("My Default Auto"); autoChooser = AutoBuilder.buildAutoChooser(); diff --git a/src/main/java/frc/robot/commands/FIREEE.java b/src/main/java/frc/robot/commands/FIREEE.java index 1ac27b3..a0a7f5d 100644 --- a/src/main/java/frc/robot/commands/FIREEE.java +++ b/src/main/java/frc/robot/commands/FIREEE.java @@ -32,12 +32,6 @@ public void initialize() { public void execute() { if (shooter.isShooterAtSpeed()){ intexer.setShooterIntake(.9); - ledSubsystem.atSpeed = true; - ledSubsystem.chargingOuttake = false; - ledSubsystem.hasNote = false; - } else { - ledSubsystem.atSpeed = false; - ledSubsystem.chargingOuttake = true; } } diff --git a/src/main/java/frc/robot/subsystems/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/LEDSubsystem.java index 44c3973..e1b047e 100644 --- a/src/main/java/frc/robot/subsystems/LEDSubsystem.java +++ b/src/main/java/frc/robot/subsystems/LEDSubsystem.java @@ -9,200 +9,245 @@ import edu.wpi.first.wpilibj.DigitalOutput; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; import frc.robot.Robot; public class LEDSubsystem extends SubsystemBase { - public DigitalOutput bit1 = new DigitalOutput(3); // Bit 1 (1) - public DigitalOutput bit2 = new DigitalOutput(4); // Bit 2 (2) - public DigitalOutput bit3 = new DigitalOutput(5); // Bit 3 (4) - public DigitalOutput bit4 = new DigitalOutput(6); // Bit 4 (8) - - // Output bits to the LEDs - public DigitalOutput[] bits = {bit1, bit2, bit3, bit4}; // Actual Outputs - boolean[] output = new boolean[4]; // Computed Outputs - - // Booleans used for Easy input - public Boolean hasNote = false; - public Boolean chargingOuttake = false; - public Boolean atSpeed = false; - - public Boolean[] inputBooleans = { - false, // Amp -0 Rainbow Pattern #1 - false, // Source -1 Rainbow Pattern #2 - false, // Climb -2 Rainbow Pattern #3 - false, // Blank/DC -3 None - false, // Note Detected -4 White Solid - false, // Charging -5 Green Pulse (HasNote) - false, // At Speed -6 Green BLink (HasNote) - false, // Charging -7 Magenta Pulse (NoNote) - false, // At Speed -8 Magenta BLink (NoNote) - false, // Note in Intake -9 Orange Blink - false, // Note in Shooter -10 Orange Solid - false, // Alliance Color -11 Red Pulse - false // Alliance Color -12 Blue Pulse - }; - - // Use subsystems - VisionSubsystem vision; - ShooterSubsystem shooter; - IntexerSubsystem intexer; - SwerveSubsystem swerve; - - - /** Creates a new LEDSubsystem. */ - public LEDSubsystem(VisionSubsystem m_VisionSubsystem, ShooterSubsystem m_ShooterSubsystem, IntexerSubsystem m_IntexerSubsystem, SwerveSubsystem m_SwerveSubsystem) { - this.vision = m_VisionSubsystem; - this.shooter = m_ShooterSubsystem; - this.intexer = m_IntexerSubsystem; - this.swerve = m_SwerveSubsystem; - } - - @Override - public void periodic() { - set(); // Decimal Phase - encoder(); // Transition Phase - send(); // Send Phase - } - - private void set() { // Decimal phase - var results = vision.getLatestResultN(); - - // Note Detected - if (results.hasTargets()) { - NoteDetected(true); - } else { - NoteDetected(false); - } - - // Driver Station Connected - if (DriverStation.isDSAttached()) { - inputBooleans[1] = false; - } else { - inputBooleans[1] = true; - } - - // HasNote for ChargingOuttake and AtSpeed - if (hasNote) { // Converts from simple inputs to boolean - if (chargingOuttake) { - inputBooleans[5] = true; inputBooleans[6] = false; - } else if (atSpeed) { - inputBooleans[5] = false; inputBooleans[6] = true; - } - inputBooleans[7] = false; inputBooleans[8] = false; - } else { - if (chargingOuttake) { - inputBooleans[7] = true; inputBooleans[8] = false; - } else if (atSpeed) { - inputBooleans[7] = false; inputBooleans[8] = true; - } - inputBooleans[5] = false; inputBooleans[6] = false; - } - - // Check if pathfinding - if (SwerveSubsystem.followingPath) { - inputBooleans[0] = true; - } else { - inputBooleans[0] = false; - } - - // Check beam breaks - if (intexer.intakeBreak()) { - inputBooleans[9] = true; - inputBooleans[10] = false; - } else if (intexer.shooterBreak()) { - inputBooleans[9] = false; - inputBooleans[10] = true; - } - } - - private void encoder() { // Transition phase - // Decimal to Binary - int pin_amount = 4; // Amount of pins - - int trueIndex = 0; // Index of selected LED sequence - - for (int i = 0; i < inputBooleans.length; i++) { // Picks the first true sequence based on priority - if (inputBooleans[i]) { - trueIndex = i; - break; - } - } - - String binaryString = Integer.toBinaryString(trueIndex); // Cast Number to Binary - int length = binaryString.length(); // Find length of Binary (How many bits) - String finalString; // Final binary string - - // Adds the amount of 0's needed for acurate transcription. Ex. 0x01 -> 0x0001 - if (length < pin_amount) { - int zerosToAdd = pin_amount - length; - StringBuilder paddedStringBuilder = new StringBuilder(); - - for (int i = 0; i < zerosToAdd; i++) { - paddedStringBuilder.append("0"); + public DigitalOutput WalterLight = new DigitalOutput(3); // Bit 1 (1) + public DigitalOutput JesseBlinkman = new DigitalOutput(4); // Bit 2 (2) + public DigitalOutput GusBling = new DigitalOutput(5); // Bit 3 (4) + public DigitalOutput SkylerBright = new DigitalOutput(6); // Bit 4 (8) + + // Output bits to the LEDs + public DigitalOutput[] bits = { WalterLight, JesseBlinkman, GusBling, SkylerBright }; // Actual Outputs + boolean[] output = new boolean[4]; // Computed Outputs + + // Booleans used for Easy input + private Boolean hasNote = false; + private Boolean chargingOuttake = false; + private Boolean atSpeed = false; + + public Boolean[] inputBooleans = { + false, // Amp -0 Rainbow Pattern #1 + false, // Source -1 Rainbow Pattern #2 + false, // Climb -2 Rainbow Pattern #3 + false, // Blank/DC -3 None + false, // Note Detected -4 White Solid + false, // Charging -5 Green Pulse (HasNote) + false, // At Speed -6 Green BLink (HasNote) + false, // Charging -7 Magenta Pulse (NoNote) + false, // At Speed -8 Magenta BLink (NoNote) + false, // Note in Intake -9 Orange Blink + false, // Note in Shooter -10 Orange Solid + false, // Alliance Color -11 Red Pulse + false // Alliance Color -12 Blue Pulse + }; + + // Use subsystems + VisionSubsystem vision; + ShooterSubsystem shooter; + IntexerSubsystem intexer; + SwerveSubsystem swerve; + + /** Creates a new LEDSubsystem. */ + public LEDSubsystem(VisionSubsystem m_VisionSubsystem, ShooterSubsystem m_ShooterSubsystem, + IntexerSubsystem m_IntexerSubsystem, SwerveSubsystem m_SwerveSubsystem) { + this.vision = m_VisionSubsystem; + this.shooter = m_ShooterSubsystem; + this.intexer = m_IntexerSubsystem; + this.swerve = m_SwerveSubsystem; + } + + @Override + public void periodic() { + set(); // Decimal Phase + encoder(); // Transition Phase + send(); // Send Phase + } + + private void set() { // Decimal phase + var results = vision.getLatestResultN(); + + // Note Detected + if (results.hasTargets()) { + NoteDetected(true); + } else { + NoteDetected(false); + } + + // Driver Station Connected + if (DriverStation.isDSAttached()) { + inputBooleans[1] = false; + } else { + inputBooleans[1] = true; + } + + if (Robot.checkRedAlliance()) { + inputBooleans[11] = true; + inputBooleans[12] = false; + } else { + inputBooleans[11] = false; + inputBooleans[12] = true; + } + + // HasNote for ChargingOuttake and AtSpeed + if (hasNote) { // Converts from simple inputs to boolean + if (chargingOuttake) { + inputBooleans[5] = true; + inputBooleans[6] = false; + } else if (atSpeed) { + inputBooleans[5] = false; + inputBooleans[6] = true; + } + inputBooleans[7] = false; + inputBooleans[8] = false; + } else { + if (chargingOuttake) { + inputBooleans[7] = true; + inputBooleans[8] = false; + } else if (atSpeed) { + inputBooleans[7] = false; + inputBooleans[8] = true; + } + inputBooleans[5] = false; + inputBooleans[6] = false; + } + + // Check if pathfinding + if (SwerveSubsystem.followingPath) { + inputBooleans[0] = true; + } else { + inputBooleans[0] = false; + } + + // Charging or At Speed with Note or without Note + if (hasNote) { // Has Note + if (shooter.isShooterAtSpeed()) { // At speed + inputBooleans[5] = false; + inputBooleans[6] = true; + } else if (shooter.getVelocity() > Constants.Shooter.idleSpeedRPM + 500) { // Charging + inputBooleans[5] = true; + inputBooleans[6] = false; + } + inputBooleans[7] = false; + inputBooleans[8] = false; + } else { // No Note + if (shooter.isShooterAtSpeed()) { // At speed + inputBooleans[7] = false; + inputBooleans[8] = true; + } else if (shooter.getVelocity() > Constants.Shooter.idleSpeedRPM + 500) { // Charging + inputBooleans[7] = true; + inputBooleans[8] = false; + } + inputBooleans[5] = false; + inputBooleans[6] = false; + } + + // Check beam breaks + if (intexer.intakeBreak()) { + inputBooleans[9] = true; // Intake + inputBooleans[10] = false; // Shooter + hasNote = true; + } else if (intexer.shooterBreak()) { + inputBooleans[9] = false; // Intake + inputBooleans[10] = true; // Shooter + hasNote = true; + } else { + inputBooleans[9] = false; // Intake + inputBooleans[10] = false; // Shooter + hasNote = false; + } + } + + private void encoder() { // Transition phase + // Decimal to Binary + int pin_amount = 4; // Amount of pins + + int trueIndex = 0; // Index of selected LED sequence + + for (int i = 0; i < inputBooleans.length; i++) { // Picks the first true sequence based on priority + if (inputBooleans[i]) { + trueIndex = i; + break; + } } - paddedStringBuilder.append(binaryString); - String paddedBinaryString = paddedStringBuilder.toString(); + String binaryString = Integer.toBinaryString(trueIndex); // Cast Number to Binary + int length = binaryString.length(); // Find length of Binary (How many bits) + String finalString; // Final binary string + + // Adds the amount of 0's needed for acurate transcription. Ex. 0x01 -> 0x0001 + if (length < pin_amount) { + int zerosToAdd = pin_amount - length; + StringBuilder paddedStringBuilder = new StringBuilder(); + + for (int i = 0; i < zerosToAdd; i++) { + paddedStringBuilder.append("0"); + } + + paddedStringBuilder.append(binaryString); + String paddedBinaryString = paddedStringBuilder.toString(); - finalString = paddedBinaryString; - } else { - finalString = binaryString; + finalString = paddedBinaryString; + } else { + finalString = binaryString; + } + + for (int i = pin_amount - 1; i >= 0; i--) { + output[i] = finalString.charAt(i) == '1'; + } } - - for (int i = pin_amount - 1; i >= 0; i--) { - output[i] = finalString.charAt(i) == '1'; + + private void send() { // Send Phase - "Redundancy is bliss" + for (int i = 0; i < output.length; i++) { + bits[i].set(output[i]); + } } - } - private void send() { // Send Phase - "Redundancy is bliss" - for (int i = 0; i < output.length; i++) { - bits[i].set(output[i]); + // SET FUNCTIONS + public void ampTele(boolean amp) { + inputBooleans[0] = amp; } - } - // SET FUNCTIONS - public void ampTele(boolean amp) { - inputBooleans[0] = amp; - } - - public void sourceTele(boolean source) { - inputBooleans[1] = source; - } + public void sourceTele(boolean source) { + inputBooleans[1] = source; + } - public void climbTele(boolean climb) { - inputBooleans[2] = climb; - } + public void climbTele(boolean climb) { + inputBooleans[2] = climb; + } - public void HasNote(boolean hasNote) { - this.hasNote = hasNote; - } + public void HasNote(boolean hasNote) { + this.hasNote = hasNote; + } - public void ChargingOuttake(boolean chargingOuttake) { - this.chargingOuttake = chargingOuttake; - } + public void ChargingOuttake(boolean chargingOuttake) { + this.chargingOuttake = chargingOuttake; + } - public void ReadyToFire(boolean fire) { - this.atSpeed = fire; - } + public void ReadyToFire(boolean fire) { + this.atSpeed = fire; + } - public void NoteInIntake(boolean noteInIntake) { - inputBooleans[9] = noteInIntake; - } + public void NoteInIntake(boolean noteInIntake) { + inputBooleans[9] = noteInIntake; + } - public void NoteInShooter(boolean noteInShooter) { - inputBooleans[10] = noteInShooter; - } + public void NoteInShooter(boolean noteInShooter) { + inputBooleans[10] = noteInShooter; + } - public void NoteDetected(boolean note) { - inputBooleans[4] = note; - } + public void NoteDetected(boolean note) { + inputBooleans[4] = note; + } - public void setAllianceColor() { - inputBooleans[11] = Robot.getAlliance(); // True is Red - inputBooleans[12] = !Robot.getAlliance(); // False is Blue - } + /* DISCLAIMER: DISCONTINUED */ + public void setAllianceColor() { + inputBooleans[11] = Robot.getAlliance(); // True is Red + inputBooleans[12] = !Robot.getAlliance(); // False is Blue + } - public void Disconnected(boolean disconnected) { // NOT CONFIRMED TO BE IN FINAL - inputBooleans[3] = disconnected; - } + public void Disconnected(boolean disconnected) { // NOT CONFIRMED TO BE IN FINAL + inputBooleans[3] = disconnected; + } } From b1703fc7fcd387f42b05df23bfc612d0f27ac6d6 Mon Sep 17 00:00:00 2001 From: Andrew-K961 Date: Tue, 12 Mar 2024 21:16:41 -0500 Subject: [PATCH 40/49] HEARTLAND VS DONIIIIIIIIII --- .pathplanner/settings.json | 4 +- .../{3N-2M-Right.auto => HBNFClBrI.auto} | 20 ++-- .../pathplanner/autos/ROMAN CHARGE.auto | 97 +++++++++++++++++++ src/main/deploy/pathplanner/navgrid.json | 2 +- .../pathplanner/paths/2nd Note TOP.path | 4 +- .../pathplanner/paths/3N Grab Note 3.path | 4 +- .../deploy/pathplanner/paths/3N Top 1.path | 4 +- .../pathplanner/paths/3N Top Shoot 1.path | 4 +- .../pathplanner/paths/3N Top Shoot 2.path | 4 +- .../pathplanner/paths/3N Top Shoot 3.path | 4 +- .../pathplanner/paths/3rd Note TOP.path | 4 +- .../pathplanner/paths/Straight Line Top.path | 4 +- .../pathplanner/paths/backCloseLeft.path | 2 +- .../pathplanner/paths/backCloseMiddle.path | 2 +- .../pathplanner/paths/backCloseRight.path | 2 +- .../pathplanner/paths/backMidAmpLeft.path | 2 +- .../pathplanner/paths/backMidAmpRight.path | 2 +- .../pathplanner/paths/backMidSourceLeft.path | 38 +++----- .../pathplanner/paths/backMidSourceRight.path | 40 +++----- .../paths/backMidSourceRightAlt.path | 49 ++++++++++ .../paths/idk what the naming scheme is.path | 52 ++++++++++ .../deploy/pathplanner/paths/toCloseLeft.path | 2 +- .../pathplanner/paths/toCloseMiddle.path | 2 +- .../pathplanner/paths/toCloseRight.path | 2 +- .../pathplanner/paths/toMidAmpRight.path | 2 +- .../pathplanner/paths/toMidSourceLeft.path | 23 +++-- .../pathplanner/paths/toMidSourceLeftAlt.path | 52 ++++++++++ .../pathplanner/paths/toMidSourceRight.path | 36 ++++--- .../paths/toMidSourceRightAlt.path | 64 ++++++++++++ .../paths/toRightCloseFromRight.path | 2 +- .../java/frc/lib/math/FiringSolutionsV3.java | 79 +++++++++++---- src/main/java/frc/robot/Constants.java | 15 +-- src/main/java/frc/robot/Robot.java | 2 + src/main/java/frc/robot/RobotContainer.java | 30 +++--- src/main/java/frc/robot/commands/AimBot.java | 14 ++- .../FIREEFORACERTAINAMOUNTOFTIME.java | 50 ++++++++++ .../java/frc/robot/commands/ManRizzt.java | 2 +- .../java/frc/robot/commands/NoteSniffer.java | 8 +- .../java/frc/robot/commands/TeleopSwerve.java | 42 ++++++-- .../robot/subsystems/ElevatorSubsystem.java | 6 +- .../frc/robot/subsystems/SwerveSubsystem.java | 11 ++- .../frc/robot/subsystems/VisionSubsystem.java | 5 +- vendordeps/PathplannerLib.json | 6 +- 43 files changed, 615 insertions(+), 184 deletions(-) rename src/main/deploy/pathplanner/autos/{3N-2M-Right.auto => HBNFClBrI.auto} (74%) create mode 100644 src/main/deploy/pathplanner/autos/ROMAN CHARGE.auto create mode 100644 src/main/deploy/pathplanner/paths/backMidSourceRightAlt.path create mode 100644 src/main/deploy/pathplanner/paths/idk what the naming scheme is.path create mode 100644 src/main/deploy/pathplanner/paths/toMidSourceLeftAlt.path create mode 100644 src/main/deploy/pathplanner/paths/toMidSourceRightAlt.path create mode 100644 src/main/java/frc/robot/commands/FIREEFORACERTAINAMOUNTOFTIME.java diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index e287b0f..101b75c 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -10,8 +10,8 @@ "autoFolders": [ "Testing" ], - "defaultMaxVel": 2.0, - "defaultMaxAccel": 3.0, + "defaultMaxVel": 4.0, + "defaultMaxAccel": 3.5, "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, "maxModuleSpeed": 4.5 diff --git a/src/main/deploy/pathplanner/autos/3N-2M-Right.auto b/src/main/deploy/pathplanner/autos/HBNFClBrI.auto similarity index 74% rename from src/main/deploy/pathplanner/autos/3N-2M-Right.auto rename to src/main/deploy/pathplanner/autos/HBNFClBrI.auto index def3347..b431f47 100644 --- a/src/main/deploy/pathplanner/autos/3N-2M-Right.auto +++ b/src/main/deploy/pathplanner/autos/HBNFClBrI.auto @@ -2,8 +2,8 @@ "version": 1.0, "startingPose": { "position": { - "x": 0.7141939224496041, - "y": 4.309393131850397 + "x": 0.744692763774372, + "y": 4.446330676429759 }, "rotation": -61.949224233821454 }, @@ -20,7 +20,7 @@ { "type": "path", "data": { - "pathName": "toMidSourceRight" + "pathName": "toMidSourceRightAlt" } }, { @@ -32,7 +32,7 @@ { "type": "path", "data": { - "pathName": "backMidSourceRight" + "pathName": "backMidSourceRightAlt" } }, { @@ -44,25 +44,19 @@ { "type": "path", "data": { - "pathName": "toMidSourceLeft" + "pathName": "toMidSourceLeftAlt" } }, { "type": "named", "data": { - "name": "Note Sniffer2" - } - }, - { - "type": "path", - "data": { - "pathName": "backMidSourceLeft" + "name": "Idle Speed" } }, { "type": "named", "data": { - "name": "Shoot" + "name": "Note Sniffer2" } } ] diff --git a/src/main/deploy/pathplanner/autos/ROMAN CHARGE.auto b/src/main/deploy/pathplanner/autos/ROMAN CHARGE.auto new file mode 100644 index 0000000..76c7f52 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/ROMAN CHARGE.auto @@ -0,0 +1,97 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.744692763774372, + "y": 4.446330676429759 + }, + "rotation": -61.949224233821454 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "toMidSourceRight" + } + }, + { + "type": "named", + "data": { + "name": "Note Sniffer" + } + }, + { + "type": "path", + "data": { + "pathName": "backMidSourceRight" + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "toMidSourceLeft" + } + }, + { + "type": "named", + "data": { + "name": "Note Sniffer2" + } + }, + { + "type": "named", + "data": { + "name": "Fire Under Stage" + } + }, + { + "type": "path", + "data": { + "pathName": "backMidSourceLeft" + } + }, + { + "type": "named", + "data": { + "name": "Force Shoot" + } + }, + { + "type": "named", + "data": { + "name": "Idle Speed" + } + }, + { + "type": "path", + "data": { + "pathName": "idk what the naming scheme is" + } + }, + { + "type": "named", + "data": { + "name": "Note Sniffer" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json index bab0da9..20b7761 100644 --- a/src/main/deploy/pathplanner/navgrid.json +++ b/src/main/deploy/pathplanner/navgrid.json @@ -1 +1 @@ -{"field_size":{"x":16.54,"y":8.21},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true],[true,true,true,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]} \ No newline at end of file +{"field_size":{"x":16.54,"y":8.21},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true],[true,true,true,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/2nd Note TOP.path b/src/main/deploy/pathplanner/paths/2nd Note TOP.path index dd95373..44e74cc 100644 --- a/src/main/deploy/pathplanner/paths/2nd Note TOP.path +++ b/src/main/deploy/pathplanner/paths/2nd Note TOP.path @@ -66,8 +66,8 @@ } ], "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.0, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/3N Grab Note 3.path b/src/main/deploy/pathplanner/paths/3N Grab Note 3.path index fb53f75..de757a4 100644 --- a/src/main/deploy/pathplanner/paths/3N Grab Note 3.path +++ b/src/main/deploy/pathplanner/paths/3N Grab Note 3.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.0, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/3N Top 1.path b/src/main/deploy/pathplanner/paths/3N Top 1.path index bfdfcd7..498a5b2 100644 --- a/src/main/deploy/pathplanner/paths/3N Top 1.path +++ b/src/main/deploy/pathplanner/paths/3N Top 1.path @@ -38,8 +38,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.0, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/3N Top Shoot 1.path b/src/main/deploy/pathplanner/paths/3N Top Shoot 1.path index bd6490b..1993c8e 100644 --- a/src/main/deploy/pathplanner/paths/3N Top Shoot 1.path +++ b/src/main/deploy/pathplanner/paths/3N Top Shoot 1.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.0, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/3N Top Shoot 2.path b/src/main/deploy/pathplanner/paths/3N Top Shoot 2.path index 5cb0994..f78350f 100644 --- a/src/main/deploy/pathplanner/paths/3N Top Shoot 2.path +++ b/src/main/deploy/pathplanner/paths/3N Top Shoot 2.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.0, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/3N Top Shoot 3.path b/src/main/deploy/pathplanner/paths/3N Top Shoot 3.path index 7b4eefb..045e830 100644 --- a/src/main/deploy/pathplanner/paths/3N Top Shoot 3.path +++ b/src/main/deploy/pathplanner/paths/3N Top Shoot 3.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.0, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/3rd Note TOP.path b/src/main/deploy/pathplanner/paths/3rd Note TOP.path index ba204bc..c105bb5 100644 --- a/src/main/deploy/pathplanner/paths/3rd Note TOP.path +++ b/src/main/deploy/pathplanner/paths/3rd Note TOP.path @@ -127,8 +127,8 @@ } ], "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.0, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Straight Line Top.path b/src/main/deploy/pathplanner/paths/Straight Line Top.path index 1205986..c18b918 100644 --- a/src/main/deploy/pathplanner/paths/Straight Line Top.path +++ b/src/main/deploy/pathplanner/paths/Straight Line Top.path @@ -38,8 +38,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.0, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/backCloseLeft.path b/src/main/deploy/pathplanner/paths/backCloseLeft.path index bbebd94..e59303b 100644 --- a/src/main/deploy/pathplanner/paths/backCloseLeft.path +++ b/src/main/deploy/pathplanner/paths/backCloseLeft.path @@ -33,7 +33,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 8.0, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/backCloseMiddle.path b/src/main/deploy/pathplanner/paths/backCloseMiddle.path index 0e9cc01..0f0d7d2 100644 --- a/src/main/deploy/pathplanner/paths/backCloseMiddle.path +++ b/src/main/deploy/pathplanner/paths/backCloseMiddle.path @@ -33,7 +33,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 8.0, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/backCloseRight.path b/src/main/deploy/pathplanner/paths/backCloseRight.path index 3216dff..eace181 100644 --- a/src/main/deploy/pathplanner/paths/backCloseRight.path +++ b/src/main/deploy/pathplanner/paths/backCloseRight.path @@ -33,7 +33,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 8.0, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/backMidAmpLeft.path b/src/main/deploy/pathplanner/paths/backMidAmpLeft.path index 2927175..3f182c2 100644 --- a/src/main/deploy/pathplanner/paths/backMidAmpLeft.path +++ b/src/main/deploy/pathplanner/paths/backMidAmpLeft.path @@ -49,7 +49,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 8.0, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/backMidAmpRight.path b/src/main/deploy/pathplanner/paths/backMidAmpRight.path index d0d5918..c803c05 100644 --- a/src/main/deploy/pathplanner/paths/backMidAmpRight.path +++ b/src/main/deploy/pathplanner/paths/backMidAmpRight.path @@ -49,7 +49,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 8.0, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/backMidSourceLeft.path b/src/main/deploy/pathplanner/paths/backMidSourceLeft.path index 01e80ad..a142a21 100644 --- a/src/main/deploy/pathplanner/paths/backMidSourceLeft.path +++ b/src/main/deploy/pathplanner/paths/backMidSourceLeft.path @@ -3,45 +3,29 @@ "waypoints": [ { "anchor": { - "x": 6.46, - "y": 2.0961980166240624 + "x": 7.344029509911752, + "y": 2.8859284321894068 }, "prevControl": null, "nextControl": { - "x": 6.136791840852092, - "y": 1.7293931318503977 + "x": 6.661546434731825, + "y": 3.344167068381643 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.3953774320850316, - "y": 2.300942939178041 + "x": 6.027812150636178, + "y": 3.782906188140168 }, "prevControl": { - "x": 4.763522936223294, - "y": 1.9429235549175612 - }, - "nextControl": { - "x": 2.3521533028814297, - "y": 2.5739361692500116 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.41, - "y": 5.57 - }, - "prevControl": { - "x": 1.8159166009543433, - "y": 4.640884911223504 + "x": 6.495800545045272, + "y": 3.412415375899636 }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "i hope this works" } ], "rotationTargets": [], @@ -49,13 +33,13 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 8.0, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, "goalEndState": { "velocity": 0, - "rotation": 0, + "rotation": -29.430000000000007, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/backMidSourceRight.path b/src/main/deploy/pathplanner/paths/backMidSourceRight.path index 6f01df6..30ac516 100644 --- a/src/main/deploy/pathplanner/paths/backMidSourceRight.path +++ b/src/main/deploy/pathplanner/paths/backMidSourceRight.path @@ -3,45 +3,29 @@ "waypoints": [ { "anchor": { - "x": 6.417802479310422, - "y": 0.8092299319990566 + "x": 6.651796676514969, + "y": 0.70198259161364 }, "prevControl": null, "nextControl": { - "x": 6.048097705126527, - "y": 0.7979593804318251 + "x": 5.6670710966125295, + "y": 1.2284695353208006 }, "isLocked": false, - "linkedName": null + "linkedName": "bottom wing line" }, { "anchor": { - "x": 2.868890488374794, - "y": 2.515437619949907 + "x": 1.3479282065452507, + "y": 3.9584018360435778 }, "prevControl": { - "x": 3.933276408891291, - "y": 1.5842649119057797 - }, - "nextControl": { - "x": 2.556898225435399, - "y": 2.7883824933930974 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.41, - "y": 5.57 - }, - "prevControl": { - "x": 1.5039243380149394, - "y": 5.030875239898781 + "x": 1.5429233708823726, + "y": 2.993175772574823 }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "SHoot from bottomn" } ], "rotationTargets": [], @@ -49,13 +33,13 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 8.0, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, "goalEndState": { "velocity": 0, - "rotation": 0, + "rotation": -48.64118525790135, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/backMidSourceRightAlt.path b/src/main/deploy/pathplanner/paths/backMidSourceRightAlt.path new file mode 100644 index 0000000..6048004 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/backMidSourceRightAlt.path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 6.798043049767811, + "y": 2.378941004912889 + }, + "prevControl": null, + "nextControl": { + "x": 5.364828591889964, + "y": 0.6922328333967845 + }, + "isLocked": false, + "linkedName": "3 Note Top Alt" + }, + { + "anchor": { + "x": 1.3479282065452507, + "y": 3.9584018360435778 + }, + "prevControl": { + "x": 1.5429233708823726, + "y": 2.993175772574823 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -48.64118525790135, + "rotateFast": false + }, + "reversed": false, + "folder": "Midfield Paths", + "previewStartingState": null, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/idk what the naming scheme is.path b/src/main/deploy/pathplanner/paths/idk what the naming scheme is.path new file mode 100644 index 0000000..9cc1024 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/idk what the naming scheme is.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 6.027812150636178, + "y": 3.782906188140168 + }, + "prevControl": null, + "nextControl": { + "x": 6.4763010286115605, + "y": 3.9681515942604335 + }, + "isLocked": false, + "linkedName": "i hope this works" + }, + { + "anchor": { + "x": 6.983288455888077, + "y": 4.1338974839469875 + }, + "prevControl": { + "x": 6.612789523297573, + "y": 4.0266781996568355 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "Midfield Paths", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/toCloseLeft.path b/src/main/deploy/pathplanner/paths/toCloseLeft.path index 8b49246..ce9ff1e 100644 --- a/src/main/deploy/pathplanner/paths/toCloseLeft.path +++ b/src/main/deploy/pathplanner/paths/toCloseLeft.path @@ -33,7 +33,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 8.0, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/toCloseMiddle.path b/src/main/deploy/pathplanner/paths/toCloseMiddle.path index bdaf095..79f39e7 100644 --- a/src/main/deploy/pathplanner/paths/toCloseMiddle.path +++ b/src/main/deploy/pathplanner/paths/toCloseMiddle.path @@ -33,7 +33,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 8.0, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/toCloseRight.path b/src/main/deploy/pathplanner/paths/toCloseRight.path index 7866d27..e609aae 100644 --- a/src/main/deploy/pathplanner/paths/toCloseRight.path +++ b/src/main/deploy/pathplanner/paths/toCloseRight.path @@ -33,7 +33,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 8.0, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/toMidAmpRight.path b/src/main/deploy/pathplanner/paths/toMidAmpRight.path index 70a9a58..a7bce57 100644 --- a/src/main/deploy/pathplanner/paths/toMidAmpRight.path +++ b/src/main/deploy/pathplanner/paths/toMidAmpRight.path @@ -49,7 +49,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 8.0, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/toMidSourceLeft.path b/src/main/deploy/pathplanner/paths/toMidSourceLeft.path index f9eed3c..d09d636 100644 --- a/src/main/deploy/pathplanner/paths/toMidSourceLeft.path +++ b/src/main/deploy/pathplanner/paths/toMidSourceLeft.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 3.6683706621570025, - "y": 2.242444389876904 + "x": 2.1571581385443066, + "y": 3.4709139252007724 }, "prevControl": null, "nextControl": { - "x": 4.887090439264016, - "y": 2.0571989837566393 + "x": 3.8926151011446923, + "y": 2.300942939178039 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 6.46, - "y": 2.0961980166240624 + "x": 6.768338999394455, + "y": 2.355489245691538 }, "prevControl": { - "x": 5.306330042588828, - "y": 1.7354569626003862 + "x": 5.1405841529022736, + "y": 1.3259671174924308 }, "nextControl": null, "isLocked": false, @@ -33,7 +33,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 8.0, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, @@ -44,6 +44,9 @@ }, "reversed": false, "folder": "Midfield Paths", - "previewStartingState": null, + "previewStartingState": { + "rotation": -50.0, + "velocity": 0 + }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/toMidSourceLeftAlt.path b/src/main/deploy/pathplanner/paths/toMidSourceLeftAlt.path new file mode 100644 index 0000000..03faadb --- /dev/null +++ b/src/main/deploy/pathplanner/paths/toMidSourceLeftAlt.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.9329136995566167, + "y": 3.3539168265984998 + }, + "prevControl": null, + "nextControl": { + "x": 3.288130091699615, + "y": 2.6031854439005797 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.5835483689969765, + "y": 0.8189796902159131 + }, + "prevControl": { + "x": 5.218582218637122, + "y": 1.540461798263264 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "Midfield Paths", + "previewStartingState": { + "rotation": -50.0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/toMidSourceRight.path b/src/main/deploy/pathplanner/paths/toMidSourceRight.path index b1db96f..92a4b04 100644 --- a/src/main/deploy/pathplanner/paths/toMidSourceRight.path +++ b/src/main/deploy/pathplanner/paths/toMidSourceRight.path @@ -3,37 +3,49 @@ "waypoints": [ { "anchor": { - "x": 0.7141939224496041, - "y": 4.309393131850397 + "x": 0.744692763774372, + "y": 4.446330676429759 }, "prevControl": null, "nextControl": { - "x": 0.8116915046181651, - "y": 4.172896516814411 + "x": 2.4789001597005584, + "y": 2.8469293993219815 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 6.42, - "y": 0.81 + "x": 6.651796676514969, + "y": 0.70198259161364 }, "prevControl": { - "x": 5.891315535600193, - "y": 0.9554763052518983 + "x": 6.037561908853036, + "y": 1.0627236456373161 }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "bottom wing line" } ], "rotationTargets": [], - "constraintZones": [], + "constraintZones": [ + { + "name": "New Constraints Zone", + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 4.25, + "maxAcceleration": 3.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + } + } + ], "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 8.0, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, @@ -45,7 +57,7 @@ "reversed": false, "folder": "Midfield Paths", "previewStartingState": { - "rotation": -60.321510074864804, + "rotation": -59.63572899983965, "velocity": 0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/toMidSourceRightAlt.path b/src/main/deploy/pathplanner/paths/toMidSourceRightAlt.path new file mode 100644 index 0000000..f6c0ccd --- /dev/null +++ b/src/main/deploy/pathplanner/paths/toMidSourceRightAlt.path @@ -0,0 +1,64 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.744692763774372, + "y": 4.446330676429759 + }, + "prevControl": null, + "nextControl": { + "x": 2.44965088504999, + "y": 2.47643858708145 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.798043049767811, + "y": 2.378941004912889 + }, + "prevControl": { + "x": 5.452576415841669, + "y": 0.9067275141676178 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "3 Note Top Alt" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "New Constraints Zone", + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 4.25, + "maxAcceleration": 3.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + } + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "Midfield Paths", + "previewStartingState": { + "rotation": -59.63572899983965, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/toRightCloseFromRight.path b/src/main/deploy/pathplanner/paths/toRightCloseFromRight.path index 04f26cb..f554d3a 100644 --- a/src/main/deploy/pathplanner/paths/toRightCloseFromRight.path +++ b/src/main/deploy/pathplanner/paths/toRightCloseFromRight.path @@ -33,7 +33,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 8.0, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/java/frc/lib/math/FiringSolutionsV3.java b/src/main/java/frc/lib/math/FiringSolutionsV3.java index aaf2ffc..081d660 100644 --- a/src/main/java/frc/lib/math/FiringSolutionsV3.java +++ b/src/main/java/frc/lib/math/FiringSolutionsV3.java @@ -27,7 +27,7 @@ public class FiringSolutionsV3 { private static final double trueAmpXRed = 16.54 - trueAmpXBlue; public static double trueAmpX; public static final double trueAmpY = 8.2; - + // Speaker Values public static double speakerTargetXOffset = .24; private static double speakerTargetXBlue = 0.0 + speakerTargetXOffset; @@ -40,7 +40,6 @@ public class FiringSolutionsV3 { private static double speakerR, ampR, customR = 1.0; public static double maxRangeWithR = 11.25; - private FiringSolutionsV3() { } @@ -56,7 +55,7 @@ public static void setAlliance(boolean redAlliance) { } } - public static void updateHeight(double laserCANValue){ + public static void updateHeight(double laserCANValue) { shooterHeight = defaultShooterHeight + (laserCANValue - laserCANOffset); } @@ -68,7 +67,8 @@ public static double getDistanceToTarget(double robotX, double robotY, double ta return Math.abs(Math.sqrt(Math.pow(targetX - robotX, 2) + Math.pow(targetY - robotY, 2))); } - public static double getRobotVelocityTowardsTarget(double robotX, double targetX, double robotVelocityX, double robotVelocityY, + public static double getRobotVelocityTowardsTarget(double robotX, double targetX, double robotVelocityX, + double robotVelocityY, double angleToTarget, double robotHeading) { if (robotX <= targetX) { if (robotVelocityX == 0) { @@ -89,7 +89,8 @@ public static double getRobotVelocityTowardsTarget(double robotX, double targetX } } - public static double getRobotVelocityPerpendicularToTarget(double robotX, double targetX, double robotVelocityX, double robotVelocityY, + public static double getRobotVelocityPerpendicularToTarget(double robotX, double targetX, double robotVelocityX, + double robotVelocityY, double angleToSpeaker, double robotHeading) { if (robotX <= targetX) { if (robotVelocityX == 0) { @@ -197,7 +198,7 @@ public static double getShooterAngleFromCustomR() { return Math.acos(customR); } - public static void resetAllR(){ + public static void resetAllR() { resetSpeakerR(); resetAmpR(); resetCustomR(); @@ -272,37 +273,75 @@ public static Optional movingTarget( // is based on the same as the input translate2ds) } - public static double getAngleToMovingTarget(double robotX, double robotY, double targetX, double targetY, double robotVelocityX, + public static double getAngleToMovingTarget(double robotX, double robotY, double targetX, double targetY, + double robotVelocityX, double robotVelocityY, double robotHeading) { - return Math.atan((movingTarget(robotX, robotY, targetX, targetY, - getRobotVelocityTowardsTarget(robotX, targetX, robotVelocityX, robotVelocityY, - getAngleToTarget(robotX, robotY, targetX, targetY), robotHeading), - getRobotVelocityPerpendicularToTarget(robotX, targetX, robotVelocityX, robotVelocityY, - getAngleToTarget(robotX, robotY, targetX, targetY), robotHeading)).get().getY() - robotY) - / (movingTarget(robotX, robotY, targetX, targetY, + //if (robotX >= targetX) { + return Math.atan((movingTarget(robotX, robotY, targetX, targetY, + getRobotVelocityTowardsTarget(robotX, targetX, robotVelocityX, robotVelocityY, + getAngleToTarget(robotX, robotY, targetX, targetY), robotHeading), + getRobotVelocityPerpendicularToTarget(robotX, targetX, robotVelocityX, robotVelocityY, + getAngleToTarget(robotX, robotY, targetX, targetY), robotHeading)) + .get().getY() - robotY) + / (movingTarget(robotX, robotY, targetX, targetY, + getRobotVelocityTowardsTarget(robotX, targetX, robotVelocityX, robotVelocityY, + getAngleToTarget(robotX, robotY, targetX, targetY), robotHeading), + getRobotVelocityPerpendicularToTarget(robotX, targetX, robotVelocityX, robotVelocityY, + getAngleToTarget(robotX, robotY, targetX, targetY), robotHeading)) + .get().getX() - robotX)); + /*} else { + if (robotY >= targetY) { + return Math.atan((movingTarget(robotX, robotY, targetX, targetY, getRobotVelocityTowardsTarget(robotX, targetX, robotVelocityX, robotVelocityY, getAngleToTarget(robotX, robotY, targetX, targetY), robotHeading), getRobotVelocityPerpendicularToTarget(robotX, targetX, robotVelocityX, robotVelocityY, - getAngleToTarget(robotX, robotY, targetX, targetY), robotHeading)).get().getX() - robotX)); + getAngleToTarget(robotX, robotY, targetX, targetY), robotHeading)) + .get().getY() - robotY) + / (movingTarget(robotX, robotY, targetX, targetY, + getRobotVelocityTowardsTarget(robotX, targetX, robotVelocityX, robotVelocityY, + getAngleToTarget(robotX, robotY, targetX, targetY), robotHeading), + getRobotVelocityPerpendicularToTarget(robotX, targetX, robotVelocityX, robotVelocityY, + getAngleToTarget(robotX, robotY, targetX, targetY), robotHeading)) + .get().getX() - robotX)) + Math.toRadians(180); + } else { + return Math.atan((movingTarget(robotX, robotY, targetX, targetY, + getRobotVelocityTowardsTarget(robotX, targetX, robotVelocityX, robotVelocityY, + getAngleToTarget(robotX, robotY, targetX, targetY), robotHeading), + getRobotVelocityPerpendicularToTarget(robotX, targetX, robotVelocityX, robotVelocityY, + getAngleToTarget(robotX, robotY, targetX, targetY), robotHeading)) + .get().getY() - robotY) + / (movingTarget(robotX, robotY, targetX, targetY, + getRobotVelocityTowardsTarget(robotX, targetX, robotVelocityX, robotVelocityY, + getAngleToTarget(robotX, robotY, targetX, targetY), robotHeading), + getRobotVelocityPerpendicularToTarget(robotX, targetX, robotVelocityX, robotVelocityY, + getAngleToTarget(robotX, robotY, targetX, targetY), robotHeading)) + .get().getX() - robotX)) - Math.toRadians(180); + } + }*/ + } - public static double getDistanceToMovingTarget(double robotX, double robotY, double targetX, double targetY, double robotVelocityX, + public static double getDistanceToMovingTarget(double robotX, double robotY, double targetX, double targetY, + double robotVelocityX, double robotVelocityY, double robotHeading) { return Math.abs(Math.sqrt( - Math.pow(movingTarget(robotX, robotY, targetX, targetY, + Math.pow(movingTarget(robotX, robotY, targetX, targetY, getRobotVelocityTowardsTarget(robotX, targetX, robotVelocityX, robotVelocityY, getAngleToTarget(robotX, robotY, targetX, targetY), robotHeading), getRobotVelocityPerpendicularToTarget(robotX, targetX, robotVelocityX, robotVelocityY, - getAngleToTarget(robotX, robotY, targetX, targetY), robotHeading)).get().getX() - robotX, 2) + getAngleToTarget(robotX, robotY, targetX, targetY), robotHeading)) + .get().getX() - robotX, 2) + Math.pow( movingTarget(robotX, robotY, targetX, targetY, getRobotVelocityTowardsTarget(robotX, targetX, robotVelocityX, robotVelocityY, getAngleToTarget(robotX, robotY, targetX, targetY), robotHeading), - getRobotVelocityPerpendicularToTarget(robotX, targetX, robotVelocityX, robotVelocityY, - getAngleToTarget(robotX, robotY, targetX, targetY), robotHeading)).get().getY() - robotY, + getRobotVelocityPerpendicularToTarget(robotX, targetX, robotVelocityX, + robotVelocityY, + getAngleToTarget(robotX, robotY, targetX, targetY), robotHeading)) + .get().getY() - robotY, 2))); } - + /** Convert meters per second to rotations per minute */ public static double convertToRPM(double velocity) { return (60 * velocity) / (slipPercent * Math.PI * .1016); diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 4a0f5b6..2ab7806 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -51,7 +51,8 @@ public static class Vision { public static final Matrix kMultiTagStdDevs = VecBuilder.fill(4, 4, 20); public static final Vector stateStdDevs = VecBuilder.fill(.5, .5, .5); // Encoder Odometry - public static final Pose2d startingPose = new Pose2d(1.35, 5.55, new Rotation2d(0)); + public static final Pose2d startingPoseBlue = new Pose2d(1.35, 5.55, new Rotation2d(0)); + public static final Pose2d startingPoseRed = new Pose2d(15.19, 5.55, new Rotation2d(Math.PI)); } private static AprilTagFieldLayout getFieldLayout() { @@ -143,7 +144,7 @@ public static final class Mod0 { public static final int driveMotorID = 1; public static final int angleMotorID = 3; public static final int canCoderID = 2; - public static final Rotation2d angleOffset = Rotation2d.fromDegrees(112.5); + public static final Rotation2d angleOffset = Rotation2d.fromDegrees(112.6757); public static final SwerveModuleConstants constants = new SwerveModuleConstants(driveMotorID, angleMotorID, canCoderID, angleOffset); } @@ -153,7 +154,7 @@ public static final class Mod1 { public static final int driveMotorID = 4; public static final int angleMotorID = 6; public static final int canCoderID = 5; - public static final Rotation2d angleOffset = Rotation2d.fromDegrees(-79.4); + public static final Rotation2d angleOffset = Rotation2d.fromDegrees(-80.5078); public static final SwerveModuleConstants constants = new SwerveModuleConstants(driveMotorID, angleMotorID, canCoderID, angleOffset); } @@ -163,7 +164,7 @@ public static final class Mod2 { public static final int driveMotorID = 7; public static final int angleMotorID = 9; public static final int canCoderID = 8; - public static final Rotation2d angleOffset = Rotation2d.fromDegrees(-80.5); + public static final Rotation2d angleOffset = Rotation2d.fromDegrees(-79.453125); public static final SwerveModuleConstants constants = new SwerveModuleConstants(driveMotorID, angleMotorID, canCoderID, angleOffset); } @@ -173,7 +174,7 @@ public static final class Mod3 { public static final int driveMotorID = 10; public static final int angleMotorID = 12; public static final int canCoderID = 11; - public static final Rotation2d angleOffset = Rotation2d.fromDegrees(114); + public static final Rotation2d angleOffset = Rotation2d.fromDegrees(112.06); public static final SwerveModuleConstants constants = new SwerveModuleConstants(driveMotorID, angleMotorID, canCoderID, angleOffset); } @@ -181,7 +182,7 @@ public static final class Mod3 { public static final class Intake { public static final double noteOutsideSpeed = 0.5; - public static final double noteInsideSpeed = 0.3; + public static final double noteInsideSpeed = 0.25; public static final double outakeSpeed = -0.5; } @@ -203,7 +204,7 @@ public static final class Elevator { } public static final class Auto { - public static final double kMaxSpeedMetersPerSecond = 1.8; + public static final double kMaxSpeedMetersPerSecond = 2.5; public static final double kMaxAccelerationMetersPerSecondSquared = 3; public static final double kMaxAngularSpeedRadiansPerSecond = 9.424778; public static final double kMaxAngularSpeedRadiansPerSecondSquared = 12.56637; diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 31f7502..b10dd7e 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -81,6 +81,8 @@ public void robotInit() { SmartDashboard.putData(PDH); + redAlliance = checkRedAlliance(); + // idk if this is useful //System.gc(); } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 245551c..c649c15 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -39,14 +39,14 @@ public class RobotContainer { private final int rightTrigger = XboxController.Axis.kRightTrigger.value; /* DRIVER BUTTONS */ - /** Driver B */ - private final JoystickButton robotCentric = new JoystickButton(driver, XboxController.Button.kB.value); + /** Driver X */ + private final JoystickButton intakeNoVision = new JoystickButton(driver, XboxController.Button.kX.value); /** Driver A */ private final JoystickButton Shoot = new JoystickButton(driver, XboxController.Button.kA.value); + /** Driver B */ + private final JoystickButton forceShoot = new JoystickButton(driver, XboxController.Button.kB.value); /** Driver Y */ - private final JoystickButton forceShoot = new JoystickButton(driver, XboxController.Button.kY.value); - /** Driver X */ - private final JoystickButton driverX = new JoystickButton(driver, XboxController.Button.kX.value); + private final JoystickButton intakeFromSource = new JoystickButton(driver, XboxController.Button.kY.value); /** Driver RB */ private final JoystickButton intex = new JoystickButton(driver, XboxController.Button.kRightBumper.value); /** Driver LB */ @@ -67,14 +67,14 @@ public class RobotContainer { private final JoystickButton resetOdom = new JoystickButton(driver, XboxController.Button.kStart.value); /* MECH BUTTONS */ - /** Mech B */ - private final JoystickButton shooterToAntiDefense = new JoystickButton(mech, XboxController.Button.kB.value); + /** Mech X */ + private final JoystickButton shooterToAntiDefense = new JoystickButton(mech, XboxController.Button.kX.value); /** Mech A */ private final JoystickButton shooterToIntake = new JoystickButton(mech, XboxController.Button.kA.value); /** Mech Y */ private final JoystickButton shooterToAmp = new JoystickButton(mech, XboxController.Button.kY.value); - /** Mech X */ - private final JoystickButton shooterToSubwoofer = new JoystickButton(mech, XboxController.Button.kX.value); + /** Mech B */ + private final JoystickButton shooterToSubwoofer = new JoystickButton(mech, XboxController.Button.kB.value); /** Mech RB */ private final JoystickButton primeShooterSpeedSpeaker = new JoystickButton(mech, XboxController.Button.kRightBumper.value); /** Mech LB */ @@ -129,14 +129,16 @@ public RobotContainer() { NamedCommands.registerCommand("Stop Shooter Intake", new InstantCommand(() -> m_IntexerSubsystem.setShooterIntake(0))); NamedCommands.registerCommand("Note Sniffer", new NoteSniffer(m_SwerveSubsystem, m_VisionSubsystem, m_IntexerSubsystem, m_ShooterSubsystem)); NamedCommands.registerCommand("Note Sniffer2", new NoteSniffer(m_SwerveSubsystem, m_VisionSubsystem, m_IntexerSubsystem, m_ShooterSubsystem)); + NamedCommands.registerCommand("Fire Under Stage", new InstantCommand(() -> m_ShooterSubsystem.setWristByAngle(Math.toRadians(10)))); + NamedCommands.registerCommand("Force Shoot", new FIREEFORACERTAINAMOUNTOFTIME(m_ShooterSubsystem, m_IntexerSubsystem, .2)); m_SwerveSubsystem.setDefaultCommand( new TeleopSwerve( - m_SwerveSubsystem, m_VisionSubsystem, m_ShooterSubsystem, + m_SwerveSubsystem, m_VisionSubsystem, m_ShooterSubsystem, m_IntexerSubsystem, () -> -driver.getRawAxis(leftVerticalAxis), () -> -driver.getRawAxis(leftHorizontalAxis), () -> -driver.getRawAxis(rightHorizontalAxis), - () -> robotCentric.getAsBoolean(), + () -> false, () -> targetAmp.getAsBoolean(), () -> targetSpeaker.getAsBoolean(), () -> intex.getAsBoolean(), driver)); @@ -181,10 +183,10 @@ private void configureButtonBindings() { // Reset Odometry resetOdom.onTrue(new InstantCommand(() -> m_SwerveSubsystem.zeroHeading()).alongWith( new InstantCommand(() -> m_SwerveSubsystem - .setPose(Constants.Vision.startingPose)))); + .setPose(Robot.getAlliance() ? Constants.Vision.startingPoseRed : Constants.Vision.startingPoseBlue)))); // Intexer - intex.whileTrue(new IntexBestHex(m_IntexerSubsystem, true, driver)); + intex.or(intakeNoVision).whileTrue(new IntexBestHex(m_IntexerSubsystem, true, driver)); outex.whileTrue(new IntexBestHex(m_IntexerSubsystem, false, driver)); // Shooter intake @@ -198,7 +200,7 @@ private void configureButtonBindings() { driverDown.whileTrue(m_SwerveSubsystem.pathToSourceChain()); // Intake from Source - driverX.whileTrue(new IntakeThroughShooter(m_ShooterSubsystem, m_IntexerSubsystem, driver)) + intakeFromSource.whileTrue(new IntakeThroughShooter(m_ShooterSubsystem, m_IntexerSubsystem, driver)) .onFalse(new IntakeThroughShooterPart2(m_ShooterSubsystem, m_IntexerSubsystem, driver)); /* MECH BUTTONS */ diff --git a/src/main/java/frc/robot/commands/AimBot.java b/src/main/java/frc/robot/commands/AimBot.java index 8f045d1..3761d08 100644 --- a/src/main/java/frc/robot/commands/AimBot.java +++ b/src/main/java/frc/robot/commands/AimBot.java @@ -12,6 +12,7 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.lib.math.FiringSolutionsV3; import frc.robot.Constants; +import frc.robot.Robot; import frc.robot.subsystems.IntexerSubsystem; import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.SwerveSubsystem; @@ -46,7 +47,18 @@ public void execute() { Pose2d pose = swerveSubsystem.getPose(); ChassisSpeeds currentSpeed = swerveSubsystem.getChassisSpeeds(); - double rotationVal = rotationPID.calculate(swerveSubsystem.getHeading().getRadians(), + double offset; + if (Robot.getAlliance()){ + if (pose.getRotation().getRadians() > 0){ + offset = -Math.toRadians(180); + } else { + offset = Math.toRadians(180); + } + } else { + offset = 0; + } + + double rotationVal = rotationPID.calculate(pose.getRotation().getRadians() + offset, FiringSolutionsV3.getAngleToMovingTarget(pose.getX(), pose.getY(), FiringSolutionsV3.speakerTargetX, FiringSolutionsV3.speakerTargetY, currentSpeed.vxMetersPerSecond, diff --git a/src/main/java/frc/robot/commands/FIREEFORACERTAINAMOUNTOFTIME.java b/src/main/java/frc/robot/commands/FIREEFORACERTAINAMOUNTOFTIME.java new file mode 100644 index 0000000..bc1a38f --- /dev/null +++ b/src/main/java/frc/robot/commands/FIREEFORACERTAINAMOUNTOFTIME.java @@ -0,0 +1,50 @@ +// 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; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.IntexerSubsystem; +import frc.robot.subsystems.ShooterSubsystem; + +public class FIREEFORACERTAINAMOUNTOFTIME extends Command { + private ShooterSubsystem shooter; + private IntexerSubsystem intexer; + private double time; + private Timer timer = new Timer(); + + public FIREEFORACERTAINAMOUNTOFTIME(ShooterSubsystem shooterSub, IntexerSubsystem intex, double time) { + shooter = shooterSub; + intexer = intex; + this.time = time; + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(intex); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + timer.reset(); + timer.start(); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + intexer.setShooterIntake(.9); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + intexer.setShooterIntake(0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return timer.get() > time; + } +} diff --git a/src/main/java/frc/robot/commands/ManRizzt.java b/src/main/java/frc/robot/commands/ManRizzt.java index b5fda3c..4b08d9e 100644 --- a/src/main/java/frc/robot/commands/ManRizzt.java +++ b/src/main/java/frc/robot/commands/ManRizzt.java @@ -41,7 +41,7 @@ public void execute() { speedValue = Math.pow(speedValue, 3); if (setAngle.getAsBoolean()) { - m_shooterSubsystem.setWristByAngle(SmartDashboard.getNumber("Set Wrist Angle", 0)); + //m_shooterSubsystem.setWristByAngle(SmartDashboard.getNumber("Set Wrist Angle", 0)); } else { if (Math.abs(speedValue) > .0) { wristIsLocked = false; diff --git a/src/main/java/frc/robot/commands/NoteSniffer.java b/src/main/java/frc/robot/commands/NoteSniffer.java index 3833630..12b3146 100644 --- a/src/main/java/frc/robot/commands/NoteSniffer.java +++ b/src/main/java/frc/robot/commands/NoteSniffer.java @@ -13,6 +13,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants; +import frc.robot.Robot; import frc.robot.subsystems.IntexerSubsystem; import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.SwerveSubsystem; @@ -27,7 +28,7 @@ public class NoteSniffer extends Command { private PIDController rotationPID = new PIDController(0.65, 0.00001, 0.04); private Timer timer = new Timer(); private boolean noteInside = false; - private double translationVal; + private double translationVal = .35; /** Creates a new IntakeWithVision. */ public NoteSniffer(SwerveSubsystem swerve, VisionSubsystem vision, IntexerSubsystem intexer, @@ -55,8 +56,9 @@ public void execute() { PhotonPipelineResult result = vision.getLatestResultN(); double rotationVal; + boolean overMidfield = Robot.getAlliance() ? (16.54 - swerveSubsystem.getPose().getX()) > 8.3 : swerveSubsystem.getPose().getX() > 8.3; - if (intexer.intakeBreak()) { + if (intexer.intakeBreak() || overMidfield) { noteInside = true; translationVal = 0; rotationVal = 0; @@ -102,6 +104,6 @@ public void end(boolean interrupted) { // Returns true when the command should end. @Override public boolean isFinished() { - return intexer.shooterBreak() || timer.get() > 3; + return intexer.shooterBreak() || timer.get() > 2; } } diff --git a/src/main/java/frc/robot/commands/TeleopSwerve.java b/src/main/java/frc/robot/commands/TeleopSwerve.java index 8f74711..f07fec8 100644 --- a/src/main/java/frc/robot/commands/TeleopSwerve.java +++ b/src/main/java/frc/robot/commands/TeleopSwerve.java @@ -3,6 +3,7 @@ import frc.lib.math.FiringSolutionsV3; import frc.robot.Constants; import frc.robot.Robot; +import frc.robot.subsystems.IntexerSubsystem; import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.SwerveSubsystem; import frc.robot.subsystems.VisionSubsystem; @@ -26,6 +27,7 @@ public class TeleopSwerve extends Command { private SwerveSubsystem swerveSubsystem; private VisionSubsystem vision; private ShooterSubsystem shooterSubsystem; + private IntexerSubsystem intexerSubsystem; private DoubleSupplier translationSup; private DoubleSupplier strafeSup; @@ -37,14 +39,17 @@ public class TeleopSwerve extends Command { private BooleanSupplier intakeOverride; private PIDController rotationPID = new PIDController(0.65, 0.00001, 0.04); - + private Joystick controller; + + private boolean noteInside = false; - public TeleopSwerve(SwerveSubsystem swerve, VisionSubsystem vision, ShooterSubsystem shooter, DoubleSupplier translationSup, DoubleSupplier strafeSup, + public TeleopSwerve(SwerveSubsystem swerve, VisionSubsystem vision, ShooterSubsystem shooter, IntexerSubsystem intexer, DoubleSupplier translationSup, DoubleSupplier strafeSup, DoubleSupplier rotationSup, BooleanSupplier robotCentricSup, BooleanSupplier shooterOverrideAmp, BooleanSupplier shooterOverrideSpeaker, BooleanSupplier intake, Joystick controller) { this.swerveSubsystem = swerve; this.vision = vision; this.shooterSubsystem = shooter; + this.intexerSubsystem = intexer; addRequirements(swerve); this.translationSup = translationSup; @@ -55,7 +60,7 @@ public TeleopSwerve(SwerveSubsystem swerve, VisionSubsystem vision, ShooterSubsy this.shooterOverrideSpeaker = shooterOverrideSpeaker; this.intakeOverride = intake; this.controller = controller; - SmartDashboard.putData(rotationPID); + SmartDashboard.putData("Lock On Rotation PID", rotationPID); } @Override @@ -70,12 +75,24 @@ public void execute() { double strafeVal = MathUtil.applyDeadband(strafeSup.getAsDouble(), Constants.stickDeadband); double rotationVal; + double offset; + + if (Robot.getAlliance()){ + if (pose.getRotation().getRadians() > 0){ + offset = -Math.toRadians(180); + } else { + offset = Math.toRadians(180); + } + } else { + offset = 0; + } + /* Exponential Drive */ translationVal = Math.copySign(Math.pow(translationVal, 2), translationVal); strafeVal = Math.copySign(Math.pow(strafeVal, 2), strafeVal); if (shooterOverrideSpeaker.getAsBoolean()) { // Lock robot angle to speaker - if (shooterSubsystem.getDistanceToSpeakerWhileMoving() >= 3.5){ + if (pose.getX() >= 3){ // "TODO: fix this" -Micah controller.setRumble(RumbleType.kBothRumble, 0.5); } else { controller.setRumble(RumbleType.kBothRumble, 0); @@ -83,7 +100,7 @@ public void execute() { ChassisSpeeds currentSpeed = swerveSubsystem.getChassisSpeeds(); - rotationVal = rotationPID.calculate(pose.getRotation().getRadians(), + rotationVal = rotationPID.calculate(pose.getRotation().getRadians() + offset, FiringSolutionsV3.getAngleToMovingTarget(pose.getX(), pose.getY(), FiringSolutionsV3.speakerTargetX, FiringSolutionsV3.speakerTargetY, currentSpeed.vxMetersPerSecond, currentSpeed.vyMetersPerSecond, @@ -95,7 +112,7 @@ public void execute() { if (FiringSolutionsV3.getDistanceToTarget(pose.getX(), pose.getY(), FiringSolutionsV3.trueAmpX, FiringSolutionsV3.trueAmpY) > 4) { - rotationVal = rotationPID.calculate(pose.getRotation().getRadians(), + rotationVal = rotationPID.calculate(pose.getRotation().getRadians() + offset, FiringSolutionsV3.getAngleToMovingTarget(pose.getX(), pose.getY(), FiringSolutionsV3.ampTargetX, FiringSolutionsV3.ampTargetY, currentSpeed.vxMetersPerSecond, currentSpeed.vyMetersPerSecond, @@ -104,18 +121,21 @@ public void execute() { rotationVal = rotationPID.calculate(pose.getRotation().getRadians(), Math.toRadians(-90)); } - } else if (intakeOverride.getAsBoolean() && result.hasTargets()) { // Lock robot towards detected note + } else if (intakeOverride.getAsBoolean() && result.hasTargets() && !noteInside) { // Lock robot towards detected note double yawToNote = Math.toRadians(result.getBestTarget().getYaw()) + swerveSubsystem.getGyroYaw().getRadians(); SmartDashboard.putNumber("Note Yaw", yawToNote); //openLoop = false; rotationVal = rotationPID.calculate(yawToNote, swerveSubsystem.getGyroYaw().getRadians()); - + translationVal = 0.35; + robotCentric = true; + } else { rotationVal = MathUtil.applyDeadband(rotationSup.getAsDouble(), Constants.stickDeadband); rotationVal = Math.copySign(Math.pow(rotationVal, 2), rotationVal); controller.setRumble(RumbleType.kBothRumble, 0); + noteInside = false; } if (Robot.getAlliance() && !robotCentric){ // Invert field oriented for always blue origin @@ -123,6 +143,12 @@ public void execute() { strafeVal = -strafeVal; } + if (intexerSubsystem.intakeBreak()) { + translationVal = 0; + rotationVal = 0; + noteInside = true; + } + /* Drive */ swerveSubsystem.drive( new Translation2d(translationVal, strafeVal).times(Constants.Swerve.maxSpeed), diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index a966639..1f3607d 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -128,11 +128,11 @@ public void stopHere(){ /** Get height from motor encoder */ public double getHeightEncoder() { - return (revolutionCount / gearRatio) * (spoolCircumference * Math.PI) + 0.2; + return (revolutionCount / gearRatio) * (spoolCircumference * Math.PI); } /** Set height IN METERS. Will run off LaserCan but will switch to encoder if it fails */ - public void setHeight(double height) { + public void setHeight(double height) { //TODO rewrite locked = true; setHeight = height; if (!lasercanFailureCheck()) { // Run off LaserCan @@ -141,7 +141,7 @@ public void setHeight(double height) { } else { // Run off encoder double rot = (height / (spoolCircumference * Math.PI)) * gearRatio; if (getHeightEncoder() < Constants.Elevator.maxHeightMeters) { - m_elevatorLeft.setControl(m_requestPosition.withPosition(rot).withSlot(1)); + m_elevatorLeft.set(elevatorPID.calculate(getHeightEncoder(), height)); } } } diff --git a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java index befbce4..64756ce 100644 --- a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java @@ -1,6 +1,7 @@ package frc.robot.subsystems; import frc.robot.SwerveModule; +import frc.lib.math.FiringSolutionsV3; import frc.robot.Constants; import frc.robot.Robot; import edu.wpi.first.math.kinematics.ChassisSpeeds; @@ -126,7 +127,7 @@ public SwerveSubsystem(VisionSubsystem vision) { Constants.Swerve.swerveKinematics, getGyroYaw(), getModulePositions(), - Constants.Vision.startingPose, + Robot.getAlliance() ? Constants.Vision.startingPoseRed : Constants.Vision.startingPoseBlue, Constants.Vision.stateStdDevs, Constants.Vision.kSingleTagStdDevs); @@ -135,7 +136,7 @@ public SwerveSubsystem(VisionSubsystem vision) { swervePublisher = NetworkTableInstance.getDefault() .getStructArrayTopic("Swerve Module States", SwerveModuleState.struct).publish(); - SmartDashboard.putData(gyro); + SmartDashboard.putData("Gyro", gyro); SmartDashboard.putData(this); SmartDashboard.putData("field", m_field); @@ -272,7 +273,7 @@ public void voltageDrive(double Voltage) { public void periodic() { updateModuleStates(); SwerveModulePosition[] modulePositions = getModulePositions(); - + // Correct pose estimate with multiple vision measurements Optional OptionalEstimatedPoseFront = vision.getEstimatedPoseFront(); if (OptionalEstimatedPoseFront.isPresent()) { @@ -317,6 +318,10 @@ public void periodic() { //SmartDashboard.putString("Obodom", getPose().toString()); SmartDashboard.putNumber("Gyro", getGyroYaw().getDegrees()); SmartDashboard.putNumber("Heading", getHeading().getDegrees()); + SmartDashboard.putNumber("Angle to target", Math.toDegrees(FiringSolutionsV3.getAngleToMovingTarget(getPose().getX(), getPose().getY(), FiringSolutionsV3.ampTargetX, FiringSolutionsV3.ampTargetY, + getChassisSpeeds().vxMetersPerSecond, + getChassisSpeeds().vyMetersPerSecond, + getPose().getRotation().getRadians()))); } diff --git a/src/main/java/frc/robot/subsystems/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/VisionSubsystem.java index 5f27517..8262ceb 100644 --- a/src/main/java/frc/robot/subsystems/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/VisionSubsystem.java @@ -25,6 +25,7 @@ package frc.robot.subsystems; import frc.robot.Constants; +import frc.robot.Robot; import edu.wpi.first.apriltag.AprilTagFieldLayout.OriginPosition; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; @@ -66,8 +67,8 @@ public VisionSubsystem() { photonEstimatorBack = new PhotonPoseEstimator( Constants.Vision.kTagLayout, PoseStrategy.LOWEST_AMBIGUITY, aprilTagCameraBack, Constants.Vision.kRobotToCamBack); - photonEstimatorFront.setLastPose(Constants.Vision.startingPose); - photonEstimatorBack.setLastPose(Constants.Vision.startingPose); + photonEstimatorFront.setLastPose(Robot.getAlliance() ? Constants.Vision.startingPoseRed : Constants.Vision.startingPoseBlue); + photonEstimatorBack.setLastPose(Robot.getAlliance() ? Constants.Vision.startingPoseRed : Constants.Vision.startingPoseBlue); // 2024 field quality makes multitag impractical //photonEstimatorFront.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json index b849e92..3ec4c12 100644 --- a/vendordeps/PathplannerLib.json +++ b/vendordeps/PathplannerLib.json @@ -1,7 +1,7 @@ { "fileName": "PathplannerLib.json", "name": "PathplannerLib", - "version": "2024.2.5", + "version": "2024.2.6", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2024", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2024.2.5" + "version": "2024.2.6" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2024.2.5", + "version": "2024.2.6", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, From 589b2d8692bb32b257d4bbe71df7bf6054c9395c Mon Sep 17 00:00:00 2001 From: Andrew-K961 Date: Tue, 12 Mar 2024 21:20:22 -0500 Subject: [PATCH 41/49] Overengineering LEDs (it doesn't work) --- src/main/java/frc/robot/RobotContainer.java | 2 +- src/main/java/frc/robot/commands/FIREEE.java | 5 +---- src/main/java/frc/robot/commands/ManRizzt.java | 6 +----- src/main/java/frc/robot/subsystems/LEDSubsystem.java | 2 -- 4 files changed, 3 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index dc0a0b4..06ff173 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -177,7 +177,7 @@ private void configureButtonBindings() { .onFalse(new InstantCommand(() -> m_ShooterSubsystem.setShooterVelocity(Constants.Shooter.idleSpeedRPM))); // Shooter - Speaker.or(targetAmp).and(Shoot).whileTrue(new FIREEE(m_ShooterSubsystem, m_IntexerSubsystem)); // Main fire + targetSpeaker.or(targetAmp).and(Shoot).whileTrue(new FIREEE(m_ShooterSubsystem, m_IntexerSubsystem)); // Main fire // Reset Odometry resetOdom.onTrue(new InstantCommand(() -> m_SwerveSubsystem.zeroHeading()).alongWith( diff --git a/src/main/java/frc/robot/commands/FIREEE.java b/src/main/java/frc/robot/commands/FIREEE.java index a0a7f5d..0d30b5c 100644 --- a/src/main/java/frc/robot/commands/FIREEE.java +++ b/src/main/java/frc/robot/commands/FIREEE.java @@ -6,18 +6,15 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.IntexerSubsystem; -import frc.robot.subsystems.LEDSubsystem; import frc.robot.subsystems.ShooterSubsystem; public class FIREEE extends Command { private ShooterSubsystem shooter; private IntexerSubsystem intexer; - private LEDSubsystem ledSubsystem; - public FIREEE(ShooterSubsystem shooterSub, IntexerSubsystem intex, LEDSubsystem ledSubsystem) { + public FIREEE(ShooterSubsystem shooterSub, IntexerSubsystem intex) { shooter = shooterSub; intexer = intex; - this.ledSubsystem = ledSubsystem; // Use addRequirements() here to declare subsystem dependencies. addRequirements(intex); } diff --git a/src/main/java/frc/robot/commands/ManRizzt.java b/src/main/java/frc/robot/commands/ManRizzt.java index 7b1bb2f..731ccd4 100644 --- a/src/main/java/frc/robot/commands/ManRizzt.java +++ b/src/main/java/frc/robot/commands/ManRizzt.java @@ -12,21 +12,18 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants; import frc.robot.subsystems.ShooterSubsystem; -import frc.robot.subsystems.LEDSubsystem; public class ManRizzt extends Command { ShooterSubsystem m_shooterSubsystem; - LEDSubsystem ledSubsystem; private DoubleSupplier speed; BooleanSupplier setAngle; double lastWristSetpoint = 0.0; boolean wristIsLocked = false; - public ManRizzt(ShooterSubsystem subsystem, LEDSubsystem ledSubsystem, DoubleSupplier speed, BooleanSupplier setAngle) { + public ManRizzt(ShooterSubsystem subsystem, DoubleSupplier speed, BooleanSupplier setAngle) { // Use addRequirements() here to declare subsystem dependencies. m_shooterSubsystem = subsystem; - this.ledSubsystem = ledSubsystem; this.setAngle = setAngle; this.speed = speed; SmartDashboard.putNumber("Set Wrist Angle", 0); @@ -64,7 +61,6 @@ public void execute() { @Override public boolean isFinished() { - ledSubsystem.ReadyToFire(true); return false; } diff --git a/src/main/java/frc/robot/subsystems/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/LEDSubsystem.java index e1b047e..01c4693 100644 --- a/src/main/java/frc/robot/subsystems/LEDSubsystem.java +++ b/src/main/java/frc/robot/subsystems/LEDSubsystem.java @@ -4,8 +4,6 @@ package frc.robot.subsystems; -import java.lang.reflect.Array; - import edu.wpi.first.wpilibj.DigitalOutput; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.SubsystemBase; From a8b7a4ed944b03d29b302c02da0649f4f73def30 Mon Sep 17 00:00:00 2001 From: Mr-A768 Date: Tue, 12 Mar 2024 22:45:17 -0500 Subject: [PATCH 42/49] cartoon_snoring_sound_effect.wav --- src/main/java/frc/robot/commands/MissileLock.java | 8 ++++---- .../java/frc/robot/commands/TeleopSwerve.java | 8 +++++--- .../frc/robot/subsystems/ShooterSubsystem.java | 15 +++++---------- 3 files changed, 14 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/robot/commands/MissileLock.java b/src/main/java/frc/robot/commands/MissileLock.java index ba5dabf..52fe9e7 100644 --- a/src/main/java/frc/robot/commands/MissileLock.java +++ b/src/main/java/frc/robot/commands/MissileLock.java @@ -31,11 +31,11 @@ public void initialize() { @Override public void execute() { if (target == "amp") { - if (shooter.getDistanceTo(FiringSolutionsV3.trueAmpX, FiringSolutionsV3.trueAmpY) > 4) { + if (shooter.outsideAllianceWing) { shooter.PointShoot(Math.toRadians(55), - FiringSolutionsV3.convertToRPM(12)); + FiringSolutionsV3.convertToRPM(shooter.getCalculatedVelocity())); } else { - shooter.setShooterVelocity(FiringSolutionsV3.convertToRPM(12)); + shooter.setShooterVelocity(FiringSolutionsV3.convertToRPM(shooter.getCalculatedVelocity())); } } else { shooter.PointShoot(shooter.getCalculatedAngleToSpeaker(), @@ -47,7 +47,7 @@ public void execute() { @Override public void end(boolean interrupted) { //shooter.setShooterVelocity(Constants.Shooter.idleSpeedRPM); - if (target != "amp" || shooter.getDistanceTo(FiringSolutionsV3.trueAmpX, FiringSolutionsV3.trueAmpY) > 4){ + if (target != "amp" || shooter.outsideAllianceWing){ shooter.setWristByAngle(Constants.Shooter.intakeAngleRadians); } } diff --git a/src/main/java/frc/robot/commands/TeleopSwerve.java b/src/main/java/frc/robot/commands/TeleopSwerve.java index f07fec8..ed24baa 100644 --- a/src/main/java/frc/robot/commands/TeleopSwerve.java +++ b/src/main/java/frc/robot/commands/TeleopSwerve.java @@ -76,14 +76,17 @@ public void execute() { double rotationVal; double offset; + double ampLockOffset; if (Robot.getAlliance()){ + ampLockOffset = 16.54 - 5; if (pose.getRotation().getRadians() > 0){ offset = -Math.toRadians(180); } else { offset = Math.toRadians(180); } } else { + ampLockOffset = 5; offset = 0; } @@ -92,7 +95,7 @@ public void execute() { strafeVal = Math.copySign(Math.pow(strafeVal, 2), strafeVal); if (shooterOverrideSpeaker.getAsBoolean()) { // Lock robot angle to speaker - if (pose.getX() >= 3){ // "TODO: fix this" -Micah + if (shooterSubsystem.getDistanceToSpeakerWhileMoving() >= 3.5){ controller.setRumble(RumbleType.kBothRumble, 0.5); } else { controller.setRumble(RumbleType.kBothRumble, 0); @@ -110,8 +113,7 @@ public void execute() { } else if (shooterOverrideAmp.getAsBoolean()) { // Lock robot angle to amp ChassisSpeeds currentSpeed = swerveSubsystem.getChassisSpeeds(); - if (FiringSolutionsV3.getDistanceToTarget(pose.getX(), pose.getY(), FiringSolutionsV3.trueAmpX, FiringSolutionsV3.trueAmpY) > 4) { - + if ((Robot.getAlliance() && pose.getX() < 16.54 - 5) ^ (!Robot.getAlliance() && pose.getX() > 5)) { rotationVal = rotationPID.calculate(pose.getRotation().getRadians() + offset, FiringSolutionsV3.getAngleToMovingTarget(pose.getX(), pose.getY(), FiringSolutionsV3.ampTargetX, FiringSolutionsV3.ampTargetY, currentSpeed.vxMetersPerSecond, diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 4fe8217..60d175f 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -8,6 +8,7 @@ import frc.lib.math.FiringSolutionsV3; import frc.lib.math.Interpolations; import frc.robot.Constants; +import frc.robot.Robot; import com.revrobotics.CANSparkBase; import com.revrobotics.RelativeEncoder; @@ -65,7 +66,7 @@ public class ShooterSubsystem extends SubsystemBase { public double wristAngleUpperBound; public double wristAngleLowerBound; - public boolean outOfAmpRange = false; + public boolean outsideAllianceWing = false; // Constants private final double wristAngleMax = 0.0; @@ -395,16 +396,10 @@ public void updateShooterMath() { // Shooter Math shooterAngleToSpeaker = Math.toRadians(interpolation.getShooterAngleFromInterpolation(distanceToMovingSpeakerTarget)); } - if (FiringSolutionsV3.getDistanceToMovingTarget(pose.getX(), pose.getY(), - FiringSolutionsV3.ampTargetX, FiringSolutionsV3.ampTargetY, - chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond, - pose.getRotation().getRadians()) > FiringSolutionsV3.maxRangeWithR) { - outOfAmpRange = true; + if ((Robot.getAlliance() && pose.getX() < 16.54 - 5) ^ (!Robot.getAlliance() && pose.getX() > 5)) { + outsideAllianceWing = true; } else { - if (outOfAmpRange) { - outOfAmpRange = false; - FiringSolutionsV3.resetAmpR(); - } + outsideAllianceWing = false; } SmartDashboard.putNumber("Target Velocity RPM", FiringSolutionsV3.convertToRPM(shooterVelocity)); From ecb2ee82ab4d7fd0608af500d260ce40e5d076b8 Mon Sep 17 00:00:00 2001 From: Andrew-K961 Date: Tue, 12 Mar 2024 22:54:09 -0500 Subject: [PATCH 43/49] Heartland scuffed --- build.gradle | 12 +++++----- .../{HBNFClBrI.auto => toothy grin.auto} | 0 src/main/java/frc/robot/RobotContainer.java | 17 +++++++------- .../java/frc/robot/commands/TeleopSwerve.java | 19 ++++++++++++---- .../robot/subsystems/ElevatorSubsystem.java | 22 ++----------------- 5 files changed, 31 insertions(+), 39 deletions(-) rename src/main/deploy/pathplanner/autos/{HBNFClBrI.auto => toothy grin.auto} (100%) diff --git a/build.gradle b/build.gradle index 539e22f..4c5d256 100644 --- a/build.gradle +++ b/build.gradle @@ -35,12 +35,12 @@ deploy { //jvmArgs.remove('-XX:+AlwaysPreTouch') // Use VisualVM for code profiling - jvmArgs.add("-Dcom.sun.management.jmxremote=true") - jvmArgs.add("-Dcom.sun.management.jmxremote.port=1099") - jvmArgs.add("-Dcom.sun.management.jmxremote.local.only=false") - jvmArgs.add("-Dcom.sun.management.jmxremote.ssl=false") - jvmArgs.add("-Dcom.sun.management.jmxremote.authenticate=false") - jvmArgs.add("-Djava.rmi.server.hostname=10.17.10.2") + //jvmArgs.add("-Dcom.sun.management.jmxremote=true") + //jvmArgs.add("-Dcom.sun.management.jmxremote.port=1099") + //jvmArgs.add("-Dcom.sun.management.jmxremote.local.only=false") + //jvmArgs.add("-Dcom.sun.management.jmxremote.ssl=false") + //jvmArgs.add("-Dcom.sun.management.jmxremote.authenticate=false") + //jvmArgs.add("-Djava.rmi.server.hostname=10.17.10.2") } // Static files artifact diff --git a/src/main/deploy/pathplanner/autos/HBNFClBrI.auto b/src/main/deploy/pathplanner/autos/toothy grin.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/HBNFClBrI.auto rename to src/main/deploy/pathplanner/autos/toothy grin.auto diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 06ff173..c5e5d4f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -40,7 +40,7 @@ public class RobotContainer { /* DRIVER BUTTONS */ /** Driver X */ - private final JoystickButton intakeNoVision = new JoystickButton(driver, XboxController.Button.kX.value); + private final JoystickButton intakeNoMove = new JoystickButton(driver, XboxController.Button.kX.value); /** Driver A */ private final JoystickButton Shoot = new JoystickButton(driver, XboxController.Button.kA.value); /** Driver B */ @@ -109,10 +109,10 @@ public class RobotContainer { /* Subsystems */ private final VisionSubsystem m_VisionSubsystem = new VisionSubsystem(); private final SwerveSubsystem m_SwerveSubsystem = new SwerveSubsystem(m_VisionSubsystem); - //private final LEDSubsystem m_LEDSubsystem = new LEDSubsystem(m_VisionSubsystem, m_Shoota, m_IntexerSubsystem, m_SwerveSubsystem); private final ElevatorSubsystem m_ElevatorSubsystem = new ElevatorSubsystem(); private final IntexerSubsystem m_IntexerSubsystem = new IntexerSubsystem(); private final ShooterSubsystem m_ShooterSubsystem = new ShooterSubsystem(m_SwerveSubsystem, m_ElevatorSubsystem); + private final LEDSubsystem m_LEDSubsystem = new LEDSubsystem(m_VisionSubsystem, m_ShooterSubsystem, m_IntexerSubsystem, m_SwerveSubsystem); private final SendableChooser autoChooser; @@ -141,7 +141,8 @@ public RobotContainer() { () -> false, () -> targetAmp.getAsBoolean(), () -> targetSpeaker.getAsBoolean(), - () -> intex.getAsBoolean(), driver)); + () -> intex.getAsBoolean(), + () -> intakeNoMove.getAsBoolean(), driver)); m_ElevatorSubsystem.setDefaultCommand( new ElevationManual( @@ -151,9 +152,7 @@ public RobotContainer() { m_ShooterSubsystem.setDefaultCommand(new ManRizzt(m_ShooterSubsystem, () -> -mech.getRawAxis(rightVerticalAxis), () -> shooterToSubwoofer.getAsBoolean())); - // Another option that allows you to specify the default auto by its name - // autoChooser = AutoBuilder.buildAutoChooser("My Default Auto"); - autoChooser = AutoBuilder.buildAutoChooser(); + autoChooser = AutoBuilder.buildAutoChooser("4 piece mcnugget"); SmartDashboard.putData("Auto Chooser", autoChooser); @@ -185,7 +184,7 @@ private void configureButtonBindings() { .setPose(Robot.getAlliance() ? Constants.Vision.startingPoseRed : Constants.Vision.startingPoseBlue)))); // Intexer - intex.or(intakeNoVision).whileTrue(new IntexBestHex(m_IntexerSubsystem, true, driver)); + intex.or(intakeNoMove).whileTrue(new IntexBestHex(m_IntexerSubsystem, true, driver)); outex.whileTrue(new IntexBestHex(m_IntexerSubsystem, false, driver)); // Shooter intake @@ -193,10 +192,10 @@ private void configureButtonBindings() { .onFalse(new InstantCommand(() -> m_IntexerSubsystem.setShooterIntake(0))); // Move to Amp - driverUp.whileTrue(m_SwerveSubsystem.pathToAmpChain()); + //driverUp.whileTrue(m_SwerveSubsystem.pathToAmpChain()); // Move to Source - driverDown.whileTrue(m_SwerveSubsystem.pathToSourceChain()); + //driverDown.whileTrue(m_SwerveSubsystem.pathToSourceChain()); // Intake from Source intakeFromSource.whileTrue(new IntakeThroughShooter(m_ShooterSubsystem, m_IntexerSubsystem, driver)) diff --git a/src/main/java/frc/robot/commands/TeleopSwerve.java b/src/main/java/frc/robot/commands/TeleopSwerve.java index f07fec8..af1f402 100644 --- a/src/main/java/frc/robot/commands/TeleopSwerve.java +++ b/src/main/java/frc/robot/commands/TeleopSwerve.java @@ -37,6 +37,7 @@ public class TeleopSwerve extends Command { private BooleanSupplier shooterOverrideAmp; private BooleanSupplier shooterOverrideSpeaker; private BooleanSupplier intakeOverride; + private BooleanSupplier intakeOverrideNoMove; private PIDController rotationPID = new PIDController(0.65, 0.00001, 0.04); @@ -45,7 +46,7 @@ public class TeleopSwerve extends Command { private boolean noteInside = false; public TeleopSwerve(SwerveSubsystem swerve, VisionSubsystem vision, ShooterSubsystem shooter, IntexerSubsystem intexer, DoubleSupplier translationSup, DoubleSupplier strafeSup, - DoubleSupplier rotationSup, BooleanSupplier robotCentricSup, BooleanSupplier shooterOverrideAmp, BooleanSupplier shooterOverrideSpeaker, BooleanSupplier intake, Joystick controller) { + DoubleSupplier rotationSup, BooleanSupplier robotCentricSup, BooleanSupplier shooterOverrideAmp, BooleanSupplier shooterOverrideSpeaker, BooleanSupplier intake, BooleanSupplier intakeNoMove, Joystick controller) { this.swerveSubsystem = swerve; this.vision = vision; this.shooterSubsystem = shooter; @@ -59,6 +60,7 @@ public TeleopSwerve(SwerveSubsystem swerve, VisionSubsystem vision, ShooterSubsy this.shooterOverrideAmp = shooterOverrideAmp; this.shooterOverrideSpeaker = shooterOverrideSpeaker; this.intakeOverride = intake; + this.intakeOverrideNoMove = intakeNoMove; this.controller = controller; SmartDashboard.putData("Lock On Rotation PID", rotationPID); } @@ -92,7 +94,7 @@ public void execute() { strafeVal = Math.copySign(Math.pow(strafeVal, 2), strafeVal); if (shooterOverrideSpeaker.getAsBoolean()) { // Lock robot angle to speaker - if (pose.getX() >= 3){ // "TODO: fix this" -Micah + if (Robot.getAlliance() ? 16.54 - pose.getX() >= 3 : pose.getX() >= 3){ controller.setRumble(RumbleType.kBothRumble, 0.5); } else { controller.setRumble(RumbleType.kBothRumble, 0); @@ -106,9 +108,10 @@ public void execute() { currentSpeed.vyMetersPerSecond, pose.getRotation().getRadians())); openLoop = false; - + } else if (shooterOverrideAmp.getAsBoolean()) { // Lock robot angle to amp ChassisSpeeds currentSpeed = swerveSubsystem.getChassisSpeeds(); + openLoop = false; if (FiringSolutionsV3.getDistanceToTarget(pose.getX(), pose.getY(), FiringSolutionsV3.trueAmpX, FiringSolutionsV3.trueAmpY) > 4) { @@ -126,11 +129,19 @@ public void execute() { SmartDashboard.putNumber("Note Yaw", yawToNote); - //openLoop = false; + openLoop = false; rotationVal = rotationPID.calculate(yawToNote, swerveSubsystem.getGyroYaw().getRadians()); translationVal = 0.35; robotCentric = true; + } else if (intakeOverrideNoMove.getAsBoolean() && result.hasTargets() && !noteInside) { + double yawToNote = Math.toRadians(result.getBestTarget().getYaw()) + swerveSubsystem.getGyroYaw().getRadians(); + + SmartDashboard.putNumber("Note Yaw", yawToNote); + + openLoop = false; + rotationVal = rotationPID.calculate(yawToNote, swerveSubsystem.getGyroYaw().getRadians()); + } else { rotationVal = MathUtil.applyDeadband(rotationSup.getAsDouble(), Constants.stickDeadband); rotationVal = Math.copySign(Math.pow(rotationVal, 2), rotationVal); diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index 1f3607d..aa8e431 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -19,7 +19,6 @@ import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; public class ElevatorSubsystem extends SubsystemBase { @@ -29,7 +28,6 @@ public class ElevatorSubsystem extends SubsystemBase { public LaserCan lasercan; // Falcon stuff - private final PositionDutyCycle m_requestPosition = new PositionDutyCycle(0); private final PositionDutyCycle lockPosition = new PositionDutyCycle(0); private final PIDController elevatorPID = new PIDController(0, 0, 0); @@ -41,7 +39,6 @@ public class ElevatorSubsystem extends SubsystemBase { private double revolutionCount; private double currentHeight; private double setHeight; - private boolean laser; private LaserCan.Measurement measurement; public boolean manualOverride = false; @@ -83,8 +80,6 @@ public ElevatorSubsystem() { SmartDashboard.putData("Elevator PID", elevatorPID); m_elevatorLeft.setPosition(0); - - laser = false; } @Override @@ -132,18 +127,10 @@ public double getHeightEncoder() { } /** Set height IN METERS. Will run off LaserCan but will switch to encoder if it fails */ - public void setHeight(double height) { //TODO rewrite + public void setHeight(double height) { locked = true; setHeight = height; - if (!lasercanFailureCheck()) { // Run off LaserCan - m_elevatorLeft.set(elevatorPID.calculate(getHeightLaserCan(), height)); - m_elevatorLeft.getFault_StatorCurrLimit().getValue(); - } else { // Run off encoder - double rot = (height / (spoolCircumference * Math.PI)) * gearRatio; - if (getHeightEncoder() < Constants.Elevator.maxHeightMeters) { - m_elevatorLeft.set(elevatorPID.calculate(getHeightEncoder(), height)); - } - } + m_elevatorLeft.set(elevatorPID.calculate(getHeight(), height)); } /** Get height IN METERS. Will run off LaserCan but will switch to encoder if it fails */ @@ -168,11 +155,6 @@ public void resetElevatorEncoder() { m_elevatorLeft.setPosition(0); } - //lasercan methods - public void useLaserCan(boolean laserCanOn) { - laser = laserCanOn; - } - /** Get height from LaserCan IN METERS */ public double getHeightLaserCan() { return currentHeight; From 012fcacd7b2c73476dbeb09598cd2df049ea64dd Mon Sep 17 00:00:00 2001 From: Andrew-K961 Date: Wed, 13 Mar 2024 12:05:20 -0500 Subject: [PATCH 44/49] ministry of truth --- build.gradle | 2 +- src/main/java/frc/lib/math/Conversions.java | 12 +- .../java/frc/lib/math/FiringSolutions.java | 4 +- .../java/frc/lib/math/FiringSolutionsV2.java | 4 +- .../java/frc/lib/math/FiringSolutionsV3.java | 4 +- .../java/frc/lib/math/Interpolations.java | 34 +-- .../lib/util/COTSTalonFXSwerveConstants.java | 5 +- .../frc/lib/util/SwerveModuleConstants.java | 3 +- src/main/java/frc/robot/CTREConfigs.java | 33 ++- src/main/java/frc/robot/Constants.java | 65 +++-- src/main/java/frc/robot/Main.java | 18 +- src/main/java/frc/robot/Robot.java | 43 ++-- src/main/java/frc/robot/RobotContainer.java | 199 +++++++++------ src/main/java/frc/robot/SwerveModule.java | 58 ++--- src/main/java/frc/robot/commands/AimBot.java | 19 +- .../frc/robot/commands/ElevationManual.java | 17 +- .../java/frc/robot/commands/ElevatorSet.java | 6 +- src/main/java/frc/robot/commands/FIREEE.java | 6 +- .../FIREEFORACERTAINAMOUNTOFTIME.java | 4 +- .../robot/commands/IntakeThroughShooter.java | 6 +- .../commands/IntakeThroughShooterPart2.java | 6 +- .../java/frc/robot/commands/IntexBestHex.java | 7 +- .../robot/commands/IntexForAutosByAutos.java | 4 +- .../java/frc/robot/commands/ManRizzt.java | 21 +- .../java/frc/robot/commands/MissileLock.java | 17 +- .../java/frc/robot/commands/NoteSniffer.java | 29 ++- .../robot/commands/ResetNoteInShooter.java | 6 +- .../commands/ResetNoteInShooterPart2.java | 6 +- .../java/frc/robot/commands/RizzLevel.java | 6 +- .../java/frc/robot/commands/TeleopSwerve.java | 106 +++++--- .../robot/commands/ToBreakOrNotToBreak.java | 6 +- .../java/frc/robot/commands/ZeroRizz.java | 1 + .../robot/subsystems/ElevatorSubsystem.java | 36 +-- .../robot/subsystems/IntexerSubsystem.java | 18 +- .../frc/robot/subsystems/LEDSubsystem.java | 41 ++-- .../robot/subsystems/ShooterSubsystem.java | 122 ++++++---- .../frc/robot/subsystems/SwerveSubsystem.java | 227 ++++++++++-------- .../frc/robot/subsystems/VisionSubsystem.java | 78 +++--- 38 files changed, 756 insertions(+), 523 deletions(-) diff --git a/build.gradle b/build.gradle index 2d20ce9..2eef37a 100644 --- a/build.gradle +++ b/build.gradle @@ -127,10 +127,10 @@ spotless { include '**/*.java' exclude '**/build/**', '**/build-*/**' } - toggleOffOn() palantirJavaFormat().style("AOSP") removeUnusedImports() trimTrailingWhitespace() + toggleOffOn() } groovyGradle { diff --git a/src/main/java/frc/lib/math/Conversions.java b/src/main/java/frc/lib/math/Conversions.java index ac68dcd..84781f3 100644 --- a/src/main/java/frc/lib/math/Conversions.java +++ b/src/main/java/frc/lib/math/Conversions.java @@ -1,13 +1,13 @@ package frc.lib.math; public class Conversions { - + /** * @param wheelRPS Wheel Velocity: (in Rotations per Second) * @param circumference Wheel Circumference: (in Meters) * @return Wheel Velocity: (in Meters per Second) */ - public static double RPSToMPS(double wheelRPS, double circumference){ + public static double RPSToMPS(double wheelRPS, double circumference) { double wheelMPS = wheelRPS * circumference; return wheelMPS; } @@ -17,7 +17,7 @@ public static double RPSToMPS(double wheelRPS, double circumference){ * @param circumference Wheel Circumference: (in Meters) * @return Wheel Velocity: (in Rotations per Second) */ - public static double MPSToRPS(double wheelMPS, double circumference){ + public static double MPSToRPS(double wheelMPS, double circumference) { double wheelRPS = wheelMPS / circumference; return wheelRPS; } @@ -27,7 +27,7 @@ public static double MPSToRPS(double wheelMPS, double circumference){ * @param circumference Wheel Circumference: (in Meters) * @return Wheel Distance: (in Meters) */ - public static double rotationsToMeters(double wheelRotations, double circumference){ + public static double rotationsToMeters(double wheelRotations, double circumference) { double wheelMeters = wheelRotations * circumference; return wheelMeters; } @@ -37,8 +37,8 @@ public static double rotationsToMeters(double wheelRotations, double circumferen * @param circumference Wheel Circumference: (in Meters) * @return Wheel Position: (in Rotations) */ - public static double metersToRotations(double wheelMeters, double circumference){ + public static double metersToRotations(double wheelMeters, double circumference) { double wheelRotations = wheelMeters / circumference; return wheelRotations; } -} \ No newline at end of file +} diff --git a/src/main/java/frc/lib/math/FiringSolutions.java b/src/main/java/frc/lib/math/FiringSolutions.java index 3c0c68b..7971a85 100644 --- a/src/main/java/frc/lib/math/FiringSolutions.java +++ b/src/main/java/frc/lib/math/FiringSolutions.java @@ -1,5 +1,5 @@ package frc.lib.math; -// format:off +// spotless:off public final class FiringSolutions { private static final double shooterHeight = 0.355; private static final double noteFallAccel = 9.8; @@ -86,5 +86,5 @@ public static double getShooterAngle(double shooterVelocityX, double shooterVelo public static double getRobotOffsetAngle(double robotVelocityPerpendicularToSpeaker, double shooterVelocityX){ return Math.atan(robotVelocityPerpendicularToSpeaker / shooterVelocityX); } - +// spotless:on } diff --git a/src/main/java/frc/lib/math/FiringSolutionsV2.java b/src/main/java/frc/lib/math/FiringSolutionsV2.java index 930ce26..96c9efe 100644 --- a/src/main/java/frc/lib/math/FiringSolutionsV2.java +++ b/src/main/java/frc/lib/math/FiringSolutionsV2.java @@ -1,5 +1,5 @@ package frc.lib.math; -// format:off +// spotless:off public class FiringSolutionsV2 { private static final double shooterHeight = 0.355; @@ -141,5 +141,5 @@ public static double getR() { public static double getShooterAngle() { return Math.acos(R); } - +// spotless:on } diff --git a/src/main/java/frc/lib/math/FiringSolutionsV3.java b/src/main/java/frc/lib/math/FiringSolutionsV3.java index 9e7d52c..3b38fa1 100644 --- a/src/main/java/frc/lib/math/FiringSolutionsV3.java +++ b/src/main/java/frc/lib/math/FiringSolutionsV3.java @@ -1,5 +1,5 @@ package frc.lib.math; -// format:off +// spotless:off import java.util.Optional; import edu.wpi.first.math.geometry.Translation2d; @@ -346,5 +346,5 @@ public static double getDistanceToMovingTarget(double robotX, double robotY, dou public static double convertToRPM(double velocity) { return (60 * velocity) / (slipPercent * Math.PI * .1016); } - +// spotless:on } diff --git a/src/main/java/frc/lib/math/Interpolations.java b/src/main/java/frc/lib/math/Interpolations.java index 16eba82..98c665a 100644 --- a/src/main/java/frc/lib/math/Interpolations.java +++ b/src/main/java/frc/lib/math/Interpolations.java @@ -9,25 +9,26 @@ public class Interpolations { public InterpolatingDoubleTreeMap shooterSpeeds = new InterpolatingDoubleTreeMap(); - public InterpolatingDoubleTreeMap bozoDefenseBotDriversHonestReactionWhenElevatorGoUp_speeds = new InterpolatingDoubleTreeMap(); + public InterpolatingDoubleTreeMap bozoDefenseBotDriversHonestReactionWhenElevatorGoUp_speeds = + new InterpolatingDoubleTreeMap(); private double offset = 1; public Interpolations() { - /* - shooterSpeeds.put(1.25, 53.0); - shooterSpeeds.put(1.5, 48.0); - shooterSpeeds.put(1.75, 44.0); - shooterSpeeds.put(2.0, 41.0); - shooterSpeeds.put(2.25, 39.0); - shooterSpeeds.put(2.5, 37.5); - shooterSpeeds.put(2.75, 35.5); - shooterSpeeds.put(3.0, 34.0); - shooterSpeeds.put(3.25, 33.0); - shooterSpeeds.put(3.5, 32.0); - shooterSpeeds.put(4.0, 30.75); - shooterSpeeds.put(4.5, 29.75); -*/ + /* + shooterSpeeds.put(1.25, 53.0); + shooterSpeeds.put(1.5, 48.0); + shooterSpeeds.put(1.75, 44.0); + shooterSpeeds.put(2.0, 41.0); + shooterSpeeds.put(2.25, 39.0); + shooterSpeeds.put(2.5, 37.5); + shooterSpeeds.put(2.75, 35.5); + shooterSpeeds.put(3.0, 34.0); + shooterSpeeds.put(3.25, 33.0); + shooterSpeeds.put(3.5, 32.0); + shooterSpeeds.put(4.0, 30.75); + shooterSpeeds.put(4.5, 29.75); + */ bozoDefenseBotDriversHonestReactionWhenElevatorGoUp_speeds.put(1.25, 44.0); bozoDefenseBotDriversHonestReactionWhenElevatorGoUp_speeds.put(1.5, 39.5); bozoDefenseBotDriversHonestReactionWhenElevatorGoUp_speeds.put(1.75, 36.0); @@ -52,7 +53,6 @@ public Interpolations() { shooterSpeeds.put(3.25, 31.0 + offset); shooterSpeeds.put(3.5, 30.0 + offset); shooterSpeeds.put(4.0, 28.25 + offset); - } public double getShooterAngleFromInterpolation(double distanceToTarget) { @@ -62,4 +62,4 @@ public double getShooterAngleFromInterpolation(double distanceToTarget) { public double getShooterAngleFromInterpolationElevatorUp(double distanceToTarget) { return bozoDefenseBotDriversHonestReactionWhenElevatorGoUp_speeds.get(distanceToTarget); } -} \ No newline at end of file +} diff --git a/src/main/java/frc/lib/util/COTSTalonFXSwerveConstants.java b/src/main/java/frc/lib/util/COTSTalonFXSwerveConstants.java index 0f4d7f9..e0930a2 100644 --- a/src/main/java/frc/lib/util/COTSTalonFXSwerveConstants.java +++ b/src/main/java/frc/lib/util/COTSTalonFXSwerveConstants.java @@ -4,7 +4,7 @@ import com.ctre.phoenix6.signals.SensorDirectionValue; import edu.wpi.first.math.util.Units; - +// spotless:off /* Contains values and required settings for common COTS swerve modules. */ public class COTSTalonFXSwerveConstants { public final double wheelDiameter; @@ -307,5 +307,4 @@ public static final class driveRatios{ } } } - - \ No newline at end of file +// spotless:on diff --git a/src/main/java/frc/lib/util/SwerveModuleConstants.java b/src/main/java/frc/lib/util/SwerveModuleConstants.java index da90a20..05bb3a2 100644 --- a/src/main/java/frc/lib/util/SwerveModuleConstants.java +++ b/src/main/java/frc/lib/util/SwerveModuleConstants.java @@ -1,7 +1,7 @@ package frc.lib.util; import edu.wpi.first.math.geometry.Rotation2d; - +// spotless:off public class SwerveModuleConstants { public final int driveMotorID; public final int angleMotorID; @@ -22,3 +22,4 @@ public SwerveModuleConstants(int driveMotorID, int angleMotorID, int canCoderID, this.angleOffset = angleOffset; } } +// spotless:on diff --git a/src/main/java/frc/robot/CTREConfigs.java b/src/main/java/frc/robot/CTREConfigs.java index 24b6695..8bbb55d 100644 --- a/src/main/java/frc/robot/CTREConfigs.java +++ b/src/main/java/frc/robot/CTREConfigs.java @@ -8,7 +8,7 @@ public final class CTREConfigs { public TalonFXConfiguration swerveDriveFXConfig = new TalonFXConfiguration(); public CANcoderConfiguration swerveCANcoderConfig = new CANcoderConfiguration(); - public CTREConfigs(){ + public CTREConfigs() { /** Swerve CANCoder Configuration */ swerveCANcoderConfig.MagnetSensor.SensorDirection = Constants.Swerve.cancoderInvert; @@ -20,12 +20,15 @@ public CTREConfigs(){ /* Gear Ratio and Wrapping Config */ swerveAngleFXConfig.Feedback.SensorToMechanismRatio = Constants.Swerve.angleGearRatio; swerveAngleFXConfig.ClosedLoopGeneral.ContinuousWrap = true; - + /* Current Limiting */ - swerveAngleFXConfig.CurrentLimits.SupplyCurrentLimitEnable = Constants.Swerve.angleEnableCurrentLimit; + swerveAngleFXConfig.CurrentLimits.SupplyCurrentLimitEnable = + Constants.Swerve.angleEnableCurrentLimit; swerveAngleFXConfig.CurrentLimits.SupplyCurrentLimit = Constants.Swerve.angleCurrentLimit; - swerveAngleFXConfig.CurrentLimits.SupplyCurrentThreshold = Constants.Swerve.angleCurrentThreshold; - swerveAngleFXConfig.CurrentLimits.SupplyTimeThreshold = Constants.Swerve.angleCurrentThresholdTime; + swerveAngleFXConfig.CurrentLimits.SupplyCurrentThreshold = + Constants.Swerve.angleCurrentThreshold; + swerveAngleFXConfig.CurrentLimits.SupplyTimeThreshold = + Constants.Swerve.angleCurrentThresholdTime; /* PID Config */ swerveAngleFXConfig.Slot0.kP = Constants.Swerve.angleKP; @@ -41,10 +44,13 @@ public CTREConfigs(){ swerveDriveFXConfig.Feedback.SensorToMechanismRatio = Constants.Swerve.driveGearRatio; /* Current Limiting */ - swerveDriveFXConfig.CurrentLimits.SupplyCurrentLimitEnable = Constants.Swerve.driveEnableCurrentLimit; + swerveDriveFXConfig.CurrentLimits.SupplyCurrentLimitEnable = + Constants.Swerve.driveEnableCurrentLimit; swerveDriveFXConfig.CurrentLimits.SupplyCurrentLimit = Constants.Swerve.driveCurrentLimit; - swerveDriveFXConfig.CurrentLimits.SupplyCurrentThreshold = Constants.Swerve.driveCurrentThreshold; - swerveDriveFXConfig.CurrentLimits.SupplyTimeThreshold = Constants.Swerve.driveCurrentThresholdTime; + swerveDriveFXConfig.CurrentLimits.SupplyCurrentThreshold = + Constants.Swerve.driveCurrentThreshold; + swerveDriveFXConfig.CurrentLimits.SupplyTimeThreshold = + Constants.Swerve.driveCurrentThresholdTime; /* PID Config */ swerveDriveFXConfig.Slot0.kP = Constants.Swerve.driveKP; @@ -52,10 +58,13 @@ public CTREConfigs(){ swerveDriveFXConfig.Slot0.kD = Constants.Swerve.driveKD; /* Open and Closed Loop Ramping */ - swerveDriveFXConfig.OpenLoopRamps.DutyCycleOpenLoopRampPeriod = Constants.Swerve.openLoopRamp; + swerveDriveFXConfig.OpenLoopRamps.DutyCycleOpenLoopRampPeriod = + Constants.Swerve.openLoopRamp; swerveDriveFXConfig.OpenLoopRamps.VoltageOpenLoopRampPeriod = Constants.Swerve.openLoopRamp; - swerveDriveFXConfig.ClosedLoopRamps.DutyCycleClosedLoopRampPeriod = Constants.Swerve.closedLoopRamp; - swerveDriveFXConfig.ClosedLoopRamps.VoltageClosedLoopRampPeriod = Constants.Swerve.closedLoopRamp; + swerveDriveFXConfig.ClosedLoopRamps.DutyCycleClosedLoopRampPeriod = + Constants.Swerve.closedLoopRamp; + swerveDriveFXConfig.ClosedLoopRamps.VoltageClosedLoopRampPeriod = + Constants.Swerve.closedLoopRamp; } -} \ 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 2ab7806..3548c15 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -1,7 +1,5 @@ package frc.robot; -import java.io.UncheckedIOException; - import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.signals.SensorDirectionValue; @@ -24,9 +22,12 @@ import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DataLogManager; + import frc.lib.util.COTSTalonFXSwerveConstants; import frc.lib.util.SwerveModuleConstants; +import java.io.UncheckedIOException; + public final class Constants { public static final double stickDeadband = 0.07; @@ -36,11 +37,17 @@ public static class Vision { public static final String kNoteCamera = "OnionRing"; public static final Transform3d kRobotToCamFront = new Transform3d( - new Translation3d(Units.inchesToMeters(11.5), Units.inchesToMeters(-1.25), Units.inchesToMeters(14.75)), + new Translation3d( + Units.inchesToMeters(11.5), + Units.inchesToMeters(-1.25), + Units.inchesToMeters(14.75)), new Rotation3d(0, Units.degreesToRadians(15), 0)); public static final Transform3d kRobotToCamBack = new Transform3d( - new Translation3d(Units.inchesToMeters(-7), Units.inchesToMeters(11), Units.inchesToMeters(14.5)), + new Translation3d( + Units.inchesToMeters(-7), + Units.inchesToMeters(11), + Units.inchesToMeters(14.5)), new Rotation3d(0, Units.degreesToRadians(15), Math.PI)); // The layout of the AprilTags on the field @@ -49,15 +56,18 @@ public static class Vision { // The standard deviations of our vision estimated poses, which affect correction rate public static final Matrix kSingleTagStdDevs = VecBuilder.fill(8, 8, 40); public static final Matrix kMultiTagStdDevs = VecBuilder.fill(4, 4, 20); - public static final Vector stateStdDevs = VecBuilder.fill(.5, .5, .5); // Encoder Odometry + public static final Vector stateStdDevs = + VecBuilder.fill(.5, .5, .5); // Encoder Odometry public static final Pose2d startingPoseBlue = new Pose2d(1.35, 5.55, new Rotation2d(0)); - public static final Pose2d startingPoseRed = new Pose2d(15.19, 5.55, new Rotation2d(Math.PI)); + public static final Pose2d startingPoseRed = + new Pose2d(15.19, 5.55, new Rotation2d(Math.PI)); } private static AprilTagFieldLayout getFieldLayout() { try { - AprilTagFieldLayout attemptedKTagLayout = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField(); + AprilTagFieldLayout attemptedKTagLayout = + AprilTagFields.k2024Crescendo.loadAprilTagLayoutField(); return attemptedKTagLayout; } catch (UncheckedIOException e) { DataLogManager.log(e.getMessage()); @@ -69,15 +79,16 @@ public static final class Swerve { public static final String canivore = "rex"; public static final int pigeonID = 13; - public static final COTSTalonFXSwerveConstants chosenModule = COTSTalonFXSwerveConstants.SDS.MK4i - .Falcon500(COTSTalonFXSwerveConstants.SDS.MK4i.driveRatios.L2); + public static final COTSTalonFXSwerveConstants chosenModule = + COTSTalonFXSwerveConstants.SDS.MK4i.Falcon500( + COTSTalonFXSwerveConstants.SDS.MK4i.driveRatios.L2); /* Drivetrain Constants */ public static final double trackWidth = Units.inchesToMeters(20.75); public static final double wheelBase = Units.inchesToMeters(20.75); public static final double wheelCircumference = chosenModule.wheelCircumference; - /* Swerve Kinematics + /* Swerve Kinematics * No need to ever change this unless you are not doing a traditional rectangular/square 4 module swerve */ public static final SwerveDriveKinematics swerveKinematics = new SwerveDriveKinematics( new Translation2d(wheelBase / 2.0, trackWidth / 2.0), @@ -132,7 +143,8 @@ public static final class Swerve { /** Meters per Second */ public static final double maxSpeed = 4.5; /** Radians per Second */ - public static final double maxAngularVelocity = 10.0; //TODO: This must be tuned to specific robot + public static final double maxAngularVelocity = + 10.0; // TODO: This must be tuned to specific robot /* Neutral Modes */ public static final NeutralModeValue angleNeutralMode = NeutralModeValue.Brake; @@ -145,8 +157,8 @@ public static final class Mod0 { public static final int angleMotorID = 3; public static final int canCoderID = 2; public static final Rotation2d angleOffset = Rotation2d.fromDegrees(112.6757); - public static final SwerveModuleConstants constants = new SwerveModuleConstants(driveMotorID, angleMotorID, - canCoderID, angleOffset); + public static final SwerveModuleConstants constants = + new SwerveModuleConstants(driveMotorID, angleMotorID, canCoderID, angleOffset); } /** Front Right Module - Module 1 */ @@ -155,8 +167,8 @@ public static final class Mod1 { public static final int angleMotorID = 6; public static final int canCoderID = 5; public static final Rotation2d angleOffset = Rotation2d.fromDegrees(-80.5078); - public static final SwerveModuleConstants constants = new SwerveModuleConstants(driveMotorID, angleMotorID, - canCoderID, angleOffset); + public static final SwerveModuleConstants constants = + new SwerveModuleConstants(driveMotorID, angleMotorID, canCoderID, angleOffset); } /** Back Left Module - Module 2 */ @@ -165,8 +177,8 @@ public static final class Mod2 { public static final int angleMotorID = 9; public static final int canCoderID = 8; public static final Rotation2d angleOffset = Rotation2d.fromDegrees(-79.453125); - public static final SwerveModuleConstants constants = new SwerveModuleConstants(driveMotorID, angleMotorID, - canCoderID, angleOffset); + public static final SwerveModuleConstants constants = + new SwerveModuleConstants(driveMotorID, angleMotorID, canCoderID, angleOffset); } /** Back Right Module - Module 3 */ @@ -175,8 +187,8 @@ public static final class Mod3 { public static final int angleMotorID = 12; public static final int canCoderID = 11; public static final Rotation2d angleOffset = Rotation2d.fromDegrees(112.06); - public static final SwerveModuleConstants constants = new SwerveModuleConstants(driveMotorID, angleMotorID, - canCoderID, angleOffset); + public static final SwerveModuleConstants constants = + new SwerveModuleConstants(driveMotorID, angleMotorID, canCoderID, angleOffset); } } @@ -208,17 +220,20 @@ public static final class Auto { public static final double kMaxAccelerationMetersPerSecondSquared = 3; public static final double kMaxAngularSpeedRadiansPerSecond = 9.424778; public static final double kMaxAngularSpeedRadiansPerSecondSquared = 12.56637; - + public static final double kPXController = 1; public static final double kPYController = 1; public static final double kPThetaController = 1; - - public static final PathConstraints PathfindingConstraints = new PathConstraints(kMaxSpeedMetersPerSecond, - kMaxAccelerationMetersPerSecondSquared, kMaxAngularSpeedRadiansPerSecond, + + public static final PathConstraints PathfindingConstraints = new PathConstraints( + kMaxSpeedMetersPerSecond, + kMaxAccelerationMetersPerSecondSquared, + kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared); /** Constraint for the motion profilied robot angle controller */ - public static final TrapezoidProfile.Constraints kThetaControllerConstraints = new TrapezoidProfile.Constraints( - kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared); + public static final TrapezoidProfile.Constraints kThetaControllerConstraints = + new TrapezoidProfile.Constraints( + kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared); } } diff --git a/src/main/java/frc/robot/Main.java b/src/main/java/frc/robot/Main.java index 8776e5d..61037e8 100644 --- a/src/main/java/frc/robot/Main.java +++ b/src/main/java/frc/robot/Main.java @@ -12,14 +12,14 @@ * call. */ public final class Main { - private Main() {} + private Main() {} - /** - * Main initialization function. Do not perform any initialization here. - * - *

If you change your main robot class, change the parameter type. - */ - public static void main(String... args) { - RobotBase.startRobot(Robot::new); - } + /** + * Main initialization function. Do not perform any initialization here. + * + *

If you change your main robot class, change the parameter type. + */ + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 663c9fc..78484b6 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,26 +4,26 @@ package frc.robot; -import java.time.LocalDateTime; -import java.time.format.DateTimeFormatter; -import java.util.Optional; -import org.littletonrobotics.urcl.URCL; - -import com.ctre.phoenix6.SignalLogger; - import edu.wpi.first.net.PortForwarder; import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.PowerDistribution; import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; +import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.livewindow.LiveWindow; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; + import frc.lib.math.FiringSolutionsV3; +import org.littletonrobotics.urcl.URCL; + +import java.time.LocalDateTime; +import java.time.format.DateTimeFormatter; +import java.util.Optional; + /** * The VM is configured to automatically run this class, and to call the functions corresponding to * each mode, as described in the TimedRobot documentation. If you change the name of this class or @@ -59,7 +59,10 @@ public void robotInit() { DriverStation.silenceJoystickConnectionWarning(true); // Starts recording to data log - DataLogManager.start("/media/sda1/logs/", DateTimeFormatter.ofPattern("yyyy-MM-dd__HH-mm-ss").format(LocalDateTime.now())+".wpilog"); + DataLogManager.start( + "/media/sda1/logs/", + DateTimeFormatter.ofPattern("yyyy-MM-dd__HH-mm-ss").format(LocalDateTime.now()) + + ".wpilog"); // Record both DS control and joystick data DriverStation.startDataLog(DataLogManager.getLog()); @@ -70,8 +73,8 @@ public void robotInit() { URCL.start(); // Log data from all CTRE devices - //SignalLogger.setPath("/media/sda1/logs/"); - //SignalLogger.start(); + // SignalLogger.setPath("/media/sda1/logs/"); + // SignalLogger.start(); // Output command scheduler to dashboard SmartDashboard.putData(CommandScheduler.getInstance()); @@ -84,7 +87,7 @@ public void robotInit() { redAlliance = checkRedAlliance(); // idk if this is useful - //System.gc(); + // System.gc(); } /** @@ -124,14 +127,13 @@ public static boolean checkRedAlliance() { public void disabledInit() { m_robotContainer.stopAll(); - if (m_autonomousCommand != null){ + if (m_autonomousCommand != null) { m_autonomousCommand.cancel(); } } @Override - public void disabledPeriodic() { - } + public void disabledPeriodic() {} /** * This autonomous runs the autonomous command selected by your @@ -140,7 +142,7 @@ public void disabledPeriodic() { @Override public void autonomousInit() { redAlliance = checkRedAlliance(); - + m_autonomousCommand = m_robotContainer.getAutonomousCommand(); FiringSolutionsV3.setAlliance(redAlliance); @@ -156,8 +158,7 @@ public void autonomousInit() { /** This function is called periodically during autonomous. */ @Override - public void autonomousPeriodic() { - } + public void autonomousPeriodic() {} @Override public void teleopInit() { @@ -177,8 +178,7 @@ public void teleopInit() { /** This function is called periodically during operator control. */ @Override - public void teleopPeriodic() { - } + public void teleopPeriodic() {} @Override public void testInit() { @@ -188,6 +188,5 @@ public void testInit() { /** This function is called periodically during test mode. */ @Override - public void testPeriodic() { - } + public void testPeriodic() {} } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c5e5d4f..017cce3 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -13,6 +13,7 @@ import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; + import frc.lib.math.FiringSolutionsV3; import frc.robot.commands.*; import frc.robot.subsystems.*; @@ -40,17 +41,22 @@ public class RobotContainer { /* DRIVER BUTTONS */ /** Driver X */ - private final JoystickButton intakeNoMove = new JoystickButton(driver, XboxController.Button.kX.value); + private final JoystickButton intakeNoMove = + new JoystickButton(driver, XboxController.Button.kX.value); /** Driver A */ private final JoystickButton Shoot = new JoystickButton(driver, XboxController.Button.kA.value); /** Driver B */ - private final JoystickButton forceShoot = new JoystickButton(driver, XboxController.Button.kB.value); + private final JoystickButton forceShoot = + new JoystickButton(driver, XboxController.Button.kB.value); /** Driver Y */ - private final JoystickButton intakeFromSource = new JoystickButton(driver, XboxController.Button.kY.value); + private final JoystickButton intakeFromSource = + new JoystickButton(driver, XboxController.Button.kY.value); /** Driver RB */ - private final JoystickButton intex = new JoystickButton(driver, XboxController.Button.kRightBumper.value); + private final JoystickButton intex = + new JoystickButton(driver, XboxController.Button.kRightBumper.value); /** Driver LB */ - private final JoystickButton outex = new JoystickButton(driver, XboxController.Button.kLeftBumper.value); + private final JoystickButton outex = + new JoystickButton(driver, XboxController.Button.kLeftBumper.value); /** Driver LT */ private final Trigger targetAmp = new Trigger(() -> driver.getRawAxis(leftTrigger) > .5); /** Driver RT */ @@ -64,21 +70,28 @@ public class RobotContainer { /** Driver Left */ private final Trigger driverRight = new Trigger(() -> driver.getPOV() == 270); /** Driver Start */ - private final JoystickButton resetOdom = new JoystickButton(driver, XboxController.Button.kStart.value); + private final JoystickButton resetOdom = + new JoystickButton(driver, XboxController.Button.kStart.value); /* MECH BUTTONS */ /** Mech X */ - private final JoystickButton shooterToAntiDefense = new JoystickButton(mech, XboxController.Button.kX.value); + private final JoystickButton shooterToAntiDefense = + new JoystickButton(mech, XboxController.Button.kX.value); /** Mech A */ - private final JoystickButton shooterToIntake = new JoystickButton(mech, XboxController.Button.kA.value); + private final JoystickButton shooterToIntake = + new JoystickButton(mech, XboxController.Button.kA.value); /** Mech Y */ - private final JoystickButton shooterToAmp = new JoystickButton(mech, XboxController.Button.kY.value); + private final JoystickButton shooterToAmp = + new JoystickButton(mech, XboxController.Button.kY.value); /** Mech B */ - private final JoystickButton shooterToSubwoofer = new JoystickButton(mech, XboxController.Button.kB.value); + private final JoystickButton shooterToSubwoofer = + new JoystickButton(mech, XboxController.Button.kB.value); /** Mech RB */ - private final JoystickButton primeShooterSpeedSpeaker = new JoystickButton(mech, XboxController.Button.kRightBumper.value); + private final JoystickButton primeShooterSpeedSpeaker = + new JoystickButton(mech, XboxController.Button.kRightBumper.value); /** Mech LB */ - private final JoystickButton resetNoteInShooter = new JoystickButton(mech, XboxController.Button.kLeftBumper.value); + private final JoystickButton resetNoteInShooter = + new JoystickButton(mech, XboxController.Button.kLeftBumper.value); /** Mech RT */ private final Trigger mechRT = new Trigger(() -> mech.getRawAxis(rightTrigger) > .5); /** Mech LT */ @@ -92,15 +105,18 @@ public class RobotContainer { /** Mech Left */ private final Trigger mechLeft = new Trigger(() -> mech.getPOV() == 270); /** Mech Start */ - private final JoystickButton autoZeroShooter = new JoystickButton(mech, XboxController.Button.kStart.value); + private final JoystickButton autoZeroShooter = + new JoystickButton(mech, XboxController.Button.kStart.value); /** Mech Back */ - private final JoystickButton zeroShooter = new JoystickButton(mech, XboxController.Button.kBack.value); + private final JoystickButton zeroShooter = + new JoystickButton(mech, XboxController.Button.kBack.value); /** Mech RS */ - private final JoystickButton mechRightStick = new JoystickButton(mech, XboxController.Button.kRightStick.value); + private final JoystickButton mechRightStick = + new JoystickButton(mech, XboxController.Button.kRightStick.value); /** Mech LS */ - private final JoystickButton mechLeftStick = new JoystickButton(mech, XboxController.Button.kLeftStick.value); + private final JoystickButton mechLeftStick = + new JoystickButton(mech, XboxController.Button.kLeftStick.value); - private final Trigger dynamicForward = new Trigger(() -> FF.getPOV() == 90); private final Trigger dynamicBackward = new Trigger(() -> FF.getPOV() == 270); private final Trigger quasistaticForward = new Trigger(() -> FF.getPOV() == 0); @@ -111,8 +127,10 @@ public class RobotContainer { private final SwerveSubsystem m_SwerveSubsystem = new SwerveSubsystem(m_VisionSubsystem); private final ElevatorSubsystem m_ElevatorSubsystem = new ElevatorSubsystem(); private final IntexerSubsystem m_IntexerSubsystem = new IntexerSubsystem(); - private final ShooterSubsystem m_ShooterSubsystem = new ShooterSubsystem(m_SwerveSubsystem, m_ElevatorSubsystem); - private final LEDSubsystem m_LEDSubsystem = new LEDSubsystem(m_VisionSubsystem, m_ShooterSubsystem, m_IntexerSubsystem, m_SwerveSubsystem); + private final ShooterSubsystem m_ShooterSubsystem = + new ShooterSubsystem(m_SwerveSubsystem, m_ElevatorSubsystem); + private final LEDSubsystem m_LEDSubsystem = new LEDSubsystem( + m_VisionSubsystem, m_ShooterSubsystem, m_IntexerSubsystem, m_SwerveSubsystem); private final SendableChooser autoChooser; @@ -120,6 +138,7 @@ public class RobotContainer { * The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { + // spotless:off // Named commands for PathPlanner autos NamedCommands.registerCommand("Intake", new IntexForAutosByAutos(m_IntexerSubsystem, m_ShooterSubsystem)); NamedCommands.registerCommand("Shoot", new AimBot(m_ShooterSubsystem, m_SwerveSubsystem, m_IntexerSubsystem, FiringSolutionsV3.convertToRPM(m_ShooterSubsystem.getCalculatedVelocity()))); @@ -131,25 +150,29 @@ public RobotContainer() { NamedCommands.registerCommand("Note Sniffer2", new NoteSniffer(m_SwerveSubsystem, m_VisionSubsystem, m_IntexerSubsystem, m_ShooterSubsystem)); NamedCommands.registerCommand("Fire Under Stage", new InstantCommand(() -> m_ShooterSubsystem.setWristByAngle(Math.toRadians(10)))); NamedCommands.registerCommand("Force Shoot", new FIREEFORACERTAINAMOUNTOFTIME(m_ShooterSubsystem, m_IntexerSubsystem, .2)); - - m_SwerveSubsystem.setDefaultCommand( - new TeleopSwerve( - m_SwerveSubsystem, m_VisionSubsystem, m_ShooterSubsystem, m_IntexerSubsystem, - () -> -driver.getRawAxis(leftVerticalAxis), - () -> -driver.getRawAxis(leftHorizontalAxis), - () -> -driver.getRawAxis(rightHorizontalAxis), - () -> false, - () -> targetAmp.getAsBoolean(), - () -> targetSpeaker.getAsBoolean(), - () -> intex.getAsBoolean(), - () -> intakeNoMove.getAsBoolean(), driver)); + // spotless:on + + m_SwerveSubsystem.setDefaultCommand(new TeleopSwerve( + m_SwerveSubsystem, + m_VisionSubsystem, + m_ShooterSubsystem, + m_IntexerSubsystem, + () -> -driver.getRawAxis(leftVerticalAxis), + () -> -driver.getRawAxis(leftHorizontalAxis), + () -> -driver.getRawAxis(rightHorizontalAxis), + () -> false, + () -> targetAmp.getAsBoolean(), + () -> targetSpeaker.getAsBoolean(), + () -> intex.getAsBoolean(), + () -> intakeNoMove.getAsBoolean(), + driver)); m_ElevatorSubsystem.setDefaultCommand( - new ElevationManual( - m_ElevatorSubsystem, - () -> -mech.getRawAxis(leftVerticalAxis))); + new ElevationManual(m_ElevatorSubsystem, () -> -mech.getRawAxis(leftVerticalAxis))); - m_ShooterSubsystem.setDefaultCommand(new ManRizzt(m_ShooterSubsystem, () -> -mech.getRawAxis(rightVerticalAxis), + m_ShooterSubsystem.setDefaultCommand(new ManRizzt( + m_ShooterSubsystem, + () -> -mech.getRawAxis(rightVerticalAxis), () -> shooterToSubwoofer.getAsBoolean())); autoChooser = AutoBuilder.buildAutoChooser("4 piece mcnugget"); @@ -170,94 +193,121 @@ private void configureButtonBindings() { /* DRIVER BUTTONS */ // Lock on to speaker - targetSpeaker.whileTrue(new MissileLock(m_ShooterSubsystem, "speaker")) - .onFalse(new InstantCommand(() -> m_ShooterSubsystem.setShooterVelocity(Constants.Shooter.idleSpeedRPM))); - targetAmp.whileTrue(new MissileLock(m_ShooterSubsystem, "amp")) - .onFalse(new InstantCommand(() -> m_ShooterSubsystem.setShooterVelocity(Constants.Shooter.idleSpeedRPM))); + targetSpeaker + .whileTrue(new MissileLock(m_ShooterSubsystem, "speaker")) + .onFalse(new InstantCommand(() -> + m_ShooterSubsystem.setShooterVelocity(Constants.Shooter.idleSpeedRPM))); + targetAmp + .whileTrue(new MissileLock(m_ShooterSubsystem, "amp")) + .onFalse(new InstantCommand(() -> + m_ShooterSubsystem.setShooterVelocity(Constants.Shooter.idleSpeedRPM))); // Shooter - targetSpeaker.or(targetAmp).and(Shoot).whileTrue(new FIREEE(m_ShooterSubsystem, m_IntexerSubsystem)); // Main fire + targetSpeaker + .or(targetAmp) + .and(Shoot) + .whileTrue(new FIREEE(m_ShooterSubsystem, m_IntexerSubsystem)); // Main fire // Reset Odometry - resetOdom.onTrue(new InstantCommand(() -> m_SwerveSubsystem.zeroHeading()).alongWith( - new InstantCommand(() -> m_SwerveSubsystem - .setPose(Robot.getAlliance() ? Constants.Vision.startingPoseRed : Constants.Vision.startingPoseBlue)))); + resetOdom.onTrue(new InstantCommand(() -> m_SwerveSubsystem.zeroHeading()) + .alongWith(new InstantCommand(() -> m_SwerveSubsystem.setPose( + Robot.getAlliance() + ? Constants.Vision.startingPoseRed + : Constants.Vision.startingPoseBlue)))); // Intexer intex.or(intakeNoMove).whileTrue(new IntexBestHex(m_IntexerSubsystem, true, driver)); outex.whileTrue(new IntexBestHex(m_IntexerSubsystem, false, driver)); // Shooter intake - forceShoot.whileTrue(new InstantCommand(() -> m_IntexerSubsystem.setShooterIntake(.9))) + forceShoot + .whileTrue(new InstantCommand(() -> m_IntexerSubsystem.setShooterIntake(.9))) .onFalse(new InstantCommand(() -> m_IntexerSubsystem.setShooterIntake(0))); // Move to Amp - //driverUp.whileTrue(m_SwerveSubsystem.pathToAmpChain()); + // driverUp.whileTrue(m_SwerveSubsystem.pathToAmpChain()); // Move to Source - //driverDown.whileTrue(m_SwerveSubsystem.pathToSourceChain()); + // driverDown.whileTrue(m_SwerveSubsystem.pathToSourceChain()); // Intake from Source - intakeFromSource.whileTrue(new IntakeThroughShooter(m_ShooterSubsystem, m_IntexerSubsystem, driver)) - .onFalse(new IntakeThroughShooterPart2(m_ShooterSubsystem, m_IntexerSubsystem, driver)); + intakeFromSource + .whileTrue(new IntakeThroughShooter(m_ShooterSubsystem, m_IntexerSubsystem, driver)) + .onFalse(new IntakeThroughShooterPart2( + m_ShooterSubsystem, m_IntexerSubsystem, driver)); /* MECH BUTTONS */ // Prime Shooter primeShooterSpeedSpeaker - .whileTrue(new InstantCommand(() -> m_ShooterSubsystem.setShooterVelocity( - FiringSolutionsV3.convertToRPM(m_ShooterSubsystem.getCalculatedVelocity())))) - .onFalse(new InstantCommand( - () -> m_ShooterSubsystem.setShooterVelocity(Constants.Shooter.idleSpeedRPM))); + .whileTrue(new InstantCommand( + () -> m_ShooterSubsystem.setShooterVelocity(FiringSolutionsV3.convertToRPM( + m_ShooterSubsystem.getCalculatedVelocity())))) + .onFalse(new InstantCommand(() -> + m_ShooterSubsystem.setShooterVelocity(Constants.Shooter.idleSpeedRPM))); // Elevator elevatorDown.onTrue(new ElevatorSet(m_ElevatorSubsystem, Constants.Elevator.minHeightMeters) - .alongWith(new RizzLevel(m_ShooterSubsystem, Constants.Shooter.intakeAngleRadians))); + .alongWith( + new RizzLevel(m_ShooterSubsystem, Constants.Shooter.intakeAngleRadians))); elevatorUp.onTrue(new ElevatorSet(m_ElevatorSubsystem, Constants.Elevator.maxHeightMeters) .alongWith(new RizzLevel(m_ShooterSubsystem, 0.0))); // Zero wrist - mechLT.negate().and(zeroShooter).onTrue(new InstantCommand( - () -> m_ShooterSubsystem.resetWristEncoders(Constants.Shooter.angleOffsetManual))); // Set encoder to zero - //autoZeroShooter.onTrue(new ZeroRizz(m_ShooterSubsystem) - // .andThen(new RizzLevel(m_ShooterSubsystem, Constants.Shooter.intakeAngleRadians))); + mechLT.negate() + .and(zeroShooter) + .onTrue(new InstantCommand(() -> m_ShooterSubsystem.resetWristEncoders( + Constants.Shooter.angleOffsetManual))); // Set encoder to zero + // autoZeroShooter.onTrue(new ZeroRizz(m_ShooterSubsystem) + // .andThen(new RizzLevel(m_ShooterSubsystem, + // Constants.Shooter.intakeAngleRadians))); // Intake Preset - shooterToIntake.onTrue(new RizzLevel(m_ShooterSubsystem, Constants.Shooter.intakeAngleRadians)) + shooterToIntake + .onTrue(new RizzLevel(m_ShooterSubsystem, Constants.Shooter.intakeAngleRadians)) .onTrue(new ElevatorSet(m_ElevatorSubsystem, Constants.Elevator.minHeightMeters)); // Amp Preset - shooterToAmp.onTrue(new RizzLevel(m_ShooterSubsystem, -0.48)) + shooterToAmp + .onTrue(new RizzLevel(m_ShooterSubsystem, -0.48)) .onTrue(new ElevatorSet(m_ElevatorSubsystem, Constants.Elevator.ampHeight)); // Subwoofer Preset - shooterToSubwoofer.onTrue(new RizzLevel(m_ShooterSubsystem, Math.toRadians(57))) + shooterToSubwoofer + .onTrue(new RizzLevel(m_ShooterSubsystem, Math.toRadians(57))) .onTrue(new ElevatorSet(m_ElevatorSubsystem, Constants.Elevator.minHeightMeters)); // Anti-Defense Preset - shooterToAntiDefense - .onTrue(new ElevatorSet(m_ElevatorSubsystem, Constants.Elevator.antiBozoSmileToasterAhhGoonBotShooterHeight)); + shooterToAntiDefense.onTrue(new ElevatorSet( + m_ElevatorSubsystem, + Constants.Elevator.antiBozoSmileToasterAhhGoonBotShooterHeight)); // Reset the R calculation in case it gets off resetR.onTrue(new InstantCommand(() -> FiringSolutionsV3.resetAllR())); // Reset Note in Shooter - resetNoteInShooter.whileTrue(new ResetNoteInShooter(m_ShooterSubsystem, m_IntexerSubsystem, mech)) - .onFalse(new ResetNoteInShooterPart2(m_ShooterSubsystem, m_IntexerSubsystem, mech)); - + resetNoteInShooter + .whileTrue(new ResetNoteInShooter(m_ShooterSubsystem, m_IntexerSubsystem, mech)) + .onFalse(new ResetNoteInShooterPart2(m_ShooterSubsystem, m_IntexerSubsystem, mech)); + // Kill Flywheels - mechLT.negate().and(mechRightStick).onTrue(new InstantCommand(() -> m_ShooterSubsystem.setShooterVelocity(0))); - + mechLT.negate() + .and(mechRightStick) + .onTrue(new InstantCommand(() -> m_ShooterSubsystem.setShooterVelocity(0))); + // Kill Wrist - mechLT.and(mechRightStick).onTrue(new InstantCommand(() -> m_ShooterSubsystem.setWristSpeedManual(0)) - .alongWith(new InstantCommand(() -> m_ShooterSubsystem.setWristToCoast()))) - .onFalse(new InstantCommand(() -> m_ShooterSubsystem.setWristToBrake())); + mechLT.and(mechRightStick) + .onTrue(new InstantCommand(() -> m_ShooterSubsystem.setWristSpeedManual(0)) + .alongWith(new InstantCommand(() -> m_ShooterSubsystem.setWristToCoast()))) + .onFalse(new InstantCommand(() -> m_ShooterSubsystem.setWristToBrake())); // Kill Elevator - mechLT.and(mechLeftStick).onTrue(new InstantCommand(() -> m_ElevatorSubsystem.setElevatorSpeedManual(0))); + mechLT.and(mechLeftStick) + .onTrue(new InstantCommand(() -> m_ElevatorSubsystem.setElevatorSpeedManual(0))); - mechLT.and(zeroShooter).onTrue(new InstantCommand( - () -> m_ShooterSubsystem.resetWristEncoders(Constants.Shooter.angleOffsetBottom))); + mechLT.and(zeroShooter) + .onTrue(new InstantCommand(() -> m_ShooterSubsystem.resetWristEncoders( + Constants.Shooter.angleOffsetBottom))); /* THIRD CONTROLLER */ // Characterization tests @@ -274,7 +324,7 @@ public void stopAll() { m_ElevatorSubsystem.setPositionWithEncoder(m_ElevatorSubsystem.getPosition()); } - public void zeroWristEncoders(){ + public void zeroWristEncoders() { m_ShooterSubsystem.resetWristEncoders(Constants.Shooter.angleOffsetBottom); } @@ -286,5 +336,4 @@ public void zeroWristEncoders(){ public Command getAutonomousCommand() { return autoChooser.getSelected(); } - } diff --git a/src/main/java/frc/robot/SwerveModule.java b/src/main/java/frc/robot/SwerveModule.java index 2cbbe1a..173f44c 100644 --- a/src/main/java/frc/robot/SwerveModule.java +++ b/src/main/java/frc/robot/SwerveModule.java @@ -11,6 +11,7 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; + import frc.lib.math.Conversions; import frc.lib.util.SwerveModuleConstants; @@ -22,7 +23,8 @@ public class SwerveModule { private TalonFX mDriveMotor; private CANcoder angleEncoder; - private final SimpleMotorFeedforward driveFeedForward = new SimpleMotorFeedforward(Constants.Swerve.driveKS, Constants.Swerve.driveKV, Constants.Swerve.driveKA); + private final SimpleMotorFeedforward driveFeedForward = new SimpleMotorFeedforward( + Constants.Swerve.driveKS, Constants.Swerve.driveKV, Constants.Swerve.driveKA); /* drive motor control requests */ private final DutyCycleOut driveDutyCycle = new DutyCycleOut(0); @@ -32,10 +34,10 @@ public class SwerveModule { /* angle motor control requests */ private final PositionVoltage anglePosition = new PositionVoltage(0); - public SwerveModule(int moduleNumber, SwerveModuleConstants moduleConstants){ + public SwerveModule(int moduleNumber, SwerveModuleConstants moduleConstants) { this.moduleNumber = moduleNumber; this.angleOffset = moduleConstants.angleOffset; - + /* Angle Encoder Config */ angleEncoder = new CANcoder(moduleConstants.cancoderID, Constants.Swerve.canivore); angleEncoder.getConfigurator().apply(Robot.ctreConfigs.swerveCANcoderConfig); @@ -51,64 +53,66 @@ public SwerveModule(int moduleNumber, SwerveModuleConstants moduleConstants){ mDriveMotor.getConfigurator().setPosition(0.0); } - public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop){ - desiredState = SwerveModuleState.optimize(desiredState, getState().angle); + public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop) { + desiredState = SwerveModuleState.optimize(desiredState, getState().angle); mAngleMotor.setControl(anglePosition.withPosition(desiredState.angle.getRotations())); setSpeed(desiredState, isOpenLoop); } - private void setSpeed(SwerveModuleState desiredState, boolean isOpenLoop){ - if(isOpenLoop){ + private void setSpeed(SwerveModuleState desiredState, boolean isOpenLoop) { + if (isOpenLoop) { driveDutyCycle.Output = desiredState.speedMetersPerSecond / Constants.Swerve.maxSpeed; mDriveMotor.setControl(driveDutyCycle); - } - else { - driveVelocity.Velocity = Conversions.MPSToRPS(desiredState.speedMetersPerSecond, Constants.Swerve.wheelCircumference); - driveVelocity.FeedForward = driveFeedForward.calculate(desiredState.speedMetersPerSecond); + } else { + driveVelocity.Velocity = Conversions.MPSToRPS( + desiredState.speedMetersPerSecond, Constants.Swerve.wheelCircumference); + driveVelocity.FeedForward = + driveFeedForward.calculate(desiredState.speedMetersPerSecond); mDriveMotor.setControl(driveVelocity); } } - public Rotation2d getCANcoder(){ + public Rotation2d getCANcoder() { return Rotation2d.fromRotations(angleEncoder.getAbsolutePosition().getValue()); } /** Set module angles to absolute position */ - public void resetToAbsolute(){ + public void resetToAbsolute() { double absolutePosition = getCANcoder().getRotations() - angleOffset.getRotations(); mAngleMotor.setPosition(absolutePosition); } - public SwerveModuleState getState(){ + public SwerveModuleState getState() { return new SwerveModuleState( - Conversions.RPSToMPS(mDriveMotor.getVelocity().getValue(), Constants.Swerve.wheelCircumference), - Rotation2d.fromRotations(mAngleMotor.getPosition().getValue()) - ); + Conversions.RPSToMPS( + mDriveMotor.getVelocity().getValue(), Constants.Swerve.wheelCircumference), + Rotation2d.fromRotations(mAngleMotor.getPosition().getValue())); } - public SwerveModulePosition getPosition(){ + public SwerveModulePosition getPosition() { return new SwerveModulePosition( - Conversions.rotationsToMeters(mDriveMotor.getPosition().getValue(), Constants.Swerve.wheelCircumference), - Rotation2d.fromRotations(mAngleMotor.getPosition().getValue()) - ); + Conversions.rotationsToMeters( + mDriveMotor.getPosition().getValue(), Constants.Swerve.wheelCircumference), + Rotation2d.fromRotations(mAngleMotor.getPosition().getValue())); } - /** + /** * Drive robot based on provided voltage value *

* Does NOT have optimization, meaning wheels have to be facing same direction */ - public void voltageDrive(double Volts){ + public void voltageDrive(double Volts) { mDriveMotor.setControl(driveCharacteriztionControl.withOutput(Volts)); } /** Get drive motor voltage */ - public double getMotorVoltage(){ + public double getMotorVoltage() { return mDriveMotor.getMotorVoltage().getValue(); } /** IN METERS PER SECOND */ - public double getMotorVelocity(){ - return Conversions.RPSToMPS(mDriveMotor.getVelocity().getValue(), Constants.Swerve.wheelCircumference); + public double getMotorVelocity() { + return Conversions.RPSToMPS( + mDriveMotor.getVelocity().getValue(), Constants.Swerve.wheelCircumference); } -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/commands/AimBot.java b/src/main/java/frc/robot/commands/AimBot.java index 3761d08..8a2917a 100644 --- a/src/main/java/frc/robot/commands/AimBot.java +++ b/src/main/java/frc/robot/commands/AimBot.java @@ -10,6 +10,7 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; + import frc.lib.math.FiringSolutionsV3; import frc.robot.Constants; import frc.robot.Robot; @@ -27,7 +28,11 @@ public class AimBot extends Command { private Timer timer = new Timer(); /** Creates a new AimBot. */ - public AimBot(ShooterSubsystem shooterSubsystem, SwerveSubsystem swerve, IntexerSubsystem intexer, double speed) { + public AimBot( + ShooterSubsystem shooterSubsystem, + SwerveSubsystem swerve, + IntexerSubsystem intexer, + double speed) { this.shooter = shooterSubsystem; this.speed = speed; this.intexer = intexer; @@ -48,8 +53,8 @@ public void execute() { ChassisSpeeds currentSpeed = swerveSubsystem.getChassisSpeeds(); double offset; - if (Robot.getAlliance()){ - if (pose.getRotation().getRadians() > 0){ + if (Robot.getAlliance()) { + if (pose.getRotation().getRadians() > 0) { offset = -Math.toRadians(180); } else { offset = Math.toRadians(180); @@ -58,8 +63,12 @@ public void execute() { offset = 0; } - double rotationVal = rotationPID.calculate(pose.getRotation().getRadians() + offset, - FiringSolutionsV3.getAngleToMovingTarget(pose.getX(), pose.getY(), FiringSolutionsV3.speakerTargetX, + double rotationVal = rotationPID.calculate( + pose.getRotation().getRadians() + offset, + FiringSolutionsV3.getAngleToMovingTarget( + pose.getX(), + pose.getY(), + FiringSolutionsV3.speakerTargetX, FiringSolutionsV3.speakerTargetY, currentSpeed.vxMetersPerSecond, currentSpeed.vyMetersPerSecond, diff --git a/src/main/java/frc/robot/commands/ElevationManual.java b/src/main/java/frc/robot/commands/ElevationManual.java index 4f6b621..57bda1e 100644 --- a/src/main/java/frc/robot/commands/ElevationManual.java +++ b/src/main/java/frc/robot/commands/ElevationManual.java @@ -4,14 +4,15 @@ package frc.robot.commands; -import java.util.function.DoubleSupplier; - import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; + import frc.robot.Constants; import frc.robot.subsystems.ElevatorSubsystem; +import java.util.function.DoubleSupplier; + public class ElevationManual extends Command { private ElevatorSubsystem m_elevatorSubsystem; double m_speed; @@ -28,8 +29,7 @@ public ElevationManual(ElevatorSubsystem elevate, DoubleSupplier control) { // Called when the command is initially scheduled. @Override - public void initialize() { - } + public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override @@ -40,9 +40,9 @@ public void execute() { value = MathUtil.applyDeadband(value, Constants.stickDeadband); value = Math.pow(value, 3); -// if (Math.abs(value) > .1) { // Crime zone - m_elevatorSubsystem.ManSpin(value); -/* } else { + // if (Math.abs(value) > .1) { // Crime zone + m_elevatorSubsystem.ManSpin(value); + /* } else { if (!m_elevatorSubsystem.locked){ lastElevatorSetpoint = m_elevatorSubsystem.getPosition(); m_elevatorSubsystem.setPositionWithEncoder(lastElevatorSetpoint); @@ -55,8 +55,7 @@ public void execute() { // Called once the command ends or is interrupted. @Override - public void end(boolean interrupted) { - } + public void end(boolean interrupted) {} // Returns true when the command should end. @Override diff --git a/src/main/java/frc/robot/commands/ElevatorSet.java b/src/main/java/frc/robot/commands/ElevatorSet.java index 030d497..5a8f0a0 100644 --- a/src/main/java/frc/robot/commands/ElevatorSet.java +++ b/src/main/java/frc/robot/commands/ElevatorSet.java @@ -5,6 +5,7 @@ package frc.robot.commands; import edu.wpi.first.wpilibj2.command.Command; + import frc.robot.subsystems.ElevatorSubsystem; public class ElevatorSet extends Command { @@ -21,7 +22,7 @@ public ElevatorSet(ElevatorSubsystem elevator, double position) { // Called when the command is initially scheduled. @Override public void initialize() { - //m_elevator.setManualOverride(true); + // m_elevator.setManualOverride(true); } // Called every time the scheduler runs while the command is scheduled. @@ -32,8 +33,7 @@ public void execute() { // Called once the command ends or is interrupted. @Override - public void end(boolean interrupted) { - } + public void end(boolean interrupted) {} // Returns true when the command should end. @Override diff --git a/src/main/java/frc/robot/commands/FIREEE.java b/src/main/java/frc/robot/commands/FIREEE.java index 0d30b5c..641d227 100644 --- a/src/main/java/frc/robot/commands/FIREEE.java +++ b/src/main/java/frc/robot/commands/FIREEE.java @@ -5,6 +5,7 @@ package frc.robot.commands; import edu.wpi.first.wpilibj2.command.Command; + import frc.robot.subsystems.IntexerSubsystem; import frc.robot.subsystems.ShooterSubsystem; @@ -21,13 +22,12 @@ public FIREEE(ShooterSubsystem shooterSub, IntexerSubsystem intex) { // Called when the command is initially scheduled. @Override - public void initialize() { - } + public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if (shooter.isShooterAtSpeed()){ + if (shooter.isShooterAtSpeed()) { intexer.setShooterIntake(.9); } } diff --git a/src/main/java/frc/robot/commands/FIREEFORACERTAINAMOUNTOFTIME.java b/src/main/java/frc/robot/commands/FIREEFORACERTAINAMOUNTOFTIME.java index bc1a38f..8f50a4b 100644 --- a/src/main/java/frc/robot/commands/FIREEFORACERTAINAMOUNTOFTIME.java +++ b/src/main/java/frc/robot/commands/FIREEFORACERTAINAMOUNTOFTIME.java @@ -6,6 +6,7 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; + import frc.robot.subsystems.IntexerSubsystem; import frc.robot.subsystems.ShooterSubsystem; @@ -15,7 +16,8 @@ public class FIREEFORACERTAINAMOUNTOFTIME extends Command { private double time; private Timer timer = new Timer(); - public FIREEFORACERTAINAMOUNTOFTIME(ShooterSubsystem shooterSub, IntexerSubsystem intex, double time) { + public FIREEFORACERTAINAMOUNTOFTIME( + ShooterSubsystem shooterSub, IntexerSubsystem intex, double time) { shooter = shooterSub; intexer = intex; this.time = time; diff --git a/src/main/java/frc/robot/commands/IntakeThroughShooter.java b/src/main/java/frc/robot/commands/IntakeThroughShooter.java index 768e6be..bcefdb8 100644 --- a/src/main/java/frc/robot/commands/IntakeThroughShooter.java +++ b/src/main/java/frc/robot/commands/IntakeThroughShooter.java @@ -7,6 +7,7 @@ import edu.wpi.first.wpilibj.GenericHID.RumbleType; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj2.command.Command; + import frc.robot.Constants; import frc.robot.subsystems.IntexerSubsystem; import frc.robot.subsystems.ShooterSubsystem; @@ -18,7 +19,8 @@ public class IntakeThroughShooter extends Command { Joystick controller; /** Creates a new IntakeFromShooter. */ - public IntakeThroughShooter(ShooterSubsystem shooterSub, IntexerSubsystem intex, Joystick controller) { + public IntakeThroughShooter( + ShooterSubsystem shooterSub, IntexerSubsystem intex, Joystick controller) { shooter = shooterSub; intexer = intex; this.controller = controller; @@ -46,7 +48,7 @@ public void end(boolean interrupted) { shooter.setWristByAngle(Constants.Shooter.intakeAngleRadians); intexer.setALL(0); } - + // Returns true when the command should end. @Override public boolean isFinished() { diff --git a/src/main/java/frc/robot/commands/IntakeThroughShooterPart2.java b/src/main/java/frc/robot/commands/IntakeThroughShooterPart2.java index f289097..42d258a 100644 --- a/src/main/java/frc/robot/commands/IntakeThroughShooterPart2.java +++ b/src/main/java/frc/robot/commands/IntakeThroughShooterPart2.java @@ -8,6 +8,7 @@ import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; + import frc.robot.Constants; import frc.robot.subsystems.IntexerSubsystem; import frc.robot.subsystems.ShooterSubsystem; @@ -21,7 +22,8 @@ public class IntakeThroughShooterPart2 extends Command { public final Timer timer = new Timer(); /** Creates a new IntakeFromShooterPart2. */ - public IntakeThroughShooterPart2(ShooterSubsystem shooterSub, IntexerSubsystem intex, Joystick controller) { + public IntakeThroughShooterPart2( + ShooterSubsystem shooterSub, IntexerSubsystem intex, Joystick controller) { shooter = shooterSub; intexer = intex; this.controller = controller; @@ -37,7 +39,7 @@ public void initialize() { controller.setRumble(RumbleType.kBothRumble, 0); shooter.setWristByAngle(Constants.Shooter.intakeAngleRadians); } - + // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { diff --git a/src/main/java/frc/robot/commands/IntexBestHex.java b/src/main/java/frc/robot/commands/IntexBestHex.java index bdab8c5..8a6e674 100644 --- a/src/main/java/frc/robot/commands/IntexBestHex.java +++ b/src/main/java/frc/robot/commands/IntexBestHex.java @@ -4,9 +4,10 @@ package frc.robot.commands; -import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.GenericHID.RumbleType; +import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj2.command.Command; + import frc.lib.math.FiringSolutionsV3; import frc.robot.Constants; import frc.robot.subsystems.IntexerSubsystem; @@ -37,7 +38,8 @@ public void execute() { if (in) { // Hood logic to run forwards or backwards if (intexer.intakeBreak() && !intexer.shooterBreak()) { // If note is not at shooter yet intexer.setALL(Constants.Intake.noteInsideSpeed); - } else if (!intexer.intakeBreak() && intexer.shooterBreak()) { // Stop note if at shooter + } else if (!intexer.intakeBreak() + && intexer.shooterBreak()) { // Stop note if at shooter controller.setRumble(RumbleType.kBothRumble, 0.75); intexer.setALL(0); } else { // Note is not in robot @@ -46,7 +48,6 @@ public void execute() { } else { // Outtake intexer.setALL(Constants.Intake.outakeSpeed); } - } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/commands/IntexForAutosByAutos.java b/src/main/java/frc/robot/commands/IntexForAutosByAutos.java index a7544b1..67889da 100644 --- a/src/main/java/frc/robot/commands/IntexForAutosByAutos.java +++ b/src/main/java/frc/robot/commands/IntexForAutosByAutos.java @@ -5,6 +5,7 @@ package frc.robot.commands; import edu.wpi.first.wpilibj2.command.Command; + import frc.robot.Constants; import frc.robot.subsystems.IntexerSubsystem; import frc.robot.subsystems.ShooterSubsystem; @@ -23,8 +24,7 @@ public IntexForAutosByAutos(IntexerSubsystem intexerSub, ShooterSubsystem shoote // Called when the command is initially scheduled. @Override - public void initialize() { - } + public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override diff --git a/src/main/java/frc/robot/commands/ManRizzt.java b/src/main/java/frc/robot/commands/ManRizzt.java index 731ccd4..1e44b33 100644 --- a/src/main/java/frc/robot/commands/ManRizzt.java +++ b/src/main/java/frc/robot/commands/ManRizzt.java @@ -4,15 +4,16 @@ package frc.robot.commands; -import java.util.function.BooleanSupplier; -import java.util.function.DoubleSupplier; - import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; + import frc.robot.Constants; import frc.robot.subsystems.ShooterSubsystem; +import java.util.function.BooleanSupplier; +import java.util.function.DoubleSupplier; + public class ManRizzt extends Command { ShooterSubsystem m_shooterSubsystem; @@ -32,8 +33,7 @@ public ManRizzt(ShooterSubsystem subsystem, DoubleSupplier speed, BooleanSupplie // Called when the command is initially scheduled. @Override - public void initialize() { - } + public void initialize() {} @Override public void execute() { @@ -41,15 +41,16 @@ public void execute() { speedValue = Math.pow(speedValue, 3); if (setAngle.getAsBoolean()) { - //m_shooterSubsystem.setWristByAngle(SmartDashboard.getNumber("Set Wrist Angle", 0)); + // m_shooterSubsystem.setWristByAngle(SmartDashboard.getNumber("Set Wrist Angle", 0)); } else { if (Math.abs(speedValue) > .0) { wristIsLocked = false; m_shooterSubsystem.setWristSpeedManual(speedValue); } else { - if (m_shooterSubsystem.isZeroed){ - if (!wristIsLocked){ - m_shooterSubsystem.setWristByAngle(m_shooterSubsystem.getCurrentShooterAngle()); + if (m_shooterSubsystem.isZeroed) { + if (!wristIsLocked) { + m_shooterSubsystem.setWristByAngle( + m_shooterSubsystem.getCurrentShooterAngle()); wristIsLocked = true; } } else { @@ -66,6 +67,6 @@ public boolean isFinished() { @Override public void end(boolean interrupted) { - //m_shooterSubsystem.setWristSpeedManual(0); + // m_shooterSubsystem.setWristSpeedManual(0); } } diff --git a/src/main/java/frc/robot/commands/MissileLock.java b/src/main/java/frc/robot/commands/MissileLock.java index 52fe9e7..6063198 100644 --- a/src/main/java/frc/robot/commands/MissileLock.java +++ b/src/main/java/frc/robot/commands/MissileLock.java @@ -5,6 +5,7 @@ package frc.robot.commands; import edu.wpi.first.wpilibj2.command.Command; + import frc.lib.math.FiringSolutionsV3; import frc.robot.Constants; import frc.robot.subsystems.ShooterSubsystem; @@ -24,21 +25,23 @@ public MissileLock(ShooterSubsystem shooterSub, String target) { // Called when the command is initially scheduled. @Override - public void initialize() { - } + public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { if (target == "amp") { if (shooter.outsideAllianceWing) { - shooter.PointShoot(Math.toRadians(55), + shooter.PointShoot( + Math.toRadians(55), FiringSolutionsV3.convertToRPM(shooter.getCalculatedVelocity())); } else { - shooter.setShooterVelocity(FiringSolutionsV3.convertToRPM(shooter.getCalculatedVelocity())); + shooter.setShooterVelocity( + FiringSolutionsV3.convertToRPM(shooter.getCalculatedVelocity())); } } else { - shooter.PointShoot(shooter.getCalculatedAngleToSpeaker(), + shooter.PointShoot( + shooter.getCalculatedAngleToSpeaker(), FiringSolutionsV3.convertToRPM(shooter.getCalculatedVelocity())); } } @@ -46,8 +49,8 @@ public void execute() { // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - //shooter.setShooterVelocity(Constants.Shooter.idleSpeedRPM); - if (target != "amp" || shooter.outsideAllianceWing){ + // shooter.setShooterVelocity(Constants.Shooter.idleSpeedRPM); + if (target != "amp" || shooter.outsideAllianceWing) { shooter.setWristByAngle(Constants.Shooter.intakeAngleRadians); } } diff --git a/src/main/java/frc/robot/commands/NoteSniffer.java b/src/main/java/frc/robot/commands/NoteSniffer.java index 12b3146..ba2b011 100644 --- a/src/main/java/frc/robot/commands/NoteSniffer.java +++ b/src/main/java/frc/robot/commands/NoteSniffer.java @@ -4,14 +4,13 @@ package frc.robot.commands; -import org.photonvision.targeting.PhotonPipelineResult; - import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; + import frc.robot.Constants; import frc.robot.Robot; import frc.robot.subsystems.IntexerSubsystem; @@ -19,6 +18,8 @@ import frc.robot.subsystems.SwerveSubsystem; import frc.robot.subsystems.VisionSubsystem; +import org.photonvision.targeting.PhotonPipelineResult; + public class NoteSniffer extends Command { private SwerveSubsystem swerveSubsystem; @@ -31,7 +32,10 @@ public class NoteSniffer extends Command { private double translationVal = .35; /** Creates a new IntakeWithVision. */ - public NoteSniffer(SwerveSubsystem swerve, VisionSubsystem vision, IntexerSubsystem intexer, + public NoteSniffer( + SwerveSubsystem swerve, + VisionSubsystem vision, + IntexerSubsystem intexer, ShooterSubsystem shooter) { this.swerveSubsystem = swerve; this.vision = vision; @@ -56,7 +60,9 @@ public void execute() { PhotonPipelineResult result = vision.getLatestResultN(); double rotationVal; - boolean overMidfield = Robot.getAlliance() ? (16.54 - swerveSubsystem.getPose().getX()) > 8.3 : swerveSubsystem.getPose().getX() > 8.3; + boolean overMidfield = Robot.getAlliance() + ? (16.54 - swerveSubsystem.getPose().getX()) > 8.3 + : swerveSubsystem.getPose().getX() > 8.3; if (intexer.intakeBreak() || overMidfield) { noteInside = true; @@ -70,7 +76,8 @@ public void execute() { SmartDashboard.putNumber("Note Yaw", yawToNote); - rotationVal = rotationPID.calculate(yawToNote, swerveSubsystem.getGyroYaw().getRadians()); + rotationVal = rotationPID.calculate( + yawToNote, swerveSubsystem.getGyroYaw().getRadians()); intexer.setFrontIntake(Constants.Intake.noteOutsideSpeed); intexer.setShooterIntake(Constants.Intake.noteInsideSpeed); @@ -78,12 +85,12 @@ public void execute() { rotationVal = 0; } - //if (shooter.getDistanceToSpeaker() < 2.5){ - swerveSubsystem.drive( - new Translation2d(translationVal, 0).times(Constants.Swerve.maxSpeed), - rotationVal * Constants.Swerve.maxAngularVelocity, - false, - false); + // if (shooter.getDistanceToSpeaker() < 2.5){ + swerveSubsystem.drive( + new Translation2d(translationVal, 0).times(Constants.Swerve.maxSpeed), + rotationVal * Constants.Swerve.maxAngularVelocity, + false, + false); /* } else { intexer.setALL(-.5); swerveSubsystem.drive( diff --git a/src/main/java/frc/robot/commands/ResetNoteInShooter.java b/src/main/java/frc/robot/commands/ResetNoteInShooter.java index d937692..4fb5633 100644 --- a/src/main/java/frc/robot/commands/ResetNoteInShooter.java +++ b/src/main/java/frc/robot/commands/ResetNoteInShooter.java @@ -7,6 +7,7 @@ import edu.wpi.first.wpilibj.GenericHID.RumbleType; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj2.command.Command; + import frc.robot.Constants; import frc.robot.subsystems.IntexerSubsystem; import frc.robot.subsystems.ShooterSubsystem; @@ -18,7 +19,8 @@ public class ResetNoteInShooter extends Command { Joystick controller; /** Creates a new IntakeFromShooter. */ - public ResetNoteInShooter(ShooterSubsystem shooterSub, IntexerSubsystem intex, Joystick controller) { + public ResetNoteInShooter( + ShooterSubsystem shooterSub, IntexerSubsystem intex, Joystick controller) { shooter = shooterSub; intexer = intex; this.controller = controller; @@ -44,7 +46,7 @@ public void end(boolean interrupted) { shooter.setWristByAngle(Constants.Shooter.intakeAngleRadians); intexer.setALL(0); } - + // Returns true when the command should end. @Override public boolean isFinished() { diff --git a/src/main/java/frc/robot/commands/ResetNoteInShooterPart2.java b/src/main/java/frc/robot/commands/ResetNoteInShooterPart2.java index b308b5e..67a6cd5 100644 --- a/src/main/java/frc/robot/commands/ResetNoteInShooterPart2.java +++ b/src/main/java/frc/robot/commands/ResetNoteInShooterPart2.java @@ -8,6 +8,7 @@ import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; + import frc.robot.Constants; import frc.robot.subsystems.IntexerSubsystem; import frc.robot.subsystems.ShooterSubsystem; @@ -20,7 +21,8 @@ public class ResetNoteInShooterPart2 extends Command { public final Timer timer = new Timer(); /** Creates a new IntakeFromShooterPart2. */ - public ResetNoteInShooterPart2(ShooterSubsystem shooterSub, IntexerSubsystem intex, Joystick controller) { + public ResetNoteInShooterPart2( + ShooterSubsystem shooterSub, IntexerSubsystem intex, Joystick controller) { shooter = shooterSub; intexer = intex; this.controller = controller; @@ -36,7 +38,7 @@ public void initialize() { controller.setRumble(RumbleType.kBothRumble, 0); shooter.setWristByAngle(Constants.Shooter.intakeAngleRadians); } - + // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { diff --git a/src/main/java/frc/robot/commands/RizzLevel.java b/src/main/java/frc/robot/commands/RizzLevel.java index 564444e..392b7d0 100644 --- a/src/main/java/frc/robot/commands/RizzLevel.java +++ b/src/main/java/frc/robot/commands/RizzLevel.java @@ -5,6 +5,7 @@ package frc.robot.commands; import edu.wpi.first.wpilibj2.command.Command; + import frc.robot.subsystems.ShooterSubsystem; public class RizzLevel extends Command { @@ -28,13 +29,12 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override - public void execute() { - } + public void execute() {} // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - //shooter.manualWristSpeed(0); + // shooter.manualWristSpeed(0); } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/commands/TeleopSwerve.java b/src/main/java/frc/robot/commands/TeleopSwerve.java index 41292ab..38885e4 100644 --- a/src/main/java/frc/robot/commands/TeleopSwerve.java +++ b/src/main/java/frc/robot/commands/TeleopSwerve.java @@ -1,5 +1,15 @@ package frc.robot.commands; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.GenericHID.RumbleType; +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; + import frc.lib.math.FiringSolutionsV3; import frc.robot.Constants; import frc.robot.Robot; @@ -8,20 +18,10 @@ import frc.robot.subsystems.SwerveSubsystem; import frc.robot.subsystems.VisionSubsystem; -import java.util.function.BooleanSupplier; -import java.util.function.DoubleSupplier; - import org.photonvision.targeting.PhotonPipelineResult; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.wpilibj.Joystick; -import edu.wpi.first.wpilibj.GenericHID.RumbleType; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; +import java.util.function.BooleanSupplier; +import java.util.function.DoubleSupplier; public class TeleopSwerve extends Command { private SwerveSubsystem swerveSubsystem; @@ -40,13 +40,26 @@ public class TeleopSwerve extends Command { private BooleanSupplier intakeOverrideNoMove; private PIDController rotationPID = new PIDController(0.65, 0.00001, 0.04); - + private Joystick controller; - + private boolean noteInside = false; - public TeleopSwerve(SwerveSubsystem swerve, VisionSubsystem vision, ShooterSubsystem shooter, IntexerSubsystem intexer, DoubleSupplier translationSup, DoubleSupplier strafeSup, - DoubleSupplier rotationSup, BooleanSupplier robotCentricSup, BooleanSupplier shooterOverrideAmp, BooleanSupplier shooterOverrideSpeaker, BooleanSupplier intake, BooleanSupplier intakeNoMove, Joystick controller) { + public TeleopSwerve( + SwerveSubsystem swerve, + VisionSubsystem vision, + ShooterSubsystem shooter, + IntexerSubsystem intexer, + DoubleSupplier translationSup, + DoubleSupplier strafeSup, + DoubleSupplier rotationSup, + BooleanSupplier robotCentricSup, + BooleanSupplier shooterOverrideAmp, + BooleanSupplier shooterOverrideSpeaker, + BooleanSupplier intake, + BooleanSupplier intakeNoMove, + Joystick controller) { + this.swerveSubsystem = swerve; this.vision = vision; this.shooterSubsystem = shooter; @@ -73,16 +86,17 @@ public void execute() { PhotonPipelineResult result = vision.getLatestResultN(); /* Get Values, Deadband */ - double translationVal = MathUtil.applyDeadband(translationSup.getAsDouble(), Constants.stickDeadband); + double translationVal = + MathUtil.applyDeadband(translationSup.getAsDouble(), Constants.stickDeadband); double strafeVal = MathUtil.applyDeadband(strafeSup.getAsDouble(), Constants.stickDeadband); double rotationVal; double offset; double ampLockOffset; - if (Robot.getAlliance()){ + if (Robot.getAlliance()) { ampLockOffset = 16.54 - 5; - if (pose.getRotation().getRadians() > 0){ + if (pose.getRotation().getRadians() > 0) { offset = -Math.toRadians(180); } else { offset = Math.toRadians(180); @@ -97,7 +111,7 @@ public void execute() { strafeVal = Math.copySign(Math.pow(strafeVal, 2), strafeVal); if (shooterOverrideSpeaker.getAsBoolean()) { // Lock robot angle to speaker - if (shooterSubsystem.getDistanceToSpeakerWhileMoving() >= 3.5){ + if (shooterSubsystem.getDistanceToSpeakerWhileMoving() >= 3.5) { controller.setRumble(RumbleType.kBothRumble, 0.5); } else { controller.setRumble(RumbleType.kBothRumble, 0); @@ -105,8 +119,13 @@ public void execute() { ChassisSpeeds currentSpeed = swerveSubsystem.getChassisSpeeds(); - rotationVal = rotationPID.calculate(pose.getRotation().getRadians() + offset, - FiringSolutionsV3.getAngleToMovingTarget(pose.getX(), pose.getY(), FiringSolutionsV3.speakerTargetX, FiringSolutionsV3.speakerTargetY, + rotationVal = rotationPID.calculate( + pose.getRotation().getRadians() + offset, + FiringSolutionsV3.getAngleToMovingTarget( + pose.getX(), + pose.getY(), + FiringSolutionsV3.speakerTargetX, + FiringSolutionsV3.speakerTargetY, currentSpeed.vxMetersPerSecond, currentSpeed.vyMetersPerSecond, pose.getRotation().getRadians())); @@ -116,42 +135,59 @@ public void execute() { ChassisSpeeds currentSpeed = swerveSubsystem.getChassisSpeeds(); openLoop = false; - if ((Robot.getAlliance() && pose.getX() < 16.54 - 5) ^ (!Robot.getAlliance() && pose.getX() > 5)) { - rotationVal = rotationPID.calculate(pose.getRotation().getRadians() + offset, - FiringSolutionsV3.getAngleToMovingTarget(pose.getX(), pose.getY(), FiringSolutionsV3.ampTargetX, FiringSolutionsV3.ampTargetY, + if ((Robot.getAlliance() && pose.getX() < 16.54 - 5) + ^ (!Robot.getAlliance() && pose.getX() > 5)) { + + rotationVal = rotationPID.calculate( + pose.getRotation().getRadians() + offset, + FiringSolutionsV3.getAngleToMovingTarget( + pose.getX(), + pose.getY(), + FiringSolutionsV3.ampTargetX, + FiringSolutionsV3.ampTargetY, currentSpeed.vxMetersPerSecond, currentSpeed.vyMetersPerSecond, pose.getRotation().getRadians())); } else { - rotationVal = rotationPID.calculate(pose.getRotation().getRadians(), Math.toRadians(-90)); + rotationVal = + rotationPID.calculate(pose.getRotation().getRadians(), Math.toRadians(-90)); } - } else if (intakeOverride.getAsBoolean() && result.hasTargets() && !noteInside) { // Lock robot towards detected note - double yawToNote = Math.toRadians(result.getBestTarget().getYaw()) + swerveSubsystem.getGyroYaw().getRadians(); + } else if (intakeOverride.getAsBoolean() + && result.hasTargets() + && !noteInside) { // Lock robot towards detected note + + double yawToNote = Math.toRadians(result.getBestTarget().getYaw()) + + swerveSubsystem.getGyroYaw().getRadians(); SmartDashboard.putNumber("Note Yaw", yawToNote); - openLoop = false; - rotationVal = rotationPID.calculate(yawToNote, swerveSubsystem.getGyroYaw().getRadians()); + rotationVal = rotationPID.calculate( + yawToNote, swerveSubsystem.getGyroYaw().getRadians()); + translationVal = 0.35; robotCentric = true; + openLoop = false; } else if (intakeOverrideNoMove.getAsBoolean() && result.hasTargets() && !noteInside) { - double yawToNote = Math.toRadians(result.getBestTarget().getYaw()) + swerveSubsystem.getGyroYaw().getRadians(); + double yawToNote = Math.toRadians(result.getBestTarget().getYaw()) + + swerveSubsystem.getGyroYaw().getRadians(); SmartDashboard.putNumber("Note Yaw", yawToNote); openLoop = false; - rotationVal = rotationPID.calculate(yawToNote, swerveSubsystem.getGyroYaw().getRadians()); + rotationVal = rotationPID.calculate( + yawToNote, swerveSubsystem.getGyroYaw().getRadians()); } else { - rotationVal = MathUtil.applyDeadband(rotationSup.getAsDouble(), Constants.stickDeadband); + rotationVal = + MathUtil.applyDeadband(rotationSup.getAsDouble(), Constants.stickDeadband); rotationVal = Math.copySign(Math.pow(rotationVal, 2), rotationVal); controller.setRumble(RumbleType.kBothRumble, 0); noteInside = false; } - if (Robot.getAlliance() && !robotCentric){ // Invert field oriented for always blue origin + if (Robot.getAlliance() && !robotCentric) { // Invert field oriented for always blue origin translationVal = -translationVal; strafeVal = -strafeVal; } @@ -169,4 +205,4 @@ public void execute() { !robotCentric, openLoop); } -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/commands/ToBreakOrNotToBreak.java b/src/main/java/frc/robot/commands/ToBreakOrNotToBreak.java index 193bb39..ec1f75d 100644 --- a/src/main/java/frc/robot/commands/ToBreakOrNotToBreak.java +++ b/src/main/java/frc/robot/commands/ToBreakOrNotToBreak.java @@ -5,6 +5,7 @@ package frc.robot.commands; import edu.wpi.first.wpilibj2.command.Command; + import frc.robot.subsystems.IntexerSubsystem; import frc.robot.subsystems.LEDSubsystem; @@ -21,13 +22,12 @@ public ToBreakOrNotToBreak(IntexerSubsystem intexer) { // Called when the command is initially scheduled. @Override - public void initialize() { - } + public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if (!intexer.shooterBreak()){ + if (!intexer.shooterBreak()) { intexer.setShooterIntake(.5); } else { intexer.setShooterIntake(0); diff --git a/src/main/java/frc/robot/commands/ZeroRizz.java b/src/main/java/frc/robot/commands/ZeroRizz.java index ff01532..6dcd871 100644 --- a/src/main/java/frc/robot/commands/ZeroRizz.java +++ b/src/main/java/frc/robot/commands/ZeroRizz.java @@ -6,6 +6,7 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; + import frc.robot.Constants; import frc.robot.subsystems.ShooterSubsystem; diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index aa8e431..6028b08 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -4,6 +4,10 @@ package frc.robot.subsystems; +import au.grapplerobotics.ConfigurationFailedException; +import au.grapplerobotics.LaserCan; +import au.grapplerobotics.LaserCan.RangingMode; + import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.controls.PositionDutyCycle; @@ -12,9 +16,6 @@ import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; -import au.grapplerobotics.ConfigurationFailedException; -import au.grapplerobotics.LaserCan; -import au.grapplerobotics.LaserCan.RangingMode; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -91,32 +92,36 @@ public void periodic() { SmartDashboard.putNumber("Set Height", setHeight); SmartDashboard.putNumber("LaserCan Meters", getHeightLaserCan()); SmartDashboard.putBoolean("LaserCan failure", lasercanFailureCheck()); - SmartDashboard.putNumber("Elevator Left Supply Current", m_elevatorLeft.getSupplyCurrent().getValueAsDouble()); - SmartDashboard.putNumber("Elevator Right Supply Current", m_elevatorRight.getSupplyCurrent().getValueAsDouble()); + SmartDashboard.putNumber( + "Elevator Left Supply Current", + m_elevatorLeft.getSupplyCurrent().getValueAsDouble()); + SmartDashboard.putNumber( + "Elevator Right Supply Current", + m_elevatorRight.getSupplyCurrent().getValueAsDouble()); SmartDashboard.putNumber("LaserCan Ambient", measurement != null ? measurement.ambient : 0); revolutionCount = m_elevatorLeft.getPosition().getValueAsDouble(); - - //FiringSolutionsV3.updateHeight(getHeight()); //TODO: test this + + // FiringSolutionsV3.updateHeight(getHeight()); //TODO: test this } - public void setElevatorSpeedManual(double value){ + public void setElevatorSpeedManual(double value) { m_elevatorLeft.set(value); } - public void setManualOverride(boolean value){ + public void setManualOverride(boolean value) { manualOverride = value; } - public double getEncoderValue(){ + public double getEncoderValue() { return revolutionCount; } - public void setPositionWithEncoder(double value){ + public void setPositionWithEncoder(double value) { locked = true; m_elevatorLeft.setControl(lockPosition.withPosition(value)); } - public void stopHere(){ + public void stopHere() { locked = true; m_elevatorLeft.setControl(lockPosition.withPosition(revolutionCount).withSlot(0)); } @@ -134,7 +139,7 @@ public void setHeight(double height) { } /** Get height IN METERS. Will run off LaserCan but will switch to encoder if it fails */ - public double getHeight(){ + public double getHeight() { if (!lasercanFailureCheck()) { // Run off LaserCan return getHeightLaserCan(); } else { // Run off encoder @@ -176,12 +181,11 @@ public boolean lasercanFailureCheck() { } } - public double getSetpoint(){ + public double getSetpoint() { return m_elevatorLeft.getClosedLoopReference().getValue(); } - public double getPosition(){ + public double getPosition() { return m_elevatorLeft.getPosition().getValueAsDouble(); } - } diff --git a/src/main/java/frc/robot/subsystems/IntexerSubsystem.java b/src/main/java/frc/robot/subsystems/IntexerSubsystem.java index d1c9217..f85cad4 100644 --- a/src/main/java/frc/robot/subsystems/IntexerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntexerSubsystem.java @@ -4,13 +4,14 @@ package frc.robot.subsystems; -import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; import com.revrobotics.CANSparkBase; -import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkBase.IdleMode; import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.CANSparkMax; + +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; public class IntexerSubsystem extends SubsystemBase { @@ -20,7 +21,6 @@ public class IntexerSubsystem extends SubsystemBase { private CANSparkBase shooterIntake; public boolean intakeThroughShooterPart2isReady = false; public boolean resetNoteInShooterPart2isReady = false; - /** Front Intake */ private DigitalInput breakingBeam; @@ -58,7 +58,7 @@ public void setALL(double speed) { shooterIntake.set(speed); } } - + public void setFrontIntake(double speed) { if (speed == 0) { left.stopMotor(); @@ -70,7 +70,7 @@ public void setFrontIntake(double speed) { } public void setShooterIntake(double speed) { - if (speed == 0){ + if (speed == 0) { shooterIntake.stopMotor(); } else { shooterIntake.set(speed); @@ -85,11 +85,11 @@ public boolean shooterBreak() { return !beamKamen.get(); } - public void setIntakeThroughShooterPart2Status(boolean value){ + public void setIntakeThroughShooterPart2Status(boolean value) { intakeThroughShooterPart2isReady = value; } - public void resetNoteInShooterPart2Status(boolean value){ + public void resetNoteInShooterPart2Status(boolean value) { resetNoteInShooterPart2isReady = value; } diff --git a/src/main/java/frc/robot/subsystems/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/LEDSubsystem.java index 01c4693..5caa236 100644 --- a/src/main/java/frc/robot/subsystems/LEDSubsystem.java +++ b/src/main/java/frc/robot/subsystems/LEDSubsystem.java @@ -7,6 +7,7 @@ import edu.wpi.first.wpilibj.DigitalOutput; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.SubsystemBase; + import frc.robot.Constants; import frc.robot.Robot; @@ -17,7 +18,8 @@ public class LEDSubsystem extends SubsystemBase { public DigitalOutput SkylerBright = new DigitalOutput(6); // Bit 4 (8) // Output bits to the LEDs - public DigitalOutput[] bits = { WalterLight, JesseBlinkman, GusBling, SkylerBright }; // Actual Outputs + public DigitalOutput[] bits = {WalterLight, JesseBlinkman, GusBling, SkylerBright + }; // Actual Outputs boolean[] output = new boolean[4]; // Computed Outputs // Booleans used for Easy input @@ -26,19 +28,19 @@ public class LEDSubsystem extends SubsystemBase { private Boolean atSpeed = false; public Boolean[] inputBooleans = { - false, // Amp -0 Rainbow Pattern #1 - false, // Source -1 Rainbow Pattern #2 - false, // Climb -2 Rainbow Pattern #3 - false, // Blank/DC -3 None - false, // Note Detected -4 White Solid - false, // Charging -5 Green Pulse (HasNote) - false, // At Speed -6 Green BLink (HasNote) - false, // Charging -7 Magenta Pulse (NoNote) - false, // At Speed -8 Magenta BLink (NoNote) - false, // Note in Intake -9 Orange Blink - false, // Note in Shooter -10 Orange Solid - false, // Alliance Color -11 Red Pulse - false // Alliance Color -12 Blue Pulse + false, // Amp -0 Rainbow Pattern #1 + false, // Source -1 Rainbow Pattern #2 + false, // Climb -2 Rainbow Pattern #3 + false, // Blank/DC -3 None + false, // Note Detected -4 White Solid + false, // Charging -5 Green Pulse (HasNote) + false, // At Speed -6 Green BLink (HasNote) + false, // Charging -7 Magenta Pulse (NoNote) + false, // At Speed -8 Magenta BLink (NoNote) + false, // Note in Intake -9 Orange Blink + false, // Note in Shooter -10 Orange Solid + false, // Alliance Color -11 Red Pulse + false // Alliance Color -12 Blue Pulse }; // Use subsystems @@ -48,8 +50,11 @@ public class LEDSubsystem extends SubsystemBase { SwerveSubsystem swerve; /** Creates a new LEDSubsystem. */ - public LEDSubsystem(VisionSubsystem m_VisionSubsystem, ShooterSubsystem m_ShooterSubsystem, - IntexerSubsystem m_IntexerSubsystem, SwerveSubsystem m_SwerveSubsystem) { + public LEDSubsystem( + VisionSubsystem m_VisionSubsystem, + ShooterSubsystem m_ShooterSubsystem, + IntexerSubsystem m_IntexerSubsystem, + SwerveSubsystem m_SwerveSubsystem) { this.vision = m_VisionSubsystem; this.shooter = m_ShooterSubsystem; this.intexer = m_IntexerSubsystem; @@ -163,7 +168,9 @@ private void encoder() { // Transition phase int trueIndex = 0; // Index of selected LED sequence - for (int i = 0; i < inputBooleans.length; i++) { // Picks the first true sequence based on priority + for (int i = 0; + i < inputBooleans.length; + i++) { // Picks the first true sequence based on priority if (inputBooleans[i]) { trueIndex = i; break; diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 60d175f..8aa7656 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -4,26 +4,26 @@ package frc.robot.subsystems; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.lib.math.FiringSolutionsV3; -import frc.lib.math.Interpolations; -import frc.robot.Constants; -import frc.robot.Robot; - import com.revrobotics.CANSparkBase; -import com.revrobotics.RelativeEncoder; -import com.revrobotics.SparkPIDController; import com.revrobotics.CANSparkBase.IdleMode; import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkMax; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.SparkPIDController; -import edu.wpi.first.wpilibj.DutyCycleEncoder; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.*; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.DutyCycleEncoder; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.*; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +import frc.lib.math.FiringSolutionsV3; +import frc.lib.math.Interpolations; +import frc.robot.Constants; +import frc.robot.Robot; public class ShooterSubsystem extends SubsystemBase { @@ -54,12 +54,14 @@ public class ShooterSubsystem extends SubsystemBase { // Vars /** In m/s */ private double shooterVelocity = 12.1; + private double shooterAngleToSpeaker, shooterAngleToAmp; private boolean ENCFAIL = false; public boolean isZeroed = false; public boolean wristIsLocked = false; /** IN RADIANS */ private double angleOffset = Constants.Shooter.angleOffsetManual; + private double distanceToMovingSpeakerTarget = .94; public double lastWristAngleSetpoint = 0.0; public boolean manualOverride = false; @@ -140,19 +142,19 @@ public ShooterSubsystem(SwerveSubsystem swerve, ElevatorSubsystem elevator) { @Override public void periodic() { // This method will be called once per scheduler run - /* + /* if (velocityP != SmartDashboard.getNumber("Velo P", velocityP)){ velocityP = SmartDashboard.getNumber("Velo P", velocityP); topPID.setP(velocityP, 0); botPID.setP(velocityP, 0); } - + if (velocityI != SmartDashboard.getNumber("Velo I", velocityI)){ velocityI = SmartDashboard.getNumber("Velo I", velocityI); topPID.setI(velocityI, 0); botPID.setI(velocityI, 0); } - + if (velocityD != SmartDashboard.getNumber("Velo D", velocityD)){ velocityD = SmartDashboard.getNumber("Velo D", velocityD); topPID.setD(velocityD, 0); @@ -161,15 +163,19 @@ public void periodic() { */ shooterVelocity = SmartDashboard.getNumber("set velocity", shooterVelocity); - FiringSolutionsV3.slipPercent = SmartDashboard.getNumber("Set Slip Offset", FiringSolutionsV3.slipPercent); - FiringSolutionsV3.speakerTargetZ = SmartDashboard.getNumber("Set Target Z", FiringSolutionsV3.speakerTargetZ); + FiringSolutionsV3.slipPercent = + SmartDashboard.getNumber("Set Slip Offset", FiringSolutionsV3.slipPercent); + FiringSolutionsV3.speakerTargetZ = + SmartDashboard.getNumber("Set Target Z", FiringSolutionsV3.speakerTargetZ); - SmartDashboard.putNumber("Top - Bottom error", + SmartDashboard.putNumber( + "Top - Bottom error", m_VelocityEncoder.getVelocity() - m_VelocityEncoder2.getVelocity()); SmartDashboard.putNumber("Current Angle Radians", getCurrentShooterAngle()); SmartDashboard.putNumber("Current Velocity", getVelocity()); SmartDashboard.putBoolean("shooter at speed", isShooterAtSpeed()); - SmartDashboard.putNumber("Current Angle Degrees", Units.radiansToDegrees(getCurrentShooterAngle())); + SmartDashboard.putNumber( + "Current Angle Degrees", Units.radiansToDegrees(getCurrentShooterAngle())); // check for encoder failure if (m_WristEncoder.isConnected()) { @@ -202,7 +208,6 @@ public void periodic() { * } */ } - } /** in RADIANs units MATTER */ @@ -227,8 +232,8 @@ public double getCalculatedVelocity() { } public double getDistanceTo(double x, double y) { - return FiringSolutionsV3.getDistanceToTarget(swerveSubsystem.getPose().getX(), swerveSubsystem.getPose().getY(), - x, y); + return FiringSolutionsV3.getDistanceToTarget( + swerveSubsystem.getPose().getX(), swerveSubsystem.getPose().getY(), x, y); } public double getDistanceToSpeakerWhileMoving() { @@ -276,7 +281,6 @@ public void PointShoot(double PointAngle, double launchVelocity) { topPID.setReference(launchVelocity, CANSparkMax.ControlType.kVelocity); } - public void resetWristEncoders(double newOffset) { angleOffset = newOffset; m_WristEncoder.reset(); @@ -300,9 +304,11 @@ public void setOffsetVelocity(double velocity) { shootaBot.stopMotor(); shootaTop.stopMotor(); } else { - botPID.setReference(velocity - SmartDashboard.getNumber("Top Bottom Offset", 400), + botPID.setReference( + velocity - SmartDashboard.getNumber("Top Bottom Offset", 400), CANSparkMax.ControlType.kVelocity); - topPID.setReference(velocity + SmartDashboard.getNumber("Top Bottom Offset", 400), + topPID.setReference( + velocity + SmartDashboard.getNumber("Top Bottom Offset", 400), CANSparkMax.ControlType.kVelocity); } } @@ -347,7 +353,7 @@ public void setWristAngleUpperBound(double wristAngleUpperBound) { public void setWristByAngle(double angle) { manualOverride = false; if (isZeroed) { - //manualOverride = false; + // manualOverride = false; updateWristAngleSetpoint(angle); } } @@ -364,10 +370,11 @@ public void setWristSpeedManual(double speed) { m_Wrist.set(speed); } - public void setWristToBrake(){ + public void setWristToBrake() { m_Wrist.setIdleMode(IdleMode.kBrake); } - public void setWristToCoast(){ + + public void setWristToCoast() { m_Wrist.setIdleMode(IdleMode.kCoast); } @@ -382,45 +389,70 @@ public void updateShooterMath() { // Shooter Math Pose2d pose = swerveSubsystem.getPose(); ChassisSpeeds chassisSpeeds = swerveSubsystem.getChassisSpeeds(); - distanceToMovingSpeakerTarget = FiringSolutionsV3.getDistanceToMovingTarget(pose.getX(), pose.getY(), - FiringSolutionsV3.speakerTargetX, FiringSolutionsV3.speakerTargetY, - chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond, pose.getRotation().getRadians()); + distanceToMovingSpeakerTarget = FiringSolutionsV3.getDistanceToMovingTarget( + pose.getX(), + pose.getY(), + FiringSolutionsV3.speakerTargetX, + FiringSolutionsV3.speakerTargetY, + chassisSpeeds.vxMetersPerSecond, + chassisSpeeds.vyMetersPerSecond, + pose.getRotation().getRadians()); FiringSolutionsV3.updateSpeakerR(distanceToMovingSpeakerTarget); // shooterAngleToSpeaker = FiringSolutionsV3.getShooterAngleFromSpeakerR(); - if (elevatorSubsystem.getHeight() > 0.3){ - shooterAngleToSpeaker = Math.toRadians(interpolation.getShooterAngleFromInterpolationElevatorUp(distanceToMovingSpeakerTarget)); + if (elevatorSubsystem.getHeight() > 0.3) { + shooterAngleToSpeaker = + Math.toRadians(interpolation.getShooterAngleFromInterpolationElevatorUp( + distanceToMovingSpeakerTarget)); } else { - shooterAngleToSpeaker = Math.toRadians(interpolation.getShooterAngleFromInterpolation(distanceToMovingSpeakerTarget)); + shooterAngleToSpeaker = Math.toRadians( + interpolation.getShooterAngleFromInterpolation(distanceToMovingSpeakerTarget)); } - if ((Robot.getAlliance() && pose.getX() < 16.54 - 5) ^ (!Robot.getAlliance() && pose.getX() > 5)) { + if ((Robot.getAlliance() && pose.getX() < 16.54 - 5) + ^ (!Robot.getAlliance() && pose.getX() > 5)) { outsideAllianceWing = true; } else { outsideAllianceWing = false; } - SmartDashboard.putNumber("Target Velocity RPM", FiringSolutionsV3.convertToRPM(shooterVelocity)); + SmartDashboard.putNumber( + "Target Velocity RPM", FiringSolutionsV3.convertToRPM(shooterVelocity)); SmartDashboard.putNumber("Calculated Angle Radians", shooterAngleToSpeaker); SmartDashboard.putNumber("Calculated Angle Degrees", Math.toDegrees(shooterAngleToSpeaker)); - SmartDashboard.putNumber("distance", FiringSolutionsV3.getDistanceToTarget(pose.getX(), pose.getY(), - FiringSolutionsV3.speakerTargetX, FiringSolutionsV3.speakerTargetY)); + SmartDashboard.putNumber( + "distance", + FiringSolutionsV3.getDistanceToTarget( + pose.getX(), + pose.getY(), + FiringSolutionsV3.speakerTargetX, + FiringSolutionsV3.speakerTargetY)); SmartDashboard.putNumber("R", FiringSolutionsV3.getSpeakerR()); SmartDashboard.putNumber("C", FiringSolutionsV3.C(distanceToMovingSpeakerTarget)); - SmartDashboard.putNumber("quarticA", - FiringSolutionsV3.quarticA(distanceToMovingSpeakerTarget, FiringSolutionsV3.speakerTargetZ)); - SmartDashboard.putNumber("quarticC", - FiringSolutionsV3.quarticC(distanceToMovingSpeakerTarget, FiringSolutionsV3.speakerTargetZ)); - SmartDashboard.putNumber("quarticE", FiringSolutionsV3.quarticE(distanceToMovingSpeakerTarget)); + SmartDashboard.putNumber( + "quarticA", + FiringSolutionsV3.quarticA( + distanceToMovingSpeakerTarget, FiringSolutionsV3.speakerTargetZ)); + SmartDashboard.putNumber( + "quarticC", + FiringSolutionsV3.quarticC( + distanceToMovingSpeakerTarget, FiringSolutionsV3.speakerTargetZ)); + SmartDashboard.putNumber( + "quarticE", FiringSolutionsV3.quarticE(distanceToMovingSpeakerTarget)); SmartDashboard.putNumber("Distance to Moving Target", distanceToMovingSpeakerTarget); SmartDashboard.putNumber("robot vx", chassisSpeeds.vxMetersPerSecond); SmartDashboard.putNumber("robot vy", chassisSpeeds.vyMetersPerSecond); - double distanceToMovingAmpTarget = FiringSolutionsV3.getDistanceToMovingTarget(pose.getX(), pose.getY(), - FiringSolutionsV3.ampTargetX, FiringSolutionsV3.ampTargetY, - chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond, pose.getRotation().getRadians()); + double distanceToMovingAmpTarget = FiringSolutionsV3.getDistanceToMovingTarget( + pose.getX(), + pose.getY(), + FiringSolutionsV3.ampTargetX, + FiringSolutionsV3.ampTargetY, + chassisSpeeds.vxMetersPerSecond, + chassisSpeeds.vyMetersPerSecond, + pose.getRotation().getRadians()); FiringSolutionsV3.updateAmpR(distanceToMovingAmpTarget); diff --git a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java index 64756ce..9e79f9c 100644 --- a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java @@ -1,17 +1,9 @@ package frc.robot.subsystems; -import frc.robot.SwerveModule; -import frc.lib.math.FiringSolutionsV3; -import frc.robot.Constants; -import frc.robot.Robot; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.kinematics.SwerveDriveKinematics; -import edu.wpi.first.math.kinematics.SwerveModulePosition; - -import java.util.Optional; -import java.util.function.BooleanSupplier; - -import org.photonvision.EstimatedRobotPose; +import static edu.wpi.first.units.MutableMeasure.mutable; +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.Volts; import com.ctre.phoenix6.configs.Pigeon2Configuration; import com.ctre.phoenix6.hardware.Pigeon2; @@ -26,6 +18,9 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; @@ -48,10 +43,15 @@ import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import static edu.wpi.first.units.Units.Meters; -import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.Volts; -import static edu.wpi.first.units.MutableMeasure.mutable; +import frc.lib.math.FiringSolutionsV3; +import frc.robot.Constants; +import frc.robot.Robot; +import frc.robot.SwerveModule; + +import org.photonvision.EstimatedRobotPose; + +import java.util.Optional; +import java.util.function.BooleanSupplier; public class SwerveSubsystem extends SubsystemBase { @@ -93,48 +93,56 @@ public SwerveSubsystem(VisionSubsystem vision) { // Swerve setup mSwerveMods = new SwerveModule[] { - new SwerveModule(0, Constants.Swerve.Mod0.constants), - new SwerveModule(1, Constants.Swerve.Mod1.constants), - new SwerveModule(2, Constants.Swerve.Mod2.constants), - new SwerveModule(3, Constants.Swerve.Mod3.constants) + new SwerveModule(0, Constants.Swerve.Mod0.constants), + new SwerveModule(1, Constants.Swerve.Mod1.constants), + new SwerveModule(2, Constants.Swerve.Mod2.constants), + new SwerveModule(3, Constants.Swerve.Mod3.constants) }; swerveModuleStates = new SwerveModuleState[] { - new SwerveModuleState(), - new SwerveModuleState(), - new SwerveModuleState(), - new SwerveModuleState() + new SwerveModuleState(), + new SwerveModuleState(), + new SwerveModuleState(), + new SwerveModuleState() }; // Auto setup AutoBuilder.configureHolonomic( this::getPose, // Robot pose supplier - this::setPose, // Method to reset odometry (will be called if your auto has a starting pose) + this::setPose, // Method to reset odometry (will be called if your auto has a + // starting pose) this::getChassisSpeeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE - this::setChassisSpeeds, // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds + this::setChassisSpeeds, // Method that will drive the robot given ROBOT RELATIVE + // ChassisSpeeds new HolonomicPathFollowerConfig( new PIDConstants(5, 0, 0), // Translation PID constants new PIDConstants(5, 0.0, 0.0), // Rotation PID constants 4.5, // Max module speed, in m/s - 0.37268, // Drive base radius in meters. Distance from robot center to furthest module. + 0.37268, // Drive base radius in meters. Distance from robot center to + // furthest module. new ReplanningConfig(true, true, .5, .25)), checkRedAlliance(), this // Reference to this subsystem to set requirements - ); + ); // Swerve obodom swerveOdomEstimator = new SwerveDrivePoseEstimator( Constants.Swerve.swerveKinematics, getGyroYaw(), getModulePositions(), - Robot.getAlliance() ? Constants.Vision.startingPoseRed : Constants.Vision.startingPoseBlue, + Robot.getAlliance() + ? Constants.Vision.startingPoseRed + : Constants.Vision.startingPoseBlue, Constants.Vision.stateStdDevs, Constants.Vision.kSingleTagStdDevs); // Logging - posePublisher = NetworkTableInstance.getDefault().getStructTopic("Fused Pose", Pose2d.struct).publish(); + posePublisher = NetworkTableInstance.getDefault() + .getStructTopic("Fused Pose", Pose2d.struct) + .publish(); swervePublisher = NetworkTableInstance.getDefault() - .getStructArrayTopic("Swerve Module States", SwerveModuleState.struct).publish(); + .getStructArrayTopic("Swerve Module States", SwerveModuleState.struct) + .publish(); SmartDashboard.putData("Gyro", gyro); SmartDashboard.putData(this); @@ -159,22 +167,21 @@ public BooleanSupplier checkRedAlliance() { } /** Teleop drive method */ - public void drive(Translation2d translation, double rotation, boolean fieldRelative, boolean isOpenLoop) { + public void drive( + Translation2d translation, double rotation, boolean fieldRelative, boolean isOpenLoop) { double translationX = translation.getX(); double translationY = translation.getY(); SwerveModuleState[] desiredStates; if (fieldRelative) { - desiredStates = Constants.Swerve.swerveKinematics - .toSwerveModuleStates(ChassisSpeeds.discretize(ChassisSpeeds.fromFieldRelativeSpeeds( - translationX, - translationY, - rotation, - getHeading()), 0.02)); + desiredStates = + Constants.Swerve.swerveKinematics.toSwerveModuleStates(ChassisSpeeds.discretize( + ChassisSpeeds.fromFieldRelativeSpeeds( + translationX, translationY, rotation, getHeading()), + 0.02)); } else { - desiredStates = Constants.Swerve.swerveKinematics.toSwerveModuleStates(new ChassisSpeeds(translationX, - translationY, - rotation)); + desiredStates = Constants.Swerve.swerveKinematics.toSwerveModuleStates( + new ChassisSpeeds(translationX, translationY, rotation)); } SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Constants.Swerve.maxSpeed); @@ -207,7 +214,8 @@ public void updateModuleStates() { public void setChassisSpeeds(ChassisSpeeds speed) { ChassisSpeeds targetSpeeds = ChassisSpeeds.discretize(speed, 0.02); - SwerveModuleState[] desiredState = Constants.Swerve.swerveKinematics.toSwerveModuleStates(targetSpeeds); + SwerveModuleState[] desiredState = + Constants.Swerve.swerveKinematics.toSwerveModuleStates(targetSpeeds); setModuleStates(desiredState); } @@ -236,13 +244,19 @@ public Rotation2d getHeading() { } public void setHeading(Rotation2d heading) { - swerveOdomEstimator.resetPosition(getGyroYaw(), getModulePositions(), + swerveOdomEstimator.resetPosition( + getGyroYaw(), + getModulePositions(), new Pose2d(getPose().getTranslation(), heading)); } public void zeroHeading() { - swerveOdomEstimator.resetPosition(getGyroYaw(), getModulePositions(), - new Pose2d(getPose().getTranslation(), new Rotation2d(Robot.getAlliance() ? Math.PI : 0))); + swerveOdomEstimator.resetPosition( + getGyroYaw(), + getModulePositions(), + new Pose2d( + getPose().getTranslation(), + new Rotation2d(Robot.getAlliance() ? Math.PI : 0))); } public Rotation2d getGyroYaw() { @@ -259,7 +273,8 @@ public void resetModulesToAbsolute() { * See * {@link SwerveDrivePoseEstimator#addVisionMeasurement(Pose2d, double, Matrix)}. */ - public void addVisionMeasurement(Pose2d visionMeasurement, double timestampSeconds, Matrix stdDevs) { + public void addVisionMeasurement( + Pose2d visionMeasurement, double timestampSeconds, Matrix stdDevs) { swerveOdomEstimator.addVisionMeasurement(visionMeasurement, timestampSeconds, stdDevs); } @@ -273,19 +288,18 @@ public void voltageDrive(double Voltage) { public void periodic() { updateModuleStates(); SwerveModulePosition[] modulePositions = getModulePositions(); - + // Correct pose estimate with multiple vision measurements Optional OptionalEstimatedPoseFront = vision.getEstimatedPoseFront(); if (OptionalEstimatedPoseFront.isPresent()) { final EstimatedRobotPose estimatedPose = OptionalEstimatedPoseFront.get(); - swerveOdomEstimator - .setVisionMeasurementStdDevs( - vision.getEstimationStdDevsFront(estimatedPose.estimatedPose.toPose2d())); + swerveOdomEstimator.setVisionMeasurementStdDevs( + vision.getEstimationStdDevsFront(estimatedPose.estimatedPose.toPose2d())); - swerveOdomEstimator.addVisionMeasurement(estimatedPose.estimatedPose.toPose2d(), - estimatedPose.timestampSeconds); + swerveOdomEstimator.addVisionMeasurement( + estimatedPose.estimatedPose.toPose2d(), estimatedPose.timestampSeconds); } Optional OptionalEstimatedPoseBack = vision.getEstimatedPoseBack(); @@ -293,12 +307,11 @@ public void periodic() { final EstimatedRobotPose estimatedPose2 = OptionalEstimatedPoseBack.get(); - swerveOdomEstimator - .setVisionMeasurementStdDevs( - vision.getEstimationStdDevsBack(estimatedPose2.estimatedPose.toPose2d())); + swerveOdomEstimator.setVisionMeasurementStdDevs( + vision.getEstimationStdDevsBack(estimatedPose2.estimatedPose.toPose2d())); - swerveOdomEstimator.addVisionMeasurement(estimatedPose2.estimatedPose.toPose2d(), - estimatedPose2.timestampSeconds); + swerveOdomEstimator.addVisionMeasurement( + estimatedPose2.estimatedPose.toPose2d(), estimatedPose2.timestampSeconds); } // Logging @@ -310,111 +323,133 @@ public void periodic() { swervePublisher.set(swerveModuleStates); for (SwerveModule mod : mSwerveMods) { - SmartDashboard.putNumber("Mod " + mod.moduleNumber + " CANcoder", mod.getCANcoder().getDegrees()); - SmartDashboard.putNumber("Mod " + mod.moduleNumber + " Angle", modulePositions[mod.moduleNumber].angle.getDegrees()); - SmartDashboard.putNumber("Mod " + mod.moduleNumber + " Velocity", swerveModuleStates[mod.moduleNumber].speedMetersPerSecond); + SmartDashboard.putNumber( + "Mod " + mod.moduleNumber + " CANcoder", mod.getCANcoder().getDegrees()); + SmartDashboard.putNumber( + "Mod " + mod.moduleNumber + " Angle", + modulePositions[mod.moduleNumber].angle.getDegrees()); + SmartDashboard.putNumber( + "Mod " + mod.moduleNumber + " Velocity", + swerveModuleStates[mod.moduleNumber].speedMetersPerSecond); } - //SmartDashboard.putString("Obodom", getPose().toString()); + // SmartDashboard.putString("Obodom", getPose().toString()); SmartDashboard.putNumber("Gyro", getGyroYaw().getDegrees()); SmartDashboard.putNumber("Heading", getHeading().getDegrees()); - SmartDashboard.putNumber("Angle to target", Math.toDegrees(FiringSolutionsV3.getAngleToMovingTarget(getPose().getX(), getPose().getY(), FiringSolutionsV3.ampTargetX, FiringSolutionsV3.ampTargetY, - getChassisSpeeds().vxMetersPerSecond, - getChassisSpeeds().vyMetersPerSecond, - getPose().getRotation().getRadians()))); - + SmartDashboard.putNumber( + "Angle to target", + Math.toDegrees(FiringSolutionsV3.getAngleToMovingTarget( + getPose().getX(), + getPose().getY(), + FiringSolutionsV3.ampTargetX, + FiringSolutionsV3.ampTargetY, + getChassisSpeeds().vxMetersPerSecond, + getChassisSpeeds().vyMetersPerSecond, + getPose().getRotation().getRadians()))); } public void sysidroutine(SysIdRoutineLog log) { log.motor("drive-BR") - .voltage( - m_appliedVoltage.mut_replace( - mSwerveMods[3].getMotorVoltage(), Volts)) - .linearPosition(m_distance.mut_replace(mSwerveMods[3].getPosition().distanceMeters, Meters)) + .voltage(m_appliedVoltage.mut_replace(mSwerveMods[3].getMotorVoltage(), Volts)) + .linearPosition( + m_distance.mut_replace(mSwerveMods[3].getPosition().distanceMeters, Meters)) .linearVelocity( m_velocity.mut_replace(mSwerveMods[3].getMotorVelocity(), MetersPerSecond)); + log.motor("drive-FL") - .voltage( - m_appliedVoltage.mut_replace( - mSwerveMods[0].getMotorVoltage(), Volts)) - .linearPosition(m_distance.mut_replace(mSwerveMods[0].getPosition().distanceMeters, Meters)) + .voltage(m_appliedVoltage.mut_replace(mSwerveMods[0].getMotorVoltage(), Volts)) + .linearPosition( + m_distance.mut_replace(mSwerveMods[0].getPosition().distanceMeters, Meters)) .linearVelocity( m_velocity.mut_replace(mSwerveMods[0].getMotorVelocity(), MetersPerSecond)); + log.motor("drive-FR") - .voltage( - m_appliedVoltage.mut_replace( - mSwerveMods[1].getMotorVoltage(), Volts)) - .linearPosition(m_distance.mut_replace(mSwerveMods[1].getPosition().distanceMeters, Meters)) + .voltage(m_appliedVoltage.mut_replace(mSwerveMods[1].getMotorVoltage(), Volts)) + .linearPosition( + m_distance.mut_replace(mSwerveMods[1].getPosition().distanceMeters, Meters)) .linearVelocity( m_velocity.mut_replace(mSwerveMods[1].getMotorVelocity(), MetersPerSecond)); + log.motor("drive-BL") - .voltage( - m_appliedVoltage.mut_replace( - mSwerveMods[2].getMotorVoltage(), Volts)) - .linearPosition(m_distance.mut_replace(mSwerveMods[2].getPosition().distanceMeters, Meters)) + .voltage(m_appliedVoltage.mut_replace(mSwerveMods[2].getMotorVoltage(), Volts)) + .linearPosition( + m_distance.mut_replace(mSwerveMods[2].getPosition().distanceMeters, Meters)) .linearVelocity( m_velocity.mut_replace(mSwerveMods[2].getMotorVelocity(), MetersPerSecond)); } public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { - return new SequentialCommandGroup(new InstantCommand(this::resetModulesToAbsolute, this), new WaitCommand(0.5), + return new SequentialCommandGroup( + new InstantCommand(this::resetModulesToAbsolute, this), + new WaitCommand(0.5), m_sysIdRoutine.quasistatic(direction)); } public Command sysIdDynamic(SysIdRoutine.Direction direction) { - return new SequentialCommandGroup(new InstantCommand(this::resetModulesToAbsolute, this), new WaitCommand(0.5), + return new SequentialCommandGroup( + new InstantCommand(this::resetModulesToAbsolute, this), + new WaitCommand(0.5), m_sysIdRoutine.dynamic(direction)); } // Pathfinding Commands public Command pathToSource() { if (!Robot.getAlliance()) { - return AutoBuilder.pathfindToPose(new Pose2d(1.21, 0.96, Rotation2d.fromDegrees(58.79)), + return AutoBuilder.pathfindToPose( + new Pose2d(1.21, 0.96, Rotation2d.fromDegrees(58.79)), Constants.Auto.PathfindingConstraints); } else { - return AutoBuilder.pathfindToPose(new Pose2d(15.47, 1.50, Rotation2d.fromDegrees(-59.53)), + return AutoBuilder.pathfindToPose( + new Pose2d(15.47, 1.50, Rotation2d.fromDegrees(-59.53)), Constants.Auto.PathfindingConstraints); } } public Command pathToAmp() { if (!Robot.getAlliance()) { - return AutoBuilder.pathfindToPose(new Pose2d(1.84, 7.59, Rotation2d.fromDegrees(90.37)), + return AutoBuilder.pathfindToPose( + new Pose2d(1.84, 7.59, Rotation2d.fromDegrees(90.37)), Constants.Auto.PathfindingConstraints); } else { - return AutoBuilder.pathfindToPose(new Pose2d(14.74, 7.52, Rotation2d.fromDegrees(90.37)), + return AutoBuilder.pathfindToPose( + new Pose2d(14.74, 7.52, Rotation2d.fromDegrees(90.37)), Constants.Auto.PathfindingConstraints); } } public Command pathToMidfieldChain() { if (!Robot.getAlliance()) { - return AutoBuilder.pathfindToPose(new Pose2d(6.29, 4.08, Rotation2d.fromDegrees(180)), + return AutoBuilder.pathfindToPose( + new Pose2d(6.29, 4.08, Rotation2d.fromDegrees(180)), Constants.Auto.PathfindingConstraints); } else { - return AutoBuilder.pathfindToPose(new Pose2d(10.26, 4.10, Rotation2d.fromDegrees(0)), + return AutoBuilder.pathfindToPose( + new Pose2d(10.26, 4.10, Rotation2d.fromDegrees(0)), Constants.Auto.PathfindingConstraints); } } public Command pathToSourceChain() { if (!Robot.getAlliance()) { - return AutoBuilder.pathfindToPose(new Pose2d(4.18, 2.79, Rotation2d.fromDegrees(59.47)), + return AutoBuilder.pathfindToPose( + new Pose2d(4.18, 2.79, Rotation2d.fromDegrees(59.47)), Constants.Auto.PathfindingConstraints); } else { - return AutoBuilder.pathfindToPose(new Pose2d(12.43, 2.90, Rotation2d.fromDegrees(121.26)), + return AutoBuilder.pathfindToPose( + new Pose2d(12.43, 2.90, Rotation2d.fromDegrees(121.26)), Constants.Auto.PathfindingConstraints); } } public Command pathToAmpChain() { if (!Robot.getAlliance()) { - return AutoBuilder.pathfindToPose(new Pose2d(4.20, 5.30, Rotation2d.fromDegrees(-58.21)), + return AutoBuilder.pathfindToPose( + new Pose2d(4.20, 5.30, Rotation2d.fromDegrees(-58.21)), Constants.Auto.PathfindingConstraints); } else { - return AutoBuilder.pathfindToPose(new Pose2d(12.50, 5.25, Rotation2d.fromDegrees(-120.96)), + return AutoBuilder.pathfindToPose( + new Pose2d(12.50, 5.25, Rotation2d.fromDegrees(-120.96)), Constants.Auto.PathfindingConstraints); } } - -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/subsystems/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/VisionSubsystem.java index 8262ceb..db858e9 100644 --- a/src/main/java/frc/robot/subsystems/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/VisionSubsystem.java @@ -24,8 +24,6 @@ package frc.robot.subsystems; -import frc.robot.Constants; -import frc.robot.Robot; import edu.wpi.first.apriltag.AprilTagFieldLayout.OriginPosition; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; @@ -34,13 +32,17 @@ import edu.wpi.first.math.numbers.N3; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import java.util.Optional; +import frc.robot.Constants; +import frc.robot.Robot; + import org.photonvision.EstimatedRobotPose; import org.photonvision.PhotonCamera; import org.photonvision.PhotonPoseEstimator; import org.photonvision.PhotonPoseEstimator.PoseStrategy; import org.photonvision.targeting.PhotonPipelineResult; +import java.util.Optional; + public class VisionSubsystem extends SubsystemBase { private final PhotonCamera aprilTagCameraFront; private final PhotonCamera aprilTagCameraBack; @@ -62,17 +64,29 @@ public VisionSubsystem() { Constants.Vision.kTagLayout.setOrigin(OriginPosition.kBlueAllianceWallRightSide); photonEstimatorFront = new PhotonPoseEstimator( - Constants.Vision.kTagLayout, PoseStrategy.LOWEST_AMBIGUITY, aprilTagCameraFront, Constants.Vision.kRobotToCamFront); + Constants.Vision.kTagLayout, + PoseStrategy.LOWEST_AMBIGUITY, + aprilTagCameraFront, + Constants.Vision.kRobotToCamFront); photonEstimatorBack = new PhotonPoseEstimator( - Constants.Vision.kTagLayout, PoseStrategy.LOWEST_AMBIGUITY, aprilTagCameraBack, Constants.Vision.kRobotToCamBack); + Constants.Vision.kTagLayout, + PoseStrategy.LOWEST_AMBIGUITY, + aprilTagCameraBack, + Constants.Vision.kRobotToCamBack); + + photonEstimatorFront.setLastPose( + Robot.getAlliance() + ? Constants.Vision.startingPoseRed + : Constants.Vision.startingPoseBlue); + photonEstimatorBack.setLastPose( + Robot.getAlliance() + ? Constants.Vision.startingPoseRed + : Constants.Vision.startingPoseBlue); - photonEstimatorFront.setLastPose(Robot.getAlliance() ? Constants.Vision.startingPoseRed : Constants.Vision.startingPoseBlue); - photonEstimatorBack.setLastPose(Robot.getAlliance() ? Constants.Vision.startingPoseRed : Constants.Vision.startingPoseBlue); - // 2024 field quality makes multitag impractical - //photonEstimatorFront.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); - //photonEstimatorBack.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); + // photonEstimatorFront.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); + // photonEstimatorBack.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); } /** Get the latest result from the front April Tag camera */ @@ -102,8 +116,7 @@ public Optional getEstimatedPoseFront() { var visionEst = photonEstimatorFront.update(); double latestTimestamp = aprilTagCameraFront.getLatestResult().getTimestampSeconds(); boolean newResult = Math.abs(latestTimestamp - lastTimeStampFront) > 1e-5; - if (newResult) - lastTimeStampFront = latestTimestamp; + if (newResult) lastTimeStampFront = latestTimestamp; return visionEst; } @@ -111,8 +124,7 @@ public Optional getEstimatedPoseBack() { var visionEst = photonEstimatorBack.update(); double latestTimestamp = aprilTagCameraBack.getLatestResult().getTimestampSeconds(); boolean newResult = Math.abs(latestTimestamp - lastEstTimestampBack) > 1e-5; - if (newResult) - lastEstTimestampBack = latestTimestamp; + if (newResult) lastEstTimestampBack = latestTimestamp; return visionEst; } @@ -132,22 +144,22 @@ public Matrix getEstimationStdDevsFront(Pose2d estimatedPose) { double avgDist = 0; for (var tgt : targets) { var tagPose = photonEstimatorFront.getFieldTags().getTagPose(tgt.getFiducialId()); - if (tagPose.isEmpty()) - continue; + if (tagPose.isEmpty()) continue; numTags++; - avgDist += tagPose.get().toPose2d().getTranslation().getDistance(estimatedPose.getTranslation()); + avgDist += tagPose.get() + .toPose2d() + .getTranslation() + .getDistance(estimatedPose.getTranslation()); } - if (numTags == 0) - return estStdDevs; + if (numTags == 0) return estStdDevs; avgDist /= numTags; // Decrease std devs if multiple targets are visible - //if (numTags > 1) + // if (numTags > 1) // estStdDevs = Constants.Vision.kMultiTagStdDevs; // Increase std devs based on (average) distance - if (/*numTags == 1 && */avgDist > maxAcceptableRange) + if (avgDist > maxAcceptableRange) estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE); - else - estStdDevs = estStdDevs.times(1 + (avgDist * avgDist / 30)); + else estStdDevs = estStdDevs.times(1 + (avgDist * avgDist / 30)); return estStdDevs; } @@ -159,23 +171,23 @@ public Matrix getEstimationStdDevsBack(Pose2d estimatedPose) { double avgDist = 0; for (var tgt : targets) { var tagPose = photonEstimatorBack.getFieldTags().getTagPose(tgt.getFiducialId()); - if (tagPose.isEmpty()) - continue; + if (tagPose.isEmpty()) continue; numTags++; - avgDist += tagPose.get().toPose2d().getTranslation().getDistance(estimatedPose.getTranslation()); + avgDist += tagPose.get() + .toPose2d() + .getTranslation() + .getDistance(estimatedPose.getTranslation()); } - if (numTags == 0) - return estStdDevs; + if (numTags == 0) return estStdDevs; avgDist /= numTags; // Decrease std devs if multiple targets are visible - //if (numTags > 1) + // if (numTags > 1) // estStdDevs = Constants.Vision.kMultiTagStdDevs; // Increase std devs based on (average) distance - if (/*numTags == 1 &&*/ avgDist > maxAcceptableRange) + if (avgDist > maxAcceptableRange) estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE); - else - estStdDevs = estStdDevs.times(1 + (avgDist * avgDist / 30)); + else estStdDevs = estStdDevs.times(1 + (avgDist * avgDist / 30)); return estStdDevs; } -} \ No newline at end of file +} From feeedad43ada3a915c735fd433d78ba9149331d1 Mon Sep 17 00:00:00 2001 From: RavenRain44 <89553123+RavenRain44@users.noreply.github.com> Date: Wed, 13 Mar 2024 12:18:12 -0500 Subject: [PATCH 45/49] Removed the unneeded code --- src/main/java/frc/robot/commands/FIREEE.java | 4 +--- src/main/java/frc/robot/commands/ManRizzt.java | 5 +---- 2 files changed, 2 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/commands/FIREEE.java b/src/main/java/frc/robot/commands/FIREEE.java index a0a7f5d..7bb4488 100644 --- a/src/main/java/frc/robot/commands/FIREEE.java +++ b/src/main/java/frc/robot/commands/FIREEE.java @@ -12,12 +12,10 @@ public class FIREEE extends Command { private ShooterSubsystem shooter; private IntexerSubsystem intexer; - private LEDSubsystem ledSubsystem; - public FIREEE(ShooterSubsystem shooterSub, IntexerSubsystem intex, LEDSubsystem ledSubsystem) { + public FIREEE(ShooterSubsystem shooterSub, IntexerSubsystem intex) { shooter = shooterSub; intexer = intex; - this.ledSubsystem = ledSubsystem; // Use addRequirements() here to declare subsystem dependencies. addRequirements(intex); } diff --git a/src/main/java/frc/robot/commands/ManRizzt.java b/src/main/java/frc/robot/commands/ManRizzt.java index 8867bb5..265076a 100644 --- a/src/main/java/frc/robot/commands/ManRizzt.java +++ b/src/main/java/frc/robot/commands/ManRizzt.java @@ -17,16 +17,14 @@ public class ManRizzt extends Command { ShooterSubsystem m_shooterSubsystem; - LEDSubsystem ledSubsystem; private DoubleSupplier speed; BooleanSupplier setAngle; double lastWristSetpoint = 0.0; boolean wristIsLocked = false; - public ManRizzt(ShooterSubsystem subsystem, LEDSubsystem ledSubsystem, DoubleSupplier speed, BooleanSupplier setAngle) { + public ManRizzt(ShooterSubsystem subsystem, DoubleSupplier speed, BooleanSupplier setAngle) { // Use addRequirements() here to declare subsystem dependencies. m_shooterSubsystem = subsystem; - this.ledSubsystem = ledSubsystem; this.setAngle = setAngle; this.speed = speed; SmartDashboard.putNumber("Set Wrist Angle", 0); @@ -64,7 +62,6 @@ public void execute() { @Override public boolean isFinished() { - ledSubsystem.ReadyToFire(true); return false; } From 111a5ad01d0c8cdd9c2afeb694773bb186caa5c1 Mon Sep 17 00:00:00 2001 From: RavenRain44 <89553123+RavenRain44@users.noreply.github.com> Date: Wed, 13 Mar 2024 12:41:37 -0500 Subject: [PATCH 46/49] Merged and Solved --- src/main/java/frc/robot/subsystems/LEDSubsystem.java | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/LEDSubsystem.java index 5caa236..bcc84e6 100644 --- a/src/main/java/frc/robot/subsystems/LEDSubsystem.java +++ b/src/main/java/frc/robot/subsystems/LEDSubsystem.java @@ -18,9 +18,8 @@ public class LEDSubsystem extends SubsystemBase { public DigitalOutput SkylerBright = new DigitalOutput(6); // Bit 4 (8) // Output bits to the LEDs - public DigitalOutput[] bits = {WalterLight, JesseBlinkman, GusBling, SkylerBright - }; // Actual Outputs - boolean[] output = new boolean[4]; // Computed Outputs + public DigitalOutput[] bits = {WalterLight, JesseBlinkman, GusBling, SkylerBright}; // Actual Outputs + private boolean[] output = new boolean[4]; // Computed Outputs // Booleans used for Easy input private Boolean hasNote = false; @@ -40,7 +39,7 @@ public class LEDSubsystem extends SubsystemBase { false, // Note in Intake -9 Orange Blink false, // Note in Shooter -10 Orange Solid false, // Alliance Color -11 Red Pulse - false // Alliance Color -12 Blue Pulse + false // Alliance Color -12 Blue Pulse }; // Use subsystems From 0ce515373987bfbdb552350114d4847889b87e74 Mon Sep 17 00:00:00 2001 From: RavenRain44 <89553123+RavenRain44@users.noreply.github.com> Date: Wed, 13 Mar 2024 13:12:22 -0500 Subject: [PATCH 47/49] SmartDashboard Values --- .../frc/robot/subsystems/LEDSubsystem.java | 44 +++++++++---------- 1 file changed, 22 insertions(+), 22 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/LEDSubsystem.java index bcc84e6..674899c 100644 --- a/src/main/java/frc/robot/subsystems/LEDSubsystem.java +++ b/src/main/java/frc/robot/subsystems/LEDSubsystem.java @@ -6,6 +6,7 @@ import edu.wpi.first.wpilibj.DigitalOutput; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; @@ -49,11 +50,7 @@ public class LEDSubsystem extends SubsystemBase { SwerveSubsystem swerve; /** Creates a new LEDSubsystem. */ - public LEDSubsystem( - VisionSubsystem m_VisionSubsystem, - ShooterSubsystem m_ShooterSubsystem, - IntexerSubsystem m_IntexerSubsystem, - SwerveSubsystem m_SwerveSubsystem) { + public LEDSubsystem(VisionSubsystem m_VisionSubsystem, ShooterSubsystem m_ShooterSubsystem, IntexerSubsystem m_IntexerSubsystem, SwerveSubsystem m_SwerveSubsystem) { this.vision = m_VisionSubsystem; this.shooter = m_ShooterSubsystem; this.intexer = m_IntexerSubsystem; @@ -122,6 +119,21 @@ private void set() { // Decimal phase inputBooleans[0] = false; } + // Check beam breaks + if (intexer.intakeBreak()) { + inputBooleans[9] = true; // Intake + inputBooleans[10] = false; // Shooter + hasNote = true; + } else if (intexer.shooterBreak()) { + inputBooleans[9] = false; // Intake + inputBooleans[10] = true; // Shooter + hasNote = true; + } else { + inputBooleans[9] = false; // Intake + inputBooleans[10] = false; // Shooter + hasNote = false; + } + // Charging or At Speed with Note or without Note if (hasNote) { // Has Note if (shooter.isShooterAtSpeed()) { // At speed @@ -145,20 +157,7 @@ private void set() { // Decimal phase inputBooleans[6] = false; } - // Check beam breaks - if (intexer.intakeBreak()) { - inputBooleans[9] = true; // Intake - inputBooleans[10] = false; // Shooter - hasNote = true; - } else if (intexer.shooterBreak()) { - inputBooleans[9] = false; // Intake - inputBooleans[10] = true; // Shooter - hasNote = true; - } else { - inputBooleans[9] = false; // Intake - inputBooleans[10] = false; // Shooter - hasNote = false; - } + SmartDashboard.putBooleanArray("Input Booleans", inputBooleans); } private void encoder() { // Transition phase @@ -167,9 +166,7 @@ private void encoder() { // Transition phase int trueIndex = 0; // Index of selected LED sequence - for (int i = 0; - i < inputBooleans.length; - i++) { // Picks the first true sequence based on priority + for (int i = 0; i < inputBooleans.length; i++) { // Picks the first true sequence based on priority if (inputBooleans[i]) { trueIndex = i; break; @@ -197,6 +194,8 @@ private void encoder() { // Transition phase finalString = binaryString; } + SmartDashboard.putString("Binary Output", finalString); + for (int i = pin_amount - 1; i >= 0; i--) { output[i] = finalString.charAt(i) == '1'; } @@ -206,6 +205,7 @@ private void send() { // Send Phase - "Redundancy is bliss" for (int i = 0; i < output.length; i++) { bits[i].set(output[i]); } + SmartDashboard.putBooleanArray("DigitalOutputs", output); } // SET FUNCTIONS From 06c33a28a65df93117b51ed15319bc8a559c9488 Mon Sep 17 00:00:00 2001 From: Andrew-K961 Date: Wed, 13 Mar 2024 14:23:32 -0500 Subject: [PATCH 48/49] me when robote expode day before comp --- src/main/java/frc/robot/RobotContainer.java | 57 +++++++------------ .../frc/robot/subsystems/LEDSubsystem.java | 15 +++-- 2 files changed, 33 insertions(+), 39 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 017cce3..605c221 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -39,24 +39,19 @@ public class RobotContainer { private final int leftTrigger = XboxController.Axis.kLeftTrigger.value; private final int rightTrigger = XboxController.Axis.kRightTrigger.value; - /* DRIVER BUTTONS */ + /* DRIVER BUTTONS spotless:off */ /** Driver X */ - private final JoystickButton intakeNoMove = - new JoystickButton(driver, XboxController.Button.kX.value); + private final JoystickButton intakeNoMove = new JoystickButton(driver, XboxController.Button.kX.value); /** Driver A */ private final JoystickButton Shoot = new JoystickButton(driver, XboxController.Button.kA.value); /** Driver B */ - private final JoystickButton forceShoot = - new JoystickButton(driver, XboxController.Button.kB.value); + private final JoystickButton forceShoot = new JoystickButton(driver, XboxController.Button.kB.value); /** Driver Y */ - private final JoystickButton intakeFromSource = - new JoystickButton(driver, XboxController.Button.kY.value); + private final JoystickButton intakeFromSource = new JoystickButton(driver, XboxController.Button.kY.value); /** Driver RB */ - private final JoystickButton intex = - new JoystickButton(driver, XboxController.Button.kRightBumper.value); + private final JoystickButton intex = new JoystickButton(driver, XboxController.Button.kRightBumper.value); /** Driver LB */ - private final JoystickButton outex = - new JoystickButton(driver, XboxController.Button.kLeftBumper.value); + private final JoystickButton outex = new JoystickButton(driver, XboxController.Button.kLeftBumper.value); /** Driver LT */ private final Trigger targetAmp = new Trigger(() -> driver.getRawAxis(leftTrigger) > .5); /** Driver RT */ @@ -70,28 +65,21 @@ public class RobotContainer { /** Driver Left */ private final Trigger driverRight = new Trigger(() -> driver.getPOV() == 270); /** Driver Start */ - private final JoystickButton resetOdom = - new JoystickButton(driver, XboxController.Button.kStart.value); + private final JoystickButton resetOdom = new JoystickButton(driver, XboxController.Button.kStart.value); /* MECH BUTTONS */ /** Mech X */ - private final JoystickButton shooterToAntiDefense = - new JoystickButton(mech, XboxController.Button.kX.value); + private final JoystickButton shooterToAntiDefense = new JoystickButton(mech, XboxController.Button.kX.value); /** Mech A */ - private final JoystickButton shooterToIntake = - new JoystickButton(mech, XboxController.Button.kA.value); + private final JoystickButton shooterToIntake = new JoystickButton(mech, XboxController.Button.kA.value); /** Mech Y */ - private final JoystickButton shooterToAmp = - new JoystickButton(mech, XboxController.Button.kY.value); + private final JoystickButton shooterToAmp = new JoystickButton(mech, XboxController.Button.kY.value); /** Mech B */ - private final JoystickButton shooterToSubwoofer = - new JoystickButton(mech, XboxController.Button.kB.value); + private final JoystickButton shooterToSubwoofer = new JoystickButton(mech, XboxController.Button.kB.value); /** Mech RB */ - private final JoystickButton primeShooterSpeedSpeaker = - new JoystickButton(mech, XboxController.Button.kRightBumper.value); + private final JoystickButton primeShooterSpeedSpeaker = new JoystickButton(mech, XboxController.Button.kRightBumper.value); /** Mech LB */ - private final JoystickButton resetNoteInShooter = - new JoystickButton(mech, XboxController.Button.kLeftBumper.value); + private final JoystickButton resetNoteInShooter = new JoystickButton(mech, XboxController.Button.kLeftBumper.value); /** Mech RT */ private final Trigger mechRT = new Trigger(() -> mech.getRawAxis(rightTrigger) > .5); /** Mech LT */ @@ -105,18 +93,14 @@ public class RobotContainer { /** Mech Left */ private final Trigger mechLeft = new Trigger(() -> mech.getPOV() == 270); /** Mech Start */ - private final JoystickButton autoZeroShooter = - new JoystickButton(mech, XboxController.Button.kStart.value); + private final JoystickButton autoZeroShooter = new JoystickButton(mech, XboxController.Button.kStart.value); /** Mech Back */ - private final JoystickButton zeroShooter = - new JoystickButton(mech, XboxController.Button.kBack.value); + private final JoystickButton zeroShooter = new JoystickButton(mech, XboxController.Button.kBack.value); /** Mech RS */ - private final JoystickButton mechRightStick = - new JoystickButton(mech, XboxController.Button.kRightStick.value); + private final JoystickButton mechRightStick = new JoystickButton(mech, XboxController.Button.kRightStick.value); /** Mech LS */ - private final JoystickButton mechLeftStick = - new JoystickButton(mech, XboxController.Button.kLeftStick.value); - + private final JoystickButton mechLeftStick = new JoystickButton(mech, XboxController.Button.kLeftStick.value); +// spotless:on private final Trigger dynamicForward = new Trigger(() -> FF.getPOV() == 90); private final Trigger dynamicBackward = new Trigger(() -> FF.getPOV() == 270); private final Trigger quasistaticForward = new Trigger(() -> FF.getPOV() == 0); @@ -216,7 +200,10 @@ private void configureButtonBindings() { : Constants.Vision.startingPoseBlue)))); // Intexer - intex.or(intakeNoMove).whileTrue(new IntexBestHex(m_IntexerSubsystem, true, driver)); + intex.or(intakeNoMove) + .whileTrue(new IntexBestHex(m_IntexerSubsystem, true, driver)) + .onFalse(new ResetNoteInShooterPart2( + m_ShooterSubsystem, m_IntexerSubsystem, driver)); outex.whileTrue(new IntexBestHex(m_IntexerSubsystem, false, driver)); // Shooter intake diff --git a/src/main/java/frc/robot/subsystems/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/LEDSubsystem.java index 674899c..5331fac 100644 --- a/src/main/java/frc/robot/subsystems/LEDSubsystem.java +++ b/src/main/java/frc/robot/subsystems/LEDSubsystem.java @@ -19,7 +19,8 @@ public class LEDSubsystem extends SubsystemBase { public DigitalOutput SkylerBright = new DigitalOutput(6); // Bit 4 (8) // Output bits to the LEDs - public DigitalOutput[] bits = {WalterLight, JesseBlinkman, GusBling, SkylerBright}; // Actual Outputs + public DigitalOutput[] bits = {WalterLight, JesseBlinkman, GusBling, SkylerBright + }; // Actual Outputs private boolean[] output = new boolean[4]; // Computed Outputs // Booleans used for Easy input @@ -40,7 +41,7 @@ public class LEDSubsystem extends SubsystemBase { false, // Note in Intake -9 Orange Blink false, // Note in Shooter -10 Orange Solid false, // Alliance Color -11 Red Pulse - false // Alliance Color -12 Blue Pulse + false // Alliance Color -12 Blue Pulse }; // Use subsystems @@ -50,7 +51,11 @@ public class LEDSubsystem extends SubsystemBase { SwerveSubsystem swerve; /** Creates a new LEDSubsystem. */ - public LEDSubsystem(VisionSubsystem m_VisionSubsystem, ShooterSubsystem m_ShooterSubsystem, IntexerSubsystem m_IntexerSubsystem, SwerveSubsystem m_SwerveSubsystem) { + public LEDSubsystem( + VisionSubsystem m_VisionSubsystem, + ShooterSubsystem m_ShooterSubsystem, + IntexerSubsystem m_IntexerSubsystem, + SwerveSubsystem m_SwerveSubsystem) { this.vision = m_VisionSubsystem; this.shooter = m_ShooterSubsystem; this.intexer = m_IntexerSubsystem; @@ -166,7 +171,9 @@ private void encoder() { // Transition phase int trueIndex = 0; // Index of selected LED sequence - for (int i = 0; i < inputBooleans.length; i++) { // Picks the first true sequence based on priority + for (int i = 0; + i < inputBooleans.length; + i++) { // Picks the first true sequence based on priority if (inputBooleans[i]) { trueIndex = i; break; From 74d373b2a81c879f364ed142f35c7a5708c13f3c Mon Sep 17 00:00:00 2001 From: Andrew-K961 Date: Wed, 13 Mar 2024 19:42:24 -0500 Subject: [PATCH 49/49] FINAL CHANGEs --- src/main/java/frc/robot/Constants.java | 8 ++++---- src/main/java/frc/robot/RobotContainer.java | 3 ++- src/main/java/frc/robot/subsystems/ElevatorSubsystem.java | 2 +- 3 files changed, 7 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 3548c15..ea136a7 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -156,7 +156,7 @@ public static final class Mod0 { public static final int driveMotorID = 1; public static final int angleMotorID = 3; public static final int canCoderID = 2; - public static final Rotation2d angleOffset = Rotation2d.fromDegrees(112.6757); + public static final Rotation2d angleOffset = Rotation2d.fromDegrees(111.7); public static final SwerveModuleConstants constants = new SwerveModuleConstants(driveMotorID, angleMotorID, canCoderID, angleOffset); } @@ -166,7 +166,7 @@ public static final class Mod1 { public static final int driveMotorID = 4; public static final int angleMotorID = 6; public static final int canCoderID = 5; - public static final Rotation2d angleOffset = Rotation2d.fromDegrees(-80.5078); + public static final Rotation2d angleOffset = Rotation2d.fromDegrees(-79.1); public static final SwerveModuleConstants constants = new SwerveModuleConstants(driveMotorID, angleMotorID, canCoderID, angleOffset); } @@ -176,7 +176,7 @@ public static final class Mod2 { public static final int driveMotorID = 7; public static final int angleMotorID = 9; public static final int canCoderID = 8; - public static final Rotation2d angleOffset = Rotation2d.fromDegrees(-79.453125); + public static final Rotation2d angleOffset = Rotation2d.fromDegrees(-80.1); public static final SwerveModuleConstants constants = new SwerveModuleConstants(driveMotorID, angleMotorID, canCoderID, angleOffset); } @@ -186,7 +186,7 @@ public static final class Mod3 { public static final int driveMotorID = 10; public static final int angleMotorID = 12; public static final int canCoderID = 11; - public static final Rotation2d angleOffset = Rotation2d.fromDegrees(112.06); + public static final Rotation2d angleOffset = Rotation2d.fromDegrees(111.44); public static final SwerveModuleConstants constants = new SwerveModuleConstants(driveMotorID, angleMotorID, canCoderID, angleOffset); } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 605c221..7c27b75 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -12,7 +12,6 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.Trigger; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; import frc.lib.math.FiringSolutionsV3; import frc.robot.commands.*; @@ -298,10 +297,12 @@ private void configureButtonBindings() { /* THIRD CONTROLLER */ // Characterization tests + /* dynamicForward.whileTrue(m_SwerveSubsystem.sysIdDynamic(Direction.kForward)); dynamicBackward.whileTrue(m_SwerveSubsystem.sysIdDynamic(Direction.kReverse)); quasistaticForward.whileTrue(m_SwerveSubsystem.sysIdQuasistatic(Direction.kForward)); quasistaticBackwards.whileTrue(m_SwerveSubsystem.sysIdQuasistatic(Direction.kReverse)); + */ } public void stopAll() { diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index 6028b08..4703e22 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -67,7 +67,7 @@ public ElevatorSubsystem() { // laser can pid shenanigans elevatorPID.setP(3); - elevatorPID.setI(0); + elevatorPID.setI(0.5); elevatorPID.setD(0); elevatorPID.setTolerance(0.02);