From 873dfc85097650e88fa7a024c74bf76276a18447 Mon Sep 17 00:00:00 2001 From: Garrett Summerfield Date: Mon, 18 Mar 2024 14:14:21 -0500 Subject: [PATCH 01/36] Add PhotonLib, manifest Vision helper class PhotonLib will help assist with the management and the communication with the robot's current onboard cameras with tags and other information from the cameras. --- src/main/java/frc/robot/Constants.java | 10 ++- src/main/java/frc/robot/RobotContainer.java | 9 ++- .../java/frc/robot/subsystems/Vision.java | 29 ++++++++ .../swervedrive/SwerveSubsystem.java | 72 +++++++++++++++---- vendordeps/photonlib.json | 57 +++++++++++++++ 5 files changed, 155 insertions(+), 22 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/Vision.java create mode 100644 vendordeps/photonlib.json diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index e750605..0753d4d 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -4,10 +4,10 @@ package frc.robot; +import com.pathplanner.lib.util.PIDConstants; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.util.Units; import swervelib.math.Matter; -import swervelib.parser.PIDFConfig; /** * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean @@ -24,12 +24,10 @@ public final class Constants { new Matter(new Translation3d(0, 0, Units.inchesToMeters(8)), ROBOT_MASS); public static final double LOOP_TIME = 0.13; // s, 20ms + 110ms sprk max velocity lag - public static final class Auton { + public static final class AutonConstants { - public static final PIDFConfig TranslationPID = new PIDFConfig(0.7, 0, 0); - public static final PIDFConfig angleAutoPID = new PIDFConfig(0.4, 0, 0.01); - - public static final double MAX_ACCELERATION = 2; + public static final PIDConstants TRANSLATION_PID = new PIDConstants(0.7, 0, 0); + public static final PIDConstants ANGLE_PID = new PIDConstants(0.4, 0, 0.01); } public static final class HardwareConstants { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d2407b3..92e8d27 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -35,6 +35,7 @@ import frc.robot.subsystems.swervedrive.SwerveSubsystem; import java.io.File; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; +import org.photonvision.PhotonCamera; /** * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -64,6 +65,9 @@ public class RobotContainer { private final LoggedDashboardChooser autoChooser; + private final PhotonCamera limeLight = new PhotonCamera("photonvision-limelight"); + private final PhotonCamera rpi = new PhotonCamera("photonvision-rpi"); + /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { @@ -185,15 +189,14 @@ private void configureBindings() { // Schedule `ExampleCommand` when `exampleCondition` changes to `true` new JoystickButton(driverXbox, 1).onTrue((new InstantCommand(drivebase::zeroGyro))); - new JoystickButton(driverXbox, 3).onTrue(new InstantCommand(drivebase::addFakeVisionReading)); + new JoystickButton(driverXbox, 3).onTrue(Commands.deferredProxy(() -> drivebase.aimAtTarget(limeLight))); new JoystickButton(driverXbox, 2) .whileTrue( Commands.deferredProxy( () -> drivebase.driveToPose( new Pose2d(new Translation2d(4, 4), Rotation2d.fromDegrees(0))))); - // new JoystickButton(driverXbox, 3).whileTrue(new RepeatCommand(new - // InstantCommand(drivebase::lock, drivebase))); + new JoystickButton(driverXbox, 4).whileTrue(new InstantCommand(drivebase::lock, drivebase)); } /** diff --git a/src/main/java/frc/robot/subsystems/Vision.java b/src/main/java/frc/robot/subsystems/Vision.java new file mode 100644 index 0000000..94ed381 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Vision.java @@ -0,0 +1,29 @@ +package frc.robot.subsystems; + +import org.photonvision.PhotonCamera; + +public class Vision { + private PhotonCamera limelight; + private PhotonCamera intakeCamera; + private PhotonCamera conveyorCamera; + + public Vision() { + limelight = new PhotonCamera("limelight"); + intakeCamera = new PhotonCamera("intake"); + conveyorCamera = new PhotonCamera("conveyor"); + } + + public PhotonCamera getLimelight() { + return limelight; + } + + public PhotonCamera getIntakeCamera() { + return intakeCamera; + } + + public PhotonCamera getConveyorCamera() { + return conveyorCamera; + } + + +} diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index 567b6a2..92f6542 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -9,7 +9,6 @@ import com.pathplanner.lib.path.PathConstraints; import com.pathplanner.lib.path.PathPlannerPath; import com.pathplanner.lib.util.HolonomicPathFollowerConfig; -import com.pathplanner.lib.util.PIDConstants; import com.pathplanner.lib.util.ReplanningConfig; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -22,10 +21,15 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; +import frc.robot.Constants.AutonConstants; import java.io.File; import java.util.function.DoubleSupplier; +import org.photonvision.PhotonCamera; +import org.photonvision.targeting.PhotonPipelineResult; import swervelib.SwerveController; import swervelib.SwerveDrive; +import swervelib.SwerveDriveTest; import swervelib.math.SwerveMath; import swervelib.parser.SwerveControllerConfiguration; import swervelib.parser.SwerveDriveConfiguration; @@ -77,7 +81,10 @@ public SwerveSubsystem(File directory) { } swerveDrive.setHeadingCorrection( false); // Heading correction should only be used while controlling the robot via angle. - + swerveDrive.setCosineCompensator( + !SwerveDriveTelemetry + .isSimulation); // Disables cosine compensation for simulations since it causes + // discrepancies not seen in real life. setupPathPlanner(); } @@ -103,12 +110,9 @@ public void setupPathPlanner() { // ChassisSpeeds new HolonomicPathFollowerConfig( // HolonomicPathFollowerConfig, this should likely live in // your Constants class - new PIDConstants(5.0, 0.0, 0.0), + AutonConstants.TRANSLATION_PID, // Translation PID constants - new PIDConstants( - swerveDrive.swerveController.config.headingPIDF.p, - swerveDrive.swerveController.config.headingPIDF.i, - swerveDrive.swerveController.config.headingPIDF.d), + AutonConstants.ANGLE_PID, // Rotation PID constants 4.5, // Max module speed, in m/s @@ -128,15 +132,37 @@ public void setupPathPlanner() { ); } + /** + * Aim the robot at the target returned by PhotonVision. + * + * @param camera {@link PhotonCamera} to communicate with. + * @return A {@link Command} which will run the alignment. + */ + public Command aimAtTarget(PhotonCamera camera) { + return run( + () -> { + PhotonPipelineResult result = camera.getLatestResult(); + if (result.hasTargets()) { + drive( + getTargetSpeeds( + 0, + 0, + Rotation2d.fromDegrees( + result + .getBestTarget() + .getYaw()))); // Not sure if this will work, more math may be required. + } + }); + } + /** * Get the path follower with events. * * @param pathName PathPlanner path name. - * @param setOdomToStart Set the odometry position to the start of the path. * @return {@link AutoBuilder#followPath(PathPlannerPath)} path command. */ public Command getAutonomousCommand(String pathName) { - // Load the path you want to follow using its name in the GUI + // Create a path following command using AutoBuilder. This will also trigger event markers. return new PathPlannerAuto(pathName); } @@ -192,7 +218,7 @@ public Command driveCommand( yInput, headingX.getAsDouble(), headingY.getAsDouble(), - swerveDrive.getYaw().getRadians(), + swerveDrive.getOdometryHeading().getRadians(), swerveDrive.getMaximumVelocity())); }); } @@ -217,11 +243,31 @@ public Command simDriveCommand( translationX.getAsDouble(), translationY.getAsDouble(), rotation.getAsDouble() * Math.PI, - swerveDrive.getYaw().getRadians(), + swerveDrive.getOdometryHeading().getRadians(), swerveDrive.getMaximumVelocity())); }); } + /** + * Command to characterize the robot drive motors using SysId + * + * @return SysId Drive Command + */ + public Command sysIdDriveMotorCommand() { + return SwerveDriveTest.generateSysIdCommand( + SwerveDriveTest.setDriveSysIdRoutine(new Config(), this, swerveDrive, 12), 3.0, 5.0, 3.0); + } + + /** + * Command to characterize the robot angle motors using SysId + * + * @return SysId Angle Command + */ + public Command sysIdAngleMotorCommand() { + return SwerveDriveTest.generateSysIdCommand( + SwerveDriveTest.setAngleSysIdRoutine(new Config(), this, swerveDrive), 3.0, 5.0, 3.0); + } + /** * Command to drive the robot using translative values and heading as angular velocity. * @@ -374,7 +420,7 @@ public Rotation2d getHeading() { * @param yInput Y joystick input for the robot to move in the Y direction. * @param headingX X joystick which controls the angle of the robot. * @param headingY Y joystick which controls the angle of the robot. - * @return {@link ChassisSpeeds} which can be sent to th Swerve Drive. + * @return {@link ChassisSpeeds} which can be sent to the Swerve Drive. */ public ChassisSpeeds getTargetSpeeds( double xInput, double yInput, double headingX, double headingY) { @@ -391,7 +437,7 @@ public ChassisSpeeds getTargetSpeeds( * @param xInput X joystick input for the robot to move in the X direction. * @param yInput Y joystick input for the robot to move in the Y direction. * @param angle The angle in as a {@link Rotation2d}. - * @return {@link ChassisSpeeds} which can be sent to th Swerve Drive. + * @return {@link ChassisSpeeds} which can be sent to the Swerve Drive. */ public ChassisSpeeds getTargetSpeeds(double xInput, double yInput, Rotation2d angle) { xInput = Math.pow(xInput, 3); diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json new file mode 100644 index 0000000..77641e4 --- /dev/null +++ b/vendordeps/photonlib.json @@ -0,0 +1,57 @@ +{ + "fileName": "photonlib.json", + "name": "photonlib", + "version": "v2024.2.9", + "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", + "frcYear": "2024", + "mavenUrls": [ + "https://maven.photonvision.org/repository/internal", + "https://maven.photonvision.org/repository/snapshots" + ], + "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json", + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-cpp", + "version": "v2024.2.9", + "libName": "photonlib", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2024.2.9", + "libName": "photontargeting", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "javaDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-java", + "version": "v2024.2.9" + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-java", + "version": "v2024.2.9" + } + ] +} \ No newline at end of file From caabd74467dbaf38c638b3781a67d3ee2b29200b Mon Sep 17 00:00:00 2001 From: Garrett Summerfield Date: Mon, 18 Mar 2024 14:34:25 -0500 Subject: [PATCH 02/36] Update camera instances from Vision subsystem Instead of creating a new object for each camera when we need to access it from the RobotContainer, it is now created as the whole class is constructed with the specified values. --- src/main/java/frc/robot/RobotContainer.java | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 92e8d27..aaa4157 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -32,6 +32,7 @@ import frc.robot.subsystems.Elevator; import frc.robot.subsystems.FlyWheel; import frc.robot.subsystems.Intake; +import frc.robot.subsystems.Vision; import frc.robot.subsystems.swervedrive.SwerveSubsystem; import java.io.File; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; @@ -65,8 +66,10 @@ public class RobotContainer { private final LoggedDashboardChooser autoChooser; - private final PhotonCamera limeLight = new PhotonCamera("photonvision-limelight"); - private final PhotonCamera rpi = new PhotonCamera("photonvision-rpi"); + private final Vision vision = new Vision(); + private final PhotonCamera limeLight = vision.getLimelight(); + private final PhotonCamera intakeCamera = vision.getIntakeCamera(); + private final PhotonCamera conveyorCamera = vision.getConveyorCamera(); /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { From 965600e5ec0a0c12fa9b83a42366e287d5a4689a Mon Sep 17 00:00:00 2001 From: Garrett Summerfield Date: Tue, 19 Mar 2024 12:04:57 -0500 Subject: [PATCH 03/36] Add constants for PhotonVision, aim controls for LimeLight Constants for PhotonVision are made to have permanent locations in 3D space for the cameras to help with positioning. --- src/main/java/frc/robot/Constants.java | 23 +++++++ src/main/java/frc/robot/RobotContainer.java | 3 +- .../java/frc/robot/subsystems/Vision.java | 61 +++++++++++++------ 3 files changed, 68 insertions(+), 19 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 0753d4d..0e20e2e 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -94,4 +94,27 @@ public static class MechanismConstants { public static final double FLYWHEEL_MOTOR_REDUCED_SPEED = 0.20; public static final double FLYWHEEL_MOTOR_SPEED_SLOW = 0.15; } + + public class VisionConstants { + public class LimeLight { + public static final String LIMELIGHT_TABLE = "limelight"; + public static final double LIMELIGHT_HEIGHT = 0.0; // inches to meters + public static final double LIMELIGHT_ANGLE = 0.0; // radians + public static final double LIMELIGHT_DISTANCE = 0.0; // inches to meters + } + + public class IntakeCamera { + public static final String INTAKE_TABLE = "intake"; + public static final double INTAKE_HEIGHT = 0.0; // inches to meters + public static final double INTAKE_ANGLE = 0.0; // radians + public static final double INTAKE_DISTANCE = 0.0; // inches to meters + } + + public class ConveyorCamera { + public static final String CONVEYOR_TABLE = "conveyor"; + public static final double CONVEYOR_HEIGHT = 0.0; // inches to meters + public static final double CONVEYOR_ANGLE = 0.0; // radians + public static final double CONVEYOR_DISTANCE = 0.0; // inches to meters + } + } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index aaa4157..c89d847 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -192,7 +192,8 @@ private void configureBindings() { // Schedule `ExampleCommand` when `exampleCondition` changes to `true` new JoystickButton(driverXbox, 1).onTrue((new InstantCommand(drivebase::zeroGyro))); - new JoystickButton(driverXbox, 3).onTrue(Commands.deferredProxy(() -> drivebase.aimAtTarget(limeLight))); + new JoystickButton(driverXbox, 3) + .onTrue(Commands.deferredProxy(() -> drivebase.aimAtTarget(limeLight))); new JoystickButton(driverXbox, 2) .whileTrue( Commands.deferredProxy( diff --git a/src/main/java/frc/robot/subsystems/Vision.java b/src/main/java/frc/robot/subsystems/Vision.java index 94ed381..bf095ef 100644 --- a/src/main/java/frc/robot/subsystems/Vision.java +++ b/src/main/java/frc/robot/subsystems/Vision.java @@ -1,29 +1,54 @@ package frc.robot.subsystems; +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; import org.photonvision.PhotonCamera; +import org.photonvision.PhotonPoseEstimator; +import org.photonvision.PhotonPoseEstimator.PoseStrategy; public class Vision { - private PhotonCamera limelight; - private PhotonCamera intakeCamera; - private PhotonCamera conveyorCamera; + private PhotonCamera limelight; + private PhotonCamera intakeCamera; + private PhotonCamera conveyorCamera; + private PhotonPoseEstimator photonPoseEstimator; + private AprilTagFieldLayout aprilTagFieldLayout; - public Vision() { - limelight = new PhotonCamera("limelight"); - intakeCamera = new PhotonCamera("intake"); - conveyorCamera = new PhotonCamera("conveyor"); - } + public Vision() { + aprilTagFieldLayout = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField(); - public PhotonCamera getLimelight() { - return limelight; - } + limelight = new PhotonCamera("limelight"); + // Cam mounted facing forward, half a meter forward of center, half a meter up from center. + Transform3d robotToLimelight = + new Transform3d(new Translation3d(0.5, 0.0, 0.5), new Rotation3d(0, 0, 0)); + // Construct a PhotonPoseEstimator with the AprilTagFieldLayout, a PoseStrategy, the camera, and + // the transform from the robot to the camera. + photonPoseEstimator = + new PhotonPoseEstimator( + aprilTagFieldLayout, + PoseStrategy.CLOSEST_TO_REFERENCE_POSE, + limelight, + robotToLimelight); + photonPoseEstimator.setMultiTagFallbackStrategy(PoseStrategy.CLOSEST_TO_REFERENCE_POSE); + intakeCamera = new PhotonCamera("intake"); + conveyorCamera = new PhotonCamera("conveyor"); + } - public PhotonCamera getIntakeCamera() { - return intakeCamera; - } + public PhotonCamera getLimelight() { + return limelight; + } - public PhotonCamera getConveyorCamera() { - return conveyorCamera; - } + public PhotonCamera getIntakeCamera() { + return intakeCamera; + } - + public PhotonCamera getConveyorCamera() { + return conveyorCamera; + } + + public int getLimelightAprilTagReading() { + return limelight.getLatestResult().getBestTarget().getFiducialId(); + } } From c12576c0762c2b362a4c52e455fc58c6d0950a8d Mon Sep 17 00:00:00 2001 From: Garrett Summerfield Date: Tue, 19 Mar 2024 15:03:19 -0500 Subject: [PATCH 04/36] Update photonlib Updated photonlib from v2024.2.9 to v2024.3.1 --- vendordeps/photonlib.json | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json index 77641e4..0e80a16 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -1,7 +1,7 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "v2024.2.9", + "version": "v2024.3.1", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", "frcYear": "2024", "mavenUrls": [ @@ -14,7 +14,7 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-cpp", - "version": "v2024.2.9", + "version": "v2024.3.1", "libName": "photonlib", "headerClassifier": "headers", "sharedLibrary": true, @@ -29,7 +29,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2024.2.9", + "version": "v2024.3.1", "libName": "photontargeting", "headerClassifier": "headers", "sharedLibrary": true, @@ -46,12 +46,12 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-java", - "version": "v2024.2.9" + "version": "v2024.3.1" }, { "groupId": "org.photonvision", "artifactId": "photontargeting-java", - "version": "v2024.2.9" + "version": "v2024.3.1" } ] } \ No newline at end of file From 8680b5623b4a9dca91ef59b382e0ce3b2425aa64 Mon Sep 17 00:00:00 2001 From: Garrett Summerfield Date: Tue, 19 Mar 2024 15:06:51 -0500 Subject: [PATCH 05/36] Update NT/PhotonVision cameras, control binds Update the camera names for accessibility of the cameras via software and NetworkTables. Update controls to use the aiming at the target on hold rather than when it is pressed. --- src/main/java/frc/robot/Constants.java | 6 +++--- src/main/java/frc/robot/RobotContainer.java | 2 +- src/main/java/frc/robot/subsystems/Vision.java | 5 ++++- 3 files changed, 8 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 0e20e2e..0edd36d 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -97,21 +97,21 @@ public static class MechanismConstants { public class VisionConstants { public class LimeLight { - public static final String LIMELIGHT_TABLE = "limelight"; + public static final String LIMELIGHT_NAME = "LIMELIGHT"; public static final double LIMELIGHT_HEIGHT = 0.0; // inches to meters public static final double LIMELIGHT_ANGLE = 0.0; // radians public static final double LIMELIGHT_DISTANCE = 0.0; // inches to meters } public class IntakeCamera { - public static final String INTAKE_TABLE = "intake"; + public static final String INTAKE_NAME = "INTAKE-CAM"; public static final double INTAKE_HEIGHT = 0.0; // inches to meters public static final double INTAKE_ANGLE = 0.0; // radians public static final double INTAKE_DISTANCE = 0.0; // inches to meters } public class ConveyorCamera { - public static final String CONVEYOR_TABLE = "conveyor"; + public static final String CONVEYOR_NAME = "CONVEYOR-CAM"; public static final double CONVEYOR_HEIGHT = 0.0; // inches to meters public static final double CONVEYOR_ANGLE = 0.0; // radians public static final double CONVEYOR_DISTANCE = 0.0; // inches to meters diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c89d847..c7f787a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -193,7 +193,7 @@ private void configureBindings() { new JoystickButton(driverXbox, 1).onTrue((new InstantCommand(drivebase::zeroGyro))); new JoystickButton(driverXbox, 3) - .onTrue(Commands.deferredProxy(() -> drivebase.aimAtTarget(limeLight))); + .whileTrue(Commands.deferredProxy(() -> drivebase.aimAtTarget(limeLight))); new JoystickButton(driverXbox, 2) .whileTrue( Commands.deferredProxy( diff --git a/src/main/java/frc/robot/subsystems/Vision.java b/src/main/java/frc/robot/subsystems/Vision.java index bf095ef..14be6b7 100644 --- a/src/main/java/frc/robot/subsystems/Vision.java +++ b/src/main/java/frc/robot/subsystems/Vision.java @@ -5,6 +5,7 @@ import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation3d; +import frc.robot.Constants.VisionConstants; import org.photonvision.PhotonCamera; import org.photonvision.PhotonPoseEstimator; import org.photonvision.PhotonPoseEstimator.PoseStrategy; @@ -19,7 +20,9 @@ public class Vision { public Vision() { aprilTagFieldLayout = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField(); - limelight = new PhotonCamera("limelight"); + limelight = new PhotonCamera(VisionConstants.LimeLight.LIMELIGHT_NAME); + conveyorCamera = new PhotonCamera(VisionConstants.ConveyorCamera.CONVEYOR_NAME); + conveyorCamera = new PhotonCamera(VisionConstants.IntakeCamera.INTAKE_NAME); // Cam mounted facing forward, half a meter forward of center, half a meter up from center. Transform3d robotToLimelight = new Transform3d(new Translation3d(0.5, 0.0, 0.5), new Rotation3d(0, 0, 0)); From 3528ed78b8a8b9b73438b27407d0cf85c9b6954f Mon Sep 17 00:00:00 2001 From: Garrett Summerfield Date: Wed, 20 Mar 2024 12:59:06 -0500 Subject: [PATCH 06/36] Add camera measurements, docs Added measurements for the camera locations in 3D space, as well as rotational data for the cameras. Updated documentation for the Vision subsystem. --- src/main/java/frc/robot/Constants.java | 33 ++++++---- .../java/frc/robot/subsystems/Vision.java | 62 +++++++++++++++---- 2 files changed, 72 insertions(+), 23 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 0edd36d..6dca6d3 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -97,24 +97,33 @@ public static class MechanismConstants { public class VisionConstants { public class LimeLight { - public static final String LIMELIGHT_NAME = "LIMELIGHT"; - public static final double LIMELIGHT_HEIGHT = 0.0; // inches to meters - public static final double LIMELIGHT_ANGLE = 0.0; // radians - public static final double LIMELIGHT_DISTANCE = 0.0; // inches to meters + public static final String NAME = "LIMELIGHT"; + public static final double LENGTH = Units.inchesToMeters(-12.5); // inches to meters + public static final double WIDTH = Units.inchesToMeters(0.0); // inches to meters + public static final double HEIGHT = Units.inchesToMeters(-23.25); // inches to meters + public static final double ROLL = Units.degreesToRadians(0.0); // degrees to radians + public static final double PITCH = Units.degreesToRadians(0.0); // degrees to radians + public static final double YAW = Units.degreesToRadians(180.0); // degrees to radians } public class IntakeCamera { - public static final String INTAKE_NAME = "INTAKE-CAM"; - public static final double INTAKE_HEIGHT = 0.0; // inches to meters - public static final double INTAKE_ANGLE = 0.0; // radians - public static final double INTAKE_DISTANCE = 0.0; // inches to meters + public static final String NAME = "INTAKE-CAM"; + public static final double LENGTH = Units.inchesToMeters(-11.75); // inches to meters + public static final double WIDTH = Units.inchesToMeters(0.0); // inches to meters + public static final double HEIGHT = Units.inchesToMeters(-12.5); // inches to meters + public static final double ROLL = Units.degreesToRadians(0.0); // degrees to radians + public static final double PITCH = Units.degreesToRadians(-20.0); // degrees to radians + public static final double YAW = Units.degreesToRadians(0.0); // degrees to radians } public class ConveyorCamera { - public static final String CONVEYOR_NAME = "CONVEYOR-CAM"; - public static final double CONVEYOR_HEIGHT = 0.0; // inches to meters - public static final double CONVEYOR_ANGLE = 0.0; // radians - public static final double CONVEYOR_DISTANCE = 0.0; // inches to meters + public static final String NAME = "CONVEYOR-CAM"; + public static final double LENGTH = Units.inchesToMeters(-5.5); // inches to meters + public static final double WIDTH = Units.inchesToMeters(0.0); // inches to meters + public static final double HEIGHT = Units.inchesToMeters(-13.5); // inches to meters + public static final double ROLL = Units.degreesToRadians(0.0); // degrees to radians + public static final double PITCH = Units.degreesToRadians(90.0); // degrees to radians + public static final double YAW = Units.degreesToRadians(180.0); // degrees to radians } } } diff --git a/src/main/java/frc/robot/subsystems/Vision.java b/src/main/java/frc/robot/subsystems/Vision.java index 14be6b7..88e400a 100644 --- a/src/main/java/frc/robot/subsystems/Vision.java +++ b/src/main/java/frc/robot/subsystems/Vision.java @@ -5,7 +5,9 @@ import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation3d; -import frc.robot.Constants.VisionConstants; +import frc.robot.Constants.VisionConstants.ConveyorCamera; +import frc.robot.Constants.VisionConstants.IntakeCamera; +import frc.robot.Constants.VisionConstants.LimeLight; import org.photonvision.PhotonCamera; import org.photonvision.PhotonPoseEstimator; import org.photonvision.PhotonPoseEstimator.PoseStrategy; @@ -14,43 +16,81 @@ public class Vision { private PhotonCamera limelight; private PhotonCamera intakeCamera; private PhotonCamera conveyorCamera; - private PhotonPoseEstimator photonPoseEstimator; + private PhotonPoseEstimator photonLimeLightPoseEstimator; private AprilTagFieldLayout aprilTagFieldLayout; public Vision() { aprilTagFieldLayout = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField(); - limelight = new PhotonCamera(VisionConstants.LimeLight.LIMELIGHT_NAME); - conveyorCamera = new PhotonCamera(VisionConstants.ConveyorCamera.CONVEYOR_NAME); - conveyorCamera = new PhotonCamera(VisionConstants.IntakeCamera.INTAKE_NAME); - // Cam mounted facing forward, half a meter forward of center, half a meter up from center. + limelight = new PhotonCamera(LimeLight.NAME); + intakeCamera = new PhotonCamera(IntakeCamera.NAME); + conveyorCamera = new PhotonCamera(ConveyorCamera.NAME); + // Cam mounted facing backwards, 12.5 in back from the robot, flat, and facing backwards Transform3d robotToLimelight = - new Transform3d(new Translation3d(0.5, 0.0, 0.5), new Rotation3d(0, 0, 0)); + new Transform3d( + new Translation3d(LimeLight.LENGTH, LimeLight.WIDTH, LimeLight.HEIGHT), + new Rotation3d(LimeLight.ROLL, LimeLight.PITCH, LimeLight.YAW)); + + // Cam mounted facing forwards, 11.75 in front of the robot, 20 degrees down, and facing + // forwards + Transform3d robotToIntakeCamera = + new Transform3d( + new Translation3d(IntakeCamera.LENGTH, IntakeCamera.WIDTH, IntakeCamera.HEIGHT), + new Rotation3d(IntakeCamera.ROLL, IntakeCamera.PITCH, IntakeCamera.YAW)); + + // Cam mounted facing downwards, 5.5 in front of the robot, 90 degrees down, and facing + // backwards + Transform3d robotToConveyorCamera = + new Transform3d( + new Translation3d(ConveyorCamera.LENGTH, ConveyorCamera.WIDTH, ConveyorCamera.HEIGHT), + new Rotation3d(ConveyorCamera.ROLL, ConveyorCamera.PITCH, ConveyorCamera.YAW)); + // Construct a PhotonPoseEstimator with the AprilTagFieldLayout, a PoseStrategy, the camera, and // the transform from the robot to the camera. - photonPoseEstimator = + photonLimeLightPoseEstimator = new PhotonPoseEstimator( aprilTagFieldLayout, PoseStrategy.CLOSEST_TO_REFERENCE_POSE, limelight, robotToLimelight); - photonPoseEstimator.setMultiTagFallbackStrategy(PoseStrategy.CLOSEST_TO_REFERENCE_POSE); - intakeCamera = new PhotonCamera("intake"); - conveyorCamera = new PhotonCamera("conveyor"); + + // Set the multi tag fallback strategy to closest to reference pose + photonLimeLightPoseEstimator.setMultiTagFallbackStrategy( + PoseStrategy.CLOSEST_TO_REFERENCE_POSE); } + /** + * Get the limelight camera + * + * @return the limelight camera + */ public PhotonCamera getLimelight() { return limelight; } + /** + * Get the intake camera + * + * @return the intake camera + */ public PhotonCamera getIntakeCamera() { return intakeCamera; } + /** + * Get the conveyor camera + * + * @return the conveyor camera + */ public PhotonCamera getConveyorCamera() { return conveyorCamera; } + /** + * Get the AprilTags ID from the limelight + * + * @return the AprilTag ID from the limelight + */ public int getLimelightAprilTagReading() { return limelight.getLatestResult().getBestTarget().getFiducialId(); } From 5fdf62184f8dd809dbdf346206b47b2c8d387353 Mon Sep 17 00:00:00 2001 From: kreylingj <102437475+kreylingj@users.noreply.github.com> Date: Wed, 20 Mar 2024 14:42:19 -0500 Subject: [PATCH 07/36] Blue alliance pathways --- .../deploy/pathplanner/autos/BL Path.auto | 2 +- .../deploy/pathplanner/autos/bl Auto.auto | 2 +- .../deploy/pathplanner/autos/rr Auto.auto | 2 +- .../paths/Blue (amp side w shoot).path | 68 +++++++++++++++++++ .../paths/Blue Amp Side w shoot.path | 52 ++++++++++++++ .../paths/Blue Center w shoot.path | 52 ++++++++++++++ 6 files changed, 175 insertions(+), 3 deletions(-) create mode 100644 src/main/deploy/pathplanner/paths/Blue (amp side w shoot).path create mode 100644 src/main/deploy/pathplanner/paths/Blue Amp Side w shoot.path create mode 100644 src/main/deploy/pathplanner/paths/Blue Center w shoot.path diff --git a/src/main/deploy/pathplanner/autos/BL Path.auto b/src/main/deploy/pathplanner/autos/BL Path.auto index 00d23e6..15e73df 100644 --- a/src/main/deploy/pathplanner/autos/BL Path.auto +++ b/src/main/deploy/pathplanner/autos/BL Path.auto @@ -14,7 +14,7 @@ { "type": "path", "data": { - "pathName": "BL Path" + "pathName": null } } ] diff --git a/src/main/deploy/pathplanner/autos/bl Auto.auto b/src/main/deploy/pathplanner/autos/bl Auto.auto index e161d3a..c3b4325 100644 --- a/src/main/deploy/pathplanner/autos/bl Auto.auto +++ b/src/main/deploy/pathplanner/autos/bl Auto.auto @@ -14,7 +14,7 @@ { "type": "path", "data": { - "pathName": "bl Path" + "pathName": null } }, { diff --git a/src/main/deploy/pathplanner/autos/rr Auto.auto b/src/main/deploy/pathplanner/autos/rr Auto.auto index 37da37c..e358e68 100644 --- a/src/main/deploy/pathplanner/autos/rr Auto.auto +++ b/src/main/deploy/pathplanner/autos/rr Auto.auto @@ -14,7 +14,7 @@ { "type": "path", "data": { - "pathName": "rr Path" + "pathName": null } }, { diff --git a/src/main/deploy/pathplanner/paths/Blue (amp side w shoot).path b/src/main/deploy/pathplanner/paths/Blue (amp side w shoot).path new file mode 100644 index 0000000..28b2afc --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Blue (amp side w shoot).path @@ -0,0 +1,68 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.5416846869729774, + "y": 6.6561862853455445 + }, + "prevControl": null, + "nextControl": { + "x": 1.5308679266007181, + "y": 6.582228659952817 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.0, + "y": 6.0 + }, + "prevControl": { + "x": 3.0, + "y": 6.0 + }, + "nextControl": { + "x": 5.0, + "y": 6.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.923673991492127, + "y": 7.377273132924644 + }, + "prevControl": { + "x": 4.961836995746063, + "y": 6.688636566462322 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 59.931417178137586, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue Amp Side w shoot.path b/src/main/deploy/pathplanner/paths/Blue Amp Side w shoot.path new file mode 100644 index 0000000..3e1fe36 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Blue Amp Side w shoot.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.5417903407235394, + "y": 4.4271562828839945 + }, + "prevControl": null, + "nextControl": { + "x": 1.5417903407235425, + "y": 4.4271562828839945 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.8609165789760156, + "y": 0.6450161471751537 + }, + "prevControl": { + "x": 1.8609165789760156, + "y": 0.6450161471751537 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": -59.5344550805402, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue Center w shoot.path b/src/main/deploy/pathplanner/paths/Blue Center w shoot.path new file mode 100644 index 0000000..a368a14 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Blue Center w shoot.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.2337167531477924, + "y": 5.55593454044051 + }, + "prevControl": null, + "nextControl": { + "x": 2.233716753147796, + "y": 5.55593454044051 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.0, + "y": 6.0 + }, + "prevControl": { + "x": 3.0, + "y": 6.0 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file From 0b487756bbe20f1083f74ea4c44c053dc6eb55fa Mon Sep 17 00:00:00 2001 From: ShaneHopkins11 <97139604+ShaneHopkins11@users.noreply.github.com> Date: Wed, 20 Mar 2024 14:58:14 -0500 Subject: [PATCH 08/36] fixed autos --- .../deploy/pathplanner/autos/BL Path.auto | 25 ----- .../deploy/pathplanner/autos/BLS Auto.auto | 43 ++++++++ .../autos/{BS Auto.auto => BMS Auto.auto} | 4 +- .../deploy/pathplanner/autos/BRS Auto.auto | 43 ++++++++ .../deploy/pathplanner/autos/RLS Auto.auto | 43 ++++++++ .../autos/{RS Auto.auto => RMS Auto.auto} | 0 .../deploy/pathplanner/autos/RR Path.auto | 25 ----- .../deploy/pathplanner/autos/RRS Auto.auto | 43 ++++++++ .../deploy/pathplanner/autos/bl Auto.auto | 4 +- .../deploy/pathplanner/paths/BLS Path.path | 76 +++++++++++++ .../deploy/pathplanner/paths/BRS Path.path | 76 +++++++++++++ .../deploy/pathplanner/paths/RLS Path.path | 102 ++++++++++++++++++ .../deploy/pathplanner/paths/RRS Path.path | 102 ++++++++++++++++++ .../deploy/pathplanner/paths/bl Path.path | 49 +++++++++ .../deploy/pathplanner/paths/bl2 Path.path | 4 +- .../swerve/modules/physicalproperties.json | 6 +- src/main/java/frc/robot/RobotContainer.java | 8 +- .../robot/commands/autoCommands/dumpBed.java | 32 ++++++ .../commands/autoCommands/shootConveyor.java | 32 ++++++ .../commands/autoCommands/shootFlyWheel.java | 32 ++++++ .../java/frc/robot/subsystems/Conveyor.java | 7 ++ .../java/frc/robot/subsystems/FlyWheel.java | 12 ++- .../swervedrive/SwerveSubsystem.java | 2 +- 23 files changed, 706 insertions(+), 64 deletions(-) delete mode 100644 src/main/deploy/pathplanner/autos/BL Path.auto create mode 100644 src/main/deploy/pathplanner/autos/BLS Auto.auto rename src/main/deploy/pathplanner/autos/{BS Auto.auto => BMS Auto.auto} (91%) create mode 100644 src/main/deploy/pathplanner/autos/BRS Auto.auto create mode 100644 src/main/deploy/pathplanner/autos/RLS Auto.auto rename src/main/deploy/pathplanner/autos/{RS Auto.auto => RMS Auto.auto} (100%) delete mode 100644 src/main/deploy/pathplanner/autos/RR Path.auto create mode 100644 src/main/deploy/pathplanner/autos/RRS Auto.auto create mode 100644 src/main/deploy/pathplanner/paths/BLS Path.path create mode 100644 src/main/deploy/pathplanner/paths/BRS Path.path create mode 100644 src/main/deploy/pathplanner/paths/RLS Path.path create mode 100644 src/main/deploy/pathplanner/paths/RRS Path.path create mode 100644 src/main/deploy/pathplanner/paths/bl Path.path create mode 100644 src/main/java/frc/robot/commands/autoCommands/dumpBed.java create mode 100644 src/main/java/frc/robot/commands/autoCommands/shootConveyor.java create mode 100644 src/main/java/frc/robot/commands/autoCommands/shootFlyWheel.java diff --git a/src/main/deploy/pathplanner/autos/BL Path.auto b/src/main/deploy/pathplanner/autos/BL Path.auto deleted file mode 100644 index 00d23e6..0000000 --- a/src/main/deploy/pathplanner/autos/BL Path.auto +++ /dev/null @@ -1,25 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 0.71583277069688, - "y": 7.110045901003483 - }, - "rotation": 0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "BL Path" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/BLS Auto.auto b/src/main/deploy/pathplanner/autos/BLS Auto.auto new file mode 100644 index 0000000..c8d1190 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/BLS Auto.auto @@ -0,0 +1,43 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.7275269251393154, + "y": 4.268366371490578 + }, + "rotation": 58.172553423326896 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "shootConveyor" + } + }, + { + "type": "named", + "data": { + "name": "shootFlyWheel" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + }, + { + "type": "path", + "data": { + "pathName": "BLS Path" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/BS Auto.auto b/src/main/deploy/pathplanner/autos/BMS Auto.auto similarity index 91% rename from src/main/deploy/pathplanner/autos/BS Auto.auto rename to src/main/deploy/pathplanner/autos/BMS Auto.auto index 4dd5075..c392660 100644 --- a/src/main/deploy/pathplanner/autos/BS Auto.auto +++ b/src/main/deploy/pathplanner/autos/BMS Auto.auto @@ -2,8 +2,8 @@ "version": 1.0, "startingPose": { "position": { - "x": 16.29244648802688, - "y": 5.557362183607978 + "x": 5.849566570928014, + "y": 4.315142989260338 }, "rotation": 0 }, diff --git a/src/main/deploy/pathplanner/autos/BRS Auto.auto b/src/main/deploy/pathplanner/autos/BRS Auto.auto new file mode 100644 index 0000000..0d0019e --- /dev/null +++ b/src/main/deploy/pathplanner/autos/BRS Auto.auto @@ -0,0 +1,43 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.79769185179396, + "y": 4.572414386994017 + }, + "rotation": -62.59242456218156 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "shootConveyor" + } + }, + { + "type": "named", + "data": { + "name": "shootFlyWheel" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + }, + { + "type": "path", + "data": { + "pathName": "BRS Path" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/RLS Auto.auto b/src/main/deploy/pathplanner/autos/RLS Auto.auto new file mode 100644 index 0000000..2062588 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/RLS Auto.auto @@ -0,0 +1,43 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 15.965010163634727, + "y": 6.6773621866284305 + }, + "rotation": -57.5288077091511 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "shootConveyor" + } + }, + { + "type": "named", + "data": { + "name": "shootFlyWheel" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + }, + { + "type": "path", + "data": { + "pathName": "RLS Path" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/RS Auto.auto b/src/main/deploy/pathplanner/autos/RMS Auto.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/RS Auto.auto rename to src/main/deploy/pathplanner/autos/RMS Auto.auto diff --git a/src/main/deploy/pathplanner/autos/RR Path.auto b/src/main/deploy/pathplanner/autos/RR Path.auto deleted file mode 100644 index 17d59b9..0000000 --- a/src/main/deploy/pathplanner/autos/RR Path.auto +++ /dev/null @@ -1,25 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 16.01178678140832, - "y": 7.028186819906402 - }, - "rotation": 180.0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "RR Path" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/RRS Auto.auto b/src/main/deploy/pathplanner/autos/RRS Auto.auto new file mode 100644 index 0000000..ae2fb20 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/RRS Auto.auto @@ -0,0 +1,43 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 15.976704318081, + "y": 4.408696224799858 + }, + "rotation": -120.5792268724892 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "shootConveyor" + } + }, + { + "type": "named", + "data": { + "name": "shootFlyWheel" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + }, + { + "type": "path", + "data": { + "pathName": "RRS Path" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/bl Auto.auto b/src/main/deploy/pathplanner/autos/bl Auto.auto index e161d3a..c7eca3e 100644 --- a/src/main/deploy/pathplanner/autos/bl Auto.auto +++ b/src/main/deploy/pathplanner/autos/bl Auto.auto @@ -2,8 +2,8 @@ "version": 1.0, "startingPose": { "position": { - "x": 0.4217011759439209, - "y": 7.400066486593781 + "x": 0.38839644630856207, + "y": 7.3322348354098414 }, "rotation": 0 }, diff --git a/src/main/deploy/pathplanner/paths/BLS Path.path b/src/main/deploy/pathplanner/paths/BLS Path.path new file mode 100644 index 0000000..efa2a7e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BLS Path.path @@ -0,0 +1,76 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.7275269251393154, + "y": 6.712444649961881 + }, + "prevControl": null, + "nextControl": { + "x": 0.7860254744404522, + "y": 6.731944166395593 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.4691504014837022, + "y": 6.27884429165533 + }, + "prevControl": { + "x": 2.4886499179174146, + "y": 6.298343808089042 + }, + "nextControl": { + "x": 2.44965088504999, + "y": 6.259344775221619 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.019361957963822, + "y": 6.27884429165533 + }, + "prevControl": { + "x": 3.2150069050731935, + "y": 6.244720137896334 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "shoot", + "waypointRelativePos": 0, + "command": { + "type": "parallel", + "data": { + "commands": [] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BRS Path.path b/src/main/deploy/pathplanner/paths/BRS Path.path new file mode 100644 index 0000000..2f5e90b --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BRS Path.path @@ -0,0 +1,76 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.79769185179396, + "y": 4.303448834817898 + }, + "prevControl": null, + "nextControl": { + "x": 0.8561904010950968, + "y": 4.3229483512516085 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.4231793192931113, + "y": 3.017091846149505 + }, + "prevControl": { + "x": 2.4426788357268236, + "y": 3.036591362583216 + }, + "nextControl": { + "x": 2.403679802859399, + "y": 2.9975923297157934 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.217782640693486, + "y": 0.7952025020859156 + }, + "prevControl": { + "x": 6.413427587802858, + "y": 0.7610783483269196 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "shoot", + "waypointRelativePos": 0, + "command": { + "type": "parallel", + "data": { + "commands": [] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/RLS Path.path b/src/main/deploy/pathplanner/paths/RLS Path.path new file mode 100644 index 0000000..0efd85c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/RLS Path.path @@ -0,0 +1,102 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 15.82468031032928, + "y": 6.770915422172725 + }, + "prevControl": null, + "nextControl": { + "x": 15.836374464771719, + "y": 6.782609576615165 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 14.24596946059989, + "y": 6.209596008935608 + }, + "prevControl": { + "x": 14.23427530615745, + "y": 6.221290163378046 + }, + "nextControl": { + "x": 14.257663615042329, + "y": 6.19790185449317 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 11.474454857741623, + "y": 6.385008325572207 + }, + "prevControl": { + "x": 11.474454857741623, + "y": 6.396702480014649 + }, + "nextControl": { + "x": 11.474454857741623, + "y": 6.373314171129766 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.936823343732158, + "y": 7.460870534276682 + }, + "prevControl": { + "x": 10.12962709686103, + "y": 6.882009889375905 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "shootConveyor", + "waypointRelativePos": 0, + "command": { + "type": "parallel", + "data": { + "commands": [] + } + } + }, + { + "name": "shootFlyWheel", + "waypointRelativePos": 0, + "command": { + "type": "parallel", + "data": { + "commands": [] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/RRS Path.path b/src/main/deploy/pathplanner/paths/RRS Path.path new file mode 100644 index 0000000..8926bf5 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/RRS Path.path @@ -0,0 +1,102 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 15.836374464771723, + "y": 4.326837143702777 + }, + "prevControl": null, + "nextControl": { + "x": 15.848068619214162, + "y": 4.3385312981452175 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 13.87175651844181, + "y": 2.2569718073909093 + }, + "prevControl": { + "x": 13.86006236399937, + "y": 2.268665961833347 + }, + "nextControl": { + "x": 13.88345067288425, + "y": 2.2452776529484715 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 11.299042541105024, + "y": 1.6839582397113522 + }, + "prevControl": { + "x": 11.299042541105024, + "y": 1.6956523941537935 + }, + "nextControl": { + "x": 11.299042541105024, + "y": 1.6722640852689108 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 9.170706432580957, + "y": 0.8068966565283563 + }, + "prevControl": { + "x": 9.170706432580957, + "y": 0.8010495793071356 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "shootConveyor", + "waypointRelativePos": 0, + "command": { + "type": "parallel", + "data": { + "commands": [] + } + } + }, + { + "name": "shootFlyWheel", + "waypointRelativePos": 0, + "command": { + "type": "parallel", + "data": { + "commands": [] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/bl Path.path b/src/main/deploy/pathplanner/paths/bl Path.path new file mode 100644 index 0000000..f9e3f02 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/bl Path.path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.38839644630856207, + "y": 7.3322348354098414 + }, + "prevControl": null, + "nextControl": { + "x": 0.4351730640783217, + "y": 7.297152372082522 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.5461177361100726, + "y": 7.3322348354098414 + }, + "prevControl": { + "x": 0.9380217051031957, + "y": 5.8704655301091 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/bl2 Path.path b/src/main/deploy/pathplanner/paths/bl2 Path.path index ee7d98b..1739aed 100644 --- a/src/main/deploy/pathplanner/paths/bl2 Path.path +++ b/src/main/deploy/pathplanner/paths/bl2 Path.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 2.5081494343531894, - "y": 7.0783244654413116 + "x": 1.2771521839339972, + "y": 6.525338178881486 }, "isLocked": false, "linkedName": null diff --git a/src/main/deploy/swerve/modules/physicalproperties.json b/src/main/deploy/swerve/modules/physicalproperties.json index b2d3972..a705295 100644 --- a/src/main/deploy/swerve/modules/physicalproperties.json +++ b/src/main/deploy/swerve/modules/physicalproperties.json @@ -2,8 +2,8 @@ "optimalVoltage": 12, "wheelGripCoefficientOfFriction": 1.19, "currentLimit": { - "drive": 25, - "angle": 30 + "drive": 50, + "angle": 40 }, "conversionFactor": { "angle": 16.8, @@ -11,6 +11,6 @@ }, "rampRate": { "drive": 0.25, - "angle": 0 + "angle": 0.05 } } \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d2407b3..b2361da 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -5,6 +5,7 @@ package frc.robot; import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.commands.PathPlannerAuto; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; @@ -72,7 +73,10 @@ public RobotContainer() { conveyor.setDefaultCommand(new ConveyorCommand(conveyor)); FlyWheel.setDefaultCommand(new FlyWCommand(FlyWheel)); elevator.setDefaultCommand(new ElevatorControl(elevator)); - + // Register Named Commands + NamedCommands.registerCommand("shootConveyor", new InstantCommand(() -> conveyor.intakeConveyor())); + NamedCommands.registerCommand("shootFlyWheel", new InstantCommand(() -> FlyWheel.enableflywheelfull())); + NamedCommands.registerCommand("dump", new InstantCommand(() -> dump.open())); // Configure the trigger bindings configureBindings(); @@ -225,4 +229,4 @@ public Command doNothing() { nothing.setName("NOTHING"); return nothing; } -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/autoCommands/dumpBed.java b/src/main/java/frc/robot/commands/autoCommands/dumpBed.java new file mode 100644 index 0000000..efce099 --- /dev/null +++ b/src/main/java/frc/robot/commands/autoCommands/dumpBed.java @@ -0,0 +1,32 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands.autoCommands; + +import edu.wpi.first.wpilibj2.command.Command; + +public class dumpBed extends Command { + /** Creates a new dumpBed. */ + public dumpBed() { + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/autoCommands/shootConveyor.java b/src/main/java/frc/robot/commands/autoCommands/shootConveyor.java new file mode 100644 index 0000000..0765334 --- /dev/null +++ b/src/main/java/frc/robot/commands/autoCommands/shootConveyor.java @@ -0,0 +1,32 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands.autoCommands; + +import edu.wpi.first.wpilibj2.command.Command; + +public class shootConveyor extends Command { + /** Creates a new shootConveyor. */ + public shootConveyor() { + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/autoCommands/shootFlyWheel.java b/src/main/java/frc/robot/commands/autoCommands/shootFlyWheel.java new file mode 100644 index 0000000..2bc10ad --- /dev/null +++ b/src/main/java/frc/robot/commands/autoCommands/shootFlyWheel.java @@ -0,0 +1,32 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands.autoCommands; + +import edu.wpi.first.wpilibj2.command.Command; + +public class shootFlyWheel extends Command { + /** Creates a new shootFlyWheel. */ + public shootFlyWheel() { + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/subsystems/Conveyor.java b/src/main/java/frc/robot/subsystems/Conveyor.java index 4e81f2a..d64e693 100644 --- a/src/main/java/frc/robot/subsystems/Conveyor.java +++ b/src/main/java/frc/robot/subsystems/Conveyor.java @@ -6,6 +6,8 @@ import com.revrobotics.CANSparkLowLevel; import com.revrobotics.CANSparkMax; + +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.MechanismConstants; @@ -44,4 +46,9 @@ public void reverseConveyor() { public void periodic() { // This method will be called once per scheduler run } + +public Command shootCommand() { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'shootCommand'"); +} } diff --git a/src/main/java/frc/robot/subsystems/FlyWheel.java b/src/main/java/frc/robot/subsystems/FlyWheel.java index 60a1a76..0eed875 100644 --- a/src/main/java/frc/robot/subsystems/FlyWheel.java +++ b/src/main/java/frc/robot/subsystems/FlyWheel.java @@ -6,6 +6,8 @@ import com.revrobotics.CANSparkLowLevel; import com.revrobotics.CANSparkMax; + +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.MechanismConstants; @@ -22,8 +24,9 @@ public FlyWheel() { flywheelMotor2.setInverted(MechanismConstants.FLYWHEEL_MOTOR_2_INVERTED); } - /** Sets the flywheel motors to 100% power. */ - public void enableflywheelfull() { + /** Sets the flywheel motors to 100% power. + * @return */ + public Command enableflywheelfull() { flywheelMotor1.set(MechanismConstants.FLYWHEEL_MOTOR_FULL_SPEED); flywheelMotor2.set(MechanismConstants.FLYWHEEL_MOTOR_FULL_SPEED); } @@ -56,4 +59,9 @@ public void reverseflywheel() { public void periodic() { // This method will be called once per scheduler run } + +public Object shootCommand() { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'shootCommand'"); +} } diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index 2b7f711..ade9c63 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -109,7 +109,7 @@ public void setupPathPlanner() { swerveDrive.swerveController.config.headingPIDF.i, swerveDrive.swerveController.config.headingPIDF.d), // Rotation PID constants - 4.5, + 4.6, // Max module speed, in m/s swerveDrive.swerveDriveConfiguration.getDriveBaseRadiusMeters(), // Drive base radius in meters. Distance from robot center to furthest module. From 541a198152fbd02bf094c6f4168fa960001d23ac Mon Sep 17 00:00:00 2001 From: ShaneHopkins11 Date: Wed, 20 Mar 2024 20:00:17 +0000 Subject: [PATCH 09/36] [Spotless] Apply formatting --- src/main/java/frc/robot/RobotContainer.java | 10 ++++++---- src/main/java/frc/robot/subsystems/Conveyor.java | 5 ++--- src/main/java/frc/robot/subsystems/FlyWheel.java | 12 +++++++----- 3 files changed, 15 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b2361da..22a7f08 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -74,9 +74,11 @@ public RobotContainer() { FlyWheel.setDefaultCommand(new FlyWCommand(FlyWheel)); elevator.setDefaultCommand(new ElevatorControl(elevator)); // Register Named Commands - NamedCommands.registerCommand("shootConveyor", new InstantCommand(() -> conveyor.intakeConveyor())); - NamedCommands.registerCommand("shootFlyWheel", new InstantCommand(() -> FlyWheel.enableflywheelfull())); - NamedCommands.registerCommand("dump", new InstantCommand(() -> dump.open())); + NamedCommands.registerCommand( + "shootConveyor", new InstantCommand(() -> conveyor.intakeConveyor())); + NamedCommands.registerCommand( + "shootFlyWheel", new InstantCommand(() -> FlyWheel.enableflywheelfull())); + NamedCommands.registerCommand("dump", new InstantCommand(() -> dump.open())); // Configure the trigger bindings configureBindings(); @@ -229,4 +231,4 @@ public Command doNothing() { nothing.setName("NOTHING"); return nothing; } -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/subsystems/Conveyor.java b/src/main/java/frc/robot/subsystems/Conveyor.java index d64e693..30099db 100644 --- a/src/main/java/frc/robot/subsystems/Conveyor.java +++ b/src/main/java/frc/robot/subsystems/Conveyor.java @@ -6,7 +6,6 @@ import com.revrobotics.CANSparkLowLevel; import com.revrobotics.CANSparkMax; - import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.MechanismConstants; @@ -47,8 +46,8 @@ public void periodic() { // This method will be called once per scheduler run } -public Command shootCommand() { + public Command shootCommand() { // TODO Auto-generated method stub throw new UnsupportedOperationException("Unimplemented method 'shootCommand'"); -} + } } diff --git a/src/main/java/frc/robot/subsystems/FlyWheel.java b/src/main/java/frc/robot/subsystems/FlyWheel.java index 0eed875..f5f1093 100644 --- a/src/main/java/frc/robot/subsystems/FlyWheel.java +++ b/src/main/java/frc/robot/subsystems/FlyWheel.java @@ -6,7 +6,6 @@ import com.revrobotics.CANSparkLowLevel; import com.revrobotics.CANSparkMax; - import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.MechanismConstants; @@ -24,8 +23,11 @@ public FlyWheel() { flywheelMotor2.setInverted(MechanismConstants.FLYWHEEL_MOTOR_2_INVERTED); } - /** Sets the flywheel motors to 100% power. - * @return */ + /** + * Sets the flywheel motors to 100% power. + * + * @return + */ public Command enableflywheelfull() { flywheelMotor1.set(MechanismConstants.FLYWHEEL_MOTOR_FULL_SPEED); flywheelMotor2.set(MechanismConstants.FLYWHEEL_MOTOR_FULL_SPEED); @@ -60,8 +62,8 @@ public void periodic() { // This method will be called once per scheduler run } -public Object shootCommand() { + public Object shootCommand() { // TODO Auto-generated method stub throw new UnsupportedOperationException("Unimplemented method 'shootCommand'"); -} + } } From d7ac57bf4767a9ccc06a730c636f57e276ddd3b0 Mon Sep 17 00:00:00 2001 From: ShaneHopkins11 <97139604+ShaneHopkins11@users.noreply.github.com> Date: Thu, 21 Mar 2024 10:34:58 -0500 Subject: [PATCH 10/36] namedCommand --- .../deploy/pathplanner/autos/bl Auto.auto | 14 +------------ src/main/java/frc/robot/RobotContainer.java | 17 +++++++-------- .../robot/commands/autoCommands/dumpBed.java | 21 +++++++++++++++---- .../java/frc/robot/subsystems/Conveyor.java | 5 ++--- .../java/frc/robot/subsystems/FlyWheel.java | 11 ++++------ vendordeps/PathplannerLib.json | 6 +++--- vendordeps/REVLib.json | 10 ++++----- 7 files changed, 40 insertions(+), 44 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/bl Auto.auto b/src/main/deploy/pathplanner/autos/bl Auto.auto index c7eca3e..ffaf58c 100644 --- a/src/main/deploy/pathplanner/autos/bl Auto.auto +++ b/src/main/deploy/pathplanner/autos/bl Auto.auto @@ -20,19 +20,7 @@ { "type": "named", "data": { - "name": "shootConveyor" - } - }, - { - "type": "named", - "data": { - "name": "shootFlyWheel" - } - }, - { - "type": "named", - "data": { - "name": "dumpBed" + "name": "Dump bed " } }, { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b2361da..3a8d2c9 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -6,7 +6,6 @@ import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; -import com.pathplanner.lib.commands.PathPlannerAuto; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -27,6 +26,7 @@ import frc.robot.commands.ElevatorControl; import frc.robot.commands.FlyWCommand; import frc.robot.commands.IntakeCommand; +import frc.robot.commands.autoCommands.dumpBed; import frc.robot.commands.swervedrive.drivebase.AbsoluteDriveAdv; import frc.robot.subsystems.Conveyor; import frc.robot.subsystems.Dump; @@ -74,9 +74,12 @@ public RobotContainer() { FlyWheel.setDefaultCommand(new FlyWCommand(FlyWheel)); elevator.setDefaultCommand(new ElevatorControl(elevator)); // Register Named Commands - NamedCommands.registerCommand("shootConveyor", new InstantCommand(() -> conveyor.intakeConveyor())); - NamedCommands.registerCommand("shootFlyWheel", new InstantCommand(() -> FlyWheel.enableflywheelfull())); - NamedCommands.registerCommand("dump", new InstantCommand(() -> dump.open())); + NamedCommands.registerCommand( + "shootConveyor", new InstantCommand(() -> conveyor.intakeConveyor())); + NamedCommands.registerCommand( + "shootFlyWheel", new InstantCommand(() -> FlyWheel.enableflywheelfull())); + NamedCommands.registerCommand("dump", new InstantCommand(() -> dump.open())); + NamedCommands.registerCommand("Dump bed", new dumpBed(dump)); // Configure the trigger bindings configureBindings(); @@ -84,10 +87,6 @@ public RobotContainer() { new LoggedDashboardChooser<>("Autonomous Chooser", AutoBuilder.buildAutoChooser()); autoChooser.addDefaultOption("Do nothing", null); - autoChooser.addOption("BL", new PathPlannerAuto("BL Path")); - autoChooser.addOption("BR", new PathPlannerAuto("BR Path")); - autoChooser.addOption("RL", new PathPlannerAuto("RR Path")); - autoChooser.addOption("RR", new PathPlannerAuto("RR Path")); final AbsoluteDriveAdv closedAbsoluteDriveAdv = new AbsoluteDriveAdv( @@ -229,4 +228,4 @@ public Command doNothing() { nothing.setName("NOTHING"); return nothing; } -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/commands/autoCommands/dumpBed.java b/src/main/java/frc/robot/commands/autoCommands/dumpBed.java index efce099..1941788 100644 --- a/src/main/java/frc/robot/commands/autoCommands/dumpBed.java +++ b/src/main/java/frc/robot/commands/autoCommands/dumpBed.java @@ -5,20 +5,33 @@ package frc.robot.commands.autoCommands; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.Dump; public class dumpBed extends Command { /** Creates a new dumpBed. */ - public dumpBed() { + private final Dump m_dump; + + private boolean m_finished = false; + + public dumpBed(Dump dump) { + m_dump = dump; + addRequirements(m_dump); // Use addRequirements() here to declare subsystem dependencies. } // Called when the command is initially scheduled. @Override - public void initialize() {} + public void initialize() { + m_finished = false; + m_dump.openThenClose(); + m_dump.openThenClose(); + } // Called every time the scheduler runs while the command is scheduled. @Override - public void execute() {} + public void execute() { + m_finished = true; + } // Called once the command ends or is interrupted. @Override @@ -27,6 +40,6 @@ public void end(boolean interrupted) {} // Returns true when the command should end. @Override public boolean isFinished() { - return false; + return m_finished; } } diff --git a/src/main/java/frc/robot/subsystems/Conveyor.java b/src/main/java/frc/robot/subsystems/Conveyor.java index d64e693..30099db 100644 --- a/src/main/java/frc/robot/subsystems/Conveyor.java +++ b/src/main/java/frc/robot/subsystems/Conveyor.java @@ -6,7 +6,6 @@ import com.revrobotics.CANSparkLowLevel; import com.revrobotics.CANSparkMax; - import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.MechanismConstants; @@ -47,8 +46,8 @@ public void periodic() { // This method will be called once per scheduler run } -public Command shootCommand() { + public Command shootCommand() { // TODO Auto-generated method stub throw new UnsupportedOperationException("Unimplemented method 'shootCommand'"); -} + } } diff --git a/src/main/java/frc/robot/subsystems/FlyWheel.java b/src/main/java/frc/robot/subsystems/FlyWheel.java index 0eed875..8f9a90d 100644 --- a/src/main/java/frc/robot/subsystems/FlyWheel.java +++ b/src/main/java/frc/robot/subsystems/FlyWheel.java @@ -6,8 +6,6 @@ import com.revrobotics.CANSparkLowLevel; import com.revrobotics.CANSparkMax; - -import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.MechanismConstants; @@ -24,9 +22,8 @@ public FlyWheel() { flywheelMotor2.setInverted(MechanismConstants.FLYWHEEL_MOTOR_2_INVERTED); } - /** Sets the flywheel motors to 100% power. - * @return */ - public Command enableflywheelfull() { + /** Sets the flywheel motors to 100% power. */ + public void enableflywheelfull() { flywheelMotor1.set(MechanismConstants.FLYWHEEL_MOTOR_FULL_SPEED); flywheelMotor2.set(MechanismConstants.FLYWHEEL_MOTOR_FULL_SPEED); } @@ -60,8 +57,8 @@ public void periodic() { // This method will be called once per scheduler run } -public Object shootCommand() { + public Object shootCommand() { // TODO Auto-generated method stub throw new UnsupportedOperationException("Unimplemented method 'shootCommand'"); -} + } } diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json index 3ec4c12..a019706 100644 --- a/vendordeps/PathplannerLib.json +++ b/vendordeps/PathplannerLib.json @@ -1,7 +1,7 @@ { "fileName": "PathplannerLib.json", "name": "PathplannerLib", - "version": "2024.2.6", + "version": "2024.2.7", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2024", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2024.2.6" + "version": "2024.2.7" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2024.2.6", + "version": "2024.2.7", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json index 60eacf8..f85acd4 100644 --- a/vendordeps/REVLib.json +++ b/vendordeps/REVLib.json @@ -1,7 +1,7 @@ { "fileName": "REVLib.json", "name": "REVLib", - "version": "2024.2.3", + "version": "2024.2.4", "frcYear": "2024", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2024.2.3" + "version": "2024.2.4" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2024.2.3", + "version": "2024.2.4", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -37,7 +37,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2024.2.3", + "version": "2024.2.4", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -55,7 +55,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2024.2.3", + "version": "2024.2.4", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false, From 9772f347cf508642001154a005af3b28e89697b8 Mon Sep 17 00:00:00 2001 From: ShaneHopkins11 <97139604+ShaneHopkins11@users.noreply.github.com> Date: Thu, 21 Mar 2024 12:24:35 -0500 Subject: [PATCH 11/36] renamed --- src/main/deploy/pathplanner/autos/bl Auto.auto | 4 ++-- src/main/deploy/pathplanner/autos/rr Auto.auto | 4 ++-- src/main/java/frc/robot/subsystems/Dump.java | 4 ++-- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/bl Auto.auto b/src/main/deploy/pathplanner/autos/bl Auto.auto index ffaf58c..90c97c8 100644 --- a/src/main/deploy/pathplanner/autos/bl Auto.auto +++ b/src/main/deploy/pathplanner/autos/bl Auto.auto @@ -14,7 +14,7 @@ { "type": "path", "data": { - "pathName": "bl Path" + "pathName": "BL Path" } }, { @@ -32,7 +32,7 @@ { "type": "path", "data": { - "pathName": "bl2 Path" + "pathName": "BL2 Path" } } ] diff --git a/src/main/deploy/pathplanner/autos/rr Auto.auto b/src/main/deploy/pathplanner/autos/rr Auto.auto index 37da37c..d195bb2 100644 --- a/src/main/deploy/pathplanner/autos/rr Auto.auto +++ b/src/main/deploy/pathplanner/autos/rr Auto.auto @@ -14,7 +14,7 @@ { "type": "path", "data": { - "pathName": "rr Path" + "pathName": "RR Path" } }, { @@ -44,7 +44,7 @@ { "type": "path", "data": { - "pathName": "rr2 Path" + "pathName": "RR2 Path" } } ] diff --git a/src/main/java/frc/robot/subsystems/Dump.java b/src/main/java/frc/robot/subsystems/Dump.java index 5c9f02e..5c21b00 100644 --- a/src/main/java/frc/robot/subsystems/Dump.java +++ b/src/main/java/frc/robot/subsystems/Dump.java @@ -30,13 +30,13 @@ public Dump() { /** Opens the piston */ public void close() { - doubleSolenoid.set(DoubleSolenoid.Value.kForward); + doubleSolenoid.set(DoubleSolenoid.Value.kReverse); SmartDashboard.putBoolean(getName(), true); } /** Closes the piston */ public void open() { - doubleSolenoid.set(DoubleSolenoid.Value.kReverse); + doubleSolenoid.set(DoubleSolenoid.Value.kForward); SmartDashboard.putBoolean(getName(), false); } From 6609dbc33e89facbc91fb45f27e513e38b01048e Mon Sep 17 00:00:00 2001 From: Kendialouge Date: Thu, 21 Mar 2024 12:34:35 -0500 Subject: [PATCH 12/36] Rename paths in auto Paths in autos not correct, this is now fixed. Co-Authored-By: ShaneHopkins11 <97139604+ShaneHopkins11@users.noreply.github.com> --- src/main/deploy/pathplanner/autos/bl Auto.auto | 4 ++-- src/main/deploy/pathplanner/autos/rr Auto.auto | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/bl Auto.auto b/src/main/deploy/pathplanner/autos/bl Auto.auto index 90c97c8..ffaf58c 100644 --- a/src/main/deploy/pathplanner/autos/bl Auto.auto +++ b/src/main/deploy/pathplanner/autos/bl Auto.auto @@ -14,7 +14,7 @@ { "type": "path", "data": { - "pathName": "BL Path" + "pathName": "bl Path" } }, { @@ -32,7 +32,7 @@ { "type": "path", "data": { - "pathName": "BL2 Path" + "pathName": "bl2 Path" } } ] diff --git a/src/main/deploy/pathplanner/autos/rr Auto.auto b/src/main/deploy/pathplanner/autos/rr Auto.auto index d195bb2..824f616 100644 --- a/src/main/deploy/pathplanner/autos/rr Auto.auto +++ b/src/main/deploy/pathplanner/autos/rr Auto.auto @@ -44,7 +44,7 @@ { "type": "path", "data": { - "pathName": "RR2 Path" + "pathName": "rr2 Path" } } ] From 72b7a38ca6953f9b817b316b5292294e48e76b38 Mon Sep 17 00:00:00 2001 From: Kendialouge Date: Thu, 21 Mar 2024 13:19:07 -0500 Subject: [PATCH 13/36] KREYLINGAUTOS --- .../pathplanner/autos/AmpShootB1 Auto.auto | 25 +++++++++++++++++++ .../pathplanner/autos/AmpShootB2 Auto.auto | 25 +++++++++++++++++++ .../pathplanner/autos/AmpShootB3 Auto.auto | 25 +++++++++++++++++++ .../deploy/pathplanner/autos/bl Auto.auto | 2 +- 4 files changed, 76 insertions(+), 1 deletion(-) create mode 100644 src/main/deploy/pathplanner/autos/AmpShootB1 Auto.auto create mode 100644 src/main/deploy/pathplanner/autos/AmpShootB2 Auto.auto create mode 100644 src/main/deploy/pathplanner/autos/AmpShootB3 Auto.auto diff --git a/src/main/deploy/pathplanner/autos/AmpShootB1 Auto.auto b/src/main/deploy/pathplanner/autos/AmpShootB1 Auto.auto new file mode 100644 index 0000000..6f34ef5 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/AmpShootB1 Auto.auto @@ -0,0 +1,25 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.5416846869729774, + "y": 6.6561862853455445 + }, + "rotation": 57.38075692880712 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Blue (amp side w shoot)" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/AmpShootB2 Auto.auto b/src/main/deploy/pathplanner/autos/AmpShootB2 Auto.auto new file mode 100644 index 0000000..0aec05b --- /dev/null +++ b/src/main/deploy/pathplanner/autos/AmpShootB2 Auto.auto @@ -0,0 +1,25 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.2337167531477924, + "y": 5.55593454044051 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Blue Center w shoot" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/AmpShootB3 Auto.auto b/src/main/deploy/pathplanner/autos/AmpShootB3 Auto.auto new file mode 100644 index 0000000..9843067 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/AmpShootB3 Auto.auto @@ -0,0 +1,25 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.5668146547560705, + "y": 4.422613915650367 + }, + "rotation": -59.53445508054021 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Blue Amp Side w shoot" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/bl Auto.auto b/src/main/deploy/pathplanner/autos/bl Auto.auto index 2f5fae3..ffaf58c 100644 --- a/src/main/deploy/pathplanner/autos/bl Auto.auto +++ b/src/main/deploy/pathplanner/autos/bl Auto.auto @@ -14,7 +14,7 @@ { "type": "path", "data": { - "pathName": null + "pathName": "bl Path" } }, { From 07405ab12972634d70ce68c3596c04d0df85761b Mon Sep 17 00:00:00 2001 From: Kendialouge Date: Thu, 21 Mar 2024 13:43:15 -0500 Subject: [PATCH 14/36] fixes --- src/main/deploy/pathplanner/paths/RRS Path.path | 16 ---------------- src/main/java/frc/robot/Constants.java | 2 +- .../java/frc/robot/commands/ConveyorCommand.java | 4 +--- .../java/frc/robot/commands/FlyWCommand.java | 2 ++ src/main/java/frc/robot/subsystems/Dump.java | 4 ++-- src/main/java/frc/robot/subsystems/FlyWheel.java | 5 +++++ 6 files changed, 11 insertions(+), 22 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/RRS Path.path b/src/main/deploy/pathplanner/paths/RRS Path.path index 8926bf5..1328608 100644 --- a/src/main/deploy/pathplanner/paths/RRS Path.path +++ b/src/main/deploy/pathplanner/paths/RRS Path.path @@ -30,22 +30,6 @@ "isLocked": false, "linkedName": null }, - { - "anchor": { - "x": 11.299042541105024, - "y": 1.6839582397113522 - }, - "prevControl": { - "x": 11.299042541105024, - "y": 1.6956523941537935 - }, - "nextControl": { - "x": 11.299042541105024, - "y": 1.6722640852689108 - }, - "isLocked": false, - "linkedName": null - }, { "anchor": { "x": 9.170706432580957, diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 6dca6d3..5cba83b 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -77,7 +77,6 @@ public static class MechanismConstants { public static final boolean INTAKE_MOTOR_FRONT_INVERTED = false; public static final double INTAKE_MOTOR_SPEED_FRONT = 0.1; public static final double INTAKE_MOTOR_SPEED_SUSHI = 1; - // Conveyor Motors public static final int CONVEYOR_MOTOR_1 = 1; public static final int CONVEYOR_MOTOR_2 = 2; @@ -93,6 +92,7 @@ public static class MechanismConstants { public static final double FLYWHEEL_MOTOR_FULL_SPEED = 0.51; public static final double FLYWHEEL_MOTOR_REDUCED_SPEED = 0.20; public static final double FLYWHEEL_MOTOR_SPEED_SLOW = 0.15; + public static final double FLYWHEEL_MOTOR_SPEED_100 = 1; } public class VisionConstants { diff --git a/src/main/java/frc/robot/commands/ConveyorCommand.java b/src/main/java/frc/robot/commands/ConveyorCommand.java index 7c60288..1697fe9 100644 --- a/src/main/java/frc/robot/commands/ConveyorCommand.java +++ b/src/main/java/frc/robot/commands/ConveyorCommand.java @@ -28,9 +28,7 @@ public void execute() { || ConveyorJoystick.getRawButton(3) || ConveyorJoystick.getRawButton(10)) { m_ConveyorSubsystem.intakeConveyor(); - } else if (ConveyorJoystick.getRawButton(6) - || ConveyorJoystick.getRawButton(4) - || ConveyorJoystick.getRawButton(9)) { + } else if (ConveyorJoystick.getRawButton(6) || ConveyorJoystick.getRawButton(4)) { m_ConveyorSubsystem.reverseConveyor(); System.out.println("Conveyor Moving in Reverse"); } else { diff --git a/src/main/java/frc/robot/commands/FlyWCommand.java b/src/main/java/frc/robot/commands/FlyWCommand.java index 8ea2c73..6aa9750 100644 --- a/src/main/java/frc/robot/commands/FlyWCommand.java +++ b/src/main/java/frc/robot/commands/FlyWCommand.java @@ -36,6 +36,8 @@ public void execute() { } else if (FlyWJoystick.getRawButton(11) || FlyWJoystick.getRawButton(7)) { m_FlyWSubsystem.reverseflywheel(); System.out.println("Flywheels Moving in Reverse"); + } else if (FlyWJoystick.getRawButton(9)) { + m_FlyWSubsystem.fastflywheel(); } else { m_FlyWSubsystem.disableflywheel(); } diff --git a/src/main/java/frc/robot/subsystems/Dump.java b/src/main/java/frc/robot/subsystems/Dump.java index 5c21b00..5c9f02e 100644 --- a/src/main/java/frc/robot/subsystems/Dump.java +++ b/src/main/java/frc/robot/subsystems/Dump.java @@ -30,13 +30,13 @@ public Dump() { /** Opens the piston */ public void close() { - doubleSolenoid.set(DoubleSolenoid.Value.kReverse); + doubleSolenoid.set(DoubleSolenoid.Value.kForward); SmartDashboard.putBoolean(getName(), true); } /** Closes the piston */ public void open() { - doubleSolenoid.set(DoubleSolenoid.Value.kForward); + doubleSolenoid.set(DoubleSolenoid.Value.kReverse); SmartDashboard.putBoolean(getName(), false); } diff --git a/src/main/java/frc/robot/subsystems/FlyWheel.java b/src/main/java/frc/robot/subsystems/FlyWheel.java index 8f9a90d..296b7b9 100644 --- a/src/main/java/frc/robot/subsystems/FlyWheel.java +++ b/src/main/java/frc/robot/subsystems/FlyWheel.java @@ -61,4 +61,9 @@ public Object shootCommand() { // TODO Auto-generated method stub throw new UnsupportedOperationException("Unimplemented method 'shootCommand'"); } + + public void fastflywheel() { + flywheelMotor1.set(MechanismConstants.FLYWHEEL_MOTOR_SPEED_100); + flywheelMotor2.set(MechanismConstants.FLYWHEEL_MOTOR_SPEED_100); + } } From c37926cd78ea5e461a71427fd0c1b87da6ddf250 Mon Sep 17 00:00:00 2001 From: ShaneHopkins11 <97139604+ShaneHopkins11@users.noreply.github.com> Date: Thu, 21 Mar 2024 16:16:17 -0500 Subject: [PATCH 15/36] auto --- .../{BLS Auto.auto => LeftShoot Auto.auto} | 8 +-- .../{BMS Auto.auto => MiddleShoot Auto.auto} | 18 +++--- .../deploy/pathplanner/autos/RL Path.auto | 25 --------- .../deploy/pathplanner/autos/RLS Auto.auto | 43 --------------- .../deploy/pathplanner/autos/RMS Auto.auto | 43 --------------- .../deploy/pathplanner/autos/RRS Auto.auto | 43 --------------- .../{BRS Auto.auto => RightShoot Auto.auto} | 4 +- .../autos/{BR Path.auto => Taxi Path.auto} | 0 .../deploy/pathplanner/autos/bl Auto.auto | 43 --------------- .../deploy/pathplanner/autos/rr Auto.auto | 55 ------------------- .../SHOOTCONVEYOR.java} | 8 +-- .../SHOOTFLYS.java} | 8 +-- 12 files changed, 23 insertions(+), 275 deletions(-) rename src/main/deploy/pathplanner/autos/{BLS Auto.auto => LeftShoot Auto.auto} (82%) rename src/main/deploy/pathplanner/autos/{BMS Auto.auto => MiddleShoot Auto.auto} (82%) delete mode 100644 src/main/deploy/pathplanner/autos/RL Path.auto delete mode 100644 src/main/deploy/pathplanner/autos/RLS Auto.auto delete mode 100644 src/main/deploy/pathplanner/autos/RMS Auto.auto delete mode 100644 src/main/deploy/pathplanner/autos/RRS Auto.auto rename src/main/deploy/pathplanner/autos/{BRS Auto.auto => RightShoot Auto.auto} (90%) rename src/main/deploy/pathplanner/autos/{BR Path.auto => Taxi Path.auto} (100%) delete mode 100644 src/main/deploy/pathplanner/autos/bl Auto.auto delete mode 100644 src/main/deploy/pathplanner/autos/rr Auto.auto rename src/main/java/frc/robot/commands/{autoCommands/shootConveyor.java => auto/SHOOTCONVEYOR.java} (83%) rename src/main/java/frc/robot/commands/{autoCommands/shootFlyWheel.java => auto/SHOOTFLYS.java} (83%) diff --git a/src/main/deploy/pathplanner/autos/BLS Auto.auto b/src/main/deploy/pathplanner/autos/LeftShoot Auto.auto similarity index 82% rename from src/main/deploy/pathplanner/autos/BLS Auto.auto rename to src/main/deploy/pathplanner/autos/LeftShoot Auto.auto index c8d1190..8f7ebc2 100644 --- a/src/main/deploy/pathplanner/autos/BLS Auto.auto +++ b/src/main/deploy/pathplanner/autos/LeftShoot Auto.auto @@ -2,8 +2,8 @@ "version": 1.0, "startingPose": { "position": { - "x": 0.7275269251393154, - "y": 4.268366371490578 + "x": 0.8311910210518773, + "y": 6.50308873064302 }, "rotation": 58.172553423326896 }, @@ -14,13 +14,13 @@ { "type": "named", "data": { - "name": "shootConveyor" + "name": "SHOOTCONVEYOR" } }, { "type": "named", "data": { - "name": "shootFlyWheel" + "name": "SHOOTFLYS" } }, { diff --git a/src/main/deploy/pathplanner/autos/BMS Auto.auto b/src/main/deploy/pathplanner/autos/MiddleShoot Auto.auto similarity index 82% rename from src/main/deploy/pathplanner/autos/BMS Auto.auto rename to src/main/deploy/pathplanner/autos/MiddleShoot Auto.auto index c392660..27122dd 100644 --- a/src/main/deploy/pathplanner/autos/BMS Auto.auto +++ b/src/main/deploy/pathplanner/autos/MiddleShoot Auto.auto @@ -2,8 +2,8 @@ "version": 1.0, "startingPose": { "position": { - "x": 5.849566570928014, - "y": 4.315142989260338 + "x": 1.3674277229789629, + "y": 5.557362183607978 }, "rotation": 0 }, @@ -12,27 +12,27 @@ "data": { "commands": [ { - "type": "path", + "type": "named", "data": { - "pathName": "BS Path" + "name": "SHOOTCONVEYOR" } }, { "type": "named", "data": { - "name": "shootConveyor" + "name": "SHOOTFLYS" } }, { - "type": "named", + "type": "wait", "data": { - "name": "shootFlyWheel" + "waitTime": 2.0 } }, { - "type": "wait", + "type": "path", "data": { - "waitTime": 2.0 + "pathName": "BS Path" } } ] diff --git a/src/main/deploy/pathplanner/autos/RL Path.auto b/src/main/deploy/pathplanner/autos/RL Path.auto deleted file mode 100644 index 60d48cd..0000000 --- a/src/main/deploy/pathplanner/autos/RL Path.auto +++ /dev/null @@ -1,25 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 15.541771563143822, - "y": 2.430613727257548 - }, - "rotation": -179.1864566650106 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "RL Path" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/RLS Auto.auto b/src/main/deploy/pathplanner/autos/RLS Auto.auto deleted file mode 100644 index 2062588..0000000 --- a/src/main/deploy/pathplanner/autos/RLS Auto.auto +++ /dev/null @@ -1,43 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 15.965010163634727, - "y": 6.6773621866284305 - }, - "rotation": -57.5288077091511 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "shootConveyor" - } - }, - { - "type": "named", - "data": { - "name": "shootFlyWheel" - } - }, - { - "type": "wait", - "data": { - "waitTime": 2.0 - } - }, - { - "type": "path", - "data": { - "pathName": "RLS Path" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/RMS Auto.auto b/src/main/deploy/pathplanner/autos/RMS Auto.auto deleted file mode 100644 index 720e4f6..0000000 --- a/src/main/deploy/pathplanner/autos/RMS Auto.auto +++ /dev/null @@ -1,43 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 15.146419352667763, - "y": 5.578111669043852 - }, - "rotation": 0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "RS Path" - } - }, - { - "type": "named", - "data": { - "name": "shootConveyor" - } - }, - { - "type": "named", - "data": { - "name": "shootFlyWheel" - } - }, - { - "type": "wait", - "data": { - "waitTime": 2.0 - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/RRS Auto.auto b/src/main/deploy/pathplanner/autos/RRS Auto.auto deleted file mode 100644 index ae2fb20..0000000 --- a/src/main/deploy/pathplanner/autos/RRS Auto.auto +++ /dev/null @@ -1,43 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 15.976704318081, - "y": 4.408696224799858 - }, - "rotation": -120.5792268724892 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "shootConveyor" - } - }, - { - "type": "named", - "data": { - "name": "shootFlyWheel" - } - }, - { - "type": "wait", - "data": { - "waitTime": 2.0 - } - }, - { - "type": "path", - "data": { - "pathName": "RRS Path" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/BRS Auto.auto b/src/main/deploy/pathplanner/autos/RightShoot Auto.auto similarity index 90% rename from src/main/deploy/pathplanner/autos/BRS Auto.auto rename to src/main/deploy/pathplanner/autos/RightShoot Auto.auto index 0d0019e..942af83 100644 --- a/src/main/deploy/pathplanner/autos/BRS Auto.auto +++ b/src/main/deploy/pathplanner/autos/RightShoot Auto.auto @@ -14,13 +14,13 @@ { "type": "named", "data": { - "name": "shootConveyor" + "name": "SHOOTCONVEYOR" } }, { "type": "named", "data": { - "name": "shootFlyWheel" + "name": "SHOOTFLYS" } }, { diff --git a/src/main/deploy/pathplanner/autos/BR Path.auto b/src/main/deploy/pathplanner/autos/Taxi Path.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/BR Path.auto rename to src/main/deploy/pathplanner/autos/Taxi Path.auto diff --git a/src/main/deploy/pathplanner/autos/bl Auto.auto b/src/main/deploy/pathplanner/autos/bl Auto.auto deleted file mode 100644 index 90c97c8..0000000 --- a/src/main/deploy/pathplanner/autos/bl Auto.auto +++ /dev/null @@ -1,43 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 0.38839644630856207, - "y": 7.3322348354098414 - }, - "rotation": 0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "BL Path" - } - }, - { - "type": "named", - "data": { - "name": "Dump bed " - } - }, - { - "type": "wait", - "data": { - "waitTime": 2.0 - } - }, - { - "type": "path", - "data": { - "pathName": "BL2 Path" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/rr Auto.auto b/src/main/deploy/pathplanner/autos/rr Auto.auto deleted file mode 100644 index d195bb2..0000000 --- a/src/main/deploy/pathplanner/autos/rr Auto.auto +++ /dev/null @@ -1,55 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 16.1521166347176, - "y": 7.390705607622041 - }, - "rotation": 0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "RR Path" - } - }, - { - "type": "named", - "data": { - "name": "shootConveyor" - } - }, - { - "type": "named", - "data": { - "name": "shootFlyWheel" - } - }, - { - "type": "named", - "data": { - "name": "dumpBed" - } - }, - { - "type": "wait", - "data": { - "waitTime": 2.0 - } - }, - { - "type": "path", - "data": { - "pathName": "RR2 Path" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/autoCommands/shootConveyor.java b/src/main/java/frc/robot/commands/auto/SHOOTCONVEYOR.java similarity index 83% rename from src/main/java/frc/robot/commands/autoCommands/shootConveyor.java rename to src/main/java/frc/robot/commands/auto/SHOOTCONVEYOR.java index 0765334..48d7c9e 100644 --- a/src/main/java/frc/robot/commands/autoCommands/shootConveyor.java +++ b/src/main/java/frc/robot/commands/auto/SHOOTCONVEYOR.java @@ -2,13 +2,13 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -package frc.robot.commands.autoCommands; +package frc.robot.commands.auto; import edu.wpi.first.wpilibj2.command.Command; -public class shootConveyor extends Command { - /** Creates a new shootConveyor. */ - public shootConveyor() { +public class SHOOT extends Command { + /** Creates a new SHOOT. */ + public SHOOT() { // Use addRequirements() here to declare subsystem dependencies. } diff --git a/src/main/java/frc/robot/commands/autoCommands/shootFlyWheel.java b/src/main/java/frc/robot/commands/auto/SHOOTFLYS.java similarity index 83% rename from src/main/java/frc/robot/commands/autoCommands/shootFlyWheel.java rename to src/main/java/frc/robot/commands/auto/SHOOTFLYS.java index 2bc10ad..d106879 100644 --- a/src/main/java/frc/robot/commands/autoCommands/shootFlyWheel.java +++ b/src/main/java/frc/robot/commands/auto/SHOOTFLYS.java @@ -2,13 +2,13 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -package frc.robot.commands.autoCommands; +package frc.robot.commands.auto; import edu.wpi.first.wpilibj2.command.Command; -public class shootFlyWheel extends Command { - /** Creates a new shootFlyWheel. */ - public shootFlyWheel() { +public class SHOOTFLYS extends Command { + /** Creates a new SHOOTFLYS. */ + public SHOOTFLYS() { // Use addRequirements() here to declare subsystem dependencies. } From ce1da14f8159dac7946df2846e84c329b75f5218 Mon Sep 17 00:00:00 2001 From: Garrett Summerfield Date: Thu, 21 Mar 2024 16:32:31 -0500 Subject: [PATCH 16/36] Remove unneeded paths --- .../pathplanner/autos/AmpShootB1 Auto.auto | 25 ----- .../pathplanner/autos/AmpShootB2 Auto.auto | 25 ----- .../pathplanner/autos/AmpShootB3 Auto.auto | 25 ----- .../deploy/pathplanner/paths/RL Path.path | 49 --------- .../deploy/pathplanner/paths/RLS Path.path | 102 ------------------ .../deploy/pathplanner/paths/RR Path.path | 49 --------- .../deploy/pathplanner/paths/RS Path.path | 86 --------------- .../deploy/pathplanner/paths/rr2 Path.path | 65 ----------- 8 files changed, 426 deletions(-) delete mode 100644 src/main/deploy/pathplanner/autos/AmpShootB1 Auto.auto delete mode 100644 src/main/deploy/pathplanner/autos/AmpShootB2 Auto.auto delete mode 100644 src/main/deploy/pathplanner/autos/AmpShootB3 Auto.auto delete mode 100644 src/main/deploy/pathplanner/paths/RL Path.path delete mode 100644 src/main/deploy/pathplanner/paths/RLS Path.path delete mode 100644 src/main/deploy/pathplanner/paths/RR Path.path delete mode 100644 src/main/deploy/pathplanner/paths/RS Path.path delete mode 100644 src/main/deploy/pathplanner/paths/rr2 Path.path diff --git a/src/main/deploy/pathplanner/autos/AmpShootB1 Auto.auto b/src/main/deploy/pathplanner/autos/AmpShootB1 Auto.auto deleted file mode 100644 index 6f34ef5..0000000 --- a/src/main/deploy/pathplanner/autos/AmpShootB1 Auto.auto +++ /dev/null @@ -1,25 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 0.5416846869729774, - "y": 6.6561862853455445 - }, - "rotation": 57.38075692880712 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Blue (amp side w shoot)" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/AmpShootB2 Auto.auto b/src/main/deploy/pathplanner/autos/AmpShootB2 Auto.auto deleted file mode 100644 index 0aec05b..0000000 --- a/src/main/deploy/pathplanner/autos/AmpShootB2 Auto.auto +++ /dev/null @@ -1,25 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 1.2337167531477924, - "y": 5.55593454044051 - }, - "rotation": 0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Blue Center w shoot" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/AmpShootB3 Auto.auto b/src/main/deploy/pathplanner/autos/AmpShootB3 Auto.auto deleted file mode 100644 index 9843067..0000000 --- a/src/main/deploy/pathplanner/autos/AmpShootB3 Auto.auto +++ /dev/null @@ -1,25 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 0.5668146547560705, - "y": 4.422613915650367 - }, - "rotation": -59.53445508054021 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Blue Amp Side w shoot" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/RL Path.path b/src/main/deploy/pathplanner/paths/RL Path.path deleted file mode 100644 index 7d07639..0000000 --- a/src/main/deploy/pathplanner/paths/RL Path.path +++ /dev/null @@ -1,49 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 15.541771563143822, - "y": 2.430613727257548 - }, - "prevControl": null, - "nextControl": { - "x": 15.5446962437617, - "y": 2.4068975552230634 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 13.700871867301933, - "y": 1.1992202606733011 - }, - "prevControl": { - "x": 13.716393538454641, - "y": 1.1975158911190236 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": -92.40840760476861, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": null, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/RLS Path.path b/src/main/deploy/pathplanner/paths/RLS Path.path deleted file mode 100644 index 0efd85c..0000000 --- a/src/main/deploy/pathplanner/paths/RLS Path.path +++ /dev/null @@ -1,102 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 15.82468031032928, - "y": 6.770915422172725 - }, - "prevControl": null, - "nextControl": { - "x": 15.836374464771719, - "y": 6.782609576615165 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 14.24596946059989, - "y": 6.209596008935608 - }, - "prevControl": { - "x": 14.23427530615745, - "y": 6.221290163378046 - }, - "nextControl": { - "x": 14.257663615042329, - "y": 6.19790185449317 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 11.474454857741623, - "y": 6.385008325572207 - }, - "prevControl": { - "x": 11.474454857741623, - "y": 6.396702480014649 - }, - "nextControl": { - "x": 11.474454857741623, - "y": 6.373314171129766 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 8.936823343732158, - "y": 7.460870534276682 - }, - "prevControl": { - "x": 10.12962709686103, - "y": 6.882009889375905 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [ - { - "name": "shootConveyor", - "waypointRelativePos": 0, - "command": { - "type": "parallel", - "data": { - "commands": [] - } - } - }, - { - "name": "shootFlyWheel", - "waypointRelativePos": 0, - "command": { - "type": "parallel", - "data": { - "commands": [] - } - } - } - ], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 0.0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": null, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/RR Path.path b/src/main/deploy/pathplanner/paths/RR Path.path deleted file mode 100644 index b566cec..0000000 --- a/src/main/deploy/pathplanner/paths/RR Path.path +++ /dev/null @@ -1,49 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 16.128728325832714, - "y": 7.414093916506922 - }, - "prevControl": null, - "nextControl": { - "x": 16.128728325832714, - "y": 7.414093916506922 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 15.052866117128234, - "y": 7.414093916506922 - }, - "prevControl": { - "x": 15.25166674264972, - "y": 5.496252587946771 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": -90.0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": null, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/RS Path.path b/src/main/deploy/pathplanner/paths/RS Path.path deleted file mode 100644 index 0d86dd1..0000000 --- a/src/main/deploy/pathplanner/paths/RS Path.path +++ /dev/null @@ -1,86 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 15.146419352667763, - "y": 5.578111669043852 - }, - "prevControl": null, - "nextControl": { - "x": 15.158113507110203, - "y": 5.589805823486293 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 14.24596946059989, - "y": 6.209596008935608 - }, - "prevControl": { - "x": 14.23427530615745, - "y": 6.221290163378046 - }, - "nextControl": { - "x": 14.257663615042329, - "y": 6.19790185449317 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 11.801891182129943, - "y": 6.326537553360007 - }, - "prevControl": { - "x": 11.801891182129943, - "y": 6.338231707802448 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [ - { - "name": "shootConveyor", - "waypointRelativePos": 0, - "command": { - "type": "parallel", - "data": { - "commands": [] - } - } - }, - { - "name": "shootFlyWheel", - "waypointRelativePos": 0, - "command": { - "type": "parallel", - "data": { - "commands": [] - } - } - } - ], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 0.0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": null, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/rr2 Path.path b/src/main/deploy/pathplanner/paths/rr2 Path.path deleted file mode 100644 index fcfdd2b..0000000 --- a/src/main/deploy/pathplanner/paths/rr2 Path.path +++ /dev/null @@ -1,65 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 15.052866117129224, - "y": 7.414093916500834 - }, - "prevControl": null, - "nextControl": { - "x": 15.041171962687761, - "y": 7.4374822253796244 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 14.175804533946229, - "y": 6.303149244469041 - }, - "prevControl": { - "x": 14.175804533947208, - "y": 6.291455090020513 - }, - "nextControl": { - "x": 14.17580453394525, - "y": 6.314843398917569 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 12.176104124289, - "y": 6.303149244469041 - }, - "prevControl": { - "x": 13.076554016356384, - "y": 6.273913858365985 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 0.0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": null, - "useDefaultConstraints": false -} \ No newline at end of file From ec501a7c9c2fb4fc98d5e5894e844741bcdbcc44 Mon Sep 17 00:00:00 2001 From: ShaneHopkins11 <97139604+ShaneHopkins11@users.noreply.github.com> Date: Thu, 21 Mar 2024 16:50:15 -0500 Subject: [PATCH 17/36] auto --- .../deploy/pathplanner/paths/RL Path.path | 49 --------- .../deploy/pathplanner/paths/RLS Path.path | 102 ------------------ .../deploy/pathplanner/paths/RR Path.path | 49 --------- .../deploy/pathplanner/paths/RRS Path.path | 86 --------------- .../deploy/pathplanner/paths/RS Path.path | 86 --------------- .../deploy/pathplanner/paths/bl Path.path | 49 --------- .../deploy/pathplanner/paths/bl2 Path.path | 71 ------------ .../deploy/pathplanner/paths/rr2 Path.path | 65 ----------- 8 files changed, 557 deletions(-) delete mode 100644 src/main/deploy/pathplanner/paths/RL Path.path delete mode 100644 src/main/deploy/pathplanner/paths/RLS Path.path delete mode 100644 src/main/deploy/pathplanner/paths/RR Path.path delete mode 100644 src/main/deploy/pathplanner/paths/RRS Path.path delete mode 100644 src/main/deploy/pathplanner/paths/RS Path.path delete mode 100644 src/main/deploy/pathplanner/paths/bl Path.path delete mode 100644 src/main/deploy/pathplanner/paths/bl2 Path.path delete mode 100644 src/main/deploy/pathplanner/paths/rr2 Path.path diff --git a/src/main/deploy/pathplanner/paths/RL Path.path b/src/main/deploy/pathplanner/paths/RL Path.path deleted file mode 100644 index 7d07639..0000000 --- a/src/main/deploy/pathplanner/paths/RL Path.path +++ /dev/null @@ -1,49 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 15.541771563143822, - "y": 2.430613727257548 - }, - "prevControl": null, - "nextControl": { - "x": 15.5446962437617, - "y": 2.4068975552230634 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 13.700871867301933, - "y": 1.1992202606733011 - }, - "prevControl": { - "x": 13.716393538454641, - "y": 1.1975158911190236 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": -92.40840760476861, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": null, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/RLS Path.path b/src/main/deploy/pathplanner/paths/RLS Path.path deleted file mode 100644 index 0efd85c..0000000 --- a/src/main/deploy/pathplanner/paths/RLS Path.path +++ /dev/null @@ -1,102 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 15.82468031032928, - "y": 6.770915422172725 - }, - "prevControl": null, - "nextControl": { - "x": 15.836374464771719, - "y": 6.782609576615165 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 14.24596946059989, - "y": 6.209596008935608 - }, - "prevControl": { - "x": 14.23427530615745, - "y": 6.221290163378046 - }, - "nextControl": { - "x": 14.257663615042329, - "y": 6.19790185449317 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 11.474454857741623, - "y": 6.385008325572207 - }, - "prevControl": { - "x": 11.474454857741623, - "y": 6.396702480014649 - }, - "nextControl": { - "x": 11.474454857741623, - "y": 6.373314171129766 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 8.936823343732158, - "y": 7.460870534276682 - }, - "prevControl": { - "x": 10.12962709686103, - "y": 6.882009889375905 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [ - { - "name": "shootConveyor", - "waypointRelativePos": 0, - "command": { - "type": "parallel", - "data": { - "commands": [] - } - } - }, - { - "name": "shootFlyWheel", - "waypointRelativePos": 0, - "command": { - "type": "parallel", - "data": { - "commands": [] - } - } - } - ], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 0.0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": null, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/RR Path.path b/src/main/deploy/pathplanner/paths/RR Path.path deleted file mode 100644 index b566cec..0000000 --- a/src/main/deploy/pathplanner/paths/RR Path.path +++ /dev/null @@ -1,49 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 16.128728325832714, - "y": 7.414093916506922 - }, - "prevControl": null, - "nextControl": { - "x": 16.128728325832714, - "y": 7.414093916506922 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 15.052866117128234, - "y": 7.414093916506922 - }, - "prevControl": { - "x": 15.25166674264972, - "y": 5.496252587946771 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": -90.0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": null, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/RRS Path.path b/src/main/deploy/pathplanner/paths/RRS Path.path deleted file mode 100644 index 1328608..0000000 --- a/src/main/deploy/pathplanner/paths/RRS Path.path +++ /dev/null @@ -1,86 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 15.836374464771723, - "y": 4.326837143702777 - }, - "prevControl": null, - "nextControl": { - "x": 15.848068619214162, - "y": 4.3385312981452175 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 13.87175651844181, - "y": 2.2569718073909093 - }, - "prevControl": { - "x": 13.86006236399937, - "y": 2.268665961833347 - }, - "nextControl": { - "x": 13.88345067288425, - "y": 2.2452776529484715 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 9.170706432580957, - "y": 0.8068966565283563 - }, - "prevControl": { - "x": 9.170706432580957, - "y": 0.8010495793071356 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [ - { - "name": "shootConveyor", - "waypointRelativePos": 0, - "command": { - "type": "parallel", - "data": { - "commands": [] - } - } - }, - { - "name": "shootFlyWheel", - "waypointRelativePos": 0, - "command": { - "type": "parallel", - "data": { - "commands": [] - } - } - } - ], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 0.0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": null, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/RS Path.path b/src/main/deploy/pathplanner/paths/RS Path.path deleted file mode 100644 index 0d86dd1..0000000 --- a/src/main/deploy/pathplanner/paths/RS Path.path +++ /dev/null @@ -1,86 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 15.146419352667763, - "y": 5.578111669043852 - }, - "prevControl": null, - "nextControl": { - "x": 15.158113507110203, - "y": 5.589805823486293 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 14.24596946059989, - "y": 6.209596008935608 - }, - "prevControl": { - "x": 14.23427530615745, - "y": 6.221290163378046 - }, - "nextControl": { - "x": 14.257663615042329, - "y": 6.19790185449317 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 11.801891182129943, - "y": 6.326537553360007 - }, - "prevControl": { - "x": 11.801891182129943, - "y": 6.338231707802448 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [ - { - "name": "shootConveyor", - "waypointRelativePos": 0, - "command": { - "type": "parallel", - "data": { - "commands": [] - } - } - }, - { - "name": "shootFlyWheel", - "waypointRelativePos": 0, - "command": { - "type": "parallel", - "data": { - "commands": [] - } - } - } - ], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 0.0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": null, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/bl Path.path b/src/main/deploy/pathplanner/paths/bl Path.path deleted file mode 100644 index f9e3f02..0000000 --- a/src/main/deploy/pathplanner/paths/bl Path.path +++ /dev/null @@ -1,49 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 0.38839644630856207, - "y": 7.3322348354098414 - }, - "prevControl": null, - "nextControl": { - "x": 0.4351730640783217, - "y": 7.297152372082522 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.5461177361100726, - "y": 7.3322348354098414 - }, - "prevControl": { - "x": 0.9380217051031957, - "y": 5.8704655301091 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": null, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/bl2 Path.path b/src/main/deploy/pathplanner/paths/bl2 Path.path deleted file mode 100644 index 1739aed..0000000 --- a/src/main/deploy/pathplanner/paths/bl2 Path.path +++ /dev/null @@ -1,71 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 1.52342385444866, - "y": 7.390316728376925 - }, - "prevControl": null, - "nextControl": { - "x": 1.2771521839339972, - "y": 6.525338178881486 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.4106518521846283, - "y": 6.337342840960248 - }, - "prevControl": { - "x": 2.4301513686183416, - "y": 6.3470925991771034 - }, - "nextControl": { - "x": 2.391152335750915, - "y": 6.327593082743392 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.5026247724725117, - "y": 6.337342840960248 - }, - "prevControl": { - "x": 2.4106518521846274, - "y": 6.3470925991771034 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.95, - "rotationDegrees": -90.60054724312275, - "rotateFast": false - } - ], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": null, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/rr2 Path.path b/src/main/deploy/pathplanner/paths/rr2 Path.path deleted file mode 100644 index fcfdd2b..0000000 --- a/src/main/deploy/pathplanner/paths/rr2 Path.path +++ /dev/null @@ -1,65 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 15.052866117129224, - "y": 7.414093916500834 - }, - "prevControl": null, - "nextControl": { - "x": 15.041171962687761, - "y": 7.4374822253796244 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 14.175804533946229, - "y": 6.303149244469041 - }, - "prevControl": { - "x": 14.175804533947208, - "y": 6.291455090020513 - }, - "nextControl": { - "x": 14.17580453394525, - "y": 6.314843398917569 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 12.176104124289, - "y": 6.303149244469041 - }, - "prevControl": { - "x": 13.076554016356384, - "y": 6.273913858365985 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 0.0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": null, - "useDefaultConstraints": false -} \ No newline at end of file From ea8568438c931739696bbc121ffb89965ba3f22e Mon Sep 17 00:00:00 2001 From: ShaneHopkins11 <97139604+ShaneHopkins11@users.noreply.github.com> Date: Thu, 21 Mar 2024 19:08:24 -0500 Subject: [PATCH 18/36] Merge branch 'new-autopaths' of https://github.com/OakvilleDynamics/2024-Robot into new-autopaths --- .../robot/commands/auto/SHOOTCONVEYOR.java | 22 ++++++++++++++----- .../frc/robot/commands/auto/SHOOTFLYS.java | 20 +++++++++++++---- 2 files changed, 33 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/commands/auto/SHOOTCONVEYOR.java b/src/main/java/frc/robot/commands/auto/SHOOTCONVEYOR.java index 48d7c9e..1e93ba1 100644 --- a/src/main/java/frc/robot/commands/auto/SHOOTCONVEYOR.java +++ b/src/main/java/frc/robot/commands/auto/SHOOTCONVEYOR.java @@ -5,20 +5,32 @@ package frc.robot.commands.auto; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.Conveyor; -public class SHOOT extends Command { +public class SHOOTCONVEYOR extends Command { /** Creates a new SHOOT. */ - public SHOOT() { + private final Conveyor m_conveyor; + + private boolean m_finished = false; + + public SHOOTCONVEYOR(Conveyor conveyor) { + m_conveyor = conveyor; + addRequirements(m_conveyor); // Use addRequirements() here to declare subsystem dependencies. } // Called when the command is initially scheduled. @Override - public void initialize() {} + public void initialize() { + m_finished = false; + m_conveyor.intakeConveyor(); + } // Called every time the scheduler runs while the command is scheduled. @Override - public void execute() {} + public void execute() { + m_finished = true; + } // Called once the command ends or is interrupted. @Override @@ -27,6 +39,6 @@ public void end(boolean interrupted) {} // Returns true when the command should end. @Override public boolean isFinished() { - return false; + return m_finished; } } diff --git a/src/main/java/frc/robot/commands/auto/SHOOTFLYS.java b/src/main/java/frc/robot/commands/auto/SHOOTFLYS.java index d106879..3e597bb 100644 --- a/src/main/java/frc/robot/commands/auto/SHOOTFLYS.java +++ b/src/main/java/frc/robot/commands/auto/SHOOTFLYS.java @@ -5,20 +5,32 @@ package frc.robot.commands.auto; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.FlyWheel; public class SHOOTFLYS extends Command { /** Creates a new SHOOTFLYS. */ - public SHOOTFLYS() { + private final FlyWheel m_flywheel; + + private boolean m_finished = false; + + public SHOOTFLYS(FlyWheel flywheel) { + m_flywheel = flywheel; + addRequirements(m_flywheel); // Use addRequirements() here to declare subsystem dependencies. } // Called when the command is initially scheduled. @Override - public void initialize() {} + public void initialize() { + m_finished = false; + m_flywheel.FLYWHEEL_MOTOR_FULL_SPEED(); + } // Called every time the scheduler runs while the command is scheduled. @Override - public void execute() {} + public void execute() { + m_finished = true; + } // Called once the command ends or is interrupted. @Override @@ -27,6 +39,6 @@ public void end(boolean interrupted) {} // Returns true when the command should end. @Override public boolean isFinished() { - return false; + return m_finished; } } From 127d40f20d91d9be64c1f26a0c8f607004cfb858 Mon Sep 17 00:00:00 2001 From: Kendialouge Date: Fri, 22 Mar 2024 10:18:10 -0500 Subject: [PATCH 19/36] edited auto commands --- src/main/java/frc/robot/commands/auto/SHOOTFLYS.java | 2 +- src/main/java/frc/robot/subsystems/Conveyor.java | 8 ++++---- src/main/java/frc/robot/subsystems/FlyWheel.java | 10 +++++----- 3 files changed, 10 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/commands/auto/SHOOTFLYS.java b/src/main/java/frc/robot/commands/auto/SHOOTFLYS.java index 3e597bb..c793b2a 100644 --- a/src/main/java/frc/robot/commands/auto/SHOOTFLYS.java +++ b/src/main/java/frc/robot/commands/auto/SHOOTFLYS.java @@ -23,7 +23,7 @@ public SHOOTFLYS(FlyWheel flywheel) { @Override public void initialize() { m_finished = false; - m_flywheel.FLYWHEEL_MOTOR_FULL_SPEED(); + m_flywheel.enableflywheelfull(); } // Called every time the scheduler runs while the command is scheduled. diff --git a/src/main/java/frc/robot/subsystems/Conveyor.java b/src/main/java/frc/robot/subsystems/Conveyor.java index 30099db..80eb77c 100644 --- a/src/main/java/frc/robot/subsystems/Conveyor.java +++ b/src/main/java/frc/robot/subsystems/Conveyor.java @@ -45,9 +45,9 @@ public void reverseConveyor() { public void periodic() { // This method will be called once per scheduler run } - - public Command shootCommand() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'shootCommand'"); + + public void SHOOTCONVEYOR(){ + conveyorMotor1.set(MechanismConstants.CONVEYOR_MOTOR_SPEED); + conveyorMotor2.set(MechanismConstants.CONVEYOR_MOTOR_SPEED); } } diff --git a/src/main/java/frc/robot/subsystems/FlyWheel.java b/src/main/java/frc/robot/subsystems/FlyWheel.java index 296b7b9..a7902a7 100644 --- a/src/main/java/frc/robot/subsystems/FlyWheel.java +++ b/src/main/java/frc/robot/subsystems/FlyWheel.java @@ -57,13 +57,13 @@ public void periodic() { // This method will be called once per scheduler run } - public Object shootCommand() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'shootCommand'"); - } - public void fastflywheel() { flywheelMotor1.set(MechanismConstants.FLYWHEEL_MOTOR_SPEED_100); flywheelMotor2.set(MechanismConstants.FLYWHEEL_MOTOR_SPEED_100); } + public void SHOOTFLYS(){ + flywheelMotor1.set(MechanismConstants.FLYWHEEL_MOTOR_FULL_SPEED); + flywheelMotor2.set(MechanismConstants.FLYWHEEL_MOTOR_FULL_SPEED); + } } + From 1db7ec24ba5528a46962b39661ddf3a58ba99c4b Mon Sep 17 00:00:00 2001 From: DLipovac93 Date: Fri, 22 Mar 2024 15:19:44 +0000 Subject: [PATCH 20/36] [Spotless] Apply formatting --- src/main/java/frc/robot/subsystems/Conveyor.java | 5 ++--- src/main/java/frc/robot/subsystems/FlyWheel.java | 4 ++-- 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Conveyor.java b/src/main/java/frc/robot/subsystems/Conveyor.java index 80eb77c..0cd28c3 100644 --- a/src/main/java/frc/robot/subsystems/Conveyor.java +++ b/src/main/java/frc/robot/subsystems/Conveyor.java @@ -6,7 +6,6 @@ import com.revrobotics.CANSparkLowLevel; import com.revrobotics.CANSparkMax; -import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.MechanismConstants; @@ -45,8 +44,8 @@ public void reverseConveyor() { public void periodic() { // This method will be called once per scheduler run } - - public void SHOOTCONVEYOR(){ + + public void SHOOTCONVEYOR() { conveyorMotor1.set(MechanismConstants.CONVEYOR_MOTOR_SPEED); conveyorMotor2.set(MechanismConstants.CONVEYOR_MOTOR_SPEED); } diff --git a/src/main/java/frc/robot/subsystems/FlyWheel.java b/src/main/java/frc/robot/subsystems/FlyWheel.java index a7902a7..a0070a9 100644 --- a/src/main/java/frc/robot/subsystems/FlyWheel.java +++ b/src/main/java/frc/robot/subsystems/FlyWheel.java @@ -61,9 +61,9 @@ public void fastflywheel() { flywheelMotor1.set(MechanismConstants.FLYWHEEL_MOTOR_SPEED_100); flywheelMotor2.set(MechanismConstants.FLYWHEEL_MOTOR_SPEED_100); } - public void SHOOTFLYS(){ + + public void SHOOTFLYS() { flywheelMotor1.set(MechanismConstants.FLYWHEEL_MOTOR_FULL_SPEED); flywheelMotor2.set(MechanismConstants.FLYWHEEL_MOTOR_FULL_SPEED); } } - From 85300be194f18e692c739d9d6777d975a9d7da38 Mon Sep 17 00:00:00 2001 From: Kendialouge Date: Fri, 22 Mar 2024 10:23:25 -0500 Subject: [PATCH 21/36] speed --- src/main/java/frc/robot/Constants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 5cba83b..59026e4 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -89,7 +89,7 @@ public static class MechanismConstants { public static final int FLYWHEEL_MOTOR_2 = 4; public static final boolean FLYWHEEL_MOTOR_1_INVERTED = true; public static final boolean FLYWHEEL_MOTOR_2_INVERTED = false; - public static final double FLYWHEEL_MOTOR_FULL_SPEED = 0.51; + public static final double FLYWHEEL_MOTOR_FULL_SPEED = 0.55; public static final double FLYWHEEL_MOTOR_REDUCED_SPEED = 0.20; public static final double FLYWHEEL_MOTOR_SPEED_SLOW = 0.15; public static final double FLYWHEEL_MOTOR_SPEED_100 = 1; From e21f0af8cdf90d1dc8f26514438963848229d0e0 Mon Sep 17 00:00:00 2001 From: Kendialouge Date: Fri, 22 Mar 2024 11:38:47 -0500 Subject: [PATCH 22/36] Update Constants.java --- src/main/java/frc/robot/Constants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 59026e4..09c2e11 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -89,7 +89,7 @@ public static class MechanismConstants { public static final int FLYWHEEL_MOTOR_2 = 4; public static final boolean FLYWHEEL_MOTOR_1_INVERTED = true; public static final boolean FLYWHEEL_MOTOR_2_INVERTED = false; - public static final double FLYWHEEL_MOTOR_FULL_SPEED = 0.55; + public static final double FLYWHEEL_MOTOR_FULL_SPEED = 0.60; public static final double FLYWHEEL_MOTOR_REDUCED_SPEED = 0.20; public static final double FLYWHEEL_MOTOR_SPEED_SLOW = 0.15; public static final double FLYWHEEL_MOTOR_SPEED_100 = 1; From 92fbe9611f6db3929d806207b382cd147307846f Mon Sep 17 00:00:00 2001 From: Kendialouge Date: Fri, 22 Mar 2024 11:49:37 -0500 Subject: [PATCH 23/36] Update Constants.java --- src/main/java/frc/robot/Constants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 09c2e11..5741f53 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -90,7 +90,7 @@ public static class MechanismConstants { public static final boolean FLYWHEEL_MOTOR_1_INVERTED = true; public static final boolean FLYWHEEL_MOTOR_2_INVERTED = false; public static final double FLYWHEEL_MOTOR_FULL_SPEED = 0.60; - public static final double FLYWHEEL_MOTOR_REDUCED_SPEED = 0.20; + public static final double FLYWHEEL_MOTOR_REDUCED_SPEED = 0.17; public static final double FLYWHEEL_MOTOR_SPEED_SLOW = 0.15; public static final double FLYWHEEL_MOTOR_SPEED_100 = 1; } From f56c306d3fc3febd54883b44fbb17dff21c24186 Mon Sep 17 00:00:00 2001 From: Kendialouge Date: Fri, 22 Mar 2024 12:13:19 -0500 Subject: [PATCH 24/36] auto --- .../autos/{LeftShoot Auto.auto => AMP SIDE Shoot Auto.auto} | 0 .../autos/{RightShoot Auto.auto => SOURCE SIDE Shoot Auto.auto} | 0 src/main/java/frc/robot/Constants.java | 2 +- 3 files changed, 1 insertion(+), 1 deletion(-) rename src/main/deploy/pathplanner/autos/{LeftShoot Auto.auto => AMP SIDE Shoot Auto.auto} (100%) rename src/main/deploy/pathplanner/autos/{RightShoot Auto.auto => SOURCE SIDE Shoot Auto.auto} (100%) diff --git a/src/main/deploy/pathplanner/autos/LeftShoot Auto.auto b/src/main/deploy/pathplanner/autos/AMP SIDE Shoot Auto.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/LeftShoot Auto.auto rename to src/main/deploy/pathplanner/autos/AMP SIDE Shoot Auto.auto diff --git a/src/main/deploy/pathplanner/autos/RightShoot Auto.auto b/src/main/deploy/pathplanner/autos/SOURCE SIDE Shoot Auto.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/RightShoot Auto.auto rename to src/main/deploy/pathplanner/autos/SOURCE SIDE Shoot Auto.auto diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 5741f53..09c2e11 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -90,7 +90,7 @@ public static class MechanismConstants { public static final boolean FLYWHEEL_MOTOR_1_INVERTED = true; public static final boolean FLYWHEEL_MOTOR_2_INVERTED = false; public static final double FLYWHEEL_MOTOR_FULL_SPEED = 0.60; - public static final double FLYWHEEL_MOTOR_REDUCED_SPEED = 0.17; + public static final double FLYWHEEL_MOTOR_REDUCED_SPEED = 0.20; public static final double FLYWHEEL_MOTOR_SPEED_SLOW = 0.15; public static final double FLYWHEEL_MOTOR_SPEED_100 = 1; } From 49cce01c6faf957db93161660a8eeee9b1e4705c Mon Sep 17 00:00:00 2001 From: Kendialouge Date: Fri, 22 Mar 2024 13:07:21 -0500 Subject: [PATCH 25/36] taxi --- src/main/deploy/pathplanner/autos/Taxi Path.auto | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/Taxi Path.auto b/src/main/deploy/pathplanner/autos/Taxi Path.auto index f70a47e..965c5c8 100644 --- a/src/main/deploy/pathplanner/autos/Taxi Path.auto +++ b/src/main/deploy/pathplanner/autos/Taxi Path.auto @@ -2,8 +2,8 @@ "version": 1.0, "startingPose": { "position": { - "x": 1.107671952684762, - "y": 2.4888707299839377 + "x": 0.41341649767378325, + "y": 2.011938478259783 }, "rotation": 0 }, From c62031b33d5b2224b8d29a17df9c5fcac999b763 Mon Sep 17 00:00:00 2001 From: Kendialouge Date: Fri, 22 Mar 2024 13:18:32 -0500 Subject: [PATCH 26/36] auto --- ...uto.auto => BLUE AMP SIDE Shoot Auto.auto} | 4 +- ... Auto.auto => BLUE Middle Shoot Auto.auto} | 4 +- ....auto => BLUE SOURCE SIDE Shoot Auto.auto} | 0 .../autos/{Taxi Path.auto => BLUE TAXI.auto} | 4 +- .../autos/RED AMP SIDE Shoot Auto.auto | 43 +++++++++++ .../autos/RED Middle Shoot Auto.auto | 43 +++++++++++ .../autos/RED SOURCE SIDE Shoot Auto.auto | 43 +++++++++++ .../deploy/pathplanner/autos/RED TAXI.auto | 25 ++++++ .../deploy/pathplanner/paths/RLS Path.path | 76 +++++++++++++++++++ .../deploy/pathplanner/paths/RR Path.path | 49 ++++++++++++ .../deploy/pathplanner/paths/RRS Path.path | 76 +++++++++++++++++++ .../deploy/pathplanner/paths/RS Path.path | 76 +++++++++++++++++++ 12 files changed, 437 insertions(+), 6 deletions(-) rename src/main/deploy/pathplanner/autos/{AMP SIDE Shoot Auto.auto => BLUE AMP SIDE Shoot Auto.auto} (91%) rename src/main/deploy/pathplanner/autos/{MiddleShoot Auto.auto => BLUE Middle Shoot Auto.auto} (91%) rename src/main/deploy/pathplanner/autos/{SOURCE SIDE Shoot Auto.auto => BLUE SOURCE SIDE Shoot Auto.auto} (100%) rename src/main/deploy/pathplanner/autos/{Taxi Path.auto => BLUE TAXI.auto} (84%) create mode 100644 src/main/deploy/pathplanner/autos/RED AMP SIDE Shoot Auto.auto create mode 100644 src/main/deploy/pathplanner/autos/RED Middle Shoot Auto.auto create mode 100644 src/main/deploy/pathplanner/autos/RED SOURCE SIDE Shoot Auto.auto create mode 100644 src/main/deploy/pathplanner/autos/RED TAXI.auto create mode 100644 src/main/deploy/pathplanner/paths/RLS Path.path create mode 100644 src/main/deploy/pathplanner/paths/RR Path.path create mode 100644 src/main/deploy/pathplanner/paths/RRS Path.path create mode 100644 src/main/deploy/pathplanner/paths/RS Path.path diff --git a/src/main/deploy/pathplanner/autos/AMP SIDE Shoot Auto.auto b/src/main/deploy/pathplanner/autos/BLUE AMP SIDE Shoot Auto.auto similarity index 91% rename from src/main/deploy/pathplanner/autos/AMP SIDE Shoot Auto.auto rename to src/main/deploy/pathplanner/autos/BLUE AMP SIDE Shoot Auto.auto index 8f7ebc2..379bf7b 100644 --- a/src/main/deploy/pathplanner/autos/AMP SIDE Shoot Auto.auto +++ b/src/main/deploy/pathplanner/autos/BLUE AMP SIDE Shoot Auto.auto @@ -2,8 +2,8 @@ "version": 1.0, "startingPose": { "position": { - "x": 0.8311910210518773, - "y": 6.50308873064302 + "x": 13.401066802133604, + "y": 6.792903726652525 }, "rotation": 58.172553423326896 }, diff --git a/src/main/deploy/pathplanner/autos/MiddleShoot Auto.auto b/src/main/deploy/pathplanner/autos/BLUE Middle Shoot Auto.auto similarity index 91% rename from src/main/deploy/pathplanner/autos/MiddleShoot Auto.auto rename to src/main/deploy/pathplanner/autos/BLUE Middle Shoot Auto.auto index 27122dd..1c9f1c3 100644 --- a/src/main/deploy/pathplanner/autos/MiddleShoot Auto.auto +++ b/src/main/deploy/pathplanner/autos/BLUE Middle Shoot Auto.auto @@ -2,8 +2,8 @@ "version": 1.0, "startingPose": { "position": { - "x": 1.3674277229789629, - "y": 5.557362183607978 + "x": 16.12709658335238, + "y": 5.551187918118234 }, "rotation": 0 }, diff --git a/src/main/deploy/pathplanner/autos/SOURCE SIDE Shoot Auto.auto b/src/main/deploy/pathplanner/autos/BLUE SOURCE SIDE Shoot Auto.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/SOURCE SIDE Shoot Auto.auto rename to src/main/deploy/pathplanner/autos/BLUE SOURCE SIDE Shoot Auto.auto diff --git a/src/main/deploy/pathplanner/autos/Taxi Path.auto b/src/main/deploy/pathplanner/autos/BLUE TAXI.auto similarity index 84% rename from src/main/deploy/pathplanner/autos/Taxi Path.auto rename to src/main/deploy/pathplanner/autos/BLUE TAXI.auto index 965c5c8..2d58202 100644 --- a/src/main/deploy/pathplanner/autos/Taxi Path.auto +++ b/src/main/deploy/pathplanner/autos/BLUE TAXI.auto @@ -2,8 +2,8 @@ "version": 1.0, "startingPose": { "position": { - "x": 0.41341649767378325, - "y": 2.011938478259783 + "x": 12.573161461170866, + "y": 2.7902796413821807 }, "rotation": 0 }, diff --git a/src/main/deploy/pathplanner/autos/RED AMP SIDE Shoot Auto.auto b/src/main/deploy/pathplanner/autos/RED AMP SIDE Shoot Auto.auto new file mode 100644 index 0000000..cd1c7a1 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/RED AMP SIDE Shoot Auto.auto @@ -0,0 +1,43 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 15.814715573093117, + "y": 6.5616559402132895 + }, + "rotation": -59.534455080540134 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "SHOOTCONVEYOR" + } + }, + { + "type": "named", + "data": { + "name": "SHOOTFLYS" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + }, + { + "type": "path", + "data": { + "pathName": "RLS Path" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/RED Middle Shoot Auto.auto b/src/main/deploy/pathplanner/autos/RED Middle Shoot Auto.auto new file mode 100644 index 0000000..4986d8b --- /dev/null +++ b/src/main/deploy/pathplanner/autos/RED Middle Shoot Auto.auto @@ -0,0 +1,43 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 15.319384055583853, + "y": 5.551187918118234 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "SHOOTCONVEYOR" + } + }, + { + "type": "named", + "data": { + "name": "SHOOTFLYS" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + }, + { + "type": "path", + "data": { + "pathName": "RS Path" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/RED SOURCE SIDE Shoot Auto.auto b/src/main/deploy/pathplanner/autos/RED SOURCE SIDE Shoot Auto.auto new file mode 100644 index 0000000..4b543d3 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/RED SOURCE SIDE Shoot Auto.auto @@ -0,0 +1,43 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 15.800262586440667, + "y": 4.538237808869985 + }, + "rotation": -120.57922687248887 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "SHOOTCONVEYOR" + } + }, + { + "type": "named", + "data": { + "name": "SHOOTFLYS" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + }, + { + "type": "path", + "data": { + "pathName": "RRS Path" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/RED TAXI.auto b/src/main/deploy/pathplanner/autos/RED TAXI.auto new file mode 100644 index 0000000..1332884 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/RED TAXI.auto @@ -0,0 +1,25 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 16.112410901029318, + "y": 2.0413098429059118 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "RR Path" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/RLS Path.path b/src/main/deploy/pathplanner/paths/RLS Path.path new file mode 100644 index 0000000..5241f5a --- /dev/null +++ b/src/main/deploy/pathplanner/paths/RLS Path.path @@ -0,0 +1,76 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 15.872527519702928, + "y": 6.792903726652525 + }, + "prevControl": null, + "nextControl": { + "x": 15.931026069004062, + "y": 6.812403243086236 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 13.747938481792456, + "y": 6.2147842605544374 + }, + "prevControl": { + "x": 13.767437998226168, + "y": 6.234283776988149 + }, + "nextControl": { + "x": 13.728438965358745, + "y": 6.195284744120726 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 11.811238270363864, + "y": 6.2147842605544374 + }, + "prevControl": { + "x": 11.006883217473236, + "y": 6.180660106795441 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "shoot", + "waypointRelativePos": 0, + "command": { + "type": "parallel", + "data": { + "commands": [] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/RR Path.path b/src/main/deploy/pathplanner/paths/RR Path.path new file mode 100644 index 0000000..85eb5dc --- /dev/null +++ b/src/main/deploy/pathplanner/paths/RR Path.path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 16.112410901029318, + "y": 2.0413098429059118 + }, + "prevControl": null, + "nextControl": { + "x": 16.132544988823533, + "y": 2.038276600221949 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 13.615844906108418, + "y": 0.6167986575686938 + }, + "prevControl": { + "x": 13.654843938975842, + "y": 0.5875493829181261 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -1.080924186660749, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/RRS Path.path b/src/main/deploy/pathplanner/paths/RRS Path.path new file mode 100644 index 0000000..3d1a356 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/RRS Path.path @@ -0,0 +1,76 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 15.858074533050477, + "y": 4.234725089168489 + }, + "prevControl": null, + "nextControl": { + "x": 15.916573082351611, + "y": 4.254224605602199 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 14.35496392119545, + "y": 2.1968539711727315 + }, + "prevControl": { + "x": 14.374463437629162, + "y": 2.2163534876064426 + }, + "nextControl": { + "x": 14.335464404761737, + "y": 2.17735445473902 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 12.794041362730612, + "y": 2.0089651446908534 + }, + "prevControl": { + "x": 11.989686309839984, + "y": 1.9748409909318574 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "shoot", + "waypointRelativePos": 0, + "command": { + "type": "parallel", + "data": { + "commands": [] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/RS Path.path b/src/main/deploy/pathplanner/paths/RS Path.path new file mode 100644 index 0000000..50324dd --- /dev/null +++ b/src/main/deploy/pathplanner/paths/RS Path.path @@ -0,0 +1,76 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 15.319384055583853, + "y": 5.551187918118234 + }, + "prevControl": null, + "nextControl": { + "x": 15.377882604884993, + "y": 5.570687434551946 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 13.615844906108418, + "y": 4.86096084893422 + }, + "prevControl": { + "x": 13.63534442254213, + "y": 4.880460365367932 + }, + "nextControl": { + "x": 13.596345389674706, + "y": 4.841461332500509 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 9.180768844543056, + "y": 7.328155479208991 + }, + "prevControl": { + "x": 8.376413791652427, + "y": 7.294031325449995 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "shoot", + "waypointRelativePos": 0, + "command": { + "type": "parallel", + "data": { + "commands": [] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file From 2673eeacada19b2f2547b224fee09fa1d92697f7 Mon Sep 17 00:00:00 2001 From: Garrett Summerfield Date: Fri, 22 Mar 2024 13:50:13 -0500 Subject: [PATCH 27/36] Fix incorrect path origin Path origins for some of the autonomous paths were incorrectly starting on the wrong alliance side. --- .../deploy/pathplanner/autos/BLUE AMP SIDE Shoot Auto.auto | 6 +++--- .../deploy/pathplanner/autos/BLUE Middle Shoot Auto.auto | 4 ++-- src/main/deploy/pathplanner/autos/BLUE TAXI.auto | 4 ++-- 3 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/BLUE AMP SIDE Shoot Auto.auto b/src/main/deploy/pathplanner/autos/BLUE AMP SIDE Shoot Auto.auto index 379bf7b..b874cca 100644 --- a/src/main/deploy/pathplanner/autos/BLUE AMP SIDE Shoot Auto.auto +++ b/src/main/deploy/pathplanner/autos/BLUE AMP SIDE Shoot Auto.auto @@ -2,10 +2,10 @@ "version": 1.0, "startingPose": { "position": { - "x": 13.401066802133604, - "y": 6.792903726652525 + "x": 0.7275269251393154, + "y": 6.712444649961881 }, - "rotation": 58.172553423326896 + "rotation": 60.13079624957095 }, "command": { "type": "sequential", diff --git a/src/main/deploy/pathplanner/autos/BLUE Middle Shoot Auto.auto b/src/main/deploy/pathplanner/autos/BLUE Middle Shoot Auto.auto index 1c9f1c3..27122dd 100644 --- a/src/main/deploy/pathplanner/autos/BLUE Middle Shoot Auto.auto +++ b/src/main/deploy/pathplanner/autos/BLUE Middle Shoot Auto.auto @@ -2,8 +2,8 @@ "version": 1.0, "startingPose": { "position": { - "x": 16.12709658335238, - "y": 5.551187918118234 + "x": 1.3674277229789629, + "y": 5.557362183607978 }, "rotation": 0 }, diff --git a/src/main/deploy/pathplanner/autos/BLUE TAXI.auto b/src/main/deploy/pathplanner/autos/BLUE TAXI.auto index 2d58202..f70a47e 100644 --- a/src/main/deploy/pathplanner/autos/BLUE TAXI.auto +++ b/src/main/deploy/pathplanner/autos/BLUE TAXI.auto @@ -2,8 +2,8 @@ "version": 1.0, "startingPose": { "position": { - "x": 12.573161461170866, - "y": 2.7902796413821807 + "x": 1.107671952684762, + "y": 2.4888707299839377 }, "rotation": 0 }, From 49346c9273193b9ea29522841c933276e55063e0 Mon Sep 17 00:00:00 2001 From: Garrett Summerfield Date: Fri, 22 Mar 2024 13:50:52 -0500 Subject: [PATCH 28/36] Update names of the registered commands for autonomous Names that were registered with PathPlanner were not correct in the PathPlannerGUI, this is now fixed. --- src/main/java/frc/robot/RobotContainer.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 42e3a98..a9cac9e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -82,9 +82,9 @@ public RobotContainer() { elevator.setDefaultCommand(new ElevatorControl(elevator)); // Register Named Commands NamedCommands.registerCommand( - "shootConveyor", new InstantCommand(() -> conveyor.intakeConveyor())); + "SHOOOTCONVEYOR", new InstantCommand(() -> conveyor.intakeConveyor())); NamedCommands.registerCommand( - "shootFlyWheel", new InstantCommand(() -> FlyWheel.enableflywheelfull())); + "SHOOTFLYS", new InstantCommand(() -> FlyWheel.enableflywheelfull())); NamedCommands.registerCommand("dump", new InstantCommand(() -> dump.open())); NamedCommands.registerCommand("Dump bed", new dumpBed(dump)); // Configure the trigger bindings From 8274f25d359faddf185900283c39f089193e338b Mon Sep 17 00:00:00 2001 From: Kendialouge Date: Fri, 22 Mar 2024 14:02:15 -0500 Subject: [PATCH 29/36] Update BLUE AMP SIDE Shoot Auto.auto --- .../deploy/pathplanner/autos/BLUE AMP SIDE Shoot Auto.auto | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/BLUE AMP SIDE Shoot Auto.auto b/src/main/deploy/pathplanner/autos/BLUE AMP SIDE Shoot Auto.auto index 379bf7b..f55c43d 100644 --- a/src/main/deploy/pathplanner/autos/BLUE AMP SIDE Shoot Auto.auto +++ b/src/main/deploy/pathplanner/autos/BLUE AMP SIDE Shoot Auto.auto @@ -2,10 +2,10 @@ "version": 1.0, "startingPose": { "position": { - "x": 13.401066802133604, - "y": 6.792903726652525 + "x": 0.7275269251393154, + "y": 6.712444649961881 }, - "rotation": 58.172553423326896 + "rotation": -120.68965128706108 }, "command": { "type": "sequential", From 89720230d547a59d714863f6301457c989fc7cd3 Mon Sep 17 00:00:00 2001 From: Kendialouge Date: Fri, 22 Mar 2024 17:59:40 -0500 Subject: [PATCH 30/36] Fixed the elevator controls --- src/main/java/frc/robot/Constants.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 09c2e11..766580a 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -44,8 +44,8 @@ public static final class DumpConstants { public static final class ElevatorConstants { // upy downy chain lift thing - public static final int IN = 10; - public static final int OUT = 11; + public static final int IN = 11; + public static final int OUT = 10; } } From c5fdbf1444580f6150c424a2ed13e488be2c99fd Mon Sep 17 00:00:00 2001 From: Garrett Summerfield Date: Fri, 22 Mar 2024 21:11:48 -0500 Subject: [PATCH 31/36] Revert "Update names of the registered commands for autonomous" This reverts commit 49346c9273193b9ea29522841c933276e55063e0. --- src/main/java/frc/robot/RobotContainer.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a9cac9e..42e3a98 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -82,9 +82,9 @@ public RobotContainer() { elevator.setDefaultCommand(new ElevatorControl(elevator)); // Register Named Commands NamedCommands.registerCommand( - "SHOOOTCONVEYOR", new InstantCommand(() -> conveyor.intakeConveyor())); + "shootConveyor", new InstantCommand(() -> conveyor.intakeConveyor())); NamedCommands.registerCommand( - "SHOOTFLYS", new InstantCommand(() -> FlyWheel.enableflywheelfull())); + "shootFlyWheel", new InstantCommand(() -> FlyWheel.enableflywheelfull())); NamedCommands.registerCommand("dump", new InstantCommand(() -> dump.open())); NamedCommands.registerCommand("Dump bed", new dumpBed(dump)); // Configure the trigger bindings From 5cb4770af74a243b9cafe7262030cf080e1c3f91 Mon Sep 17 00:00:00 2001 From: Garrett Summerfield Date: Fri, 22 Mar 2024 21:14:30 -0500 Subject: [PATCH 32/36] Add new registered commands Instead of dealing with renaming already existing commands, creating new commands that use the newly created commands and have already existing commands still exist without any changes. --- src/main/java/frc/robot/RobotContainer.java | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 42e3a98..707c89f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -26,6 +26,8 @@ import frc.robot.commands.ElevatorControl; import frc.robot.commands.FlyWCommand; import frc.robot.commands.IntakeCommand; +import frc.robot.commands.auto.SHOOTCONVEYOR; +import frc.robot.commands.auto.SHOOTFLYS; import frc.robot.commands.autoCommands.dumpBed; import frc.robot.commands.swervedrive.drivebase.AbsoluteDriveAdv; import frc.robot.subsystems.Conveyor; @@ -72,6 +74,9 @@ public class RobotContainer { private final PhotonCamera intakeCamera = vision.getIntakeCamera(); private final PhotonCamera conveyorCamera = vision.getConveyorCamera(); + private final SHOOTCONVEYOR shootconveyorcmd = new SHOOTCONVEYOR(conveyor); + private final SHOOTFLYS shootflyscmd = new SHOOTFLYS(FlyWheel); + /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { @@ -87,6 +92,8 @@ public RobotContainer() { "shootFlyWheel", new InstantCommand(() -> FlyWheel.enableflywheelfull())); NamedCommands.registerCommand("dump", new InstantCommand(() -> dump.open())); NamedCommands.registerCommand("Dump bed", new dumpBed(dump)); + NamedCommands.registerCommand("SHOOTCONVEYOR", shootconveyorcmd); + NamedCommands.registerCommand("SHOOTFLYS", shootflyscmd); // Configure the trigger bindings configureBindings(); From ef1d490840af2ffc98e6178c12f24ab8fcb770ae Mon Sep 17 00:00:00 2001 From: Kendialouge Date: Sat, 23 Mar 2024 09:46:04 -0500 Subject: [PATCH 33/36] fixes --- ...hoot Auto.auto => BLUE AMP SIDE Auto.auto} | 0 ...t Auto.auto => BLUE SOURCE SIDE Auto.auto} | 2 +- ...Shoot Auto.auto => RED AMP SIDE Auto.auto} | 8 +-- ...ot Auto.auto => RED SOURCE SIDE Auto.auto} | 8 +-- .../paths/Straight Red Amp Path.path | 52 +++++++++++++++++++ .../paths/Straight Red Source Path.path | 52 +++++++++++++++++++ 6 files changed, 113 insertions(+), 9 deletions(-) rename src/main/deploy/pathplanner/autos/{BLUE AMP SIDE Shoot Auto.auto => BLUE AMP SIDE Auto.auto} (100%) rename src/main/deploy/pathplanner/autos/{BLUE SOURCE SIDE Shoot Auto.auto => BLUE SOURCE SIDE Auto.auto} (95%) rename src/main/deploy/pathplanner/autos/{RED SOURCE SIDE Shoot Auto.auto => RED AMP SIDE Auto.auto} (82%) rename src/main/deploy/pathplanner/autos/{RED AMP SIDE Shoot Auto.auto => RED SOURCE SIDE Auto.auto} (82%) create mode 100644 src/main/deploy/pathplanner/paths/Straight Red Amp Path.path create mode 100644 src/main/deploy/pathplanner/paths/Straight Red Source Path.path diff --git a/src/main/deploy/pathplanner/autos/BLUE AMP SIDE Shoot Auto.auto b/src/main/deploy/pathplanner/autos/BLUE AMP SIDE Auto.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/BLUE AMP SIDE Shoot Auto.auto rename to src/main/deploy/pathplanner/autos/BLUE AMP SIDE Auto.auto diff --git a/src/main/deploy/pathplanner/autos/BLUE SOURCE SIDE Shoot Auto.auto b/src/main/deploy/pathplanner/autos/BLUE SOURCE SIDE Auto.auto similarity index 95% rename from src/main/deploy/pathplanner/autos/BLUE SOURCE SIDE Shoot Auto.auto rename to src/main/deploy/pathplanner/autos/BLUE SOURCE SIDE Auto.auto index 942af83..af696be 100644 --- a/src/main/deploy/pathplanner/autos/BLUE SOURCE SIDE Shoot Auto.auto +++ b/src/main/deploy/pathplanner/autos/BLUE SOURCE SIDE Auto.auto @@ -5,7 +5,7 @@ "x": 0.79769185179396, "y": 4.572414386994017 }, - "rotation": -62.59242456218156 + "rotation": -125.24061873233006 }, "command": { "type": "sequential", diff --git a/src/main/deploy/pathplanner/autos/RED SOURCE SIDE Shoot Auto.auto b/src/main/deploy/pathplanner/autos/RED AMP SIDE Auto.auto similarity index 82% rename from src/main/deploy/pathplanner/autos/RED SOURCE SIDE Shoot Auto.auto rename to src/main/deploy/pathplanner/autos/RED AMP SIDE Auto.auto index 4b543d3..bec19dc 100644 --- a/src/main/deploy/pathplanner/autos/RED SOURCE SIDE Shoot Auto.auto +++ b/src/main/deploy/pathplanner/autos/RED AMP SIDE Auto.auto @@ -2,10 +2,10 @@ "version": 1.0, "startingPose": { "position": { - "x": 15.800262586440667, - "y": 4.538237808869985 + "x": 16.147134266099517, + "y": 6.9085276198721415 }, - "rotation": -120.57922687248887 + "rotation": 0.0 }, "command": { "type": "sequential", @@ -32,7 +32,7 @@ { "type": "path", "data": { - "pathName": "RRS Path" + "pathName": "Straight Red Amp Path" } } ] diff --git a/src/main/deploy/pathplanner/autos/RED AMP SIDE Shoot Auto.auto b/src/main/deploy/pathplanner/autos/RED SOURCE SIDE Auto.auto similarity index 82% rename from src/main/deploy/pathplanner/autos/RED AMP SIDE Shoot Auto.auto rename to src/main/deploy/pathplanner/autos/RED SOURCE SIDE Auto.auto index cd1c7a1..f10d3dc 100644 --- a/src/main/deploy/pathplanner/autos/RED AMP SIDE Shoot Auto.auto +++ b/src/main/deploy/pathplanner/autos/RED SOURCE SIDE Auto.auto @@ -2,10 +2,10 @@ "version": 1.0, "startingPose": { "position": { - "x": 15.814715573093117, - "y": 6.5616559402132895 + "x": 16.233852186014232, + "y": 4.176913142558679 }, - "rotation": -59.534455080540134 + "rotation": 0.0 }, "command": { "type": "sequential", @@ -32,7 +32,7 @@ { "type": "path", "data": { - "pathName": "RLS Path" + "pathName": "Straight Red Source Path" } } ] diff --git a/src/main/deploy/pathplanner/paths/Straight Red Amp Path.path b/src/main/deploy/pathplanner/paths/Straight Red Amp Path.path new file mode 100644 index 0000000..b2f8724 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Straight Red Amp Path.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 16.147134266099517, + "y": 6.9085276198721415 + }, + "prevControl": null, + "nextControl": { + "x": 13.950280294926786, + "y": 7.7034418857570115 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 11.204212830960874, + "y": 7.732347859061915 + }, + "prevControl": { + "x": 13.747938481792456, + "y": 7.862424738933985 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Straight Red Source Path.path b/src/main/deploy/pathplanner/paths/Straight Red Source Path.path new file mode 100644 index 0000000..6f288f7 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Straight Red Source Path.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 16.233852186014232, + "y": 4.176913142558679 + }, + "prevControl": null, + "nextControl": { + "x": 14.716288587506751, + "y": 3.757776529637567 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 11.117494911046158, + "y": 1.8355293048614274 + }, + "prevControl": { + "x": 13.73663367045045, + "y": 1.503110611855028 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file From fd9474bcf44db85d36f3dc7391af0e32a379fc7e Mon Sep 17 00:00:00 2001 From: ShaneHopkins11 <97139604+ShaneHopkins11@users.noreply.github.com> Date: Sat, 23 Mar 2024 10:20:52 -0500 Subject: [PATCH 34/36] fix --- ...hoot Auto.auto => BLUE AMP SIDE Auto.auto} | 6 +-- .../autos/BLUE SOURCE SIDE Auto.auto | 25 +++++++++++ .../autos/BLUE SOURCE SIDE Shoot Auto.auto | 43 ------------------- .../deploy/pathplanner/autos/BLUE TAXI.auto | 4 +- .../pathplanner/autos/RED AMP SIDE Auto.auto | 25 +++++++++++ .../autos/RED AMP SIDE Shoot Auto.auto | 43 ------------------- .../autos/RED SOURCE SIDE Auto.auto | 25 +++++++++++ .../autos/RED SOURCE SIDE Shoot Auto.auto | 43 ------------------- .../deploy/pathplanner/paths/BLS Path.path | 28 ++++++------ .../deploy/pathplanner/paths/BRS Path.path | 8 ++-- .../deploy/pathplanner/paths/RLS Path.path | 28 ++++++------ .../deploy/pathplanner/paths/RRS Path.path | 16 +++---- 12 files changed, 120 insertions(+), 174 deletions(-) rename src/main/deploy/pathplanner/autos/{BLUE AMP SIDE Shoot Auto.auto => BLUE AMP SIDE Auto.auto} (87%) create mode 100644 src/main/deploy/pathplanner/autos/BLUE SOURCE SIDE Auto.auto delete mode 100644 src/main/deploy/pathplanner/autos/BLUE SOURCE SIDE Shoot Auto.auto create mode 100644 src/main/deploy/pathplanner/autos/RED AMP SIDE Auto.auto delete mode 100644 src/main/deploy/pathplanner/autos/RED AMP SIDE Shoot Auto.auto create mode 100644 src/main/deploy/pathplanner/autos/RED SOURCE SIDE Auto.auto delete mode 100644 src/main/deploy/pathplanner/autos/RED SOURCE SIDE Shoot Auto.auto diff --git a/src/main/deploy/pathplanner/autos/BLUE AMP SIDE Shoot Auto.auto b/src/main/deploy/pathplanner/autos/BLUE AMP SIDE Auto.auto similarity index 87% rename from src/main/deploy/pathplanner/autos/BLUE AMP SIDE Shoot Auto.auto rename to src/main/deploy/pathplanner/autos/BLUE AMP SIDE Auto.auto index b874cca..84a6d73 100644 --- a/src/main/deploy/pathplanner/autos/BLUE AMP SIDE Shoot Auto.auto +++ b/src/main/deploy/pathplanner/autos/BLUE AMP SIDE Auto.auto @@ -2,10 +2,10 @@ "version": 1.0, "startingPose": { "position": { - "x": 0.7275269251393154, - "y": 6.712444649961881 + "x": 0.4234789096358818, + "y": 6.981410202136645 }, - "rotation": 60.13079624957095 + "rotation": 0.0 }, "command": { "type": "sequential", diff --git a/src/main/deploy/pathplanner/autos/BLUE SOURCE SIDE Auto.auto b/src/main/deploy/pathplanner/autos/BLUE SOURCE SIDE Auto.auto new file mode 100644 index 0000000..d6e72d5 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/BLUE SOURCE SIDE Auto.auto @@ -0,0 +1,25 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.400090600751002, + "y": 4.209895599278378 + }, + "rotation": 0.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "BRS Path" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/BLUE SOURCE SIDE Shoot Auto.auto b/src/main/deploy/pathplanner/autos/BLUE SOURCE SIDE Shoot Auto.auto deleted file mode 100644 index 942af83..0000000 --- a/src/main/deploy/pathplanner/autos/BLUE SOURCE SIDE Shoot Auto.auto +++ /dev/null @@ -1,43 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 0.79769185179396, - "y": 4.572414386994017 - }, - "rotation": -62.59242456218156 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "SHOOTCONVEYOR" - } - }, - { - "type": "named", - "data": { - "name": "SHOOTFLYS" - } - }, - { - "type": "wait", - "data": { - "waitTime": 2.0 - } - }, - { - "type": "path", - "data": { - "pathName": "BRS Path" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/BLUE TAXI.auto b/src/main/deploy/pathplanner/autos/BLUE TAXI.auto index f70a47e..a9f9f7a 100644 --- a/src/main/deploy/pathplanner/autos/BLUE TAXI.auto +++ b/src/main/deploy/pathplanner/autos/BLUE TAXI.auto @@ -2,8 +2,8 @@ "version": 1.0, "startingPose": { "position": { - "x": 1.107671952684762, - "y": 2.4888707299839377 + "x": 0.400090600751002, + "y": 1.9997004096572308 }, "rotation": 0 }, diff --git a/src/main/deploy/pathplanner/autos/RED AMP SIDE Auto.auto b/src/main/deploy/pathplanner/autos/RED AMP SIDE Auto.auto new file mode 100644 index 0000000..21afb2b --- /dev/null +++ b/src/main/deploy/pathplanner/autos/RED AMP SIDE Auto.auto @@ -0,0 +1,25 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 16.0936458625054, + "y": 6.922939429924444 + }, + "rotation": 0.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "RLS Path" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/RED AMP SIDE Shoot Auto.auto b/src/main/deploy/pathplanner/autos/RED AMP SIDE Shoot Auto.auto deleted file mode 100644 index cd1c7a1..0000000 --- a/src/main/deploy/pathplanner/autos/RED AMP SIDE Shoot Auto.auto +++ /dev/null @@ -1,43 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 15.814715573093117, - "y": 6.5616559402132895 - }, - "rotation": -59.534455080540134 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "SHOOTCONVEYOR" - } - }, - { - "type": "named", - "data": { - "name": "SHOOTFLYS" - } - }, - { - "type": "wait", - "data": { - "waitTime": 2.0 - } - }, - { - "type": "path", - "data": { - "pathName": "RLS Path" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/RED SOURCE SIDE Auto.auto b/src/main/deploy/pathplanner/autos/RED SOURCE SIDE Auto.auto new file mode 100644 index 0000000..b5b6605 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/RED SOURCE SIDE Auto.auto @@ -0,0 +1,25 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 16.1521166347176, + "y": 4.234725089168489 + }, + "rotation": -1.7858847665219348 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "RRS Path" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/RED SOURCE SIDE Shoot Auto.auto b/src/main/deploy/pathplanner/autos/RED SOURCE SIDE Shoot Auto.auto deleted file mode 100644 index 4b543d3..0000000 --- a/src/main/deploy/pathplanner/autos/RED SOURCE SIDE Shoot Auto.auto +++ /dev/null @@ -1,43 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 15.800262586440667, - "y": 4.538237808869985 - }, - "rotation": -120.57922687248887 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "SHOOTCONVEYOR" - } - }, - { - "type": "named", - "data": { - "name": "SHOOTFLYS" - } - }, - { - "type": "wait", - "data": { - "waitTime": 2.0 - } - }, - { - "type": "path", - "data": { - "pathName": "RRS Path" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BLS Path.path b/src/main/deploy/pathplanner/paths/BLS Path.path index efa2a7e..702e6fe 100644 --- a/src/main/deploy/pathplanner/paths/BLS Path.path +++ b/src/main/deploy/pathplanner/paths/BLS Path.path @@ -3,41 +3,41 @@ "waypoints": [ { "anchor": { - "x": 0.7275269251393154, - "y": 6.712444649961881 + "x": 0.4234789096358818, + "y": 6.981410202136645 }, "prevControl": null, "nextControl": { - "x": 0.7860254744404522, - "y": 6.731944166395593 + "x": 0.48197745893701854, + "y": 7.000909718570356 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.4691504014837022, - "y": 6.27884429165533 + "x": 2.855863033663389, + "y": 7.659671159798161 }, "prevControl": { - "x": 2.4886499179174146, - "y": 6.298343808089042 + "x": 2.8753625500971016, + "y": 7.679170676231872 }, "nextControl": { - "x": 2.44965088504999, - "y": 6.259344775221619 + "x": 2.8363635172296764, + "y": 7.640171643364449 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 4.019361957963822, - "y": 6.27884429165533 + "x": 5.311635466575775, + "y": 7.659671159798161 }, "prevControl": { - "x": 3.2150069050731935, - "y": 6.244720137896334 + "x": 4.507280413685147, + "y": 7.625547006039165 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/BRS Path.path b/src/main/deploy/pathplanner/paths/BRS Path.path index 2f5e90b..061428c 100644 --- a/src/main/deploy/pathplanner/paths/BRS Path.path +++ b/src/main/deploy/pathplanner/paths/BRS Path.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 0.79769185179396, - "y": 4.303448834817898 + "x": 0.400090600751002, + "y": 4.209895599278378 }, "prevControl": null, "nextControl": { - "x": 0.8561904010950968, - "y": 4.3229483512516085 + "x": 0.45858915005213896, + "y": 4.229395115712089 }, "isLocked": false, "linkedName": null diff --git a/src/main/deploy/pathplanner/paths/RLS Path.path b/src/main/deploy/pathplanner/paths/RLS Path.path index 5241f5a..20c218c 100644 --- a/src/main/deploy/pathplanner/paths/RLS Path.path +++ b/src/main/deploy/pathplanner/paths/RLS Path.path @@ -3,41 +3,41 @@ "waypoints": [ { "anchor": { - "x": 15.872527519702928, - "y": 6.792903726652525 + "x": 16.0936458625054, + "y": 6.922939429924444 }, "prevControl": null, "nextControl": { - "x": 15.931026069004062, - "y": 6.812403243086236 + "x": 16.152144411806535, + "y": 6.9424389463581555 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 13.747938481792456, - "y": 6.2147842605544374 + "x": 13.731426665132533, + "y": 7.6947536231254805 }, "prevControl": { - "x": 13.767437998226168, - "y": 6.234283776988149 + "x": 13.750926181566244, + "y": 7.714253139559192 }, "nextControl": { - "x": 13.728438965358745, - "y": 6.195284744120726 + "x": 13.711927148698821, + "y": 7.675254106691769 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 11.811238270363864, - "y": 6.2147842605544374 + "x": 11.205489305565505, + "y": 7.519341306488881 }, "prevControl": { - "x": 11.006883217473236, - "y": 6.180660106795441 + "x": 11.231224680658903, + "y": 7.502458900427612 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/RRS Path.path b/src/main/deploy/pathplanner/paths/RRS Path.path index 3d1a356..55cf11a 100644 --- a/src/main/deploy/pathplanner/paths/RRS Path.path +++ b/src/main/deploy/pathplanner/paths/RRS Path.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 15.858074533050477, - "y": 4.234725089168489 + "x": 16.058563399178077, + "y": 4.198201444835939 }, "prevControl": null, "nextControl": { - "x": 15.916573082351611, - "y": 4.254224605602199 + "x": 16.11706194847921, + "y": 4.217700961269649 }, "isLocked": false, "linkedName": null @@ -32,12 +32,12 @@ }, { "anchor": { - "x": 12.794041362730612, - "y": 2.0089651446908534 + "x": 11.240571768892824, + "y": 0.7601200387585969 }, "prevControl": { - "x": 11.989686309839984, - "y": 1.9748409909318574 + "x": 11.265813024784427, + "y": 0.7625494581674139 }, "nextControl": null, "isLocked": false, From 709621db709e3a62c24c3a4c5641936e7826b8ba Mon Sep 17 00:00:00 2001 From: Kendialouge Date: Sat, 23 Mar 2024 10:27:39 -0500 Subject: [PATCH 35/36] Update BLUE AMP SIDE Auto.auto --- src/main/deploy/pathplanner/autos/BLUE AMP SIDE Auto.auto | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/deploy/pathplanner/autos/BLUE AMP SIDE Auto.auto b/src/main/deploy/pathplanner/autos/BLUE AMP SIDE Auto.auto index 1e955ae..649472d 100644 --- a/src/main/deploy/pathplanner/autos/BLUE AMP SIDE Auto.auto +++ b/src/main/deploy/pathplanner/autos/BLUE AMP SIDE Auto.auto @@ -5,7 +5,7 @@ "x": 0.4234789096358818, "y": 6.981410202136645 }, - "rotation": -120.68965128706108 + "rotation": 2.145150039040434 }, "command": { "type": "sequential", From 5db008e76a793debca50b85c63814d26393d6dfb Mon Sep 17 00:00:00 2001 From: Kendialouge Date: Sat, 23 Mar 2024 10:31:49 -0500 Subject: [PATCH 36/36] Update BR Path.path --- src/main/deploy/pathplanner/paths/BR Path.path | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/BR Path.path b/src/main/deploy/pathplanner/paths/BR Path.path index 921f65f..95c294e 100644 --- a/src/main/deploy/pathplanner/paths/BR Path.path +++ b/src/main/deploy/pathplanner/paths/BR Path.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.107671952684762, - "y": 2.4888707299839377 + "x": 0.4281021799968474, + "y": 1.99725279593672 }, "prevControl": null, "nextControl": { - "x": 1.1278060404789791, - "y": 2.4858374872999747 + "x": 0.4482362677910645, + "y": 1.994219553252757 }, "isLocked": false, "linkedName": null