From 526856eb7143fb6869024cab666bad600f1f0383 Mon Sep 17 00:00:00 2001 From: Paden O'Neal Date: Fri, 1 Mar 2024 15:45:27 -0700 Subject: [PATCH 01/12] Update IntakeDefault.java Spelt until wrong --- src/main/java/frc/robot/commands/IntakeDefault.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/commands/IntakeDefault.java b/src/main/java/frc/robot/commands/IntakeDefault.java index f14ad42..ed114dc 100644 --- a/src/main/java/frc/robot/commands/IntakeDefault.java +++ b/src/main/java/frc/robot/commands/IntakeDefault.java @@ -14,7 +14,7 @@ public class IntakeDefault extends Command { private Boolean first; /** - * Run intake untill note is obtained or manualy disabled + * Run intake until note is obtained or manualy disabled * * @param s_Intake Intake object */ From be4a30221824cee9a784448a3ecc9c5ca5d87350 Mon Sep 17 00:00:00 2001 From: Paden O'Neal Date: Sat, 2 Mar 2024 09:18:16 -0700 Subject: [PATCH 02/12] Update Swerve.java Basic code for aligning --- .../java/frc/robot/subsystems/Swerve.java | 23 +++++++++++++++++++ 1 file changed, 23 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 0908e2b..3d073ef 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -24,6 +24,29 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; + + +/* +double X_from_camera = NetworkTableInstance.getDefault().getTable("limelight").getEntry("targetpose_robotspace").getDoubleArray(new double[0]); +double Y_from_camera = NetworkTableInstance.getDefault().getTable("limelight").getEntry("targetpose_robotspace").getDoubleArray(new double[1]); +double X_rotation_from_camera = NetworkTableInstance.getDefault().getTable("limelight").getEntry("targetpose_robotspace").getDoubleArray(new double[1]); +if (X_from_camera <= 0.5) { + if (X_from_camera >= 2) { + swerve_X_speed = 1; + } else { + swerve_X_speed = 1.5 / X_from_camera - 1; + } +} else if (X_from_camera >= -0.5) { + if (X_from_camera <= -2) { + swerve_X_speed = -1; + } else { + swerve_X_speed = 1.5 / X_from_camera + 1; + } +} else { + +} +*/ + public class Swerve extends SubsystemBase { // Delcare variables public SwerveDriveOdometry swerveOdometry; From 70e850722e1270d795f9fe1762043e94053cd6f1 Mon Sep 17 00:00:00 2001 From: Paden O'Neal Date: Sat, 2 Mar 2024 09:27:39 -0700 Subject: [PATCH 03/12] added shoot --- src/main/java/frc/robot/subsystems/Swerve.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 3d073ef..c60ca2f 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -43,7 +43,7 @@ swerve_X_speed = 1.5 / X_from_camera + 1; } } else { - + shoot } */ From 1ca9962c273cd6f597f487bd0ded7fbe88ce596b Mon Sep 17 00:00:00 2001 From: Paden O'Neal Date: Sat, 2 Mar 2024 10:18:24 -0700 Subject: [PATCH 04/12] Finished basic shooter outline in comments --- .../java/frc/robot/subsystems/Swerve.java | 25 +++++++++++++++++-- 1 file changed, 23 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index c60ca2f..f859561 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -25,8 +25,9 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; - -/* +/* +NetworkTableInstance.getDefault().getTable("limelight").getEntry("priorityid").setNumber(8); +int tag_id = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tid").getNumber(); double X_from_camera = NetworkTableInstance.getDefault().getTable("limelight").getEntry("targetpose_robotspace").getDoubleArray(new double[0]); double Y_from_camera = NetworkTableInstance.getDefault().getTable("limelight").getEntry("targetpose_robotspace").getDoubleArray(new double[1]); double X_rotation_from_camera = NetworkTableInstance.getDefault().getTable("limelight").getEntry("targetpose_robotspace").getDoubleArray(new double[1]); @@ -43,6 +44,26 @@ swerve_X_speed = 1.5 / X_from_camera + 1; } } else { + shoot_x = true; +} + +if (Y_from_camera <= 0.5) { + if (Y_from_camera >= 2) { + swerve_Y_speed = 1; + } else { + swerve_Y_speed = 1.5 / Y_from_camera - 1; + } +} else if (Y_from_camera >= -0.5) { + if (Y_from_camera <= -2) { + swerve_Y_speed = -1; + } else { + swerve_Y_speed = 1.5 / Y_from_camera + 1; + } +} else { + shoot_y = true; +} + +if (shoot_y && shoot_x) { shoot } */ From 62d3327df492214dd21618d2db4f5f86e6eaa6c4 Mon Sep 17 00:00:00 2001 From: StoneCommander <67015041+StoneCommander@users.noreply.github.com> Date: Tue, 5 Mar 2024 18:11:00 -0700 Subject: [PATCH 05/12] AddTargetSwerve --- src/main/java/frc/robot/RobotContainer.java | 29 ++-- .../java/frc/robot/commands/TargetSwerve.java | 129 ++++++++++++++++++ .../java/frc/robot/subsystems/Swerve.java | 2 +- 3 files changed, 151 insertions(+), 9 deletions(-) create mode 100644 src/main/java/frc/robot/commands/TargetSwerve.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9377bc2..2dac885 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -33,6 +33,8 @@ import java.util.ArrayList; import java.util.List; +import javax.management.remote.TargetedNotification; + import frc.robot.autos.*; import frc.robot.commands.*; import frc.robot.subsystems.*; @@ -161,7 +163,6 @@ public RobotContainer() { NamedCommands.registerCommand("RunIntake", new IntakeRun(intake, LIM, new JoystickButton(secondary, 3), theLEDs)); NamedCommands.registerCommand("LaunchASAP", new LaunchASAP(intake,launcher,theLEDs)); - // Configure the button bindings configureButtonBindings(); @@ -236,6 +237,17 @@ private void configureButtonBindings() { new JoystickButton(secondary, 2).onTrue(new IntakeRun(intake, LIM, new JoystickButton(secondary, 2),theLEDs)); new JoystickButton(secondary, 3).onTrue(new IntakeFix(intake, LIM, new JoystickButton(secondary, 3),theLEDs)); new JoystickButton(secondary, 1).onTrue(new IntakeAmp(intake, LIM, new JoystickButton(secondary, 1), new JoystickButton(secondary, 6),theLEDs)); + new JoystickButton(driver, 4).onTrue( + new TargetSwerve( + s_Swerve, + () -> getTranslationAxisMaybe(), + () -> getStrafeAxisMaybe(), + () -> getRotationAxisMaybe(), + () -> getThrottleAxisMaybe(), + () -> robotCentric.getAsBoolean(), + theLEDs, + new JoystickButton(driver, 4) + )); // NEW STUFF I ADDED! PLZ CHECK BECAUSE I KNOW I DID NOT DO THIS RIGHT! //I DONT WANT TO SHOVE THIS IN A RANDOM COMMAND FOR FEAR THAT IT WILL NO LONGER BE ABLE TO ACCESS THE main_failed VARIABLE @@ -262,18 +274,19 @@ public void updateInfo() { SmartDashboard.putData("intake", intake); SmartDashboard.putData("Launcher", launcher); + SmartDashboard.putData("Swerve", s_Swerve); SmartDashboard.putData("Gyro",s_Swerve.gyro); SmartDashboard.putBoolean("ControlorA", new JoystickButton(secondary, 1).getAsBoolean()); SmartDashboard.putBoolean("ControlorB", new JoystickButton(secondary, 2).getAsBoolean()); SmartDashboard.putBoolean("ControlorX", new JoystickButton(secondary, 3).getAsBoolean()); - SmartDashboard.putData(PDP); - System.out.println("Radio"); - System.out.println(PDP.getCurrent(15)); - System.out.println("RIO"); - System.out.println(PDP.getCurrent(20)); - System.out.println("Total"); - System.out.println(PDP.getTotalCurrent()); + // SmartDashboard.putData(PDP); + // System.out.println("Radio"); + // System.out.println(PDP.getCurrent(15)); + // System.out.println("RIO"); + // System.out.println(PDP.getCurrent(20)); + // System.out.println("Total"); + // System.out.println(PDP.getTotalCurrent()); intake.updateData(); diff --git a/src/main/java/frc/robot/commands/TargetSwerve.java b/src/main/java/frc/robot/commands/TargetSwerve.java new file mode 100644 index 0000000..a39faad --- /dev/null +++ b/src/main/java/frc/robot/commands/TargetSwerve.java @@ -0,0 +1,129 @@ +package frc.robot.commands; + +import java.util.function.BooleanSupplier; +import java.util.function.DoubleSupplier; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants; +import frc.robot.subsystems.LEDS; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import frc.robot.subsystems.Swerve; + + +public class TargetSwerve extends Command { + private Swerve s_Swerve; + private DoubleSupplier translationSup; + private DoubleSupplier strafeSup; + private DoubleSupplier rotationSup; + private DoubleSupplier throtleSupplier; + private BooleanSupplier robotCentricSup; + + private JoystickButton overrideButton; + + + private double swerve_X_speed; + private double swerve_Y_speed; + private boolean shoot_x; + private boolean shoot_y; + private boolean shoot; + + + private LEDS led; + + /** + * Default Teleop mode, takes the swerve and conrolor + * + * @param s_Swerve swerve base + * @param translationSup forward and back suplier, typicaly a controlor axis + * @param strafeSup left and right suplier, typicaly a controlor axis + * @param rotationSup Rotation suplier, typicaly a controlor axis + * @param robotCentricSup wether to drive relative to the robot. + */ + public TargetSwerve(Swerve s_Swerve, DoubleSupplier translationSup, DoubleSupplier strafeSup, DoubleSupplier rotationSup, DoubleSupplier throtleSupplier, BooleanSupplier robotCentricSup, LEDS led, JoystickButton overrideButton) { + this.s_Swerve = s_Swerve; + addRequirements(s_Swerve); + + this.translationSup = translationSup; + this.strafeSup = strafeSup; + this.rotationSup = rotationSup; + this.robotCentricSup = robotCentricSup; + this.throtleSupplier = throtleSupplier; + this.overrideButton = overrideButton; + this.led = led; + } + + @Override + public void execute() { + /* Get Values, Deadband*/ + double translationVal = MathUtil.applyDeadband(translationSup.getAsDouble(), Constants.stickDeadband); + double strafeVal = MathUtil.applyDeadband(strafeSup.getAsDouble(), Constants.stickDeadband); + double rotationVal = MathUtil.applyDeadband(rotationSup.getAsDouble(), Constants.stickDeadband); + double throtleVal = 1-((throtleSupplier.getAsDouble()+1)/2); + // SmartDashboard.putNumber("Throttle", throtleVal); + + // SmartDashboard.get + NetworkTableInstance.getDefault().getTable("limelight").getEntry("priorityid").setNumber(7); + NetworkTable LLtbl = NetworkTableInstance.getDefault().getTable("limelight"); + // int tag_id = LLtbl.getEntry("tid"). + // double X_from_camera = NetworkTableInstance.getDefault().getTable("limelight").getEntry("targetpose_robotspace").getDoubleArray(new double[1]); + // double Y_from_camera = NetworkTableInstance.getDefault().getTable("limelight").getEntry("targetpose_robotspace").getDoubleArray(new double[1]); + // double X_rotation_from_camera = NetworkTableInstance.getDefault().getTable("limelight").getEntry("targetpose_robotspace").getDoubleArray(new double[1]); + + double X_from_camera = LLtbl.getValue("tx").getDouble(); + double Y_from_camera = LLtbl.getValue("ty").getDouble(); + + if (X_from_camera <= 0.5) { + if (X_from_camera >= 2) { + swerve_X_speed = 1; + } else { + swerve_X_speed = 1.5 / X_from_camera - 1; + } + } else if (X_from_camera >= -0.5) { + if (X_from_camera <= -2) { + swerve_X_speed = -1; + } else { + swerve_X_speed = 1.5 / X_from_camera + 1; + } + } else { + shoot_x = true; + } + + if (Y_from_camera <= 0.5) { + if (Y_from_camera >= 2) { + swerve_Y_speed = 1; + } else { + swerve_Y_speed = 1.5 / Y_from_camera - 1; + } + } else if (Y_from_camera >= -0.5) { + if (Y_from_camera <= -2) { + swerve_Y_speed = -1; + } else { + swerve_Y_speed = 1.5 / Y_from_camera + 1; + } + } else { + shoot_y = true; + } + + if (shoot_y && shoot_x) { + shoot = true; + } + + /* Drive */ + s_Swerve.drive( + new Translation2d(swerve_X_speed, swerve_Y_speed).times(Constants.Swerve.maxSpeed), + (rotationVal * throtleVal) * Constants.Swerve.maxAngularVelocity, + !robotCentricSup.getAsBoolean(), + true + ); + } + + @Override + public boolean isFinished() { + return overrideButton.getAsBoolean(); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index f859561..7fbc42e 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -26,7 +26,7 @@ /* -NetworkTableInstance.getDefault().getTable("limelight").getEntry("priorityid").setNumber(8); +NetworkTableInstance.getDefault().getTable("limelight").getEntry("priorityid").setNumber(7); int tag_id = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tid").getNumber(); double X_from_camera = NetworkTableInstance.getDefault().getTable("limelight").getEntry("targetpose_robotspace").getDoubleArray(new double[0]); double Y_from_camera = NetworkTableInstance.getDefault().getTable("limelight").getEntry("targetpose_robotspace").getDoubleArray(new double[1]); From 5c3409d39e3118c795ced2a0437f5fafa87b5fdf Mon Sep 17 00:00:00 2001 From: StoneCommander <67015041+StoneCommander@users.noreply.github.com> Date: Tue, 5 Mar 2024 18:22:43 -0700 Subject: [PATCH 06/12] update --- src/main/java/frc/robot/RobotContainer.java | 8 ++++---- .../java/frc/robot/commands/TargetSwerve.java | 18 +++++++++++++----- 2 files changed, 17 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8b41da0..cd1345a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -214,10 +214,10 @@ private void configureButtonBindings() { new JoystickButton(driver, 4).onTrue( new TargetSwerve( s_Swerve, - () -> getTranslationAxisMaybe(), - () -> getStrafeAxisMaybe(), - () -> getRotationAxisMaybe(), - () -> getThrottleAxisMaybe(), + () -> -driver.getRawAxis(translationAxis), + () -> -driver.getRawAxis(strafeAxis), + () -> -driver.getRawAxis(rotationAxis), + () -> -driver.getRawAxis(throttleAxis), () -> robotCentric.getAsBoolean(), theLEDs, new JoystickButton(driver, 4) diff --git a/src/main/java/frc/robot/commands/TargetSwerve.java b/src/main/java/frc/robot/commands/TargetSwerve.java index a39faad..c1368b9 100644 --- a/src/main/java/frc/robot/commands/TargetSwerve.java +++ b/src/main/java/frc/robot/commands/TargetSwerve.java @@ -31,6 +31,7 @@ public class TargetSwerve extends Command { private boolean shoot_x; private boolean shoot_y; private boolean shoot; + private Boolean bHold; private LEDS led; @@ -57,8 +58,15 @@ public TargetSwerve(Swerve s_Swerve, DoubleSupplier translationSup, DoubleSuppli this.led = led; } + + @Override + public void initialize() { + this.bHold = true; + } + @Override public void execute() { + if (bHold) bHold = overrideButton.getAsBoolean(); /* Get Values, Deadband*/ double translationVal = MathUtil.applyDeadband(translationSup.getAsDouble(), Constants.stickDeadband); double strafeVal = MathUtil.applyDeadband(strafeSup.getAsDouble(), Constants.stickDeadband); @@ -81,13 +89,13 @@ public void execute() { if (X_from_camera >= 2) { swerve_X_speed = 1; } else { - swerve_X_speed = 1.5 / X_from_camera - 1; + swerve_X_speed = 1.5 / X_from_camera + 1; } } else if (X_from_camera >= -0.5) { if (X_from_camera <= -2) { swerve_X_speed = -1; } else { - swerve_X_speed = 1.5 / X_from_camera + 1; + swerve_X_speed = 1.5 / X_from_camera - 1; } } else { shoot_x = true; @@ -97,13 +105,13 @@ public void execute() { if (Y_from_camera >= 2) { swerve_Y_speed = 1; } else { - swerve_Y_speed = 1.5 / Y_from_camera - 1; + swerve_Y_speed = 1.5 / Y_from_camera + 1; } } else if (Y_from_camera >= -0.5) { if (Y_from_camera <= -2) { swerve_Y_speed = -1; } else { - swerve_Y_speed = 1.5 / Y_from_camera + 1; + swerve_Y_speed = 1.5 / Y_from_camera - 1; } } else { shoot_y = true; @@ -124,6 +132,6 @@ public void execute() { @Override public boolean isFinished() { - return overrideButton.getAsBoolean(); + return (overrideButton.getAsBoolean() && !bHold); } } \ No newline at end of file From a83276b51ba02d02c97ae62bf1fd5b764cdfbf46 Mon Sep 17 00:00:00 2001 From: StoneCommander <67015041+StoneCommander@users.noreply.github.com> Date: Sat, 16 Mar 2024 08:22:51 -0600 Subject: [PATCH 07/12] targetSwever work. --- src/main/java/frc/robot/Constants.java | 17 ++- src/main/java/frc/robot/RobotContainer.java | 96 ++++++++----- .../frc/robot/commands/LaunchControled.java | 7 +- .../java/frc/robot/commands/TargetSwerve.java | 133 ++++++++++++------ src/main/java/frc/robot/subsystems/LEDS.java | 10 +- 5 files changed, 179 insertions(+), 84 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index df70e97..558fa04 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -189,6 +189,17 @@ public static final class Launcher { public static final double InPoint = -1000; } + public static final class TargetSwerve { + // public static final double P = 0.05; + // public static final double I = 0; + // public static final double D = 0.001; + public static final double P = 0.08; + public static final double I = 0; + public static final double D = 0.003; + + + } + // holds constants for mode system public static final class Mode { public static final double blinkTime = 5.0; @@ -213,8 +224,10 @@ public static final class Colors { public static final Color IA = Color.kPink; // Intake Amp Color public static final Color N = Color.kYellow; // Note Color public static final Color LS = Color.kGreen; // Launcher Spinup Color - public static final Color L = Color.kAqua; // Color Color - public static final Color DIS = Color.kPurple; //Disable Color + public static final Color LT = Color.kPurple; // Launcher Targeting Color + public static final Color LK = Color.kGreen; // Launcher Lock Color + public static final Color L = Color.kAqua; // Launcher Launching Color + // public static final Color DIS = Color.kPurple;//Disable Color } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index cd1345a..477a0ed 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -1,46 +1,45 @@ package frc.robot; import org.opencv.core.Mat; -import org.opencv.core.Point; -import org.opencv.core.MatOfPoint; -import org.opencv.core.Scalar; -import org.opencv.imgproc.Imgproc; -import com.ctre.phoenix6.Orchestra; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.path.PathPlannerPath; -import com.revrobotics.CANSparkMax; import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.cscore.CvSink; import edu.wpi.first.cscore.CvSource; import edu.wpi.first.cscore.UsbCamera; -import edu.wpi.first.networktables.DoubleTopic; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.util.sendable.Sendable; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.PowerDistribution; import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; - -import java.util.ArrayList; -import java.util.List; - -import javax.management.remote.TargetedNotification; - -import frc.robot.autos.*; -import frc.robot.commands.*; -import frc.robot.subsystems.*; +import frc.robot.commands.IntakeAmp; +import frc.robot.commands.IntakeDefault; +import frc.robot.commands.IntakeFix; +import frc.robot.commands.IntakeRun; +import frc.robot.commands.LaunchASAP; +import frc.robot.commands.LaunchControled; +import frc.robot.commands.TargetSwerve; +import frc.robot.commands.TeleopSwerve; +import frc.robot.subsystems.Band; import frc.robot.subsystems.Intake; +import frc.robot.subsystems.LEDS; import frc.robot.subsystems.Launcher; +import frc.robot.subsystems.Swerve; /** * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -87,7 +86,7 @@ public class RobotContainer { private final NetworkTableInstance tableInstance = NetworkTableInstance.getDefault(); /* Subsystems */ - private final LEDS theLEDs = new LEDS(0,60); + private final LEDS theLEDs = new LEDS(9,60); private final Band theBand = new Band(); @@ -107,6 +106,9 @@ public class RobotContainer { private final SendableChooser autoChooser; private final SendableChooser musiChooser; + private Field2d m_field = new Field2d(); + private Field2d m_LLfield = new Field2d(); + private Thread m_visionThread; @@ -195,6 +197,15 @@ public RobotContainer() { }); m_visionThread.setDaemon(true); m_visionThread.start(); + + + + + SmartDashboard.putNumber("Xpid", 0); + SmartDashboard.putNumber("Ypid", 0); + SmartDashboard.putBoolean("Xshoot", false); + SmartDashboard.putBoolean("Yshoot", false); + SmartDashboard.putBoolean("Shoot", false); } /** @@ -220,9 +231,12 @@ private void configureButtonBindings() { () -> -driver.getRawAxis(throttleAxis), () -> robotCentric.getAsBoolean(), theLEDs, - new JoystickButton(driver, 4) + launcher, + intake, + new JoystickButton(driver, 4), + new JoystickButton(secondary, 6) )); - new JoystickButton(driver, 3).onTrue(smartLaunch()); + // new JoystickButton(driver, 3).onTrue(smartLaunch()); } public void updateInfo() { @@ -240,23 +254,39 @@ public void updateInfo() { SmartDashboard.putBoolean("LS", theLEDs.getMode() == "LS"); SmartDashboard.putBoolean("L", theLEDs.getMode() == "L"); + Pose2d pose = s_Swerve.getPose(); + m_field.setRobotPose(pose); + + // NetworkTableInstance.getDefault().getTable("limelight").getEntry("priorityid").setNumber(7); + NetworkTable LLtbl = NetworkTableInstance.getDefault().getTable("limelight"); + double[] LLpose = LLtbl.getEntry("botpose").getDoubleArray(new double[6]); + + + m_LLfield.setRobotPose(new Pose2d(LLpose[0], LLpose[1], new Rotation2d(LLpose[3],LLpose[4]))); + SmartDashboard.putData("intake", intake); SmartDashboard.putData("Launcher", launcher); SmartDashboard.putData("Swerve", s_Swerve); SmartDashboard.putData("Gyro",s_Swerve.gyro); - SmartDashboard.putBoolean("ControlorA", new JoystickButton(secondary, 1).getAsBoolean()); - SmartDashboard.putBoolean("ControlorB", new JoystickButton(secondary, 2).getAsBoolean()); - SmartDashboard.putBoolean("ControlorX", new JoystickButton(secondary, 3).getAsBoolean()); - - SmartDashboard.putData(PDP); - System.out.println("Radio"); - System.out.println(PDP.getCurrent(15)); - System.out.println("RIO"); - System.out.println(PDP.getCurrent(20)); - System.out.println("Total"); - System.out.println(PDP.getTotalCurrent()); - - intake.updateData(); + SmartDashboard.putData("BotPose", m_field); + SmartDashboard.putData("BotPoseLL", m_LLfield); + // SmartDashboard.putBoolean("ControlorA", new JoystickButton(secondary, 1).getAsBoolean()); + // SmartDashboard.putBoolean("ControlorB", new JoystickButton(secondary, 2).getAsBoolean()); + // SmartDashboard.putBoolean("ControlorX", new JoystickButton(secondary, 3).getAsBoolean()); + + // SmartDashboard.putData(PDP); + // System.out.println("Radio"); + // System.out.println(PDP.getCurrent(15)); + // System.out.println("RIO"); + // System.out.println(PDP.getCurrent(20)); + // System.out.println("Total"); + // System.out.println(PDP.getTotalCurrent()); + + + // NetworkTableInstance.getDefault().getTable("limelight").getEntry("priorityid").setNumber(7); + // NetworkTable LLtbl = NetworkTableInstance.getDefault().getTable("limelight"); + + // intake.updateData(); double throtleVal = (driver.getRawAxis(throttleAxis)+1)/2; SmartDashboard.putNumber("Throttle", throtleVal); diff --git a/src/main/java/frc/robot/commands/LaunchControled.java b/src/main/java/frc/robot/commands/LaunchControled.java index e7c19e5..07b56b4 100644 --- a/src/main/java/frc/robot/commands/LaunchControled.java +++ b/src/main/java/frc/robot/commands/LaunchControled.java @@ -14,6 +14,7 @@ public class LaunchControled extends Command{ private Launcher s_Launcher; private Timer timer; private boolean launch; + private boolean bHold; private LEDS theLEDs; private JoystickButton overrideButton; private JoystickButton launchButton; @@ -25,6 +26,7 @@ public LaunchControled(Intake s_Intake, Launcher s_Launcher, LEDS theLEDs, Joyst this.timer = new Timer(); this.launch = false; this.theLEDs = theLEDs; + this.bHold = true; this.overrideButton = overrideButton; this.launchButton = launchButton; @@ -37,6 +39,7 @@ public void initialize() { timer = new Timer(); // timer.start(); launch = false; + bHold = true; s_Intake.lightPull(false); s_Launcher.startLaunch(); System.out.println("Start LaunchASAP"); @@ -47,7 +50,7 @@ public void initialize() { @Override public void execute() { - + if (bHold) bHold = overrideButton.getAsBoolean(); if (launchButton.getAsBoolean()) { timer.start(); launch = true; @@ -69,6 +72,6 @@ public void end(boolean interrupted) { @Override public boolean isFinished() { - return (timer.get() >= (Constants.Launcher.LaunchStopTime) && launch); + return (timer.get() >= (Constants.Launcher.LaunchStopTime) && launch) || (overrideButton.getAsBoolean() && !bHold); } } diff --git a/src/main/java/frc/robot/commands/TargetSwerve.java b/src/main/java/frc/robot/commands/TargetSwerve.java index c1368b9..373765a 100644 --- a/src/main/java/frc/robot/commands/TargetSwerve.java +++ b/src/main/java/frc/robot/commands/TargetSwerve.java @@ -2,8 +2,10 @@ import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -11,12 +13,16 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants; import frc.robot.subsystems.LEDS; +import frc.robot.subsystems.Launcher; +import frc.robot.subsystems.Intake; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc.robot.subsystems.Swerve; public class TargetSwerve extends Command { private Swerve s_Swerve; + private Launcher s_Launcher; + private Intake s_Intake; private DoubleSupplier translationSup; private DoubleSupplier strafeSup; private DoubleSupplier rotationSup; @@ -24,14 +30,28 @@ public class TargetSwerve extends Command { private BooleanSupplier robotCentricSup; private JoystickButton overrideButton; + private JoystickButton launchButton; private double swerve_X_speed; private double swerve_Y_speed; + private double swerve_X_speedt; + private double swerve_Y_speedt; private boolean shoot_x; private boolean shoot_y; + private boolean shoot; + private boolean spin; + private boolean end; + private Timer timer; + + + private Boolean bHold; + private NetworkTable LLtbl; + + private PIDController Xpid; + private PIDController Ypid; private LEDS led; @@ -45,9 +65,13 @@ public class TargetSwerve extends Command { * @param rotationSup Rotation suplier, typicaly a controlor axis * @param robotCentricSup wether to drive relative to the robot. */ - public TargetSwerve(Swerve s_Swerve, DoubleSupplier translationSup, DoubleSupplier strafeSup, DoubleSupplier rotationSup, DoubleSupplier throtleSupplier, BooleanSupplier robotCentricSup, LEDS led, JoystickButton overrideButton) { + public TargetSwerve(Swerve s_Swerve, DoubleSupplier translationSup, DoubleSupplier strafeSup, DoubleSupplier rotationSup, DoubleSupplier throtleSupplier, BooleanSupplier robotCentricSup, LEDS led, Launcher s_Launcher, Intake s_Intake, JoystickButton overrideButton, JoystickButton launchButton) { this.s_Swerve = s_Swerve; + this.s_Launcher = s_Launcher; + this.s_Intake = s_Intake; addRequirements(s_Swerve); + addRequirements(s_Launcher); + addRequirements(s_Intake); this.translationSup = translationSup; this.strafeSup = strafeSup; @@ -55,6 +79,7 @@ public TargetSwerve(Swerve s_Swerve, DoubleSupplier translationSup, DoubleSuppli this.robotCentricSup = robotCentricSup; this.throtleSupplier = throtleSupplier; this.overrideButton = overrideButton; + this.launchButton = launchButton; this.led = led; } @@ -62,6 +87,28 @@ public TargetSwerve(Swerve s_Swerve, DoubleSupplier translationSup, DoubleSuppli @Override public void initialize() { this.bHold = true; + + Xpid = new PIDController(Constants.TargetSwerve.P, Constants.TargetSwerve.I, Constants.TargetSwerve.D); + Ypid = new PIDController(Constants.TargetSwerve.P, Constants.TargetSwerve.I, Constants.TargetSwerve.D); + + Xpid.setTolerance(3); + Ypid.setTolerance(3); + + Xpid.setSetpoint(0); + Ypid.setSetpoint(0); + + timer = new Timer(); + shoot = false; + spin = false; + end = false; + + led.setMode("LT"); + System.out.println("set LT"); + + s_Launcher.startLaunch(); + s_Intake.lightPull(false); + + LLtbl = NetworkTableInstance.getDefault().getTable("limelight"); } @Override @@ -72,66 +119,60 @@ public void execute() { double strafeVal = MathUtil.applyDeadband(strafeSup.getAsDouble(), Constants.stickDeadband); double rotationVal = MathUtil.applyDeadband(rotationSup.getAsDouble(), Constants.stickDeadband); double throtleVal = 1-((throtleSupplier.getAsDouble()+1)/2); - // SmartDashboard.putNumber("Throttle", throtleVal); - - // SmartDashboard.get - NetworkTableInstance.getDefault().getTable("limelight").getEntry("priorityid").setNumber(7); - NetworkTable LLtbl = NetworkTableInstance.getDefault().getTable("limelight"); - // int tag_id = LLtbl.getEntry("tid"). - // double X_from_camera = NetworkTableInstance.getDefault().getTable("limelight").getEntry("targetpose_robotspace").getDoubleArray(new double[1]); - // double Y_from_camera = NetworkTableInstance.getDefault().getTable("limelight").getEntry("targetpose_robotspace").getDoubleArray(new double[1]); - // double X_rotation_from_camera = NetworkTableInstance.getDefault().getTable("limelight").getEntry("targetpose_robotspace").getDoubleArray(new double[1]); double X_from_camera = LLtbl.getValue("tx").getDouble(); double Y_from_camera = LLtbl.getValue("ty").getDouble(); - if (X_from_camera <= 0.5) { - if (X_from_camera >= 2) { - swerve_X_speed = 1; - } else { - swerve_X_speed = 1.5 / X_from_camera + 1; - } - } else if (X_from_camera >= -0.5) { - if (X_from_camera <= -2) { - swerve_X_speed = -1; - } else { - swerve_X_speed = 1.5 / X_from_camera - 1; - } - } else { - shoot_x = true; - } - if (Y_from_camera <= 0.5) { - if (Y_from_camera >= 2) { - swerve_Y_speed = 1; - } else { - swerve_Y_speed = 1.5 / Y_from_camera + 1; - } - } else if (Y_from_camera >= -0.5) { - if (Y_from_camera <= -2) { - swerve_Y_speed = -1; - } else { - swerve_Y_speed = 1.5 / Y_from_camera - 1; - } - } else { - shoot_y = true; + swerve_X_speed = MathUtil.clamp(Xpid.calculate(X_from_camera), -1,1); + swerve_Y_speed = MathUtil.clamp(Ypid.calculate(Y_from_camera), -1,1); + + shoot = Xpid.atSetpoint() && Ypid.atSetpoint(); + if(Xpid.atSetpoint()) swerve_X_speed = 0; + if(Ypid.atSetpoint()) swerve_Y_speed = 0; + + if(shoot && !end && led.getMode()!="LK") { + led.setMode("LK"); + System.out.println("set LK"); + } else if (!end && led.getMode()!="LT") { + led.setMode("LT"); + System.out.println("set LT"); + } + + if(launchButton.getAsBoolean() && !end) { + s_Intake.pushIntake(false); + end = true; + timer.start(); + led.setMode("L"); + System.out.println("set L"); } - if (shoot_y && shoot_x) { - shoot = true; - } + SmartDashboard.putNumber("Xpid", swerve_X_speed); + SmartDashboard.putNumber("Ypid", swerve_Y_speed); + SmartDashboard.putBoolean("Xshoot", Xpid.atSetpoint()); + SmartDashboard.putBoolean("Yshoot", Ypid.atSetpoint()); + SmartDashboard.putBoolean("Shoot", shoot); /* Drive */ s_Swerve.drive( - new Translation2d(swerve_X_speed, swerve_Y_speed).times(Constants.Swerve.maxSpeed), + new Translation2d(-swerve_Y_speed * throtleVal, -swerve_X_speed * throtleVal).times(Constants.Swerve.maxSpeed), (rotationVal * throtleVal) * Constants.Swerve.maxAngularVelocity, - !robotCentricSup.getAsBoolean(), - true + false, + false ); } + + @Override + public void end(boolean interrupted) { + s_Intake.pushIntake(true); + s_Launcher.endLaunch(); + led.setMode("D"); + System.out.println("set D"); + } @Override public boolean isFinished() { - return (overrideButton.getAsBoolean() && !bHold); + + return (overrideButton.getAsBoolean() && !bHold) || (end && timer.get() >= Constants.Launcher.LaunchStopTime); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/LEDS.java b/src/main/java/frc/robot/subsystems/LEDS.java index a68592c..d8d2da2 100644 --- a/src/main/java/frc/robot/subsystems/LEDS.java +++ b/src/main/java/frc/robot/subsystems/LEDS.java @@ -142,6 +142,14 @@ public void updateMode() { SetFull(Constants.Mode.Colors.LS); System.out.println("LS"); break; + case "LT": + SetFull(Constants.Mode.Colors.LT); + System.out.println("LT"); + break; + case "LK": + SetFull(Constants.Mode.Colors.LK); + System.out.println("LK"); + break; case "L": SetFull(Constants.Mode.Colors.L); System.out.println("L"); @@ -153,7 +161,7 @@ public void updateMode() { } public void setMode(String modeID) { - String[] vModes = {"D","IO","IR","IF","IA","N","LS","L"}; + String[] vModes = {"D","IO","IR","IF","IA","N","LS","L","LT","LK"}; if (Arrays.asList(vModes).contains(modeID)) { mode = modeID; } From c41ff9e6f2ba65ef14893f3f26c0fa1fc0370808 Mon Sep 17 00:00:00 2001 From: StoneCommander <67015041+StoneCommander@users.noreply.github.com> Date: Mon, 18 Mar 2024 10:46:23 -0600 Subject: [PATCH 08/12] update git ignore --- .gitignore | 1 + 1 file changed, 1 insertion(+) diff --git a/.gitignore b/.gitignore index 5528d4f..4d15a70 100644 --- a/.gitignore +++ b/.gitignore @@ -176,3 +176,4 @@ logs/ # Folder that has CTRE Phoenix Sim device config storage ctre_sim/ +src/main/java/frc/robot/BuildConstants.java From e5ff1052e02120e0ffbe2ba1cd9927469b9e9d5f Mon Sep 17 00:00:00 2001 From: StoneCommander <67015041+StoneCommander@users.noreply.github.com> Date: Wed, 20 Mar 2024 10:30:59 -0600 Subject: [PATCH 09/12] more work --- .pathplanner/settings.json | 2 +- build.gradle | 2 +- .../deploy/pathplanner/autos/Mid4Lime.auto | 100 +++++++++++ .../deploy/pathplanner/autos/New Auto.auto | 25 --- src/main/deploy/pathplanner/autos/test.auto | 8 +- .../deploy/pathplanner/paths/BTM-SHOOT.path | 2 +- .../deploy/pathplanner/paths/BTM-TAXI.path | 2 +- .../deploy/pathplanner/paths/BTM-TO-2.path | 2 +- .../deploy/pathplanner/paths/BTM-TO-3.path | 2 +- .../deploy/pathplanner/paths/BTM-TO-M4.path | 2 +- .../deploy/pathplanner/paths/BTM-TO-M5.path | 2 +- .../deploy/pathplanner/paths/M-SHOOT.path | 6 +- src/main/deploy/pathplanner/paths/M-TO-1.path | 6 +- src/main/deploy/pathplanner/paths/M-TO-2.path | 6 +- src/main/deploy/pathplanner/paths/M-TO-3.path | 6 +- .../deploy/pathplanner/paths/MB-TO-SHOOT.path | 2 +- .../pathplanner/paths/SmartLaunchMid.path | 6 +- .../deploy/pathplanner/paths/T-SHOOT.path | 2 +- src/main/deploy/pathplanner/paths/T-TO-1.path | 2 +- src/main/deploy/pathplanner/paths/T-TO-2.path | 2 +- src/main/deploy/pathplanner/paths/T-TO-3.path | 2 +- .../deploy/pathplanner/paths/T-TO-M1.path | 2 +- .../deploy/pathplanner/paths/T-TO-M2.path | 2 +- .../deploy/pathplanner/paths/T-TO-M3.path | 2 +- src/main/deploy/pathplanner/paths/TO-1.path | 52 ++++++ src/main/deploy/pathplanner/paths/TO-2.path | 52 ++++++ src/main/deploy/pathplanner/paths/TO-3.path | 52 ++++++ src/main/java/frc/robot/Constants.java | 12 +- src/main/java/frc/robot/RobotContainer.java | 9 + .../java/frc/robot/commands/TargetSwerve.java | 24 ++- .../frc/robot/commands/TargetSwerveAuto.java | 158 ++++++++++++++++++ .../java/frc/robot/subsystems/Intake.java | 13 +- 32 files changed, 493 insertions(+), 74 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/Mid4Lime.auto delete mode 100644 src/main/deploy/pathplanner/autos/New Auto.auto create mode 100644 src/main/deploy/pathplanner/paths/TO-1.path create mode 100644 src/main/deploy/pathplanner/paths/TO-2.path create mode 100644 src/main/deploy/pathplanner/paths/TO-3.path create mode 100644 src/main/java/frc/robot/commands/TargetSwerveAuto.java diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index 58c4307..59652a2 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -11,7 +11,7 @@ "autoFolders": [ "TEST" ], - "defaultMaxVel": 3.0, + "defaultMaxVel": 4.0, "defaultMaxAccel": 3.0, "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, diff --git a/build.gradle b/build.gradle index c73a804..462fb1d 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.3.2" } java { diff --git a/src/main/deploy/pathplanner/autos/Mid4Lime.auto b/src/main/deploy/pathplanner/autos/Mid4Lime.auto new file mode 100644 index 0000000..880137f --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Mid4Lime.auto @@ -0,0 +1,100 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.171904793952038, + "y": 5.531335051274091 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "TargetSwerve" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "TO-2" + } + }, + { + "type": "named", + "data": { + "name": "RunIntake" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "TargetSwerve" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "TO-3" + } + }, + { + "type": "named", + "data": { + "name": "RunIntake" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "TargetSwerve" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "TO-1" + } + }, + { + "type": "named", + "data": { + "name": "RunIntake" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "TargetSwerve" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/New Auto.auto b/src/main/deploy/pathplanner/autos/New Auto.auto deleted file mode 100644 index c4472d3..0000000 --- a/src/main/deploy/pathplanner/autos/New Auto.auto +++ /dev/null @@ -1,25 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 1.160210639509598, - "y": 5.531335051274091 - }, - "rotation": 0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "SmartLaunchMid" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/test.auto b/src/main/deploy/pathplanner/autos/test.auto index 9a003af..eb1171f 100644 --- a/src/main/deploy/pathplanner/autos/test.auto +++ b/src/main/deploy/pathplanner/autos/test.auto @@ -11,16 +11,10 @@ "type": "sequential", "data": { "commands": [ - { - "type": "path", - "data": { - "pathName": "M-SHOOT" - } - }, { "type": "named", "data": { - "name": "LaunchASAP" + "name": "TargetSwerve" } } ] diff --git a/src/main/deploy/pathplanner/paths/BTM-SHOOT.path b/src/main/deploy/pathplanner/paths/BTM-SHOOT.path index 6d42fb5..0297766 100644 --- a/src/main/deploy/pathplanner/paths/BTM-SHOOT.path +++ b/src/main/deploy/pathplanner/paths/BTM-SHOOT.path @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, + "maxVelocity": 4.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 diff --git a/src/main/deploy/pathplanner/paths/BTM-TAXI.path b/src/main/deploy/pathplanner/paths/BTM-TAXI.path index 6ac425c..3e3074e 100644 --- a/src/main/deploy/pathplanner/paths/BTM-TAXI.path +++ b/src/main/deploy/pathplanner/paths/BTM-TAXI.path @@ -44,7 +44,7 @@ ], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, + "maxVelocity": 4.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 diff --git a/src/main/deploy/pathplanner/paths/BTM-TO-2.path b/src/main/deploy/pathplanner/paths/BTM-TO-2.path index 642b0a9..e1d780f 100644 --- a/src/main/deploy/pathplanner/paths/BTM-TO-2.path +++ b/src/main/deploy/pathplanner/paths/BTM-TO-2.path @@ -43,7 +43,7 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, + "maxVelocity": 4.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 diff --git a/src/main/deploy/pathplanner/paths/BTM-TO-3.path b/src/main/deploy/pathplanner/paths/BTM-TO-3.path index 72d8c43..029d8c8 100644 --- a/src/main/deploy/pathplanner/paths/BTM-TO-3.path +++ b/src/main/deploy/pathplanner/paths/BTM-TO-3.path @@ -50,7 +50,7 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, + "maxVelocity": 4.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 diff --git a/src/main/deploy/pathplanner/paths/BTM-TO-M4.path b/src/main/deploy/pathplanner/paths/BTM-TO-M4.path index 2ba3da8..939a6f5 100644 --- a/src/main/deploy/pathplanner/paths/BTM-TO-M4.path +++ b/src/main/deploy/pathplanner/paths/BTM-TO-M4.path @@ -68,7 +68,7 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, + "maxVelocity": 4.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 diff --git a/src/main/deploy/pathplanner/paths/BTM-TO-M5.path b/src/main/deploy/pathplanner/paths/BTM-TO-M5.path index 5c38c08..20ecae1 100644 --- a/src/main/deploy/pathplanner/paths/BTM-TO-M5.path +++ b/src/main/deploy/pathplanner/paths/BTM-TO-M5.path @@ -62,7 +62,7 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, + "maxVelocity": 4.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 diff --git a/src/main/deploy/pathplanner/paths/M-SHOOT.path b/src/main/deploy/pathplanner/paths/M-SHOOT.path index 4c9740b..e4835d6 100644 --- a/src/main/deploy/pathplanner/paths/M-SHOOT.path +++ b/src/main/deploy/pathplanner/paths/M-SHOOT.path @@ -16,11 +16,11 @@ }, { "anchor": { - "x": 1.5928943538798759, + "x": 1.4993411183403562, "y": 5.5430292057165325 }, "prevControl": { - "x": 1.5333576812770562, + "x": 1.4398044457375365, "y": 5.535587121641179 }, "nextControl": null, @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, + "maxVelocity": 4.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 diff --git a/src/main/deploy/pathplanner/paths/M-TO-1.path b/src/main/deploy/pathplanner/paths/M-TO-1.path index bb9a10b..0d74a7b 100644 --- a/src/main/deploy/pathplanner/paths/M-TO-1.path +++ b/src/main/deploy/pathplanner/paths/M-TO-1.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 1.5928943538798759, + "x": 1.4993411183403562, "y": 5.5430292057165325 }, "prevControl": null, "nextControl": { - "x": 1.5812001994374358, + "x": 1.4876469638979162, "y": 6.303149244475128 }, "isLocked": false, @@ -50,7 +50,7 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, + "maxVelocity": 4.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 diff --git a/src/main/deploy/pathplanner/paths/M-TO-2.path b/src/main/deploy/pathplanner/paths/M-TO-2.path index 5d3fc47..8634bec 100644 --- a/src/main/deploy/pathplanner/paths/M-TO-2.path +++ b/src/main/deploy/pathplanner/paths/M-TO-2.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 1.5928943538798759, + "x": 1.4993411183403562, "y": 5.5430292057165325 }, "prevControl": null, "nextControl": { - "x": 1.663059280534515, + "x": 1.5695060449949956, "y": 5.55472336015897 }, "isLocked": false, @@ -62,7 +62,7 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, + "maxVelocity": 4.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 diff --git a/src/main/deploy/pathplanner/paths/M-TO-3.path b/src/main/deploy/pathplanner/paths/M-TO-3.path index 75e82f4..a7bd9a6 100644 --- a/src/main/deploy/pathplanner/paths/M-TO-3.path +++ b/src/main/deploy/pathplanner/paths/M-TO-3.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 1.5928943538798759, + "x": 1.4993411183403562, "y": 5.5430292057165325 }, "prevControl": null, "nextControl": { - "x": 1.3005404928188773, + "x": 1.2069872572793576, "y": 4.420390379242298 }, "isLocked": false, @@ -50,7 +50,7 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, + "maxVelocity": 4.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 diff --git a/src/main/deploy/pathplanner/paths/MB-TO-SHOOT.path b/src/main/deploy/pathplanner/paths/MB-TO-SHOOT.path index 7000475..9b2d3ea 100644 --- a/src/main/deploy/pathplanner/paths/MB-TO-SHOOT.path +++ b/src/main/deploy/pathplanner/paths/MB-TO-SHOOT.path @@ -44,7 +44,7 @@ ], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, + "maxVelocity": 4.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 diff --git a/src/main/deploy/pathplanner/paths/SmartLaunchMid.path b/src/main/deploy/pathplanner/paths/SmartLaunchMid.path index 803b271..e480076 100644 --- a/src/main/deploy/pathplanner/paths/SmartLaunchMid.path +++ b/src/main/deploy/pathplanner/paths/SmartLaunchMid.path @@ -16,11 +16,11 @@ }, { "anchor": { - "x": 1.5928943538798759, + "x": 1.4993411183403562, "y": 5.5430292057165325 }, "prevControl": { - "x": 1.3005404928188773, + "x": 1.2069872572793576, "y": 5.5430292057165325 }, "nextControl": null, @@ -50,7 +50,7 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, + "maxVelocity": 4.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 diff --git a/src/main/deploy/pathplanner/paths/T-SHOOT.path b/src/main/deploy/pathplanner/paths/T-SHOOT.path index a6efb21..ecd5cd5 100644 --- a/src/main/deploy/pathplanner/paths/T-SHOOT.path +++ b/src/main/deploy/pathplanner/paths/T-SHOOT.path @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, + "maxVelocity": 4.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 diff --git a/src/main/deploy/pathplanner/paths/T-TO-1.path b/src/main/deploy/pathplanner/paths/T-TO-1.path index 7fbf13c..889d102 100644 --- a/src/main/deploy/pathplanner/paths/T-TO-1.path +++ b/src/main/deploy/pathplanner/paths/T-TO-1.path @@ -50,7 +50,7 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, + "maxVelocity": 4.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 diff --git a/src/main/deploy/pathplanner/paths/T-TO-2.path b/src/main/deploy/pathplanner/paths/T-TO-2.path index 960ca4a..04055a7 100644 --- a/src/main/deploy/pathplanner/paths/T-TO-2.path +++ b/src/main/deploy/pathplanner/paths/T-TO-2.path @@ -50,7 +50,7 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, + "maxVelocity": 4.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 diff --git a/src/main/deploy/pathplanner/paths/T-TO-3.path b/src/main/deploy/pathplanner/paths/T-TO-3.path index 8463bc3..0fe36a6 100644 --- a/src/main/deploy/pathplanner/paths/T-TO-3.path +++ b/src/main/deploy/pathplanner/paths/T-TO-3.path @@ -50,7 +50,7 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, + "maxVelocity": 4.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 diff --git a/src/main/deploy/pathplanner/paths/T-TO-M1.path b/src/main/deploy/pathplanner/paths/T-TO-M1.path index 6001ca6..89d1d73 100644 --- a/src/main/deploy/pathplanner/paths/T-TO-M1.path +++ b/src/main/deploy/pathplanner/paths/T-TO-M1.path @@ -77,7 +77,7 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, + "maxVelocity": 4.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 diff --git a/src/main/deploy/pathplanner/paths/T-TO-M2.path b/src/main/deploy/pathplanner/paths/T-TO-M2.path index 5dfa5e0..b4cd3a4 100644 --- a/src/main/deploy/pathplanner/paths/T-TO-M2.path +++ b/src/main/deploy/pathplanner/paths/T-TO-M2.path @@ -77,7 +77,7 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, + "maxVelocity": 4.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 diff --git a/src/main/deploy/pathplanner/paths/T-TO-M3.path b/src/main/deploy/pathplanner/paths/T-TO-M3.path index dd71b42..962666a 100644 --- a/src/main/deploy/pathplanner/paths/T-TO-M3.path +++ b/src/main/deploy/pathplanner/paths/T-TO-M3.path @@ -75,7 +75,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, + "maxVelocity": 4.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 diff --git a/src/main/deploy/pathplanner/paths/TO-1.path b/src/main/deploy/pathplanner/paths/TO-1.path new file mode 100644 index 0000000..3013b0d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/TO-1.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.037272222692593, + "y": 6.0692661556263285 + }, + "prevControl": null, + "nextControl": { + "x": 2.329626083753592, + "y": 6.256372626705367 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.832474724778509, + "y": 6.876162812154685 + }, + "prevControl": { + "x": 2.4582617826204305, + "y": 6.607197259978565 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 57.99461679191646, + "rotateFast": false + }, + "reversed": false, + "folder": "MID", + "previewStartingState": { + "rotation": 35.537677791974396, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/TO-2.path b/src/main/deploy/pathplanner/paths/TO-2.path new file mode 100644 index 0000000..acf125d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/TO-2.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.9904956049228335, + "y": 5.566417514601411 + }, + "prevControl": null, + "nextControl": { + "x": 2.1900526364941553, + "y": 5.5797213167061654 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.59859163592971, + "y": 5.566417514601411 + }, + "prevControl": { + "x": 2.1776020760018726, + "y": 5.589805823486292 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0, + "rotateFast": false + }, + "reversed": false, + "folder": "MID", + "previewStartingState": { + "rotation": 4.3987053549955135, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/TO-3.path b/src/main/deploy/pathplanner/paths/TO-3.path new file mode 100644 index 0000000..2bb2b20 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/TO-3.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.9554131415955136, + "y": 4.934933174709655 + }, + "prevControl": null, + "nextControl": { + "x": 2.247767002656512, + "y": 4.572414386994017 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.68045071702679, + "y": 4.198201444835939 + }, + "prevControl": { + "x": 2.4231793192931113, + "y": 4.373613761472538 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -34.50852298766841, + "rotateFast": false + }, + "reversed": false, + "folder": "MID", + "previewStartingState": { + "rotation": -35.537677791974374, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ 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 558fa04..2d9af7b 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -175,6 +175,7 @@ public static final class Launcher { public static final double P = 0.08; public static final double I = 0.05; public static final double D = 0.00; + public static final double FF = 0.000156; @@ -187,6 +188,7 @@ public static final class Launcher { public static final double OutPoint = 1000; public static final double InPoint = -1000; + } public static final class TargetSwerve { @@ -196,6 +198,8 @@ public static final class TargetSwerve { public static final double P = 0.08; public static final double I = 0; public static final double D = 0.003; + + public static final double SmartLaunchTime = 0.5; } @@ -232,10 +236,10 @@ public static final class Colors { } public static final class AutoConstants { //TODO: The below constants are used in the example auto, and must be tuned to specific robot - public static final double kMaxSpeedMetersPerSecond = 1; - public static final double kMaxAccelerationMetersPerSecondSquared = 0.5; - public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI; - public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI; + public static final double kMaxSpeedMetersPerSecond = 1.2; + public static final double kMaxAccelerationMetersPerSecondSquared = 0.8; + public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI*1.5; + public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI*1.5; public static final double kPXController = 1; public static final double kPYController = 1; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 477a0ed..077abbf 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -34,6 +34,7 @@ import frc.robot.commands.LaunchASAP; import frc.robot.commands.LaunchControled; import frc.robot.commands.TargetSwerve; +import frc.robot.commands.TargetSwerveAuto; import frc.robot.commands.TeleopSwerve; import frc.robot.subsystems.Band; import frc.robot.subsystems.Intake; @@ -137,6 +138,13 @@ public RobotContainer() { NamedCommands.registerCommand("RunIntake", new IntakeRun(intake, LIM, new JoystickButton(secondary, 3), theLEDs)); NamedCommands.registerCommand("EndIntake", new IntakeDefault(intake, LIM)); NamedCommands.registerCommand("LaunchASAP", new LaunchASAP(intake,launcher,theLEDs)); + NamedCommands.registerCommand("TargetSwerve", + new TargetSwerveAuto( + s_Swerve, + theLEDs, + launcher, + intake + )); // Configure the button bindings configureButtonBindings(); @@ -148,6 +156,7 @@ public RobotContainer() { musiChooser = theBand.Buildchoser(); SmartDashboard.putData("Music Choser",musiChooser); + SmartDashboard.putNumber("SmartLaunch",0); secondary.getPOV(); diff --git a/src/main/java/frc/robot/commands/TargetSwerve.java b/src/main/java/frc/robot/commands/TargetSwerve.java index 373765a..bc4982b 100644 --- a/src/main/java/frc/robot/commands/TargetSwerve.java +++ b/src/main/java/frc/robot/commands/TargetSwerve.java @@ -43,7 +43,9 @@ public class TargetSwerve extends Command { private boolean shoot; private boolean spin; private boolean end; + private boolean launching; private Timer timer; + private Timer timerL; @@ -98,9 +100,11 @@ public void initialize() { Ypid.setSetpoint(0); timer = new Timer(); + timerL = new Timer(); shoot = false; spin = false; end = false; + launching = false; led.setMode("LT"); System.out.println("set LT"); @@ -120,31 +124,41 @@ public void execute() { double rotationVal = MathUtil.applyDeadband(rotationSup.getAsDouble(), Constants.stickDeadband); double throtleVal = 1-((throtleSupplier.getAsDouble()+1)/2); + + // Get Targert X and Y double X_from_camera = LLtbl.getValue("tx").getDouble(); double Y_from_camera = LLtbl.getValue("ty").getDouble(); - - + // Calculate PID for movement swerve_X_speed = MathUtil.clamp(Xpid.calculate(X_from_camera), -1,1); swerve_Y_speed = MathUtil.clamp(Ypid.calculate(Y_from_camera), -1,1); + // If target reached set shoot to true shoot = Xpid.atSetpoint() && Ypid.atSetpoint(); + // if target reached zero motion if(Xpid.atSetpoint()) swerve_X_speed = 0; if(Ypid.atSetpoint()) swerve_Y_speed = 0; - if(shoot && !end && led.getMode()!="LK") { + // set led modes + if(shoot && !end && !launching) { + if(led.getMode()!="LK"){ led.setMode("LK"); System.out.println("set LK"); - } else if (!end && led.getMode()!="LT") { + timerL.reset(); + timerL.start(); + } + } else if (!end && led.getMode()!="LT" && !launching) { led.setMode("LT"); System.out.println("set LT"); } - if(launchButton.getAsBoolean() && !end) { + SmartDashboard.putNumber("SmartLaunch", timerL.get()); + if((launchButton.getAsBoolean() && !end) || (timerL.hasElapsed(Constants.TargetSwerve.SmartLaunchTime) && !launching)) { s_Intake.pushIntake(false); end = true; timer.start(); led.setMode("L"); System.out.println("set L"); + launching = true; } SmartDashboard.putNumber("Xpid", swerve_X_speed); diff --git a/src/main/java/frc/robot/commands/TargetSwerveAuto.java b/src/main/java/frc/robot/commands/TargetSwerveAuto.java new file mode 100644 index 0000000..d00236d --- /dev/null +++ b/src/main/java/frc/robot/commands/TargetSwerveAuto.java @@ -0,0 +1,158 @@ +package frc.robot.commands; + +import java.util.function.BooleanSupplier; +import java.util.function.DoubleSupplier; +import edu.wpi.first.wpilibj.Timer; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants; +import frc.robot.subsystems.LEDS; +import frc.robot.subsystems.Launcher; +import frc.robot.subsystems.Intake; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import frc.robot.subsystems.Swerve; + + +public class TargetSwerveAuto extends Command { + private Swerve s_Swerve; + private Launcher s_Launcher; + private Intake s_Intake; + + private double swerve_X_speed; + private double swerve_Y_speed; + + private boolean shoot; + private boolean end; + private boolean launching; + private Timer timer; + private Timer timerL; + + private NetworkTable LLtbl; + + private PIDController Xpid; + private PIDController Ypid; + + + private LEDS led; + + /** + * Default Teleop mode, takes the swerve and conrolor + * + * @param s_Swerve swerve base + * @param translationSup forward and back suplier, typicaly a controlor axis + * @param strafeSup left and right suplier, typicaly a controlor axis + * @param rotationSup Rotation suplier, typicaly a controlor axis + * @param robotCentricSup wether to drive relative to the robot. + */ + public TargetSwerveAuto(Swerve s_Swerve, LEDS led, Launcher s_Launcher, Intake s_Intake) { + this.s_Swerve = s_Swerve; + this.s_Launcher = s_Launcher; + this.s_Intake = s_Intake; + addRequirements(s_Swerve); + addRequirements(s_Launcher); + addRequirements(s_Intake); + + this.led = led; + } + + + @Override + public void initialize() { + + Xpid = new PIDController(Constants.TargetSwerve.P, Constants.TargetSwerve.I, Constants.TargetSwerve.D); + Ypid = new PIDController(Constants.TargetSwerve.P, Constants.TargetSwerve.I, Constants.TargetSwerve.D); + + Xpid.setTolerance(3); + Ypid.setTolerance(3); + + Xpid.setSetpoint(0); + Ypid.setSetpoint(0); + + timer = new Timer(); + timerL = new Timer(); + shoot = false; + end = false; + launching = false; + + led.setMode("LT"); + System.out.println("set LT"); + + s_Launcher.startLaunch(); + s_Intake.lightPull(false); + + LLtbl = NetworkTableInstance.getDefault().getTable("limelight"); + } + + @Override + public void execute() { + + // Get Targert X and Y + double X_from_camera = LLtbl.getValue("tx").getDouble(); + double Y_from_camera = LLtbl.getValue("ty").getDouble(); + // Calculate PID for movement + swerve_X_speed = MathUtil.clamp(Xpid.calculate(X_from_camera), -1,1); + swerve_Y_speed = MathUtil.clamp(Ypid.calculate(Y_from_camera), -1,1); + + // If target reached set shoot to true + shoot = Xpid.atSetpoint() && Ypid.atSetpoint(); + // if target reached zero motion + if(Xpid.atSetpoint()) swerve_X_speed = 0; + if(Ypid.atSetpoint()) swerve_Y_speed = 0; + + // set led modes + if(shoot && !end && !launching) { + if(led.getMode()!="LK"){ + led.setMode("LK"); + System.out.println("set LK"); + timerL.reset(); + timerL.start(); + } + } else if (!end && led.getMode()!="LT" && !launching) { + led.setMode("LT"); + System.out.println("set LT"); + } + + SmartDashboard.putNumber("SmartLaunch", timerL.get()); + if(timerL.hasElapsed(Constants.TargetSwerve.SmartLaunchTime) && !launching) { + s_Intake.pushIntake(false); + end = true; + timer.start(); + led.setMode("L"); + System.out.println("set L"); + launching = true; + } + + SmartDashboard.putNumber("Xpid", swerve_X_speed); + SmartDashboard.putNumber("Ypid", swerve_Y_speed); + SmartDashboard.putBoolean("Xshoot", Xpid.atSetpoint()); + SmartDashboard.putBoolean("Yshoot", Ypid.atSetpoint()); + SmartDashboard.putBoolean("Shoot", shoot); + + /* Drive */ + s_Swerve.drive( + new Translation2d(-swerve_Y_speed * 0.5, -swerve_X_speed * 0.5).times(Constants.Swerve.maxSpeed), + 0 * Constants.Swerve.maxAngularVelocity, + false, + false + ); + } + + @Override + public void end(boolean interrupted) { + s_Intake.pushIntake(true); + s_Launcher.endLaunch(); + led.setMode("D"); + System.out.println("set D"); + } + + @Override + public boolean isFinished() { + return end && timer.get() >= Constants.Launcher.LaunchStopTime; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 8833ac2..769fc63 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -8,6 +8,8 @@ import frc.robot.Constants; import java.util.function.IntSupplier; +import java.sql.Time; +import java.util.concurrent.TimeUnit; import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.VictorSPXControlMode; @@ -76,14 +78,21 @@ public Intake(int m_barMotorId, int m_intakeMotor1ID, int m_intakeMotor2ID, Xbox } public void startIntake() { - m_intakeMotor1.set(VictorSPXControlMode.PercentOutput, Constants.Intake.intakeVel); - m_intakeMotor2.set(VictorSPXControlMode.PercentOutput, Constants.Intake.intakeVel); // barPID.setReference(0.3, CANSparkMax.ControlType.kDutyCycle); m_barMotor.enableSoftLimit(CANSparkMax.SoftLimitDirection.kForward, true); m_barMotor.enableSoftLimit(CANSparkMax.SoftLimitDirection.kReverse, false); m_barMotor.set(Constants.Intake.acuateVel); System.out.println("Start"); + // wait(1000); + + try { + TimeUnit.MILLISECONDS.sleep(500); + } catch(InterruptedException e){ + System.out.println("interuption, sleeping"); + } + m_intakeMotor1.set(VictorSPXControlMode.PercentOutput, Constants.Intake.intakeVel); + m_intakeMotor2.set(VictorSPXControlMode.PercentOutput, Constants.Intake.intakeVel); } public void endIntake() { From db5b4afb9af5e9a3910a1034f3fc74d4ea8fdf5e Mon Sep 17 00:00:00 2001 From: StoneCommander <67015041+StoneCommander@users.noreply.github.com> Date: Wed, 20 Mar 2024 22:48:13 -0600 Subject: [PATCH 10/12] clean and separeate --- src/main/java/frc/robot/RobotContainer.java | 21 ++- .../java/frc/robot/commands/TargetSwerve.java | 24 +-- .../frc/robot/commands/TargetSwervePlus.java | 178 ++++++++++++++++++ 3 files changed, 196 insertions(+), 27 deletions(-) create mode 100644 src/main/java/frc/robot/commands/TargetSwervePlus.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 077abbf..5aef700 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -34,6 +34,7 @@ import frc.robot.commands.LaunchASAP; import frc.robot.commands.LaunchControled; import frc.robot.commands.TargetSwerve; +import frc.robot.commands.TargetSwervePlus; import frc.robot.commands.TargetSwerveAuto; import frc.robot.commands.TeleopSwerve; import frc.robot.subsystems.Band; @@ -87,7 +88,7 @@ public class RobotContainer { private final NetworkTableInstance tableInstance = NetworkTableInstance.getDefault(); /* Subsystems */ - private final LEDS theLEDs = new LEDS(9,60); + private final LEDS theLEDs = new LEDS(9,176); private final Band theBand = new Band(); @@ -234,11 +235,19 @@ private void configureButtonBindings() { new JoystickButton(driver, 4).onTrue( new TargetSwerve( s_Swerve, - () -> -driver.getRawAxis(translationAxis), () -> -driver.getRawAxis(strafeAxis), () -> -driver.getRawAxis(rotationAxis), - () -> -driver.getRawAxis(throttleAxis), - () -> robotCentric.getAsBoolean(), + theLEDs, + launcher, + intake, + new JoystickButton(driver, 4), + new JoystickButton(secondary, 6) + )); + new JoystickButton(driver, 3).onTrue( + new TargetSwervePlus( + s_Swerve, + () -> -driver.getRawAxis(strafeAxis), + () -> -driver.getRawAxis(rotationAxis), theLEDs, launcher, intake, @@ -342,7 +351,7 @@ public void teleopPeriodic() { } public void autonomousPeriodic() { - // theLEDs.rainbow(); + theLEDs.rainbow(1); } public void testInit() { @@ -350,7 +359,7 @@ public void testInit() { } public void testPeriodic() { - theLEDs.rainbow(); + theLEDs.rainbow(1); } public void disabledInit () { diff --git a/src/main/java/frc/robot/commands/TargetSwerve.java b/src/main/java/frc/robot/commands/TargetSwerve.java index bc4982b..a94f873 100644 --- a/src/main/java/frc/robot/commands/TargetSwerve.java +++ b/src/main/java/frc/robot/commands/TargetSwerve.java @@ -23,11 +23,7 @@ public class TargetSwerve extends Command { private Swerve s_Swerve; private Launcher s_Launcher; private Intake s_Intake; - private DoubleSupplier translationSup; - private DoubleSupplier strafeSup; private DoubleSupplier rotationSup; - private DoubleSupplier throtleSupplier; - private BooleanSupplier robotCentricSup; private JoystickButton overrideButton; private JoystickButton launchButton; @@ -35,10 +31,6 @@ public class TargetSwerve extends Command { private double swerve_X_speed; private double swerve_Y_speed; - private double swerve_X_speedt; - private double swerve_Y_speedt; - private boolean shoot_x; - private boolean shoot_y; private boolean shoot; private boolean spin; @@ -62,12 +54,10 @@ public class TargetSwerve extends Command { * Default Teleop mode, takes the swerve and conrolor * * @param s_Swerve swerve base - * @param translationSup forward and back suplier, typicaly a controlor axis - * @param strafeSup left and right suplier, typicaly a controlor axis * @param rotationSup Rotation suplier, typicaly a controlor axis * @param robotCentricSup wether to drive relative to the robot. */ - public TargetSwerve(Swerve s_Swerve, DoubleSupplier translationSup, DoubleSupplier strafeSup, DoubleSupplier rotationSup, DoubleSupplier throtleSupplier, BooleanSupplier robotCentricSup, LEDS led, Launcher s_Launcher, Intake s_Intake, JoystickButton overrideButton, JoystickButton launchButton) { + public TargetSwerve(Swerve s_Swerve, DoubleSupplier strafeSup, DoubleSupplier rotationSup, LEDS led, Launcher s_Launcher, Intake s_Intake, JoystickButton overrideButton, JoystickButton launchButton) { this.s_Swerve = s_Swerve; this.s_Launcher = s_Launcher; this.s_Intake = s_Intake; @@ -75,11 +65,7 @@ public TargetSwerve(Swerve s_Swerve, DoubleSupplier translationSup, DoubleSuppli addRequirements(s_Launcher); addRequirements(s_Intake); - this.translationSup = translationSup; - this.strafeSup = strafeSup; this.rotationSup = rotationSup; - this.robotCentricSup = robotCentricSup; - this.throtleSupplier = throtleSupplier; this.overrideButton = overrideButton; this.launchButton = launchButton; this.led = led; @@ -119,10 +105,8 @@ public void initialize() { public void execute() { if (bHold) bHold = overrideButton.getAsBoolean(); /* Get Values, Deadband*/ - double translationVal = MathUtil.applyDeadband(translationSup.getAsDouble(), Constants.stickDeadband); - double strafeVal = MathUtil.applyDeadband(strafeSup.getAsDouble(), Constants.stickDeadband); double rotationVal = MathUtil.applyDeadband(rotationSup.getAsDouble(), Constants.stickDeadband); - double throtleVal = 1-((throtleSupplier.getAsDouble()+1)/2); + double throtleVal = 0.5; // Get Targert X and Y @@ -143,8 +127,6 @@ public void execute() { if(led.getMode()!="LK"){ led.setMode("LK"); System.out.println("set LK"); - timerL.reset(); - timerL.start(); } } else if (!end && led.getMode()!="LT" && !launching) { led.setMode("LT"); @@ -152,7 +134,7 @@ public void execute() { } SmartDashboard.putNumber("SmartLaunch", timerL.get()); - if((launchButton.getAsBoolean() && !end) || (timerL.hasElapsed(Constants.TargetSwerve.SmartLaunchTime) && !launching)) { + if((launchButton.getAsBoolean() && !end)) { s_Intake.pushIntake(false); end = true; timer.start(); diff --git a/src/main/java/frc/robot/commands/TargetSwervePlus.java b/src/main/java/frc/robot/commands/TargetSwervePlus.java new file mode 100644 index 0000000..e55cd67 --- /dev/null +++ b/src/main/java/frc/robot/commands/TargetSwervePlus.java @@ -0,0 +1,178 @@ +package frc.robot.commands; + +import java.util.function.BooleanSupplier; +import java.util.function.DoubleSupplier; +import edu.wpi.first.wpilibj.Timer; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants; +import frc.robot.subsystems.LEDS; +import frc.robot.subsystems.Launcher; +import frc.robot.subsystems.Intake; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import frc.robot.subsystems.Swerve; + + +public class TargetSwervePlus extends Command { + private Swerve s_Swerve; + private Launcher s_Launcher; + private Intake s_Intake; + private DoubleSupplier rotationSup; + + private JoystickButton overrideButton; + private JoystickButton launchButton; + + + private double swerve_X_speed; + private double swerve_Y_speed; + + private boolean shoot; + private boolean spin; + private boolean end; + private boolean launching; + private Timer timer; + private Timer timerL; + + + + private Boolean bHold; + private NetworkTable LLtbl; + + private PIDController Xpid; + private PIDController Ypid; + + + private LEDS led; + + /** + * Default Teleop mode, takes the swerve and conrolor + * + * @param s_Swerve swerve base + * @param translationSup forward and back suplier, typicaly a controlor axis + * @param strafeSup left and right suplier, typicaly a controlor axis + * @param rotationSup Rotation suplier, typicaly a controlor axis + * @param robotCentricSup wether to drive relative to the robot. + */ + public TargetSwervePlus(Swerve s_Swerve, DoubleSupplier strafeSup, DoubleSupplier rotationSup, LEDS led, Launcher s_Launcher, Intake s_Intake, JoystickButton overrideButton, JoystickButton launchButton) { + this.s_Swerve = s_Swerve; + this.s_Launcher = s_Launcher; + this.s_Intake = s_Intake; + addRequirements(s_Swerve); + addRequirements(s_Launcher); + addRequirements(s_Intake); + + this.rotationSup = rotationSup; + this.overrideButton = overrideButton; + this.launchButton = launchButton; + this.led = led; + } + + + @Override + public void initialize() { + this.bHold = true; + + Xpid = new PIDController(Constants.TargetSwerve.P, Constants.TargetSwerve.I, Constants.TargetSwerve.D); + Ypid = new PIDController(Constants.TargetSwerve.P, Constants.TargetSwerve.I, Constants.TargetSwerve.D); + + Xpid.setTolerance(3); + Ypid.setTolerance(3); + + Xpid.setSetpoint(0); + Ypid.setSetpoint(0); + + timer = new Timer(); + timerL = new Timer(); + shoot = false; + spin = false; + end = false; + launching = false; + + led.setMode("LT"); + System.out.println("set LT"); + + s_Launcher.startLaunch(); + s_Intake.lightPull(false); + + LLtbl = NetworkTableInstance.getDefault().getTable("limelight"); + } + + @Override + public void execute() { + if (bHold) bHold = overrideButton.getAsBoolean(); + /* Get Values, Deadband*/ + double rotationVal = MathUtil.applyDeadband(rotationSup.getAsDouble(), Constants.stickDeadband); + double throtleVal = 0.5; + + + // Get Targert X and Y + double X_from_camera = LLtbl.getValue("tx").getDouble(); + double Y_from_camera = LLtbl.getValue("ty").getDouble(); + // Calculate PID for movement + swerve_X_speed = MathUtil.clamp(Xpid.calculate(X_from_camera), -1,1); + swerve_Y_speed = MathUtil.clamp(Ypid.calculate(Y_from_camera), -1,1); + + // If target reached set shoot to true + shoot = Xpid.atSetpoint() && Ypid.atSetpoint(); + // if target reached zero motion + if(Xpid.atSetpoint()) swerve_X_speed = 0; + if(Ypid.atSetpoint()) swerve_Y_speed = 0; + + // set led modes + if(shoot && !end && !launching) { + if(led.getMode()!="LK"){ + led.setMode("LK"); + System.out.println("set LK"); + timerL.reset(); + timerL.start(); + } + } else if (!end && led.getMode()!="LT" && !launching) { + led.setMode("LT"); + System.out.println("set LT"); + } + + SmartDashboard.putNumber("SmartLaunch", timerL.get()); + if((launchButton.getAsBoolean() && !end) || (timerL.hasElapsed(Constants.TargetSwerve.SmartLaunchTime) && !launching)) { + s_Intake.pushIntake(false); + end = true; + timer.start(); + led.setMode("L"); + System.out.println("set L"); + launching = true; + } + + SmartDashboard.putNumber("Xpid", swerve_X_speed); + SmartDashboard.putNumber("Ypid", swerve_Y_speed); + SmartDashboard.putBoolean("Xshoot", Xpid.atSetpoint()); + SmartDashboard.putBoolean("Yshoot", Ypid.atSetpoint()); + SmartDashboard.putBoolean("Shoot", shoot); + + /* Drive */ + s_Swerve.drive( + new Translation2d(-swerve_Y_speed * throtleVal, -swerve_X_speed * throtleVal).times(Constants.Swerve.maxSpeed), + (rotationVal * throtleVal) * Constants.Swerve.maxAngularVelocity, + false, + false + ); + } + + @Override + public void end(boolean interrupted) { + s_Intake.pushIntake(true); + s_Launcher.endLaunch(); + led.setMode("D"); + System.out.println("set D"); + } + + @Override + public boolean isFinished() { + + return (overrideButton.getAsBoolean() && !bHold) || (end && timer.get() >= Constants.Launcher.LaunchStopTime); + } +} \ No newline at end of file From 1a5a990964d67e9dee98811bc53f61c1c6829580 Mon Sep 17 00:00:00 2001 From: StoneCommander <67015041+StoneCommander@users.noreply.github.com> Date: Thu, 28 Mar 2024 16:07:32 -0600 Subject: [PATCH 11/12] update --- .Glass/glass.json | 19 +++++ .pathplanner/settings.json | 4 +- src/main/deploy/pathplanner/autos/Btmalt.auto | 6 ++ src/main/deploy/pathplanner/autos/Mid4B.auto | 85 +++++++++++++++++++ .../deploy/pathplanner/autos/TopMain.auto | 24 ++++-- src/main/deploy/pathplanner/autos/test.auto | 4 +- .../deploy/pathplanner/paths/BTM-SHOOT.path | 10 +-- .../deploy/pathplanner/paths/BTM-TAXI.path | 2 +- .../deploy/pathplanner/paths/BTM-TO-2.path | 10 +-- .../deploy/pathplanner/paths/BTM-TO-3.path | 18 ++-- .../deploy/pathplanner/paths/BTM-TO-M4.path | 10 +-- .../deploy/pathplanner/paths/BTM-TO-M5.path | 14 +-- .../deploy/pathplanner/paths/M-SHOOT.path | 18 ++-- src/main/deploy/pathplanner/paths/M-TO-1.path | 14 +-- src/main/deploy/pathplanner/paths/M-TO-2.path | 14 +-- src/main/deploy/pathplanner/paths/M-TO-3.path | 14 +-- .../deploy/pathplanner/paths/MB-TO-SHOOT.path | 12 +-- .../pathplanner/paths/SmartLaunchMid.path | 6 +- .../deploy/pathplanner/paths/T-SHOOT.path | 18 ++-- src/main/deploy/pathplanner/paths/T-TO-1.path | 10 +-- src/main/deploy/pathplanner/paths/T-TO-2.path | 10 +-- src/main/deploy/pathplanner/paths/T-TO-3.path | 10 +-- .../deploy/pathplanner/paths/T-TO-M1.path | 32 +++---- .../deploy/pathplanner/paths/T-TO-M2.path | 10 +-- .../deploy/pathplanner/paths/T-TO-M3.path | 10 +-- src/main/deploy/pathplanner/paths/TO-1.path | 2 +- src/main/deploy/pathplanner/paths/TO-2.path | 2 +- src/main/deploy/pathplanner/paths/TO-3.path | 2 +- src/main/java/frc/robot/Constants.java | 33 +++---- src/main/java/frc/robot/RobotContainer.java | 7 +- .../java/frc/robot/commands/SmartShoot.java | 45 ---------- .../java/frc/robot/commands/TargetSwerve.java | 15 +++- 32 files changed, 291 insertions(+), 199 deletions(-) create mode 100644 .Glass/glass.json create mode 100644 src/main/deploy/pathplanner/autos/Mid4B.auto delete mode 100644 src/main/java/frc/robot/commands/SmartShoot.java diff --git a/.Glass/glass.json b/.Glass/glass.json new file mode 100644 index 0000000..2b9c849 --- /dev/null +++ b/.Glass/glass.json @@ -0,0 +1,19 @@ +{ + "NetworkTables": { + "Persistent Values": { + "open": false + }, + "Retained Values": { + "open": false + }, + "Transitory Values": { + "open": false + } + }, + "NetworkTables Info": { + "Connections": { + "open": true + }, + "visible": true + } +} diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index 59652a2..837ebc7 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -12,8 +12,8 @@ "TEST" ], "defaultMaxVel": 4.0, - "defaultMaxAccel": 3.0, + "defaultMaxAccel": 2.0, "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, - "maxModuleSpeed": 4.5 + "maxModuleSpeed": 5.0 } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Btmalt.auto b/src/main/deploy/pathplanner/autos/Btmalt.auto index 302dad8..e72ebf7 100644 --- a/src/main/deploy/pathplanner/autos/Btmalt.auto +++ b/src/main/deploy/pathplanner/autos/Btmalt.auto @@ -52,6 +52,12 @@ "data": { "pathName": "MB-TO-SHOOT" } + }, + { + "type": "named", + "data": { + "name": "LaunchASAP" + } } ] } diff --git a/src/main/deploy/pathplanner/autos/Mid4B.auto b/src/main/deploy/pathplanner/autos/Mid4B.auto new file mode 100644 index 0000000..e7965f1 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Mid4B.auto @@ -0,0 +1,85 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.171904793952038, + "y": 5.531335051274091 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "M-SHOOT" + } + }, + { + "type": "named", + "data": { + "name": "LaunchASAP" + } + }, + { + "type": "path", + "data": { + "pathName": "TO-2" + } + }, + { + "type": "path", + "data": { + "pathName": "M-SHOOT" + } + }, + { + "type": "named", + "data": { + "name": "LaunchASAP" + } + }, + { + "type": "path", + "data": { + "pathName": "TO-1" + } + }, + { + "type": "path", + "data": { + "pathName": "M-SHOOT" + } + }, + { + "type": "named", + "data": { + "name": "LaunchASAP" + } + }, + { + "type": "path", + "data": { + "pathName": "TO-3" + } + }, + { + "type": "path", + "data": { + "pathName": "M-SHOOT" + } + }, + { + "type": "named", + "data": { + "name": "LaunchASAP" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/TopMain.auto b/src/main/deploy/pathplanner/autos/TopMain.auto index 55266dd..a305c48 100644 --- a/src/main/deploy/pathplanner/autos/TopMain.auto +++ b/src/main/deploy/pathplanner/autos/TopMain.auto @@ -11,6 +11,12 @@ "type": "sequential", "data": { "commands": [ + { + "type": "path", + "data": { + "pathName": "T-SHOOT" + } + }, { "type": "named", "data": { @@ -18,9 +24,15 @@ } }, { - "type": "wait", + "type": "path", "data": { - "waitTime": 0.0 + "pathName": "T-TO-1" + } + }, + { + "type": "path", + "data": { + "pathName": "T-SHOOT" } }, { @@ -32,19 +44,19 @@ { "type": "path", "data": { - "pathName": "T-SHOOT" + "pathName": "T-TO-M1" } }, { "type": "path", "data": { - "pathName": "T-TO-2" + "pathName": "T-SHOOT" } }, { - "type": "path", + "type": "named", "data": { - "pathName": "T-SHOOT" + "name": "LaunchASAP" } } ] diff --git a/src/main/deploy/pathplanner/autos/test.auto b/src/main/deploy/pathplanner/autos/test.auto index eb1171f..aaac7df 100644 --- a/src/main/deploy/pathplanner/autos/test.auto +++ b/src/main/deploy/pathplanner/autos/test.auto @@ -12,9 +12,9 @@ "data": { "commands": [ { - "type": "named", + "type": "path", "data": { - "name": "TargetSwerve" + "pathName": "M-SHOOT" } } ] diff --git a/src/main/deploy/pathplanner/paths/BTM-SHOOT.path b/src/main/deploy/pathplanner/paths/BTM-SHOOT.path index 0297766..57b73b1 100644 --- a/src/main/deploy/pathplanner/paths/BTM-SHOOT.path +++ b/src/main/deploy/pathplanner/paths/BTM-SHOOT.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 0.9029392417759192, - "y": 4.256672217048139 + "x": 0.973104168430559, + "y": 4.513943614781817 }, "prevControl": { - "x": 1.1952931028369176, - "y": 4.139730672623739 + "x": 1.2654580294915565, + "y": 4.397002070357417 }, "nextControl": null, "isLocked": false, @@ -33,7 +33,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 3.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/BTM-TAXI.path b/src/main/deploy/pathplanner/paths/BTM-TAXI.path index 3e3074e..2227f38 100644 --- a/src/main/deploy/pathplanner/paths/BTM-TAXI.path +++ b/src/main/deploy/pathplanner/paths/BTM-TAXI.path @@ -45,7 +45,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 3.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/BTM-TO-2.path b/src/main/deploy/pathplanner/paths/BTM-TO-2.path index e1d780f..bcb016c 100644 --- a/src/main/deploy/pathplanner/paths/BTM-TO-2.path +++ b/src/main/deploy/pathplanner/paths/BTM-TO-2.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 0.9029392417759192, - "y": 4.256672217048139 + "x": 0.973104168430559, + "y": 4.513943614781817 }, "prevControl": null, "nextControl": { - "x": 1.9029392417759186, - "y": 4.256672217048139 + "x": 1.9731041684305581, + "y": 4.513943614781817 }, "isLocked": false, "linkedName": "BTM SHOOT" @@ -44,7 +44,7 @@ ], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 3.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/BTM-TO-3.path b/src/main/deploy/pathplanner/paths/BTM-TO-3.path index 029d8c8..c669d19 100644 --- a/src/main/deploy/pathplanner/paths/BTM-TO-3.path +++ b/src/main/deploy/pathplanner/paths/BTM-TO-3.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 0.9029392417759192, - "y": 4.256672217048139 + "x": 0.973104168430559, + "y": 4.513943614781817 }, "prevControl": null, "nextControl": { - "x": 1.1602106395095981, - "y": 4.256672217048139 + "x": 1.2303755661642377, + "y": 4.513943614781817 }, "isLocked": false, "linkedName": "BTM SHOOT" }, { "anchor": { - "x": 2.4699559370628714, - "y": 4.128036518181299 + "x": 2.70383902591167, + "y": 4.1046482092964185 }, "prevControl": { - "x": 1.6981417438618356, - "y": 4.139730672623739 + "x": 1.9320248327106342, + "y": 4.116342363738859 }, "nextControl": null, "isLocked": false, @@ -51,7 +51,7 @@ ], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 3.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/BTM-TO-M4.path b/src/main/deploy/pathplanner/paths/BTM-TO-M4.path index 939a6f5..3f22626 100644 --- a/src/main/deploy/pathplanner/paths/BTM-TO-M4.path +++ b/src/main/deploy/pathplanner/paths/BTM-TO-M4.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 0.9029392417759192, - "y": 4.256672217048139 + "x": 0.973104168430559, + "y": 4.513943614781817 }, "prevControl": null, "nextControl": { - "x": 1.9029392417759181, - "y": 4.256672217048139 + "x": 0.6807503073695607, + "y": 1.6605699308264719 }, "isLocked": false, "linkedName": "BTM SHOOT" @@ -69,7 +69,7 @@ ], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 3.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/BTM-TO-M5.path b/src/main/deploy/pathplanner/paths/BTM-TO-M5.path index 20ecae1..ba27366 100644 --- a/src/main/deploy/pathplanner/paths/BTM-TO-M5.path +++ b/src/main/deploy/pathplanner/paths/BTM-TO-M5.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 0.9029392417759192, - "y": 4.256672217048139 + "x": 0.973104168430559, + "y": 4.513943614781817 }, "prevControl": null, "nextControl": { - "x": 1.3940937283583967, - "y": 2.1049477996391905 + "x": 0.3533139829812422, + "y": 0.9238382009527565 }, "isLocked": false, "linkedName": "BTM SHOOT" @@ -20,8 +20,8 @@ "y": 0.7835083476434771 }, "prevControl": { - "x": 5.440271165442615, - "y": 0.573013567679558 + "x": 4.972504987745018, + "y": 0.1754123166365992 }, "nextControl": null, "isLocked": false, @@ -63,7 +63,7 @@ ], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 3.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/M-SHOOT.path b/src/main/deploy/pathplanner/paths/M-SHOOT.path index e4835d6..eb6c6e8 100644 --- a/src/main/deploy/pathplanner/paths/M-SHOOT.path +++ b/src/main/deploy/pathplanner/paths/M-SHOOT.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.183598948394478, - "y": 5.5430292057165325 + "x": 1.6045885083223155, + "y": 5.507946742389212 }, "prevControl": null, "nextControl": { - "x": 1.2435065695868026, - "y": 5.546357406893884 + "x": 1.4993411183403562, + "y": 5.507946742389212 }, "isLocked": false, "linkedName": null @@ -17,11 +17,11 @@ { "anchor": { "x": 1.4993411183403562, - "y": 5.5430292057165325 + "y": 5.507946742389212 }, "prevControl": { - "x": 1.4398044457375365, - "y": 5.535587121641179 + "x": 1.6279768172071956, + "y": 5.5196408968316515 }, "nextControl": null, "isLocked": false, @@ -33,13 +33,13 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 3.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, "goalEndState": { "velocity": 0, - "rotation": 0, + "rotation": 0.0, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/M-TO-1.path b/src/main/deploy/pathplanner/paths/M-TO-1.path index 0d74a7b..19c03b6 100644 --- a/src/main/deploy/pathplanner/paths/M-TO-1.path +++ b/src/main/deploy/pathplanner/paths/M-TO-1.path @@ -4,24 +4,24 @@ { "anchor": { "x": 1.4993411183403562, - "y": 5.5430292057165325 + "y": 5.507946742389212 }, "prevControl": null, "nextControl": { "x": 1.4876469638979162, - "y": 6.303149244475128 + "y": 6.268066781147807 }, "isLocked": false, "linkedName": "MID SHOOT" }, { "anchor": { - "x": 2.5752033270448305, - "y": 6.841080348827365 + "x": 2.9377221147604686, + "y": 6.993104356579084 }, "prevControl": { - "x": 1.5578118905525564, - "y": 6.794303731057606 + "x": 1.9203306782681946, + "y": 6.946327738809325 }, "nextControl": null, "isLocked": false, @@ -51,7 +51,7 @@ ], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 3.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/M-TO-2.path b/src/main/deploy/pathplanner/paths/M-TO-2.path index 8634bec..4734b8b 100644 --- a/src/main/deploy/pathplanner/paths/M-TO-2.path +++ b/src/main/deploy/pathplanner/paths/M-TO-2.path @@ -4,24 +4,24 @@ { "anchor": { "x": 1.4993411183403562, - "y": 5.5430292057165325 + "y": 5.507946742389212 }, "prevControl": null, "nextControl": { "x": 1.5695060449949956, - "y": 5.55472336015897 + "y": 5.51964089683165 }, "isLocked": false, "linkedName": "MID SHOOT" }, { "anchor": { - "x": 2.5284267092750707, - "y": 5.556066607628711 + "x": 2.855863033663389, + "y": 5.5430292057165325 }, "prevControl": { - "x": 1.943718987153074, - "y": 5.55472336015897 + "x": 2.271155311541392, + "y": 5.541685958246791 }, "nextControl": null, "isLocked": false, @@ -63,7 +63,7 @@ ], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 3.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/M-TO-3.path b/src/main/deploy/pathplanner/paths/M-TO-3.path index a7bd9a6..7e0edd1 100644 --- a/src/main/deploy/pathplanner/paths/M-TO-3.path +++ b/src/main/deploy/pathplanner/paths/M-TO-3.path @@ -4,24 +4,24 @@ { "anchor": { "x": 1.4993411183403562, - "y": 5.5430292057165325 + "y": 5.507946742389212 }, "prevControl": null, "nextControl": { "x": 1.2069872572793576, - "y": 4.420390379242298 + "y": 4.385307915914978 }, "isLocked": false, "linkedName": "MID SHOOT" }, { "anchor": { - "x": 2.61028579037215, - "y": 4.326837143702777 + "x": 2.69214487146923, + "y": 4.268366371490578 }, "prevControl": { - "x": 2.493344245947751, - "y": 4.736132549188175 + "x": 2.575203327044831, + "y": 4.6776617769759765 }, "nextControl": null, "isLocked": false, @@ -51,7 +51,7 @@ ], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 3.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/MB-TO-SHOOT.path b/src/main/deploy/pathplanner/paths/MB-TO-SHOOT.path index 9b2d3ea..323d269 100644 --- a/src/main/deploy/pathplanner/paths/MB-TO-SHOOT.path +++ b/src/main/deploy/pathplanner/paths/MB-TO-SHOOT.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 0.9029392417759192, - "y": 4.256672217048139 + "x": 0.973104168430559, + "y": 4.513943614781817 }, "prevControl": { - "x": 1.0315749406427586, - "y": 2.432384124027509 + "x": 1.1017398672973977, + "y": 2.6896555217611873 }, "nextControl": null, "isLocked": false, @@ -45,13 +45,13 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 3.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, "goalEndState": { "velocity": 0, - "rotation": -59.264512298079936, + "rotation": -47.489552921999234, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/SmartLaunchMid.path b/src/main/deploy/pathplanner/paths/SmartLaunchMid.path index e480076..d3f88a5 100644 --- a/src/main/deploy/pathplanner/paths/SmartLaunchMid.path +++ b/src/main/deploy/pathplanner/paths/SmartLaunchMid.path @@ -17,11 +17,11 @@ { "anchor": { "x": 1.4993411183403562, - "y": 5.5430292057165325 + "y": 5.507946742389212 }, "prevControl": { "x": 1.2069872572793576, - "y": 5.5430292057165325 + "y": 5.507946742389212 }, "nextControl": null, "isLocked": false, @@ -51,7 +51,7 @@ ], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 3.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/T-SHOOT.path b/src/main/deploy/pathplanner/paths/T-SHOOT.path index ecd5cd5..7464326 100644 --- a/src/main/deploy/pathplanner/paths/T-SHOOT.path +++ b/src/main/deploy/pathplanner/paths/T-SHOOT.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 1.6396709716496354, - "y": 6.689056341075646 + "x": 1.3239288017037572, + "y": 6.759221267730286 }, "prevControl": null, "nextControl": { - "x": 1.008186631757879, - "y": 6.607197259978566 + "x": 1.0198807862003185, + "y": 6.595503105536126 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 0.8795509328910394, - "y": 6.689056341075646 + "x": 0.8327743151212799, + "y": 6.560420642208806 }, "prevControl": { - "x": 1.2420697206066778, - "y": 6.747527113287845 + "x": 1.195293102836918, + "y": 6.618891414421005 }, "nextControl": null, "isLocked": false, @@ -33,7 +33,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 3.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/T-TO-1.path b/src/main/deploy/pathplanner/paths/T-TO-1.path index 889d102..e17bcbd 100644 --- a/src/main/deploy/pathplanner/paths/T-TO-1.path +++ b/src/main/deploy/pathplanner/paths/T-TO-1.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 0.8795509328910394, - "y": 6.689056341075646 + "x": 0.8327743151212799, + "y": 6.560420642208806 }, "prevControl": null, "nextControl": { - "x": 1.3707054194735169, - "y": 6.817692039942485 + "x": 1.3239288017037574, + "y": 6.689056341075645 }, "isLocked": false, "linkedName": "TOP SHOOT" @@ -51,7 +51,7 @@ ], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 3.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/T-TO-2.path b/src/main/deploy/pathplanner/paths/T-TO-2.path index 04055a7..640a7f9 100644 --- a/src/main/deploy/pathplanner/paths/T-TO-2.path +++ b/src/main/deploy/pathplanner/paths/T-TO-2.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 0.8795509328910394, - "y": 6.689056341075646 + "x": 0.8327743151212799, + "y": 6.560420642208806 }, "prevControl": null, "nextControl": { - "x": 1.6162826627647553, - "y": 6.303149244475128 + "x": 1.5695060449949958, + "y": 6.174513545608288 }, "isLocked": false, "linkedName": "TOP SHOOT" @@ -51,7 +51,7 @@ ], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 3.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/T-TO-3.path b/src/main/deploy/pathplanner/paths/T-TO-3.path index 0fe36a6..9c6511d 100644 --- a/src/main/deploy/pathplanner/paths/T-TO-3.path +++ b/src/main/deploy/pathplanner/paths/T-TO-3.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 0.8795509328910394, - "y": 6.689056341075646 + "x": 0.8327743151212799, + "y": 6.560420642208806 }, "prevControl": null, "nextControl": { - "x": 2.844168879220949, - "y": 6.221290163378049 + "x": 2.79739226145119, + "y": 6.092654464511209 }, "isLocked": false, "linkedName": "TOP SHOOT" @@ -51,7 +51,7 @@ ], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 3.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/T-TO-M1.path b/src/main/deploy/pathplanner/paths/T-TO-M1.path index 89d1d73..1e5fca4 100644 --- a/src/main/deploy/pathplanner/paths/T-TO-M1.path +++ b/src/main/deploy/pathplanner/paths/T-TO-M1.path @@ -3,41 +3,41 @@ "waypoints": [ { "anchor": { - "x": 0.8795509328910394, - "y": 6.689056341075646 + "x": 0.8327743151212799, + "y": 6.560420642208806 }, "prevControl": null, "nextControl": { - "x": 1.9889153137819637, - "y": 6.199087072848821 + "x": 2.61028579037215, + "y": 6.630585568863446 }, "isLocked": false, "linkedName": "TOP SHOOT" }, { "anchor": { - "x": 2.917573402714373, - "y": 6.286398158381903 + "x": 3.0429695047424277, + "y": 6.560420642208806 }, "prevControl": { - "x": 2.5200511662284586, - "y": 6.304887564730085 + "x": 2.6472040803080072, + "y": 6.518761123847288 }, "nextControl": { - "x": 3.2793314510542197, - "y": 6.269572202645167 + "x": 3.9317252423678632, + "y": 6.653973877748326 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.9545143705672015, - "y": 7.226987445427883 + "x": 8.57430455601652, + "y": 7.402399762064483 }, "prevControl": { - "x": 7.483034508688559, - "y": 6.690794661330605 + "x": 6.375803520837811, + "y": 7.566117924258642 }, "nextControl": null, "isLocked": false, @@ -78,13 +78,13 @@ ], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 3.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, "goalEndState": { "velocity": 0, - "rotation": 27.050597007086274, + "rotation": -3.57633437499725, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/T-TO-M2.path b/src/main/deploy/pathplanner/paths/T-TO-M2.path index b4cd3a4..313520a 100644 --- a/src/main/deploy/pathplanner/paths/T-TO-M2.path +++ b/src/main/deploy/pathplanner/paths/T-TO-M2.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 0.8795509328910394, - "y": 6.689056341075646 + "x": 0.8327743151212799, + "y": 6.560420642208806 }, "prevControl": null, "nextControl": { - "x": 1.9889153137819633, - "y": 6.199087072848821 + "x": 1.9421386960122038, + "y": 6.070451373981981 }, "isLocked": false, "linkedName": "TOP SHOOT" @@ -78,7 +78,7 @@ ], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 3.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/T-TO-M3.path b/src/main/deploy/pathplanner/paths/T-TO-M3.path index 962666a..d7f453b 100644 --- a/src/main/deploy/pathplanner/paths/T-TO-M3.path +++ b/src/main/deploy/pathplanner/paths/T-TO-M3.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 0.8795509328910394, - "y": 6.689056341075646 + "x": 0.8327743151212799, + "y": 6.560420642208806 }, "prevControl": null, "nextControl": { - "x": 1.9889153137819628, - "y": 6.199087072848821 + "x": 1.9421386960122033, + "y": 6.070451373981981 }, "isLocked": false, "linkedName": "TOP SHOOT" @@ -76,7 +76,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 3.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/TO-1.path b/src/main/deploy/pathplanner/paths/TO-1.path index 3013b0d..d7a4833 100644 --- a/src/main/deploy/pathplanner/paths/TO-1.path +++ b/src/main/deploy/pathplanner/paths/TO-1.path @@ -33,7 +33,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 3.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/TO-2.path b/src/main/deploy/pathplanner/paths/TO-2.path index acf125d..d88b0fd 100644 --- a/src/main/deploy/pathplanner/paths/TO-2.path +++ b/src/main/deploy/pathplanner/paths/TO-2.path @@ -33,7 +33,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 3.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/TO-3.path b/src/main/deploy/pathplanner/paths/TO-3.path index 2bb2b20..36da75c 100644 --- a/src/main/deploy/pathplanner/paths/TO-3.path +++ b/src/main/deploy/pathplanner/paths/TO-3.path @@ -33,7 +33,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 3.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 2d9af7b..a5d8dbc 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -47,13 +47,13 @@ public static final class Swerve { public static final SensorDirectionValue cancoderInvert = chosenModule.cancoderInvert; /* Swerve Current Limiting */ - public static final int angleCurrentLimit = 15; - public static final int angleCurrentThreshold = 30; + public static final int angleCurrentLimit = 12; + public static final int angleCurrentThreshold = 25; public static final double angleCurrentThresholdTime = 0.1; public static final boolean angleEnableCurrentLimit = true; - public static final int driveCurrentLimit = 20; - public static final int driveCurrentThreshold = 40; + public static final int driveCurrentLimit = 30; + public static final int driveCurrentThreshold = 45; public static final double driveCurrentThresholdTime = 0.1; public static final boolean driveEnableCurrentLimit = true; @@ -82,7 +82,10 @@ public static final class Swerve { /** Meters per Second */ public static final double maxSpeed = 4.5; //TODOx: This must be tuned to specific robot DONE /** Radians per Second */ - public static final double maxAngularVelocity = 10.0; //TODOx: This must be tuned to specific robot DONE + public static final double maxAngularVelocity = 7.0; //TODOx: This must be tuned to specific robot DONE + + /** Rotation control dampening */ + public static final double rotationDampening = 0.75; /* Neutral Modes */ public static final NeutralModeValue angleNeutralMode = NeutralModeValue.Coast; @@ -91,8 +94,8 @@ public static final class Swerve { /* Module Specific Constants */ /* Front Left Module - Module 0 */ public static final class Mod0 { //TODOx: This must be tuned to specific robot DONE - public static final int driveMotorID = 3; - public static final int angleMotorID = 4; + public static final int driveMotorID = 5; + public static final int angleMotorID = 6; public static final int canCoderID = 18; public static final Rotation2d angleOffset = Rotation2d.fromDegrees(11.25); public static final SwerveModuleConstants constants = @@ -101,8 +104,8 @@ public static final class Mod0 { //TODOx: This must be tuned to specific robot D /* Front Right Module - Module 1 */ public static final class Mod1 { //TODOx: This must be tuned to specific robot DONE - public static final int driveMotorID = 1; - public static final int angleMotorID = 2; + public static final int driveMotorID = 3; + public static final int angleMotorID = 4; public static final int canCoderID = 17; public static final Rotation2d angleOffset = Rotation2d.fromDegrees(-93.25); public static final SwerveModuleConstants constants = @@ -111,20 +114,20 @@ public static final class Mod1 { //TODOx: This must be tuned to specific robot D /* Back Left Module - Module 2 */ public static final class Mod2 { //TODOx: This must be tuned to specific robot DONE - public static final int driveMotorID = 5; - public static final int angleMotorID = 6; + public static final int driveMotorID = 7; + public static final int angleMotorID = 8; public static final int canCoderID = 16; - public static final Rotation2d angleOffset = Rotation2d.fromDegrees(-42.45); + public static final Rotation2d angleOffset = Rotation2d.fromDegrees(-220.9+90); public static final SwerveModuleConstants constants = new SwerveModuleConstants(driveMotorID, angleMotorID, canCoderID, angleOffset); } /* Back Right Module - Module 3 */ public static final class Mod3 { //TODOx: This must be tuned to specific robot DONE - public static final int driveMotorID = 7; - public static final int angleMotorID = 8; + public static final int driveMotorID = 1; + public static final int angleMotorID = 2; public static final int canCoderID = 19; - public static final Rotation2d angleOffset = Rotation2d.fromDegrees(55.89); + public static final Rotation2d angleOffset = Rotation2d.fromDegrees(-313.4+90); 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 5aef700..abe96c9 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -136,6 +136,7 @@ public RobotContainer() { intake.setDefaultCommand(new IntakeDefault(intake,LIM)); + NamedCommands.registerCommand("RunIntake", new IntakeRun(intake, LIM, new JoystickButton(secondary, 3), theLEDs)); NamedCommands.registerCommand("EndIntake", new IntakeDefault(intake, LIM)); NamedCommands.registerCommand("LaunchASAP", new LaunchASAP(intake,launcher,theLEDs)); @@ -164,7 +165,7 @@ public RobotContainer() { // theLEDs.SetFull(255, 60, 0); theLEDs.setMode("D"); - + m_visionThread = new Thread( () -> { @@ -230,6 +231,7 @@ private void configureButtonBindings() { // new JoystickButton(secondary, 4).onTrue(new LaunchASAP(intake,launcher,theLEDs)); new JoystickButton(secondary, 4).onTrue(new LaunchControled(intake,launcher,theLEDs,new JoystickButton(secondary, 4),new JoystickButton(secondary, 6))); new JoystickButton(secondary, 2).onTrue(new IntakeRun(intake, LIM, new JoystickButton(secondary, 2),theLEDs)); + new JoystickButton(driver, 5).onTrue(new IntakeRun(intake, LIM, new JoystickButton(driver, 5),theLEDs)); new JoystickButton(secondary, 3).onTrue(new IntakeFix(intake, LIM, new JoystickButton(secondary, 3),new JoystickButton(secondary, 6),theLEDs)); new JoystickButton(secondary, 1).onTrue(new IntakeAmp(intake, LIM, new JoystickButton(secondary, 1), new JoystickButton(secondary, 6),theLEDs)); new JoystickButton(driver, 4).onTrue( @@ -241,7 +243,8 @@ private void configureButtonBindings() { launcher, intake, new JoystickButton(driver, 4), - new JoystickButton(secondary, 6) + new JoystickButton(secondary, 6), + new JoystickButton(driver, 6) )); new JoystickButton(driver, 3).onTrue( new TargetSwervePlus( diff --git a/src/main/java/frc/robot/commands/SmartShoot.java b/src/main/java/frc/robot/commands/SmartShoot.java deleted file mode 100644 index ef16016..0000000 --- a/src/main/java/frc/robot/commands/SmartShoot.java +++ /dev/null @@ -1,45 +0,0 @@ -package frc.robot.commands; - -import edu.wpi.first.networktables.PubSub; -import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.button.JoystickButton; -import java.util.function.BooleanSupplier; - -import frc.robot.Constants; -import frc.robot.subsystems.Intake; -import frc.robot.subsystems.LEDS; - -public class SmartShoot extends Command { - - - /** - * Run intake untill note is obtained or manualy disabled - * - * @param s_Intake Intake object - */ - public SmartShoot() { - } - - @Override - public void initialize() { - - } - - @Override - public void execute() { - - } - - @Override - public void end(boolean interrupted) { - - } - - @Override - public boolean isFinished() { - return false; - } - -} diff --git a/src/main/java/frc/robot/commands/TargetSwerve.java b/src/main/java/frc/robot/commands/TargetSwerve.java index a94f873..22393b3 100644 --- a/src/main/java/frc/robot/commands/TargetSwerve.java +++ b/src/main/java/frc/robot/commands/TargetSwerve.java @@ -27,7 +27,7 @@ public class TargetSwerve extends Command { private JoystickButton overrideButton; private JoystickButton launchButton; - + private JoystickButton altLaunch; private double swerve_X_speed; private double swerve_Y_speed; @@ -57,7 +57,15 @@ public class TargetSwerve extends Command { * @param rotationSup Rotation suplier, typicaly a controlor axis * @param robotCentricSup wether to drive relative to the robot. */ - public TargetSwerve(Swerve s_Swerve, DoubleSupplier strafeSup, DoubleSupplier rotationSup, LEDS led, Launcher s_Launcher, Intake s_Intake, JoystickButton overrideButton, JoystickButton launchButton) { + public TargetSwerve(Swerve s_Swerve, + DoubleSupplier strafeSup, + DoubleSupplier rotationSup, + LEDS led, + Launcher s_Launcher, + Intake s_Intake, + JoystickButton overrideButton, + JoystickButton launchButton, + JoystickButton altLaunch) { this.s_Swerve = s_Swerve; this.s_Launcher = s_Launcher; this.s_Intake = s_Intake; @@ -68,6 +76,7 @@ public TargetSwerve(Swerve s_Swerve, DoubleSupplier strafeSup, DoubleSupplier ro this.rotationSup = rotationSup; this.overrideButton = overrideButton; this.launchButton = launchButton; + this.altLaunch = altLaunch; this.led = led; } @@ -134,7 +143,7 @@ public void execute() { } SmartDashboard.putNumber("SmartLaunch", timerL.get()); - if((launchButton.getAsBoolean() && !end)) { + if((launchButton.getAsBoolean() && !end) || (altLaunch.getAsBoolean() && !end)) { s_Intake.pushIntake(false); end = true; timer.start(); From 39c1247684a34900dfa18fc91edf990e63c8557e Mon Sep 17 00:00:00 2001 From: StoneCommander <67015041+StoneCommander@users.noreply.github.com> Date: Tue, 2 Jul 2024 09:43:49 -0600 Subject: [PATCH 12/12] Last Push --- .../deploy/pathplanner/paths/M-SHOOT.path | 8 +++---- src/main/deploy/pathplanner/paths/M-TO-1.path | 8 +++---- src/main/deploy/pathplanner/paths/M-TO-2.path | 8 +++---- src/main/deploy/pathplanner/paths/M-TO-3.path | 8 +++---- .../pathplanner/paths/SmartLaunchMid.path | 12 +++++----- src/main/java/frc/robot/Constants.java | 22 +++++++++++-------- .../java/frc/robot/commands/TargetSwerve.java | 6 ++--- .../frc/robot/commands/TargetSwerveAuto.java | 5 +++-- .../frc/robot/commands/TargetSwervePlus.java | 6 ++--- src/main/java/frc/robot/subsystems/LEDS.java | 3 +-- .../java/frc/robot/subsystems/Swerve.java | 3 +++ 11 files changed, 48 insertions(+), 41 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/M-SHOOT.path b/src/main/deploy/pathplanner/paths/M-SHOOT.path index eb6c6e8..e748879 100644 --- a/src/main/deploy/pathplanner/paths/M-SHOOT.path +++ b/src/main/deploy/pathplanner/paths/M-SHOOT.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 1.4993411183403562, - "y": 5.507946742389212 + "x": 1.3122346472613169, + "y": 5.531335051274091 }, "prevControl": { - "x": 1.6279768172071956, - "y": 5.5196408968316515 + "x": 1.4408703461281562, + "y": 5.543029205716531 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/M-TO-1.path b/src/main/deploy/pathplanner/paths/M-TO-1.path index 19c03b6..a575a60 100644 --- a/src/main/deploy/pathplanner/paths/M-TO-1.path +++ b/src/main/deploy/pathplanner/paths/M-TO-1.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.4993411183403562, - "y": 5.507946742389212 + "x": 1.3122346472613169, + "y": 5.531335051274091 }, "prevControl": null, "nextControl": { - "x": 1.4876469638979162, - "y": 6.268066781147807 + "x": 1.3005404928188768, + "y": 6.291455090032686 }, "isLocked": false, "linkedName": "MID SHOOT" diff --git a/src/main/deploy/pathplanner/paths/M-TO-2.path b/src/main/deploy/pathplanner/paths/M-TO-2.path index 4734b8b..ecc7bb4 100644 --- a/src/main/deploy/pathplanner/paths/M-TO-2.path +++ b/src/main/deploy/pathplanner/paths/M-TO-2.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.4993411183403562, - "y": 5.507946742389212 + "x": 1.3122346472613169, + "y": 5.531335051274091 }, "prevControl": null, "nextControl": { - "x": 1.5695060449949956, - "y": 5.51964089683165 + "x": 1.3823995739159562, + "y": 5.543029205716529 }, "isLocked": false, "linkedName": "MID SHOOT" diff --git a/src/main/deploy/pathplanner/paths/M-TO-3.path b/src/main/deploy/pathplanner/paths/M-TO-3.path index 7e0edd1..527a15d 100644 --- a/src/main/deploy/pathplanner/paths/M-TO-3.path +++ b/src/main/deploy/pathplanner/paths/M-TO-3.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.4993411183403562, - "y": 5.507946742389212 + "x": 1.3122346472613169, + "y": 5.531335051274091 }, "prevControl": null, "nextControl": { - "x": 1.2069872572793576, - "y": 4.385307915914978 + "x": 1.0198807862003183, + "y": 4.408696224799857 }, "isLocked": false, "linkedName": "MID SHOOT" diff --git a/src/main/deploy/pathplanner/paths/SmartLaunchMid.path b/src/main/deploy/pathplanner/paths/SmartLaunchMid.path index d3f88a5..c98ea25 100644 --- a/src/main/deploy/pathplanner/paths/SmartLaunchMid.path +++ b/src/main/deploy/pathplanner/paths/SmartLaunchMid.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 1.5812001994374358, - "y": 5.55472336015897 + "x": 1.2771521839339972, + "y": 5.531335051274091 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 1.4993411183403562, - "y": 5.507946742389212 + "x": 1.3122346472613169, + "y": 5.531335051274091 }, "prevControl": { - "x": 1.2069872572793576, - "y": 5.507946742389212 + "x": 1.0198807862003183, + "y": 5.531335051274091 }, "nextControl": null, "isLocked": false, diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index a5d8dbc..579cecb 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -68,11 +68,13 @@ public static final class Swerve { public static final double angleKD = chosenModule.angleKD; /* Drive Motor PID Values */ - public static final double driveKP = 0.0; //TODO: This must be tuned to specific robot + public static final double driveKP = 0.8; //TODO: This must be tuned to specific robot public static final double driveKI = 0.0; - public static final double driveKD = 0.0; + public static final double driveKD = 0.3; public static final double driveKF = 0.0; + + /* Drive Motor Characterization Values From SYSID */ public static final double driveKS = 0.32; //TODO: This must be tuned to specific robot public static final double driveKV = 1.51; @@ -80,7 +82,7 @@ public static final class Swerve { /* Swerve Profiling Values */ /** Meters per Second */ - public static final double maxSpeed = 4.5; //TODOx: This must be tuned to specific robot DONE + public static final double maxSpeed = 5.5; //TODOx: This must be tuned to specific robot DONE /** Radians per Second */ public static final double maxAngularVelocity = 7.0; //TODOx: This must be tuned to specific robot DONE @@ -116,8 +118,8 @@ public static final class Mod1 { //TODOx: This must be tuned to specific robot D public static final class Mod2 { //TODOx: This must be tuned to specific robot DONE public static final int driveMotorID = 7; public static final int angleMotorID = 8; - public static final int canCoderID = 16; - public static final Rotation2d angleOffset = Rotation2d.fromDegrees(-220.9+90); + public static final int canCoderID = 19; + public static final Rotation2d angleOffset = Rotation2d.fromDegrees(-42.45); public static final SwerveModuleConstants constants = new SwerveModuleConstants(driveMotorID, angleMotorID, canCoderID, angleOffset); } @@ -126,8 +128,8 @@ public static final class Mod2 { //TODOx: This must be tuned to specific robot D public static final class Mod3 { //TODOx: This must be tuned to specific robot DONE public static final int driveMotorID = 1; public static final int angleMotorID = 2; - public static final int canCoderID = 19; - public static final Rotation2d angleOffset = Rotation2d.fromDegrees(-313.4+90); + public static final int canCoderID = 16; + public static final Rotation2d angleOffset = Rotation2d.fromDegrees(55.89); public static final SwerveModuleConstants constants = new SwerveModuleConstants(driveMotorID, angleMotorID, canCoderID, angleOffset); } @@ -198,9 +200,11 @@ public static final class TargetSwerve { // public static final double P = 0.05; // public static final double I = 0; // public static final double D = 0.001; - public static final double P = 0.08; + public static final double P = 0.06; public static final double I = 0; - public static final double D = 0.003; + public static final double D = 0.002; + + public static final double clamp = 0.75; public static final double SmartLaunchTime = 0.5; diff --git a/src/main/java/frc/robot/commands/TargetSwerve.java b/src/main/java/frc/robot/commands/TargetSwerve.java index 22393b3..9ce835a 100644 --- a/src/main/java/frc/robot/commands/TargetSwerve.java +++ b/src/main/java/frc/robot/commands/TargetSwerve.java @@ -115,15 +115,15 @@ public void execute() { if (bHold) bHold = overrideButton.getAsBoolean(); /* Get Values, Deadband*/ double rotationVal = MathUtil.applyDeadband(rotationSup.getAsDouble(), Constants.stickDeadband); - double throtleVal = 0.5; + double throtleVal = 0.3; // Get Targert X and Y double X_from_camera = LLtbl.getValue("tx").getDouble(); double Y_from_camera = LLtbl.getValue("ty").getDouble(); // Calculate PID for movement - swerve_X_speed = MathUtil.clamp(Xpid.calculate(X_from_camera), -1,1); - swerve_Y_speed = MathUtil.clamp(Ypid.calculate(Y_from_camera), -1,1); + swerve_X_speed = MathUtil.clamp(Xpid.calculate(X_from_camera), -Constants.TargetSwerve.clamp,Constants.TargetSwerve.clamp); + swerve_Y_speed = MathUtil.clamp(Ypid.calculate(Y_from_camera), -Constants.TargetSwerve.clamp,Constants.TargetSwerve.clamp); // If target reached set shoot to true shoot = Xpid.atSetpoint() && Ypid.atSetpoint(); diff --git a/src/main/java/frc/robot/commands/TargetSwerveAuto.java b/src/main/java/frc/robot/commands/TargetSwerveAuto.java index d00236d..06fcd17 100644 --- a/src/main/java/frc/robot/commands/TargetSwerveAuto.java +++ b/src/main/java/frc/robot/commands/TargetSwerveAuto.java @@ -96,8 +96,8 @@ public void execute() { double X_from_camera = LLtbl.getValue("tx").getDouble(); double Y_from_camera = LLtbl.getValue("ty").getDouble(); // Calculate PID for movement - swerve_X_speed = MathUtil.clamp(Xpid.calculate(X_from_camera), -1,1); - swerve_Y_speed = MathUtil.clamp(Ypid.calculate(Y_from_camera), -1,1); + swerve_X_speed = MathUtil.clamp(Xpid.calculate(X_from_camera), -Constants.TargetSwerve.clamp,Constants.TargetSwerve.clamp); + swerve_Y_speed = MathUtil.clamp(Ypid.calculate(Y_from_camera), -Constants.TargetSwerve.clamp,Constants.TargetSwerve.clamp); // If target reached set shoot to true shoot = Xpid.atSetpoint() && Ypid.atSetpoint(); @@ -128,6 +128,7 @@ public void execute() { launching = true; } + SmartDashboard.putNumber("Xpid", swerve_X_speed); SmartDashboard.putNumber("Ypid", swerve_Y_speed); SmartDashboard.putBoolean("Xshoot", Xpid.atSetpoint()); diff --git a/src/main/java/frc/robot/commands/TargetSwervePlus.java b/src/main/java/frc/robot/commands/TargetSwervePlus.java index e55cd67..a4e1f21 100644 --- a/src/main/java/frc/robot/commands/TargetSwervePlus.java +++ b/src/main/java/frc/robot/commands/TargetSwervePlus.java @@ -108,15 +108,15 @@ public void execute() { if (bHold) bHold = overrideButton.getAsBoolean(); /* Get Values, Deadband*/ double rotationVal = MathUtil.applyDeadband(rotationSup.getAsDouble(), Constants.stickDeadband); - double throtleVal = 0.5; + double throtleVal = 0.3; // Get Targert X and Y double X_from_camera = LLtbl.getValue("tx").getDouble(); double Y_from_camera = LLtbl.getValue("ty").getDouble(); // Calculate PID for movement - swerve_X_speed = MathUtil.clamp(Xpid.calculate(X_from_camera), -1,1); - swerve_Y_speed = MathUtil.clamp(Ypid.calculate(Y_from_camera), -1,1); + swerve_X_speed = MathUtil.clamp(Xpid.calculate(X_from_camera), -Constants.TargetSwerve.clamp,Constants.TargetSwerve.clamp); + swerve_Y_speed = MathUtil.clamp(Ypid.calculate(Y_from_camera), -Constants.TargetSwerve.clamp,Constants.TargetSwerve.clamp); // If target reached set shoot to true shoot = Xpid.atSetpoint() && Ypid.atSetpoint(); diff --git a/src/main/java/frc/robot/subsystems/LEDS.java b/src/main/java/frc/robot/subsystems/LEDS.java index d8d2da2..0436990 100644 --- a/src/main/java/frc/robot/subsystems/LEDS.java +++ b/src/main/java/frc/robot/subsystems/LEDS.java @@ -191,8 +191,7 @@ public void rainbow() { m_led.setData(m_ledBuffer); } - - + /** * Rainbow function, called perodicly, shifts a rainbow through the strip * diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index f8f421e..2baa06e 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -244,6 +244,9 @@ public void periodic(){ SmartDashboard.putNumber("Mod " + mod.moduleNumber + " Angle", mod.getPosition().angle.getDegrees()); SmartDashboard.putNumber("Mod " + mod.moduleNumber + " Velocity", mod.getState().speedMetersPerSecond); } + + // put module states to logger + } } \ No newline at end of file