From fb84e0581f1256cf9313d61a7954c3a7bf25544e Mon Sep 17 00:00:00 2001 From: Ghost <145223182+GhostR406@users.noreply.github.com> Date: Thu, 5 Mar 2026 18:13:09 -0500 Subject: [PATCH 1/8] Done with all sim that is relevant --- .../frc/lib/W8/io/motor/MotorIOTalonFX.java | 20 +-- src/main/java/frc/robot/Constants.java | 151 ++++++++++++++---- src/main/java/frc/robot/Robot.java | 16 +- src/main/java/frc/robot/RobotContainer.java | 102 +++++++++--- .../java/frc/robot/subsystems/Climber.java | 31 ++-- .../java/frc/robot/subsystems/Intake.java | 34 ++-- .../java/frc/robot/subsystems/Shooter.java | 123 +++++++++----- .../frc/robot/subsystems/shooter/Shooter.java | 130 +++++++-------- 8 files changed, 404 insertions(+), 203 deletions(-) diff --git a/src/main/java/frc/lib/W8/io/motor/MotorIOTalonFX.java b/src/main/java/frc/lib/W8/io/motor/MotorIOTalonFX.java index 83c744e..2bbd047 100644 --- a/src/main/java/frc/lib/W8/io/motor/MotorIOTalonFX.java +++ b/src/main/java/frc/lib/W8/io/motor/MotorIOTalonFX.java @@ -75,9 +75,9 @@ public record TalonFXFollower(Device.CAN id, MotorAlignmentValue opposesMain) {} // Preconfigured control objects reused for efficiency protected final CoastOut coastControl = new CoastOut(); protected final StaticBrake brakeControl = new StaticBrake(); - protected final VoltageOut voltageControl = new VoltageOut(0).withEnableFOC(true); + protected final VoltageOut voltageControl = new VoltageOut(0); protected final TorqueCurrentFOC currentControl = new TorqueCurrentFOC(0); - protected final DutyCycleOut dutyCycleControl = new DutyCycleOut(0).withEnableFOC(true); + protected final DutyCycleOut dutyCycleControl = new DutyCycleOut(0); protected final DynamicMotionMagicTorqueCurrentFOC positionControl = new DynamicMotionMagicTorqueCurrentFOC(0, 0, 0); protected final VelocityTorqueCurrentFOC velocityControl = new VelocityTorqueCurrentFOC(0); @@ -166,7 +166,7 @@ protected boolean isRunningPositionControl() { return (control instanceof PositionTorqueCurrentFOC) || (control instanceof PositionVoltage) || (control instanceof MotionMagicTorqueCurrentFOC) - || (control instanceof DynamicMotionMagicTorqueCurrentFOC) + || (control instanceof MotionMagicDutyCycle) || (control instanceof MotionMagicVoltage); } @@ -181,7 +181,7 @@ protected boolean isRunningVelocityControl() { var control = motor.getAppliedControl(); return (control instanceof VelocityTorqueCurrentFOC) || (control instanceof VelocityVoltage) - || (control instanceof MotionMagicVelocityTorqueCurrentFOC) + || (control instanceof MotionMagicVelocityDutyCycle) || (control instanceof MotionMagicVelocityVoltage); } @@ -196,8 +196,8 @@ protected boolean isRunningVelocityControl() { protected boolean isRunningMotionMagic() { var control = motor.getAppliedControl(); return (control instanceof MotionMagicTorqueCurrentFOC) - || (control instanceof DynamicMotionMagicTorqueCurrentFOC) - || (control instanceof MotionMagicVelocityTorqueCurrentFOC) + || (control instanceof MotionMagicDutyCycle) + || (control instanceof MotionMagicVelocityDutyCycle) || (control instanceof MotionMagicVoltage) || (control instanceof MotionMagicVelocityVoltage); } @@ -395,13 +395,7 @@ public void runPosition( Velocity maxJerk, PIDSlot slot) { this.goalPosition = position; - motor.setControl( - positionControl - .withPosition(position) - .withVelocity(cruiseVelocity) - .withAcceleration(acceleration) - .withJerk(maxJerk) - .withSlot(slot.getNum())); + motor.setControl(positionControl.withPosition(position).withSlot(slot.getNum())); } /** diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 99569da..f38ebaf 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -154,6 +154,10 @@ public class Ports { public static final Device.CAN HopperRoller = new CAN(3, "rio"); public static final Device.CAN ClimberLinearMechanism = new CAN(4, "rio"); public static final Device.CAN ShooterRoller = new CAN(5, "rio"); + public static final Device.CAN IntakePivot = new CAN(6, "rio"); + public static final Device.CAN ShooterTower = new CAN(7, "rio"); + public static final Device.CAN ShooterFeeder = new CAN(8, "rio"); + public static final Device.CAN ShooterHood = new CAN(9, "rio"); } public class HopperConstants { @@ -258,10 +262,10 @@ public final class ShooterConstants { public static final Angle STARTING_ANGLE = Rotations.of(0.0); public static final Distance WHEEL_RADIUS = Meters.of(0.5); - public static final double IDLE_SPEED_RPM = (1.0); - public static final double HUB_SPEED_RPM = (1.0); - public static final double TOWER_SPEED_RPM = (1.0); - public static final double DEFAULT_SPEED_RPM = (1.0); + public static final AngularVelocity IDLE_SPEED_RPM = RotationsPerSecond.of(1.0); + public static final AngularVelocity HUB_SPEED_RPM = RotationsPerSecond.of(1.0); + public static final AngularVelocity TOWER_SPEED_RPM = RotationsPerSecond.of(1.0); + public static final AngularVelocity DEFAULT_SPEED_RPM = RotationsPerSecond.of(1.0); public static final double FLYWHEEL_VELOCITY_TOLERANCE = 1.0; // Hood Constants @@ -278,9 +282,58 @@ public final class ShooterConstants { new RotaryMechCharacteristics(OFFSET, WHEEL_RADIUS, MIN_ANGLE, MAX_ANGLE, STARTING_ANGLE); } - public class ShooterFlywheelConstants { + // Needs tuned + public class ShooterFeederConstants { + public static String NAME = "ShooterFeederFlywheel"; - public static String NAME = "ShooterFlywheel"; + public static final AngularVelocity MAX_VELOCITY = RadiansPerSecond.of(2 * Math.PI); + public static final AngularAcceleration MAX_ACCELERATION = MAX_VELOCITY.per(Second); + + private static final double GEARING = (2.0 / 1.0); + + public static final AngularVelocity TOLERANCE = MAX_VELOCITY.times(0.1); + + public static final DCMotor DCMOTOR = DCMotor.getKrakenX60(1); + public static final MomentOfInertia MOI = KilogramSquareMeters.of(1.0); + + // Velocity PID + private static Slot0Configs SLOT0CONFIG = + new Slot0Configs().withKP(1000.0).withKI(0.0).withKD(0.0); + + public static TalonFXConfiguration getFXConfig() { + TalonFXConfiguration config = new TalonFXConfiguration(); + + config.CurrentLimits.SupplyCurrentLimitEnable = Robot.isReal(); + config.CurrentLimits.SupplyCurrentLimit = 40.0; + config.CurrentLimits.SupplyCurrentLowerLimit = 40.0; + config.CurrentLimits.SupplyCurrentLowerTime = 0.1; + + config.CurrentLimits.StatorCurrentLimitEnable = Robot.isReal(); + config.CurrentLimits.StatorCurrentLimit = 80.0; + + config.Voltage.PeakForwardVoltage = 12.0; + config.Voltage.PeakReverseVoltage = -12.0; + + config.MotorOutput.NeutralMode = NeutralModeValue.Brake; + config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + + config.SoftwareLimitSwitch.ForwardSoftLimitEnable = false; + + config.SoftwareLimitSwitch.ReverseSoftLimitEnable = false; + + config.Feedback.RotorToSensorRatio = 1.0; + + config.Feedback.SensorToMechanismRatio = GEARING; + + config.Slot0 = SLOT0CONFIG; + + return config; + } + } + + // Needs tuned + public class ShooterTowerConstants { + public static String NAME = "ShooterTowerFlywheel"; public static final AngularVelocity MAX_VELOCITY = RadiansPerSecond.of(2 * Math.PI); public static final AngularAcceleration MAX_ACCELERATION = MAX_VELOCITY.per(Second); @@ -327,6 +380,54 @@ public static TalonFXConfiguration getFXConfig() { } } + public class ShooterFlywheelConstants { + public static String NAME = "ShooterFlywheel"; + + public static final AngularVelocity MAX_VELOCITY = RadiansPerSecond.of(2 * Math.PI); + public static final AngularAcceleration MAX_ACCELERATION = MAX_VELOCITY.per(Second); + + private static final double GEARING = (5.0 / 1.0); + + public static final AngularVelocity TOLERANCE = MAX_VELOCITY.times(0.1); + + public static final DCMotor DCMOTOR = DCMotor.getKrakenX60(1); + public static final MomentOfInertia MOI = KilogramSquareMeters.of(0.2); + + // Velocity PID + private static Slot0Configs SLOT0CONFIG = + new Slot0Configs().withKP(0.75).withKI(0.0).withKD(0.0); + + public static TalonFXConfiguration getFXConfig() { + TalonFXConfiguration config = new TalonFXConfiguration(); + + config.CurrentLimits.SupplyCurrentLimitEnable = Robot.isReal(); + config.CurrentLimits.SupplyCurrentLimit = 40.0; + config.CurrentLimits.SupplyCurrentLowerLimit = 40.0; + config.CurrentLimits.SupplyCurrentLowerTime = 0.1; + + config.CurrentLimits.StatorCurrentLimitEnable = Robot.isReal(); + config.CurrentLimits.StatorCurrentLimit = 80.0; + + config.Voltage.PeakForwardVoltage = 12.0; + config.Voltage.PeakReverseVoltage = -12.0; + + config.MotorOutput.NeutralMode = NeutralModeValue.Brake; + config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + + config.SoftwareLimitSwitch.ForwardSoftLimitEnable = false; + + config.SoftwareLimitSwitch.ReverseSoftLimitEnable = false; + + config.Feedback.RotorToSensorRatio = 1.0; + + config.Feedback.SensorToMechanismRatio = GEARING; + + config.Slot0 = SLOT0CONFIG; + + return config; + } + } + public class ShooterRotaryConstants { public static String NAME = "ShooterRotary"; @@ -337,13 +438,13 @@ public class ShooterRotaryConstants { // CRUISE_VELOCITY.div(0.1).per(Units.Second); // public static final Velocity JERK = ACCELERATION.per(Second); - private static final double ROTOR_TO_SENSOR = (2.0 / 1.0); - private static final double SENSOR_TO_MECHANISM = (2.0 / 1.0); + private static final double ROTOR_TO_SENSOR = (1.0 / 1.0); + private static final double SENSOR_TO_MECHANISM = (50.0 / 1.0); public static final Translation3d OFFSET = Translation3d.kZero; public static final Angle MIN_ANGLE = Degrees.of(0.0); - public static final Angle MAX_ANGLE = Rotations.of(45.0); + public static final Angle MAX_ANGLE = Degrees.of(45.0); public static final Angle STARTING_ANGLE = Rotations.of(0.0); public static final Distance ARM_LENGTH = Meters.of(1.0); @@ -382,12 +483,12 @@ public class ShooterRotaryConstants { public static TalonFXConfiguration getFXConfig() { TalonFXConfiguration config = new TalonFXConfiguration(); - config.CurrentLimits.SupplyCurrentLimitEnable = false; + config.CurrentLimits.SupplyCurrentLimitEnable = Robot.isReal(); config.CurrentLimits.SupplyCurrentLimit = 40.0; config.CurrentLimits.SupplyCurrentLowerLimit = 40.0; config.CurrentLimits.SupplyCurrentLowerTime = 0.1; - config.CurrentLimits.StatorCurrentLimitEnable = false; + config.CurrentLimits.StatorCurrentLimitEnable = Robot.isReal(); config.CurrentLimits.StatorCurrentLimit = 80.0; config.Voltage.PeakForwardVoltage = 12.0; @@ -405,8 +506,6 @@ public static TalonFXConfiguration getFXConfig() { config.Feedback.RotorToSensorRatio = ROTOR_TO_SENSOR; config.Feedback.SensorToMechanismRatio = SENSOR_TO_MECHANISM; - config.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RemoteCANcoder; - config.Slot0 = SLOT0CONFIG; return config; @@ -439,10 +538,10 @@ public class IntakeFlywheelConstants { public static final Angle MIN_ANGLE = Rotations.of(0.0); public static final Angle MAX_ANGLE = Rotations.of(1); public static final Angle STARTING_ANGLE = Rotations.of(0.0); - public static final double PICKUP_SPEED = 0.0; + public static final double PICKUP_SPEED = 10.0; public static final Distance WHEEL_RADIUS = Meters.of(0.05); public static final Translation3d OFFSET = Translation3d.kZero; - public static final MomentOfInertia MOI = KilogramSquareMeters.of(0.0028125); + public static final MomentOfInertia MOI = KilogramSquareMeters.of(0.2); public static final RotaryMechCharacteristics CONSTANTS = new RotaryMechCharacteristics(OFFSET, WHEEL_RADIUS, MIN_ANGLE, MAX_ANGLE, STARTING_ANGLE); public static final String MOTOR_NAME = "Intake Flywheel"; @@ -571,7 +670,10 @@ public static TalonFXConfiguration getFXConfig() { public class IntakePivotConstants { public static final String NAME = "Intake"; - public static final Angle PICKUP_ANGLE = Rotations.of(0.0); + public static final Angle MIN_ANGLE = Degrees.of(0.0); + public static final Angle MAX_ANGLE = Degrees.of(130.0); + + public static final Angle PICKUP_ANGLE = MAX_ANGLE; public static final Angle STOW_ANGLE = Rotations.of(0.0); public static final Angle TOLERANCE = Degrees.of(1.0); @@ -581,11 +683,8 @@ public class IntakePivotConstants { public static final Velocity JERK = RadiansPerSecondPerSecond.per(Second).of(0.1); - private static final double ROTOR_TO_SENSOR = (50.0 / 1.0); - private static final double SENSOR_TO_MECHANISM = 1.0; - - public static final Angle MIN_ANGLE = Degrees.of(0.0); - public static final Angle MAX_ANGLE = Degrees.of(130.0); + private static final double ROTOR_TO_SENSOR = (1.0 / 1.0); + private static final double SENSOR_TO_MECHANISM = (50.0 / 1.0); public static final Angle STARTING_ANGLE = Radians.zero(); public static final Distance ARM_LENGTH = Foot.one(); @@ -594,12 +693,13 @@ public class IntakePivotConstants { new Translation3d(), ARM_LENGTH, MIN_ANGLE, MAX_ANGLE, STARTING_ANGLE); public static final DCMotor DCMOTOR = DCMotor.getKrakenX60(1); - public static final MomentOfInertia MOI = KilogramSquareMeters.of(0.25); + public static final MomentOfInertia MOI = KilogramSquareMeters.of(1); // Positional PID public static final Slot0Configs SLOT_0_CONFIG = new Slot0Configs().withKP(100.0).withKI(0.0).withKD(0).withKS(0.07).withKV(0.1); - // ^^^ CHANGE + + // ^^^ CHANGE public static TalonFXConfiguration getFXConfig() { TalonFXConfiguration config = new TalonFXConfiguration(); @@ -638,11 +738,6 @@ public static TalonFXConfiguration getFXConfig() { } } - public class FeederConstants { - public static final AngularVelocity FEED_SPEED = RotationsPerSecond.of(0.0); - public static final AngularAcceleration FEED_ACCELERATION = RotationsPerSecondPerSecond.of(0.0); - } - public class ClimberConstants { public static final DCMotor DCMOTOR = DCMotor.getKrakenX60(1); public static final Mass CARRIAGE_MASS = Kilograms.of(2.5); diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index e2e3ff8..112672b 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -21,10 +21,10 @@ import com.ctre.phoenix6.swerve.SwerveModuleConstants.DriveMotorArrangement; import com.ctre.phoenix6.swerve.SwerveModuleConstants.SteerMotorArrangement; import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc.lib.Rebuilt2026.FuelSim; @@ -199,9 +199,15 @@ public void simulationInit() { robotContainer.drive ::getChassisSpeeds); // Supplier of field-centric chassis speed - fuelSim.registerIntake(0.4, 0.5, -0.4, 0.4, - () -> robotContainer.intake.canIntake(), - () -> {robotContainer.intake.simBalls++;}); + fuelSim.registerIntake( + 0.4, + 0.5, + -0.4, + 0.4, + () -> robotContainer.intake.canIntake(), + () -> { + robotContainer.intake.simBalls++; + }); fuelSim.start(); // enables the simulation to run (updateSim must still be called periodically) @@ -215,7 +221,7 @@ public void simulationPeriodic() { fuelSim.updateSim(); Logger.recordOutput("Zero Pose", new Pose3d()); - + SmartDashboard.putNumber("Sim Balls", (double) robotContainer.intake.simBalls); } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 34a9873..bb0d245 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -17,6 +17,7 @@ import com.pathplanner.lib.auto.AutoBuilder; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.units.AngleUnit; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; @@ -24,19 +25,26 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.lib.W8.io.motor.*; +import frc.lib.W8.io.vision.VisionIO; import frc.lib.W8.io.vision.VisionIOPhotonVision; import frc.lib.W8.io.vision.VisionIOPhotonVisionSim; import frc.lib.W8.mechanisms.flywheel.*; +import frc.lib.W8.mechanisms.linear.LinearMechanism; +import frc.lib.W8.mechanisms.linear.LinearMechanismReal; +import frc.lib.W8.mechanisms.linear.LinearMechanismSim; import frc.lib.W8.mechanisms.rotary.RotaryMechanism; import frc.lib.W8.mechanisms.rotary.RotaryMechanismReal; import frc.lib.W8.mechanisms.rotary.RotaryMechanismSim; +import frc.robot.Constants.ClimberConstants; import frc.robot.Constants.HopperConstants; import frc.robot.Constants.IntakeFlywheelConstants; import frc.robot.Constants.IntakeFlywheelConstants.VisionConstants; import frc.robot.Constants.IntakePivotConstants; import frc.robot.Constants.Ports; +import frc.robot.Constants.ShooterFeederConstants; import frc.robot.Constants.ShooterFlywheelConstants; import frc.robot.Constants.ShooterRotaryConstants; +import frc.robot.Constants.ShooterTowerConstants; import frc.robot.commands.DriveCommands; import frc.robot.generated.TunerConstants; import frc.robot.subsystems.BallCounter; @@ -51,6 +59,9 @@ import frc.robot.subsystems.drive.ModuleIO; import frc.robot.subsystems.drive.ModuleIOSim; import frc.robot.subsystems.drive.ModuleIOTalonFX; + +import static edu.wpi.first.units.Units.Degrees; + import java.util.Optional; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; import org.photonvision.PhotonPoseEstimator.PoseStrategy; @@ -66,6 +77,7 @@ public class RobotContainer { // Subsystems public final Drive drive; private final Hopper hopper; + private final Climber climber; private final Shooter shooter; public final Intake intake; private final BallCounter ballCounter; @@ -109,14 +121,19 @@ public RobotContainer() { Ports.ShooterRoller)), new FlywheelMechanismReal( new MotorIOTalonFX( - ShooterFlywheelConstants.NAME, - ShooterFlywheelConstants.getFXConfig(), - Ports.ShooterRoller)), + ShooterFeederConstants.NAME, + ShooterFeederConstants.getFXConfig(), + Ports.ShooterFeeder)), + new FlywheelMechanismReal( + new MotorIOTalonFX( + ShooterTowerConstants.NAME, + ShooterTowerConstants.getFXConfig(), + Ports.ShooterTower)), new RotaryMechanismReal( new MotorIOTalonFX( ShooterRotaryConstants.NAME, ShooterRotaryConstants.getFXConfig(), - Ports.ShooterRoller), + Ports.ShooterHood), Constants.ShooterRotaryConstants.CONSTANTS, java.util.Optional.empty())); @@ -141,6 +158,15 @@ public RobotContainer() { VisionConstants.robotToCamera0, VisionConstants.aprilTagLayout, PoseStrategy.CONSTRAINED_SOLVEPNP)); + climber = + new Climber( + new LinearMechanismReal( + new MotorIOTalonFX( + ClimberConstants.MOTOR_NAME, + ClimberConstants.getFXConfig(), + Ports.ClimberLinearMechanism), + ClimberConstants.CHARACTERISTICS)); + break; case SIM: @@ -175,20 +201,28 @@ public RobotContainer() { ShooterFlywheelConstants.TOLERANCE), new FlywheelMechanismSim( new MotorIOTalonFXSim( - ShooterFlywheelConstants.NAME, - ShooterFlywheelConstants.getFXConfig(), - Ports.ShooterRoller), - ShooterFlywheelConstants.DCMOTOR, - ShooterFlywheelConstants.MOI, - ShooterFlywheelConstants.TOLERANCE), + ShooterFeederConstants.NAME, + ShooterFeederConstants.getFXConfig(), + Ports.ShooterFeeder), + ShooterFeederConstants.DCMOTOR, + ShooterFeederConstants.MOI, + ShooterFeederConstants.TOLERANCE), + new FlywheelMechanismSim( + new MotorIOTalonFXSim( + ShooterTowerConstants.NAME, + ShooterTowerConstants.getFXConfig(), + Ports.ShooterTower), + ShooterTowerConstants.DCMOTOR, + ShooterTowerConstants.MOI, + ShooterTowerConstants.TOLERANCE), new RotaryMechanismSim( new MotorIOTalonFXSim( ShooterRotaryConstants.NAME, ShooterRotaryConstants.getFXConfig(), - Ports.ShooterRoller), + Ports.ShooterHood), ShooterRotaryConstants.DCMOTOR, ShooterRotaryConstants.MOI, - true, + false, ShooterRotaryConstants.CONSTANTS, java.util.Optional.empty())); @@ -206,7 +240,7 @@ public RobotContainer() { new MotorIOTalonFXSim( IntakePivotConstants.NAME, IntakePivotConstants.getFXConfig(), - Ports.IntakeRoller), + Ports.IntakePivot), IntakePivotConstants.DCMOTOR, IntakePivotConstants.MOI, false, @@ -221,6 +255,18 @@ public RobotContainer() { VisionConstants.aprilTagLayout, PoseStrategy.CONSTRAINED_SOLVEPNP, VisionConstants.getSystemSim())); + + climber = + new Climber( + new LinearMechanismSim( + new MotorIOTalonFXSim( + ClimberConstants.MOTOR_NAME, + ClimberConstants.getFXConfig(), + Ports.ClimberLinearMechanism), + ClimberConstants.DCMOTOR, + ClimberConstants.CARRIAGE_MASS, + ClimberConstants.CHARACTERISTICS, + false)); break; default: @@ -237,6 +283,7 @@ public RobotContainer() { shooter = new Shooter( + new FlywheelMechanism() {}, new FlywheelMechanism() {}, new FlywheelMechanism() {}, new RotaryMechanism(null, null) {}); @@ -245,7 +292,11 @@ public RobotContainer() { new Intake( new FlywheelMechanism() {}, new RotaryMechanism(IntakePivotConstants.NAME, IntakePivotConstants.CONSTANTS) {}); - vision = new Vision(null); + vision = new Vision(new VisionIO() {}); + climber = + new Climber( + new LinearMechanism( + ClimberConstants.MOTOR_NAME, ClimberConstants.CHARACTERISTICS) {}); break; } @@ -315,18 +366,19 @@ private void configureButtonBindings() { .x() .onTrue(Commands.runOnce(() -> hopper.setGoal(HopperConstants.HOPPER_POSITION), hopper)); - controller - .rightTrigger() - .onTrue(Commands.runOnce(() -> shooter.simShoot())); + // Commands for sim testing! + // controller.rightTrigger().onTrue(Commands.runOnce(() -> shooter.simShoot())); + + // controller + // .leftTrigger() + // .onTrue( + // Commands.runOnce( + // () -> { + // Robot.fuelSim.clearFuel(); + // Robot.fuelSim.spawnStartingFuel(); + // intake.simBalls = 0; + // })); - controller - .leftTrigger() - .onTrue(Commands.runOnce(() -> { - Robot.fuelSim.clearFuel(); - Robot.fuelSim.spawnStartingFuel(); - intake.simBalls = 0; - })); - controller.y().onTrue(Commands.runOnce(() -> intake.setVelocity(1))); controller.leftBumper().onTrue(intake.intake()); controller.rightBumper().onTrue(intake.stowAndStopRollers()); } diff --git a/src/main/java/frc/robot/subsystems/Climber.java b/src/main/java/frc/robot/subsystems/Climber.java index 4aade98..ebca328 100644 --- a/src/main/java/frc/robot/subsystems/Climber.java +++ b/src/main/java/frc/robot/subsystems/Climber.java @@ -2,13 +2,11 @@ import static edu.wpi.first.units.Units.Amps; import static edu.wpi.first.units.Units.Meters; -import static edu.wpi.first.units.Units.Volts; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.units.measure.Distance; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.lib.W8.io.motor.MotorIO.PIDSlot; @@ -43,21 +41,6 @@ public boolean isAboveCurrentLimit() { } } - @Override - public void periodic() { - _io.periodic(); - - double z = Math.abs(Math.sin(Timer.getFPGATimestamp()) * 0.33); // Placeholder for position - - // The z of the Translation3D should be - // 'ClimberConstants.CONVERTER.toDistance(_io.getPosition()).in(Meters)', change after fixing - // motor configs. - Logger.recordOutput( - "3DField/4_Climber", new Pose3d(new Translation3d(0, 0, z), new Rotation3d(0, 0, 0))); - - _io.runVoltage(Volts.of(Math.sin(Timer.getFPGATimestamp()) * 0.25)); - } - public Command runClimber() { return this.runOnce( () -> @@ -79,4 +62,18 @@ public boolean nearGoalposition() { return false; } } + + @Override + public void periodic() { + _io.periodic(); + + Logger.recordOutput( + "3DField/4_Climber", + new Pose3d( + new Translation3d( + 0, 0, ClimberConstants.CONVERTER.toDistance(_io.getPosition()).in(Meters)), + new Rotation3d(0, 0, 0))); + + // _io.runVoltage(Volts.of(Math.sin(Timer.getFPGATimestamp()) * 0.25)); // For testing climber motion! + } } diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 2442832..8abfe0e 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -3,7 +3,6 @@ import static edu.wpi.first.units.Units.Degree; import static edu.wpi.first.units.Units.Radians; import static edu.wpi.first.units.Units.RotationsPerSecond; -import static edu.wpi.first.units.Units.Volts; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; @@ -16,6 +15,7 @@ import frc.lib.W8.io.motor.MotorIO.PIDSlot; import frc.lib.W8.mechanisms.flywheel.FlywheelMechanism; import frc.lib.W8.mechanisms.rotary.RotaryMechanism; +import frc.lib.W8.util.LoggerHelper; import frc.robot.Constants.IntakeFlywheelConstants; import frc.robot.Constants.IntakePivotConstants; import org.littletonrobotics.junction.Logger; @@ -42,16 +42,13 @@ public void setVelocity(double velocity) { _rollerIO.runVelocity(angVelo, IntakeFlywheelConstants.ACCELERATION, PIDSlot.SLOT_0); } - public Command setPivotAngle(Angle pivotAngle) { - return this.runOnce( - () -> - _pivotIO.runPosition( + public void setPivotAngle(Angle pivotAngle) { + _pivotIO.runPosition( pivotAngle, IntakeFlywheelConstants.CRUISE_VELOCITY, IntakeFlywheelConstants.ACCELERATION, IntakeFlywheelConstants.JERK, - PIDSlot.SLOT_0)); - // .withName("Go To " + setpoint.toString() + " Setpoint"); + PIDSlot.SLOT_0); } public AngularVelocity getVelocity() { @@ -68,8 +65,9 @@ public void stop() { public Command intake() { return Commands.sequence( - Commands.run(() -> setVelocity(IntakeFlywheelConstants.PICKUP_SPEED)), - setPivotAngle(IntakePivotConstants.PICKUP_ANGLE)); + Commands.runOnce(() -> this.setPivotAngle(IntakePivotConstants.PICKUP_ANGLE)), + Commands.runOnce(() -> this.setVelocity(IntakeFlywheelConstants.PICKUP_SPEED)) + ); } public boolean isIntendedAngle() { @@ -77,9 +75,10 @@ public boolean isIntendedAngle() { <= IntakePivotConstants.TOLERANCE.magnitude(); } - public boolean canIntake() - { - return _pivotIO.getPosition().in(Degree) > (IntakePivotConstants.MAX_ANGLE.in(Degree) - 10) && simBalls < 45 && simBalls >= 0; + public boolean canIntake() { + return _pivotIO.getPosition().in(Degree) > (IntakePivotConstants.MAX_ANGLE.in(Degree) - 10) + && simBalls < 45 + && simBalls >= 0; } public Command stowAndStopRollers() { @@ -101,9 +100,11 @@ private Command setStowAngle(Angle stowAngle) { @Override public void periodic() { - if (_pivotIO.getPosition().in(Degree) < IntakePivotConstants.MAX_ANGLE.in(Degree)) _pivotIO.runVoltage(Volts.of(0.25)); + LoggerHelper.recordCurrentCommand("1_Intake", this); _pivotIO.periodic(); + _rollerIO.periodic(); + Logger.recordOutput( "3DField/1_Intake", new Pose3d( @@ -115,6 +116,11 @@ public void periodic() { new Translation3d(Math.sin(_pivotIO.getPosition().in(Radians) * 0.1055), 0, 0), new Rotation3d(0, 0, 0))); - //_pivotIO.runVoltage(Volts.of(Math.sin(Timer.getFPGATimestamp())*0.25)); //--- Tests the pivot + // For testing purposes, opens hopper in sim + // if (_pivotIO.getPosition().in(Degrees) < IntakePivotConstants.MAX_ANGLE.in(Degrees) - 10) + // _pivotIO.runVoltage(Volts.of(7)); + + // _pivotIO.runVoltage(Volts.of(Math.sin(Timer.getFPGATimestamp())*5.0)); //--- Tests the pivot + // _rollerIO.runVoltage(Volts.of(Math.sin(Timer.getFPGATimestamp())*5.0)); //--- Tests the pivot } } diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 3ad1ebe..d3aa7a5 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -3,27 +3,24 @@ import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.Radians; import static edu.wpi.first.units.Units.RotationsPerSecond; -import static edu.wpi.first.units.Units.Volts; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.units.Units; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.lib.W8.io.motor.MotorIO.PIDSlot; import frc.lib.W8.mechanisms.flywheel.FlywheelMechanism; import frc.lib.W8.mechanisms.rotary.RotaryMechanism; -import frc.robot.Constants.FeederConstants; import frc.robot.Constants.FieldConstants; import frc.robot.Constants.ShooterConstants; +import frc.robot.Constants.ShooterFeederConstants; +import frc.robot.Constants.ShooterTowerConstants; import frc.robot.Robot; import org.littletonrobotics.junction.Logger; @@ -31,22 +28,36 @@ public class Shooter extends SubsystemBase { private final FlywheelMechanism _flywheel; private final FlywheelMechanism _feeder; + private final FlywheelMechanism _tower; private final RotaryMechanism _hood; // desired target values private double desiredVelo; private double hoodAngle; - public Shooter(FlywheelMechanism flywheel, FlywheelMechanism feeder, RotaryMechanism hood) { + public Shooter( + FlywheelMechanism flywheel, + FlywheelMechanism feeder, + FlywheelMechanism tower, + RotaryMechanism hood) { _flywheel = flywheel; _feeder = feeder; + _tower = tower; _hood = hood; } // Sets feeder motor speed public void runFeeder() { _feeder.runVelocity( - FeederConstants.FEED_SPEED, FeederConstants.FEED_ACCELERATION, PIDSlot.SLOT_2); + ShooterFeederConstants.MAX_VELOCITY, + ShooterFeederConstants.MAX_ACCELERATION, + PIDSlot.SLOT_2); + } + + // Sets tower motor speed + public void runTower() { + _feeder.runVelocity( + ShooterTowerConstants.MAX_VELOCITY, ShooterTowerConstants.MAX_ACCELERATION, PIDSlot.SLOT_2); } // Sets the flywheel velocity based on an input. @@ -58,20 +69,21 @@ public void setFlywheelVelocity(double velocity) { _flywheel.runVelocity(angVelo, ShooterConstants.ACCELERATION, PIDSlot.SLOT_0); } - public enum State { - OFF(Units.RevolutionsPerSecond.of(0.0)), - IDLE(Units.RevolutionsPerSecond.of(ShooterConstants.IDLE_SPEED_RPM / 60)), - SHOOT_FROM_HUB(Units.RevolutionsPerSecond.of(ShooterConstants.HUB_SPEED_RPM / 60)), - SHOOT_FROM_TOWER(Units.RevolutionsPerSecond.of(ShooterConstants.TOWER_SPEED_RPM / 60)), - SHOOT(Units.RevolutionsPerSecond.of(ShooterConstants.DEFAULT_SPEED_RPM / 60)), - SHOOT_ON_MOVE(Units.RevolutionsPerSecond.of(ShooterConstants.DEFAULT_SPEED_RPM / 60)); + // // Broken aha !! + // public enum State { + // OFF(Units.RevolutionsPerSecond.of(0.0)), + // IDLE(Units.RevolutionsPerSecond.of(ShooterConstants.IDLE_SPEED_RPM / 60)), + // SHOOT_FROM_HUB(Units.RevolutionsPerSecond.of(ShooterConstants.HUB_SPEED_RPM / 60)), + // SHOOT_FROM_TOWER(Units.RevolutionsPerSecond.of(ShooterConstants.TOWER_SPEED_RPM / 60)), + // SHOOT(Units.RevolutionsPerSecond.of(ShooterConstants.DEFAULT_SPEED_RPM / 60)), + // SHOOT_ON_MOVE(Units.RevolutionsPerSecond.of(ShooterConstants.DEFAULT_SPEED_RPM / 60)); - private final AngularVelocity stateVelocity; + // private final AngularVelocity stateVelocity; - State(AngularVelocity stateVelocity) { - this.stateVelocity = stateVelocity; - } - } + // State(AngularVelocity stateVelocity) { + // this.stateVelocity = stateVelocity; + // } + // } // Checks if the flywheel is at speed and returns a boolean public boolean flyAtVelocity() { @@ -126,36 +138,75 @@ public Command shoot(double velocity) { Commands.run(() -> setHoodAngle(hoodAngle)).until(this::hoodAtAngle)), // feed once ready Commands.runOnce(() -> runFeeder()), + Commands.runOnce(() -> runTower()), // stop flywheel when finished Commands.runOnce(() -> setFlywheelVelocity(0))); } - public void simShoot() - { + public void simShoot() { if (Robot.robotContainer.intake.simBalls <= 0) return; - double flywheelSpeed = 6; Translation2d robotPose2d = Robot.robotContainer.drive.getPose().getTranslation(); + Pose3d robotPose3d = + new Pose3d( + new Translation3d(robotPose2d.getX(), robotPose2d.getY(), 0), + new Rotation3d(robotPose2d.getAngle())); + Pose3d shooterPose3d = + new Pose3d( + new Translation3d(-0.0075, 0.0, 0.523), + new Rotation3d(0, _hood.getPosition().in(Radians), 0)); + + double flywheelSpeed = _flywheel.getVelocity().magnitude(); + double Yaw = Robot.robotContainer.drive.getPose().getRotation().getRadians(); - Pose3d robotPose3d = new Pose3d(new Translation3d(robotPose2d.getX(),robotPose2d.getY(),0), new Rotation3d(robotPose2d.getAngle())); - Pose3d shooterPose3d = new Pose3d(new Translation3d(-0.0075,0.0,0.523), new Rotation3d(0, _hood.getPosition().in(Radians), 0)); - - double V_xy = Math.sin(Math.PI/2-(_hood.getPosition().in(Radians) + Degrees.of(12).in(Radians)))*flywheelSpeed; + double V_xy = + Math.sin(Math.PI / 2 - (_hood.getPosition().in(Radians) + Degrees.of(12).in(Radians))) + * flywheelSpeed; + + // ChassisSpeeds driveChassisSpeeds = Robot.robotContainer.drive.getChassisSpeeds(); + // Translation3d driveSpeed3d = new Translation3d( + // 0.0, + // 0.0, + // 0.0 + // ); + + Robot.fuelSim.spawnFuel( + robotPose3d + .plus( + new Transform3d( + shooterPose3d.getX(), + shooterPose3d.getY(), + shooterPose3d.getZ(), + new Rotation3d(0, 0, 0))) + .getTranslation(), + new Translation3d( + V_xy * Math.cos(Yaw), + V_xy * Math.sin(Yaw), + Math.sin(Math.PI / 2 - (_hood.getPosition().in(Radians) + Degrees.of(12).in(Radians))) + * flywheelSpeed)); - Robot.fuelSim.spawnFuel(robotPose3d.plus(new Transform3d(shooterPose3d.getX(), shooterPose3d.getY(), shooterPose3d.getZ(), new Rotation3d(0, 0, 0))).getTranslation(), new Translation3d(V_xy*Math.cos(Yaw), V_xy*Math.sin(Yaw), Math.sin(Math.PI/2-(_hood.getPosition().in(Radians) + Degrees.of(12).in(Radians))) *flywheelSpeed)); Robot.robotContainer.intake.simBalls--; } public void periodic() { _hood.periodic(); - // _feeder.periodic(); - // _flywheel.periodic(); - - double pitch = Math.toRadians(Math.abs(Math.sin(Timer.getFPGATimestamp())*45)); // Placeholder for position - - // The pitch of the Rotation3D should be '_hood.getPosition().in(Radians)', change after fixing motor configs. - Logger.recordOutput("3DField/3_Hood", new Pose3d(new Translation3d(-0.0075,0.0,0.523), new Rotation3d(0, pitch, 0))); - - _hood.runVoltage(Volts.of(Math.sin(Timer.getFPGATimestamp()) * 0.25)); + _feeder.periodic(); + _tower.periodic(); + _flywheel.periodic(); + + Logger.recordOutput( + "3DField/3_Hood", + new Pose3d( + new Translation3d(-0.0075, 0.0, 0.523), + new Rotation3d(0, _hood.getPosition().in(Radians), 0))); + + // For testing purposes, raises the hood + // if (_hood.getPosition().in(Degrees) < ShooterRotaryConstants.MAX_ANGLE.in(Degrees) - 10) + // _hood.runVoltage(Volts.of(7)); + + // _hood.runVoltage(Volts.of(Math.sin(Timer.getFPGATimestamp()) * 5)); + // _feeder.runVoltage(Volts.of(5)); + // _tower.runVoltage(Volts.of(5)); + // _flywheel.runVoltage(Volts.of(5)); } } diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index fd86a44..7413bfa 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -1,79 +1,79 @@ -package frc.robot.subsystems.shooter; +// package frc.robot.subsystems.shooter; -import static edu.wpi.first.units.Units.RotationsPerSecond; +// import static edu.wpi.first.units.Units.RotationsPerSecond; -import edu.wpi.first.units.Units; -import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.lib.W8.io.motor.MotorIO.PIDSlot; -import frc.lib.W8.mechanisms.flywheel.FlywheelMechanism; -import frc.robot.Constants.FeederConstants; -import frc.robot.Constants.ShooterConstants; +// import edu.wpi.first.units.Units; +// import edu.wpi.first.units.measure.AngularVelocity; +// import edu.wpi.first.wpilibj2.command.Command; +// import edu.wpi.first.wpilibj2.command.Commands; +// import edu.wpi.first.wpilibj2.command.SubsystemBase; +// import frc.lib.W8.io.motor.MotorIO.PIDSlot; +// import frc.lib.W8.mechanisms.flywheel.FlywheelMechanism; +// import frc.robot.Constants.FeederConstants; +// import frc.robot.Constants.ShooterConstants; -public class Shooter extends SubsystemBase { +// public class Shooter extends SubsystemBase { - private FlywheelMechanism _flywheel; - private FlywheelMechanism _feeder; - public double desiredVelo; +// private FlywheelMechanism _flywheel; +// private FlywheelMechanism _feeder; +// public double desiredVelo; - public Shooter(FlywheelMechanism flywheel, FlywheelMechanism feeder) { - _flywheel = flywheel; - _feeder = feeder; - } +// public Shooter(FlywheelMechanism flywheel, FlywheelMechanism feeder) { +// _flywheel = flywheel; +// _feeder = feeder; +// } - // Sets feeder motor speed - public void runFeeder() { - _feeder.runVelocity( - FeederConstants.FEED_SPEED, FeederConstants.FEED_ACCELERATION, PIDSlot.SLOT_2); - } +// // Sets feeder motor speed +// public void runFeeder() { +// _feeder.runVelocity( +// FeederConstants.FEED_SPEED, FeederConstants.FEED_ACCELERATION, PIDSlot.SLOT_2); +// } - // Sets the flywheel velocity based on an input. - public void setFlywheelVelocity(double velocity) { - AngularVelocity angVelo = RotationsPerSecond.of(velocity); - velocity = desiredVelo; +// // Sets the flywheel velocity based on an input. +// public void setFlywheelVelocity(double velocity) { +// AngularVelocity angVelo = RotationsPerSecond.of(velocity); +// velocity = desiredVelo; - _flywheel.runVelocity(angVelo, ShooterConstants.ACCELERATION, PIDSlot.SLOT_0); - } +// _flywheel.runVelocity(angVelo, ShooterConstants.ACCELERATION, PIDSlot.SLOT_0); +// } - public enum State { - OFF(Units.RevolutionsPerSecond.of(0.0)), - IDLE(Units.RevolutionsPerSecond.of(ShooterConstants.IDLE_SPEED_RPM / 60)), - SHOOT_FROM_HUB(Units.RevolutionsPerSecond.of(ShooterConstants.HUB_SPEED_RPM / 60)), - SHOOT_FROM_TOWER(Units.RevolutionsPerSecond.of(ShooterConstants.TOWER_SPEED_RPM / 60)), - SHOOT(Units.RevolutionsPerSecond.of(ShooterConstants.DEFAULT_SPEED_RPM / 60)), - SHOOT_ON_MOVE(Units.RevolutionsPerSecond.of(ShooterConstants.DEFAULT_SPEED_RPM / 60)); +// public enum State { +// OFF(Units.RevolutionsPerSecond.of(0.0)), +// IDLE(Units.RevolutionsPerSecond.of(ShooterConstants.IDLE_SPEED_RPM / 60)), +// SHOOT_FROM_HUB(Units.RevolutionsPerSecond.of(ShooterConstants.HUB_SPEED_RPM / 60)), +// SHOOT_FROM_TOWER(Units.RevolutionsPerSecond.of(ShooterConstants.TOWER_SPEED_RPM / 60)), +// SHOOT(Units.RevolutionsPerSecond.of(ShooterConstants.DEFAULT_SPEED_RPM / 60)), +// SHOOT_ON_MOVE(Units.RevolutionsPerSecond.of(ShooterConstants.DEFAULT_SPEED_RPM / 60)); - private final AngularVelocity stateVelocity; +// private final AngularVelocity stateVelocity; - State(AngularVelocity stateVelocity) { - this.stateVelocity = stateVelocity; - } - } +// State(AngularVelocity stateVelocity) { +// this.stateVelocity = stateVelocity; +// } +// } - // Checks if the flywheel is at speed and returns a boolean - public boolean flyAtVelocity() { - return Math.abs(desiredVelo - _flywheel.getVelocity().in(RotationsPerSecond)) - <= ShooterConstants.FLYWHEEL_VELOCITY_TOLERANCE; - } +// // Checks if the flywheel is at speed and returns a boolean +// public boolean flyAtVelocity() { +// return Math.abs(desiredVelo - _flywheel.getVelocity().in(RotationsPerSecond)) +// <= ShooterConstants.FLYWHEEL_VELOCITY_TOLERANCE; +// } - public Command shoot(double velocity) { - return Commands.run( - () -> { - setFlywheelVelocity(velocity); - }) - .until(() -> flyAtVelocity()) - .andThen( - () -> { - runFeeder(); - }) - .andThen( - () -> { - setFlywheelVelocity(0); - }); - } +// public Command shoot(double velocity) { +// return Commands.run( +// () -> { +// setFlywheelVelocity(velocity); +// }) +// .until(() -> flyAtVelocity()) +// .andThen( +// () -> { +// runFeeder(); +// }) +// .andThen( +// () -> { +// setFlywheelVelocity(0); +// }); +// } - @Override - public void periodic() {} -} +// @Override +// public void periodic() {} +// } From 21f2c0ecec2baef707cb0249a24a587622275fe0 Mon Sep 17 00:00:00 2001 From: Ghost <145223182+GhostR406@users.noreply.github.com> Date: Thu, 5 Mar 2026 18:51:29 -0500 Subject: [PATCH 2/8] please no merge conflicts i beg --- src/main/java/frc/robot/Robot.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 112672b..cfd0191 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -40,7 +40,7 @@ * The VM is configured to automatically run this class, and to call the functions corresponding to * each mode, as described in the TimedRobot documentation. If you change the name of this class or * the package after creating this project, you must also update the build.gradle file in the - * project. + * project. */ public class Robot extends LoggedRobot { private Command autonomousCommand; From ca20c0d2e4c6c0fb574d86b34c519fdba7c785d2 Mon Sep 17 00:00:00 2001 From: Ghost <145223182+GhostR406@users.noreply.github.com> Date: Thu, 5 Mar 2026 19:22:39 -0500 Subject: [PATCH 3/8] great value unicorn sparkle ice cream --- src/main/java/frc/robot/Robot.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 6 +----- src/main/java/frc/robot/subsystems/Climber.java | 3 ++- src/main/java/frc/robot/subsystems/Intake.java | 13 ++++++------- src/main/java/frc/robot/subsystems/Shooter.java | 3 ++- 5 files changed, 12 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index cfd0191..112672b 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -40,7 +40,7 @@ * The VM is configured to automatically run this class, and to call the functions corresponding to * each mode, as described in the TimedRobot documentation. If you change the name of this class or * the package after creating this project, you must also update the build.gradle file in the - * project. + * project. */ public class Robot extends LoggedRobot { private Command autonomousCommand; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b7f111e..26f864a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -17,7 +17,6 @@ import com.pathplanner.lib.auto.AutoBuilder; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.units.AngleUnit; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; @@ -59,9 +58,6 @@ import frc.robot.subsystems.drive.ModuleIO; import frc.robot.subsystems.drive.ModuleIOSim; import frc.robot.subsystems.drive.ModuleIOTalonFX; - -import static edu.wpi.first.units.Units.Degrees; - import java.util.Optional; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; import org.photonvision.PhotonPoseEstimator.PoseStrategy; @@ -378,7 +374,7 @@ private void configureButtonBindings() { .x() .onTrue(Commands.runOnce(() -> hopper.setGoal(HopperConstants.HOPPER_POSITION), hopper)); - // Commands for sim testing! + // Commands for sim testing! // controller.rightTrigger().onTrue(Commands.runOnce(() -> shooter.simShoot())); // controller diff --git a/src/main/java/frc/robot/subsystems/Climber.java b/src/main/java/frc/robot/subsystems/Climber.java index ebca328..7354a3a 100644 --- a/src/main/java/frc/robot/subsystems/Climber.java +++ b/src/main/java/frc/robot/subsystems/Climber.java @@ -74,6 +74,7 @@ public void periodic() { 0, 0, ClimberConstants.CONVERTER.toDistance(_io.getPosition()).in(Meters)), new Rotation3d(0, 0, 0))); - // _io.runVoltage(Volts.of(Math.sin(Timer.getFPGATimestamp()) * 0.25)); // For testing climber motion! + // _io.runVoltage(Volts.of(Math.sin(Timer.getFPGATimestamp()) * 0.25)); // For testing climber + // motion! } } diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 8abfe0e..1961f6c 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -44,11 +44,11 @@ public void setVelocity(double velocity) { public void setPivotAngle(Angle pivotAngle) { _pivotIO.runPosition( - pivotAngle, - IntakeFlywheelConstants.CRUISE_VELOCITY, - IntakeFlywheelConstants.ACCELERATION, - IntakeFlywheelConstants.JERK, - PIDSlot.SLOT_0); + pivotAngle, + IntakeFlywheelConstants.CRUISE_VELOCITY, + IntakeFlywheelConstants.ACCELERATION, + IntakeFlywheelConstants.JERK, + PIDSlot.SLOT_0); } public AngularVelocity getVelocity() { @@ -66,8 +66,7 @@ public void stop() { public Command intake() { return Commands.sequence( Commands.runOnce(() -> this.setPivotAngle(IntakePivotConstants.PICKUP_ANGLE)), - Commands.runOnce(() -> this.setVelocity(IntakeFlywheelConstants.PICKUP_SPEED)) - ); + Commands.runOnce(() -> this.setVelocity(IntakeFlywheelConstants.PICKUP_SPEED))); } public boolean isIntendedAngle() { diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 8bd7025..0d8931a 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -165,7 +165,8 @@ public void simShoot() { new Translation3d(-0.0075, 0.0, 0.523), new Rotation3d(0, _hood.getPosition().in(Radians), 0)); - double flywheelSpeed = _lflywheel.getVelocity().magnitude() + _rflywheel.getVelocity().magnitude(); + double flywheelSpeed = + _lflywheel.getVelocity().magnitude() + _rflywheel.getVelocity().magnitude(); double Yaw = Robot.robotContainer.drive.getPose().getRotation().getRadians(); double V_xy = From cc233918bcff7884e6a5e46b560fb42ab482c279 Mon Sep 17 00:00:00 2001 From: Ghost <145223182+GhostR406@users.noreply.github.com> Date: Sat, 7 Mar 2026 15:55:52 -0500 Subject: [PATCH 4/8] sim done part 46^(12534221) --- src/main/java/frc/robot/Constants.java | 15 ++------ src/main/java/frc/robot/RobotContainer.java | 34 +++++++++---------- .../java/frc/robot/subsystems/Climber.java | 2 ++ .../java/frc/robot/subsystems/Intake.java | 3 ++ .../java/frc/robot/subsystems/Shooter.java | 26 +++++++++----- 5 files changed, 42 insertions(+), 38 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 406dbf6..ca1ba8f 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -14,16 +14,6 @@ package frc.robot; import static edu.wpi.first.units.Units.*; -import static edu.wpi.first.units.Units.Inches; -import static edu.wpi.first.units.Units.KilogramSquareMeters; -import static edu.wpi.first.units.Units.Kilograms; -import static edu.wpi.first.units.Units.Meters; -import static edu.wpi.first.units.Units.RadiansPerSecond; -import static edu.wpi.first.units.Units.Rotations; -import static edu.wpi.first.units.Units.RotationsPerSecond; -import static edu.wpi.first.units.Units.RotationsPerSecondPerSecond; -import static edu.wpi.first.units.Units.Second; -import static edu.wpi.first.units.Units.Volts; import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.CANdleConfiguration; @@ -356,7 +346,7 @@ public class ShooterTowerConstants { // Velocity PID private static Slot0Configs SLOT0CONFIG = - new Slot0Configs().withKP(1000.0).withKI(0.0).withKD(0.0); + new Slot0Configs().withKP(0.0).withKI(0.0).withKD(0.0); public static TalonFXConfiguration getFXConfig() { TalonFXConfiguration config = new TalonFXConfiguration(); @@ -390,7 +380,8 @@ public static TalonFXConfiguration getFXConfig() { } public class ShooterFlywheelConstants { - public static String NAME = "ShooterFlywheel"; + public static String NAME_L = "ShooterFlywheelLeft"; + public static String NAME_R = "ShooterFlywheelRight"; public static final AngularVelocity MAX_VELOCITY = RadiansPerSecond.of(2 * Math.PI); public static final AngularAcceleration MAX_ACCELERATION = MAX_VELOCITY.per(Second); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 26f864a..4ed57d2 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -112,12 +112,12 @@ public RobotContainer() { new Shooter( new FlywheelMechanismReal( new MotorIOTalonFX( - ShooterFlywheelConstants.NAME, + ShooterFlywheelConstants.NAME_L, ShooterFlywheelConstants.getFXConfig(), Ports.LeftFlywheel)), new FlywheelMechanismReal( new MotorIOTalonFX( - ShooterFlywheelConstants.NAME, + ShooterFlywheelConstants.NAME_R, ShooterFlywheelConstants.getFXConfig(), Ports.RightFlywheel)), new FlywheelMechanismReal( @@ -192,7 +192,7 @@ public RobotContainer() { new Shooter( new FlywheelMechanismSim( new MotorIOTalonFXSim( - ShooterFlywheelConstants.NAME, + ShooterFlywheelConstants.NAME_L, ShooterFlywheelConstants.getFXConfig(), Ports.LeftFlywheel), ShooterFlywheelConstants.DCMOTOR, @@ -200,7 +200,7 @@ public RobotContainer() { ShooterFlywheelConstants.TOLERANCE), new FlywheelMechanismSim( new MotorIOTalonFXSim( - ShooterFlywheelConstants.NAME, + ShooterFlywheelConstants.NAME_R, ShooterFlywheelConstants.getFXConfig(), Ports.RightFlywheel), ShooterFlywheelConstants.DCMOTOR, @@ -375,21 +375,21 @@ private void configureButtonBindings() { .onTrue(Commands.runOnce(() -> hopper.setGoal(HopperConstants.HOPPER_POSITION), hopper)); // Commands for sim testing! - // controller.rightTrigger().onTrue(Commands.runOnce(() -> shooter.simShoot())); + controller.rightTrigger().onTrue(Commands.runOnce(() -> shooter.simShoot())); - // controller - // .leftTrigger() - // .onTrue( - // Commands.runOnce( - // () -> { - // Robot.fuelSim.clearFuel(); - // Robot.fuelSim.spawnStartingFuel(); - // intake.simBalls = 0; - // })); + controller + .leftTrigger() + .onTrue( + Commands.runOnce( + () -> { + Robot.fuelSim.clearFuel(); + Robot.fuelSim.spawnStartingFuel(); + intake.simBalls = 0; + })); - controller.leftBumper().onTrue(intake.intake()); - controller.rightBumper().onTrue(intake.stowAndStopRollers()); - controller.a().onTrue(shooter.runFlywheel()); + // controller.leftBumper().onTrue(intake.intake()); + // controller.rightBumper().onTrue(intake.stowAndStopRollers()); + // controller.a().onTrue(shooter.runFlywheel()); } /** diff --git a/src/main/java/frc/robot/subsystems/Climber.java b/src/main/java/frc/robot/subsystems/Climber.java index 7354a3a..e151de5 100644 --- a/src/main/java/frc/robot/subsystems/Climber.java +++ b/src/main/java/frc/robot/subsystems/Climber.java @@ -2,11 +2,13 @@ import static edu.wpi.first.units.Units.Amps; import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.Volts; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.lib.W8.io.motor.MotorIO.PIDSlot; diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 1961f6c..b4f3e73 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -1,14 +1,17 @@ package frc.robot.subsystems; import static edu.wpi.first.units.Units.Degree; +import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.Radians; import static edu.wpi.first.units.Units.RotationsPerSecond; +import static edu.wpi.first.units.Units.Volts; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 0d8931a..73b00e0 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -3,14 +3,18 @@ import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.Radians; import static edu.wpi.first.units.Units.RotationsPerSecond; +import static edu.wpi.first.units.Units.RotationsPerSecondPerSecond; +import static edu.wpi.first.units.Units.Volts; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -20,7 +24,9 @@ import frc.robot.Constants.FieldConstants; import frc.robot.Constants.ShooterConstants; import frc.robot.Constants.ShooterFeederConstants; +import frc.robot.Constants.ShooterRotaryConstants; import frc.robot.Constants.ShooterTowerConstants; +import frc.robot.generated.TunerConstants; import frc.robot.Robot; import org.littletonrobotics.junction.Logger; @@ -173,12 +179,12 @@ public void simShoot() { Math.sin(Math.PI / 2 - (_hood.getPosition().in(Radians) + Degrees.of(12).in(Radians))) * flywheelSpeed; - // ChassisSpeeds driveChassisSpeeds = Robot.robotContainer.drive.getChassisSpeeds(); - // Translation3d driveSpeed3d = new Translation3d( - // 0.0, - // 0.0, - // 0.0 - // ); + ChassisSpeeds driveChassisSpeeds = Robot.robotContainer.drive.getChassisSpeeds(); + Translation3d driveSpeed3d = new Translation3d( + 0.0, + 0.0, + 0.0 + ); Robot.fuelSim.spawnFuel( robotPose3d @@ -193,7 +199,8 @@ public void simShoot() { V_xy * Math.cos(Yaw), V_xy * Math.sin(Yaw), Math.sin(Math.PI / 2 - (_hood.getPosition().in(Radians) + Degrees.of(12).in(Radians))) - * flywheelSpeed)); + * flywheelSpeed) + .plus(driveSpeed3d)); Robot.robotContainer.intake.simBalls--; } @@ -213,11 +220,12 @@ public void periodic() { // For testing purposes, raises the hood // if (_hood.getPosition().in(Degrees) < ShooterRotaryConstants.MAX_ANGLE.in(Degrees) - 10) - // _hood.runVoltage(Volts.of(7)); + // _hood.runVelocity(RotationsPerSecond.of(1), RotationsPerSecondPerSecond.of(0.01), PIDSlot.SLOT_0); // _hood.runVoltage(Volts.of(Math.sin(Timer.getFPGATimestamp()) * 5)); // _feeder.runVoltage(Volts.of(5)); // _tower.runVoltage(Volts.of(5)); - // _flywheel.runVoltage(Volts.of(5)); + // _lflywheel.runVoltage(Volts.of(3)); + // _rflywheel.runVoltage(Volts.of(3)); } } From 819fa84587e2e8b0aaaf023846efd5e90d47a62e Mon Sep 17 00:00:00 2001 From: Ghost <145223182+GhostR406@users.noreply.github.com> Date: Sat, 7 Mar 2026 15:57:06 -0500 Subject: [PATCH 5/8] i forgot spotless --- .../java/frc/robot/subsystems/Climber.java | 2 -- .../java/frc/robot/subsystems/Intake.java | 3 --- .../java/frc/robot/subsystems/Shooter.java | 26 +++++++------------ 3 files changed, 10 insertions(+), 21 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Climber.java b/src/main/java/frc/robot/subsystems/Climber.java index e151de5..7354a3a 100644 --- a/src/main/java/frc/robot/subsystems/Climber.java +++ b/src/main/java/frc/robot/subsystems/Climber.java @@ -2,13 +2,11 @@ import static edu.wpi.first.units.Units.Amps; import static edu.wpi.first.units.Units.Meters; -import static edu.wpi.first.units.Units.Volts; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.units.measure.Distance; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.lib.W8.io.motor.MotorIO.PIDSlot; diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index b4f3e73..1961f6c 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -1,17 +1,14 @@ package frc.robot.subsystems; import static edu.wpi.first.units.Units.Degree; -import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.Radians; import static edu.wpi.first.units.Units.RotationsPerSecond; -import static edu.wpi.first.units.Units.Volts; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 73b00e0..eb85b4e 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -3,8 +3,6 @@ import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.Radians; import static edu.wpi.first.units.Units.RotationsPerSecond; -import static edu.wpi.first.units.Units.RotationsPerSecondPerSecond; -import static edu.wpi.first.units.Units.Volts; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; @@ -14,7 +12,6 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -24,9 +21,7 @@ import frc.robot.Constants.FieldConstants; import frc.robot.Constants.ShooterConstants; import frc.robot.Constants.ShooterFeederConstants; -import frc.robot.Constants.ShooterRotaryConstants; import frc.robot.Constants.ShooterTowerConstants; -import frc.robot.generated.TunerConstants; import frc.robot.Robot; import org.littletonrobotics.junction.Logger; @@ -180,11 +175,7 @@ public void simShoot() { * flywheelSpeed; ChassisSpeeds driveChassisSpeeds = Robot.robotContainer.drive.getChassisSpeeds(); - Translation3d driveSpeed3d = new Translation3d( - 0.0, - 0.0, - 0.0 - ); + Translation3d driveSpeed3d = new Translation3d(0.0, 0.0, 0.0); Robot.fuelSim.spawnFuel( robotPose3d @@ -196,11 +187,13 @@ public void simShoot() { new Rotation3d(0, 0, 0))) .getTranslation(), new Translation3d( - V_xy * Math.cos(Yaw), - V_xy * Math.sin(Yaw), - Math.sin(Math.PI / 2 - (_hood.getPosition().in(Radians) + Degrees.of(12).in(Radians))) - * flywheelSpeed) - .plus(driveSpeed3d)); + V_xy * Math.cos(Yaw), + V_xy * Math.sin(Yaw), + Math.sin( + Math.PI / 2 + - (_hood.getPosition().in(Radians) + Degrees.of(12).in(Radians))) + * flywheelSpeed) + .plus(driveSpeed3d)); Robot.robotContainer.intake.simBalls--; } @@ -220,7 +213,8 @@ public void periodic() { // For testing purposes, raises the hood // if (_hood.getPosition().in(Degrees) < ShooterRotaryConstants.MAX_ANGLE.in(Degrees) - 10) - // _hood.runVelocity(RotationsPerSecond.of(1), RotationsPerSecondPerSecond.of(0.01), PIDSlot.SLOT_0); + // _hood.runVelocity(RotationsPerSecond.of(1), RotationsPerSecondPerSecond.of(0.01), + // PIDSlot.SLOT_0); // _hood.runVoltage(Volts.of(Math.sin(Timer.getFPGATimestamp()) * 5)); // _feeder.runVoltage(Volts.of(5)); From 4854745a91ba477b0cb7437387250108644e22e6 Mon Sep 17 00:00:00 2001 From: rhit-collinkn Date: Sat, 14 Mar 2026 22:13:00 -0400 Subject: [PATCH 6/8] Revert "Merge branch 'main' into Robot-Sim" This reverts commit cd38c48a8b34ed143e91707777e77f5a8f9a8d54, reversing changes made to 819fa84587e2e8b0aaaf023846efd5e90d47a62e. reverting merge --- .../pathplanner/autos/Example Auto.auto | 19 ++ .../deploy/pathplanner/autos/JustClimb1.auto | 31 --- .../deploy/pathplanner/autos/JustClimb2.auto | 31 --- .../autos/LB.Sweep.S1.M2.Climb.auto | 88 -------- .../deploy/pathplanner/autos/S3.M4.Climb.auto | 108 ---------- .../pathplanner/autos/UB.Sweep.S3.Climb.auto | 50 ----- .../pathplanner/paths/Depot-Climb1.path | 59 ------ .../pathplanner/paths/Depot-Shoot1.path | 54 ----- .../pathplanner/paths/Depot-Shoot2.path | 54 ----- .../pathplanner/paths/Depot-Shoot3.path | 54 ----- .../{Shoot2-Main1.path => Example Path.path} | 38 ++-- .../deploy/pathplanner/paths/JustClimb1.path | 54 ----- .../deploy/pathplanner/paths/JustClimb2.path | 54 ----- .../deploy/pathplanner/paths/LBump-Climb.path | 59 ------ .../deploy/pathplanner/paths/LBump-Main3.path | 66 ------ .../deploy/pathplanner/paths/LBump-Main4.path | 61 ------ .../pathplanner/paths/LBump-Shoot2.path | 54 ----- .../pathplanner/paths/LBump-Shoot3.path | 54 ----- .../paths/LBump-SweepUp-Shoot1.path | 150 -------------- .../pathplanner/paths/LBump-SweepUp.path | 59 ------ .../pathplanner/paths/LTrench-Climb2.path | 54 ----- .../pathplanner/paths/LTrench-Shoot3.path | 54 ----- .../pathplanner/paths/Main1-Climb1.path | 75 ------- .../pathplanner/paths/Main1-Shoot1.path | 59 ------ .../pathplanner/paths/Main1-Shoot2.path | 54 ----- .../pathplanner/paths/Main2-Climb1.path | 75 ------- .../pathplanner/paths/Main2-Shoot1.path | 59 ------ .../pathplanner/paths/Main2-Shoot2.path | 54 ----- .../pathplanner/paths/Main3-Climb2.path | 75 ------- .../pathplanner/paths/Main3-Shoot2.path | 54 ----- .../pathplanner/paths/Main3-Shoot3.path | 59 ------ .../pathplanner/paths/Main4-Climb2.path | 75 ------- .../pathplanner/paths/Main4-Shoot2.path | 54 ----- .../pathplanner/paths/Main4-Shoot3.path | 59 ------ .../pathplanner/paths/Shoot1-Center.path | 59 ------ .../pathplanner/paths/Shoot1-Climb1.path | 59 ------ .../pathplanner/paths/Shoot1-Depot.path | 66 ------ .../pathplanner/paths/Shoot1-Main1.path | 54 ----- .../pathplanner/paths/Shoot1-Main2.path | 54 ----- .../pathplanner/paths/Shoot1-SweepDown.path | 75 ------- .../pathplanner/paths/Shoot2-Climb1.path | 59 ------ .../pathplanner/paths/Shoot2-Depot.path | 66 ------ .../pathplanner/paths/Shoot2-Main2.path | 70 ------- .../pathplanner/paths/Shoot2-Main3.path | 70 ------- .../pathplanner/paths/Shoot2-Main4.path | 70 ------- .../pathplanner/paths/Shoot2-SweepDown.path | 91 -------- .../pathplanner/paths/Shoot2-SweepUp.path | 91 -------- .../pathplanner/paths/Shoot3-Climb2.path | 75 ------- .../pathplanner/paths/Shoot3-Depot.path | 66 ------ .../pathplanner/paths/Shoot3-Main3.path | 59 ------ .../pathplanner/paths/Shoot3-Main4.path | 59 ------ .../paths/Shoot3-SweepUp-Shoot1.path | 160 -------------- .../pathplanner/paths/Shoot3-SweepUp.path | 75 ------- .../pathplanner/paths/SweepDown-Climb2.path | 75 ------- .../pathplanner/paths/SweepDown-LBump.path | 59 ------ .../pathplanner/paths/SweepDown-Shoot2.path | 75 ------- .../pathplanner/paths/SweepDown-Shoot3.path | 75 ------- .../deploy/pathplanner/paths/SweepDown.path | 54 ----- .../pathplanner/paths/SweepUp-Climb.path | 59 ------ .../pathplanner/paths/SweepUp-Shoot1.path | 59 ------ .../pathplanner/paths/SweepUp-Shoot2.path | 70 ------- .../pathplanner/paths/SweepUp-UBump.path | 59 ------ .../deploy/pathplanner/paths/SweepUp.path | 54 ----- .../deploy/pathplanner/paths/UBump-Climb.path | 59 ------ .../deploy/pathplanner/paths/UBump-Main1.path | 61 ------ .../deploy/pathplanner/paths/UBump-Main2.path | 61 ------ .../pathplanner/paths/UBump-Shoot1.path | 54 ----- .../pathplanner/paths/UBump-Shoot2.path | 54 ----- .../paths/UBump-SweepDown-Shoot3.path | 160 -------------- .../pathplanner/paths/UBump-SweepDown.path | 59 ------ .../pathplanner/paths/UTrench-Climb1.path | 54 ----- .../pathplanner/paths/UTrench-Shoot1.path | 54 ----- src/main/deploy/pathplanner/settings.json | 39 +--- .../AbsoluteEncoderIOCANCoder.java | 8 +- .../DistanceSensorIOCANRange.java | 7 - .../frc/lib/W8/io/motor/MotorIOTalonFX.java | 103 ++++----- .../rotary/RotaryMechanismReal.java | 5 - src/main/java/frc/robot/Constants.java | 196 +++++------------- src/main/java/frc/robot/RobotContainer.java | 154 ++++---------- .../frc/robot/generated/TunerConstants.java | 43 ++-- .../java/frc/robot/subsystems/Climber.java | 68 +----- .../java/frc/robot/subsystems/Hopper.java | 24 +-- .../java/frc/robot/subsystems/Intake.java | 34 +-- .../java/frc/robot/subsystems/Shooter.java | 101 ++------- .../subsystems/drive/ModuleIOTalonFX.java | 38 ++-- vendordeps/AdvantageKit.json | 6 +- ...enix6-26.1.1.json => Phoenix6-26.1.0.json} | 62 +++--- vendordeps/REVLib.json | 18 +- vendordeps/photonlib.json | 12 +- 89 files changed, 281 insertions(+), 5327 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/Example Auto.auto delete mode 100644 src/main/deploy/pathplanner/autos/JustClimb1.auto delete mode 100644 src/main/deploy/pathplanner/autos/JustClimb2.auto delete mode 100644 src/main/deploy/pathplanner/autos/LB.Sweep.S1.M2.Climb.auto delete mode 100644 src/main/deploy/pathplanner/autos/S3.M4.Climb.auto delete mode 100644 src/main/deploy/pathplanner/autos/UB.Sweep.S3.Climb.auto delete mode 100644 src/main/deploy/pathplanner/paths/Depot-Climb1.path delete mode 100644 src/main/deploy/pathplanner/paths/Depot-Shoot1.path delete mode 100644 src/main/deploy/pathplanner/paths/Depot-Shoot2.path delete mode 100644 src/main/deploy/pathplanner/paths/Depot-Shoot3.path rename src/main/deploy/pathplanner/paths/{Shoot2-Main1.path => Example Path.path} (60%) delete mode 100644 src/main/deploy/pathplanner/paths/JustClimb1.path delete mode 100644 src/main/deploy/pathplanner/paths/JustClimb2.path delete mode 100644 src/main/deploy/pathplanner/paths/LBump-Climb.path delete mode 100644 src/main/deploy/pathplanner/paths/LBump-Main3.path delete mode 100644 src/main/deploy/pathplanner/paths/LBump-Main4.path delete mode 100644 src/main/deploy/pathplanner/paths/LBump-Shoot2.path delete mode 100644 src/main/deploy/pathplanner/paths/LBump-Shoot3.path delete mode 100644 src/main/deploy/pathplanner/paths/LBump-SweepUp-Shoot1.path delete mode 100644 src/main/deploy/pathplanner/paths/LBump-SweepUp.path delete mode 100644 src/main/deploy/pathplanner/paths/LTrench-Climb2.path delete mode 100644 src/main/deploy/pathplanner/paths/LTrench-Shoot3.path delete mode 100644 src/main/deploy/pathplanner/paths/Main1-Climb1.path delete mode 100644 src/main/deploy/pathplanner/paths/Main1-Shoot1.path delete mode 100644 src/main/deploy/pathplanner/paths/Main1-Shoot2.path delete mode 100644 src/main/deploy/pathplanner/paths/Main2-Climb1.path delete mode 100644 src/main/deploy/pathplanner/paths/Main2-Shoot1.path delete mode 100644 src/main/deploy/pathplanner/paths/Main2-Shoot2.path delete mode 100644 src/main/deploy/pathplanner/paths/Main3-Climb2.path delete mode 100644 src/main/deploy/pathplanner/paths/Main3-Shoot2.path delete mode 100644 src/main/deploy/pathplanner/paths/Main3-Shoot3.path delete mode 100644 src/main/deploy/pathplanner/paths/Main4-Climb2.path delete mode 100644 src/main/deploy/pathplanner/paths/Main4-Shoot2.path delete mode 100644 src/main/deploy/pathplanner/paths/Main4-Shoot3.path delete mode 100644 src/main/deploy/pathplanner/paths/Shoot1-Center.path delete mode 100644 src/main/deploy/pathplanner/paths/Shoot1-Climb1.path delete mode 100644 src/main/deploy/pathplanner/paths/Shoot1-Depot.path delete mode 100644 src/main/deploy/pathplanner/paths/Shoot1-Main1.path delete mode 100644 src/main/deploy/pathplanner/paths/Shoot1-Main2.path delete mode 100644 src/main/deploy/pathplanner/paths/Shoot1-SweepDown.path delete mode 100644 src/main/deploy/pathplanner/paths/Shoot2-Climb1.path delete mode 100644 src/main/deploy/pathplanner/paths/Shoot2-Depot.path delete mode 100644 src/main/deploy/pathplanner/paths/Shoot2-Main2.path delete mode 100644 src/main/deploy/pathplanner/paths/Shoot2-Main3.path delete mode 100644 src/main/deploy/pathplanner/paths/Shoot2-Main4.path delete mode 100644 src/main/deploy/pathplanner/paths/Shoot2-SweepDown.path delete mode 100644 src/main/deploy/pathplanner/paths/Shoot2-SweepUp.path delete mode 100644 src/main/deploy/pathplanner/paths/Shoot3-Climb2.path delete mode 100644 src/main/deploy/pathplanner/paths/Shoot3-Depot.path delete mode 100644 src/main/deploy/pathplanner/paths/Shoot3-Main3.path delete mode 100644 src/main/deploy/pathplanner/paths/Shoot3-Main4.path delete mode 100644 src/main/deploy/pathplanner/paths/Shoot3-SweepUp-Shoot1.path delete mode 100644 src/main/deploy/pathplanner/paths/Shoot3-SweepUp.path delete mode 100644 src/main/deploy/pathplanner/paths/SweepDown-Climb2.path delete mode 100644 src/main/deploy/pathplanner/paths/SweepDown-LBump.path delete mode 100644 src/main/deploy/pathplanner/paths/SweepDown-Shoot2.path delete mode 100644 src/main/deploy/pathplanner/paths/SweepDown-Shoot3.path delete mode 100644 src/main/deploy/pathplanner/paths/SweepDown.path delete mode 100644 src/main/deploy/pathplanner/paths/SweepUp-Climb.path delete mode 100644 src/main/deploy/pathplanner/paths/SweepUp-Shoot1.path delete mode 100644 src/main/deploy/pathplanner/paths/SweepUp-Shoot2.path delete mode 100644 src/main/deploy/pathplanner/paths/SweepUp-UBump.path delete mode 100644 src/main/deploy/pathplanner/paths/SweepUp.path delete mode 100644 src/main/deploy/pathplanner/paths/UBump-Climb.path delete mode 100644 src/main/deploy/pathplanner/paths/UBump-Main1.path delete mode 100644 src/main/deploy/pathplanner/paths/UBump-Main2.path delete mode 100644 src/main/deploy/pathplanner/paths/UBump-Shoot1.path delete mode 100644 src/main/deploy/pathplanner/paths/UBump-Shoot2.path delete mode 100644 src/main/deploy/pathplanner/paths/UBump-SweepDown-Shoot3.path delete mode 100644 src/main/deploy/pathplanner/paths/UBump-SweepDown.path delete mode 100644 src/main/deploy/pathplanner/paths/UTrench-Climb1.path delete mode 100644 src/main/deploy/pathplanner/paths/UTrench-Shoot1.path rename vendordeps/{Phoenix6-26.1.1.json => Phoenix6-26.1.0.json} (92%) diff --git a/src/main/deploy/pathplanner/autos/Example Auto.auto b/src/main/deploy/pathplanner/autos/Example Auto.auto new file mode 100644 index 0000000..70b7ab2 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Example Auto.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Example Path" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/JustClimb1.auto b/src/main/deploy/pathplanner/autos/JustClimb1.auto deleted file mode 100644 index ab0d5a6..0000000 --- a/src/main/deploy/pathplanner/autos/JustClimb1.auto +++ /dev/null @@ -1,31 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "ExtendClimber" - } - }, - { - "type": "path", - "data": { - "pathName": "JustClimb1" - } - }, - { - "type": "named", - "data": { - "name": "Climb" - } - } - ] - } - }, - "resetOdom": true, - "folder": "Only Climb", - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/JustClimb2.auto b/src/main/deploy/pathplanner/autos/JustClimb2.auto deleted file mode 100644 index c15137c..0000000 --- a/src/main/deploy/pathplanner/autos/JustClimb2.auto +++ /dev/null @@ -1,31 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "ExtendClimber" - } - }, - { - "type": "path", - "data": { - "pathName": "JustClimb2" - } - }, - { - "type": "named", - "data": { - "name": "Climb" - } - } - ] - } - }, - "resetOdom": true, - "folder": "Only Climb", - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/LB.Sweep.S1.M2.Climb.auto b/src/main/deploy/pathplanner/autos/LB.Sweep.S1.M2.Climb.auto deleted file mode 100644 index 4a8b995..0000000 --- a/src/main/deploy/pathplanner/autos/LB.Sweep.S1.M2.Climb.auto +++ /dev/null @@ -1,88 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Shoot3-SweepUp-Shoot1" - } - }, - { - "type": "named", - "data": { - "name": "Shoot" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Shoot1-Main2" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 1.5 - } - }, - { - "type": "named", - "data": { - "name": "IntakeOn" - } - } - ] - } - } - ] - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "IntakeOff" - } - }, - { - "type": "path", - "data": { - "pathName": "Main2-Climb1" - } - }, - { - "type": "named", - "data": { - "name": "ExtendClimber" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "Climb" - } - } - ] - } - }, - "resetOdom": true, - "folder": "Sweep First", - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/S3.M4.Climb.auto b/src/main/deploy/pathplanner/autos/S3.M4.Climb.auto deleted file mode 100644 index 18b9d6b..0000000 --- a/src/main/deploy/pathplanner/autos/S3.M4.Climb.auto +++ /dev/null @@ -1,108 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "LBump-Shoot3" - } - }, - { - "type": "named", - "data": { - "name": "ReadyUp" - } - }, - { - "type": "named", - "data": { - "name": "IntakeDown" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "Shoot" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Shoot3-Main4" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 1.5 - } - }, - { - "type": "named", - "data": { - "name": "IntakeOn" - } - } - ] - } - } - ] - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Main4-Climb2" - } - }, - { - "type": "named", - "data": { - "name": "ExtendClimber" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "Climb" - } - } - ] - } - } - ] - } - }, - "resetOdom": true, - "folder": "Shoot First", - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/UB.Sweep.S3.Climb.auto b/src/main/deploy/pathplanner/autos/UB.Sweep.S3.Climb.auto deleted file mode 100644 index d1b130b..0000000 --- a/src/main/deploy/pathplanner/autos/UB.Sweep.S3.Climb.auto +++ /dev/null @@ -1,50 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "UBump-SweepDown-Shoot3" - } - }, - { - "type": "named", - "data": { - "name": "Shoot" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Shoot3-Climb2" - } - }, - { - "type": "named", - "data": { - "name": "ExtendClimber" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "Climb" - } - } - ] - } - }, - "resetOdom": true, - "folder": "Sweep First", - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Depot-Climb1.path b/src/main/deploy/pathplanner/paths/Depot-Climb1.path deleted file mode 100644 index ca333f2..0000000 --- a/src/main/deploy/pathplanner/paths/Depot-Climb1.path +++ /dev/null @@ -1,59 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 0.5375257072239912, - "y": 5.9245833333333335 - }, - "prevControl": null, - "nextControl": { - "x": 0.7576434822031876, - "y": 6.04310828909459 - }, - "isLocked": false, - "linkedName": "Depot" - }, - { - "anchor": { - "x": 1.0716738049959909, - "y": 4.599583975068654 - }, - "prevControl": { - "x": 2.4268716151814416, - "y": 4.535752194154123 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "Climb1" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.643428571428569, - "rotationDegrees": 90.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 90.0 - }, - "reversed": false, - "folder": "Climb Paths ✔", - "idealStartingState": { - "velocity": 0, - "rotation": 180.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Depot-Shoot1.path b/src/main/deploy/pathplanner/paths/Depot-Shoot1.path deleted file mode 100644 index 650b756..0000000 --- a/src/main/deploy/pathplanner/paths/Depot-Shoot1.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 0.5375257072239912, - "y": 5.9245833333333335 - }, - "prevControl": null, - "nextControl": { - "x": 1.5375257072239912, - "y": 5.9245833333333335 - }, - "isLocked": false, - "linkedName": "Depot" - }, - { - "anchor": { - "x": 2.6085176282051274, - "y": 1.4477243589743591 - }, - "prevControl": { - "x": 2.637588141025641, - "y": 2.610544871794872 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "S3" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 39.09385888622944 - }, - "reversed": false, - "folder": "Depot✔", - "idealStartingState": { - "velocity": 0, - "rotation": 180.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Depot-Shoot2.path b/src/main/deploy/pathplanner/paths/Depot-Shoot2.path deleted file mode 100644 index acc1efe..0000000 --- a/src/main/deploy/pathplanner/paths/Depot-Shoot2.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 0.5375257072239912, - "y": 5.9245833333333335 - }, - "prevControl": null, - "nextControl": { - "x": 1.5375257072239912, - "y": 5.9245833333333335 - }, - "isLocked": false, - "linkedName": "Depot" - }, - { - "anchor": { - "x": 3.495168269230769, - "y": 3.976858974358975 - }, - "prevControl": { - "x": 2.495168269230769, - "y": 3.976858974358975 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "S2" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 1.101706115206352 - }, - "reversed": false, - "folder": "Depot✔", - "idealStartingState": { - "velocity": 0, - "rotation": 180.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Depot-Shoot3.path b/src/main/deploy/pathplanner/paths/Depot-Shoot3.path deleted file mode 100644 index d2fa138..0000000 --- a/src/main/deploy/pathplanner/paths/Depot-Shoot3.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 0.5375257072239912, - "y": 5.9245833333333335 - }, - "prevControl": null, - "nextControl": { - "x": 1.5375257072239912, - "y": 5.9245833333333335 - }, - "isLocked": false, - "linkedName": "Depot" - }, - { - "anchor": { - "x": 2.971899038461538, - "y": 5.9245833333333335 - }, - "prevControl": { - "x": 1.971899038461538, - "y": 5.924583333333333 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "S1" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -45.0 - }, - "reversed": false, - "folder": "Depot✔", - "idealStartingState": { - "velocity": 0, - "rotation": 180.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Shoot2-Main1.path b/src/main/deploy/pathplanner/paths/Example Path.path similarity index 60% rename from src/main/deploy/pathplanner/paths/Shoot2-Main1.path rename to src/main/deploy/pathplanner/paths/Example Path.path index 8bae84f..303dbb2 100644 --- a/src/main/deploy/pathplanner/paths/Shoot2-Main1.path +++ b/src/main/deploy/pathplanner/paths/Example Path.path @@ -3,45 +3,45 @@ "waypoints": [ { "anchor": { - "x": 3.495168269230769, - "y": 3.976858974358975 + "x": 2.0, + "y": 7.0 }, "prevControl": null, "nextControl": { - "x": 3.592847900557547, - "y": 4.963312327872699 + "x": 3.013282910175676, + "y": 6.5274984191074985 }, "isLocked": false, - "linkedName": "S2" + "linkedName": null }, { "anchor": { - "x": 3.76778313253012, - "y": 5.291686746987953 + "x": 5.166769560390973, + "y": 5.0964860911203305 }, "prevControl": { - "x": 3.5745839880885217, - "y": 5.101240702945876 + "x": 4.166769560390973, + "y": 6.0964860911203305 }, "nextControl": { - "x": 4.225742841389206, - "y": 5.743120500149518 + "x": 6.166769560390973, + "y": 4.0964860911203305 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.675728952774968, - "y": 5.9918925393593625 + "x": 7.0, + "y": 1.0 }, "prevControl": { - "x": 6.456, - "y": 5.88178313253012 + "x": 6.75, + "y": 2.5 }, "nextControl": null, "isLocked": false, - "linkedName": "M1" + "linkedName": null } ], "rotationTargets": [], @@ -61,10 +61,10 @@ "rotation": 0.0 }, "reversed": false, - "folder": "Shoot✔️ ", + "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 1.101706115206352 + "rotation": 0.0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/JustClimb1.path b/src/main/deploy/pathplanner/paths/JustClimb1.path deleted file mode 100644 index 32581dd..0000000 --- a/src/main/deploy/pathplanner/paths/JustClimb1.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 1.4627440246710683, - "y": 4.57531457311145 - }, - "prevControl": null, - "nextControl": { - "x": 1.7127430750220372, - "y": 4.576003647573122 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.0716738049959909, - "y": 4.599583975068654 - }, - "prevControl": { - "x": 0.7923425408940383, - "y": 4.599031203468609 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "Climb1" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 90.0 - }, - "reversed": false, - "folder": "Climb Paths ✔", - "idealStartingState": { - "velocity": 0, - "rotation": 90.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/JustClimb2.path b/src/main/deploy/pathplanner/paths/JustClimb2.path deleted file mode 100644 index dec627e..0000000 --- a/src/main/deploy/pathplanner/paths/JustClimb2.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 0.6732340615750425, - "y": 2.8801012661030256 - }, - "prevControl": null, - "nextControl": { - "x": 0.9232331119260113, - "y": 2.8807903405646975 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.0422131290243875, - "y": 2.8801012661030256 - }, - "prevControl": { - "x": 0.7628818649224349, - "y": 2.8795484945029806 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "Climb2" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -90.0 - }, - "reversed": false, - "folder": "Climb Paths ✔", - "idealStartingState": { - "velocity": 0, - "rotation": -90.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LBump-Climb.path b/src/main/deploy/pathplanner/paths/LBump-Climb.path deleted file mode 100644 index 13b50f6..0000000 --- a/src/main/deploy/pathplanner/paths/LBump-Climb.path +++ /dev/null @@ -1,59 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.6, - "y": 2.51 - }, - "prevControl": null, - "nextControl": { - "x": 3.0456045805726717, - "y": 1.8737948147497536 - }, - "isLocked": false, - "linkedName": "LB" - }, - { - "anchor": { - "x": 1.0422131290243875, - "y": 2.8801012661030256 - }, - "prevControl": { - "x": -0.1993986099955909, - "y": 2.81140296010582 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "Climb2" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.5, - "rotationDegrees": -90.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -90.0 - }, - "reversed": false, - "folder": "Climb Paths ✔", - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LBump-Main3.path b/src/main/deploy/pathplanner/paths/LBump-Main3.path deleted file mode 100644 index 9f43872..0000000 --- a/src/main/deploy/pathplanner/paths/LBump-Main3.path +++ /dev/null @@ -1,66 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.6, - "y": 2.51 - }, - "prevControl": null, - "nextControl": { - "x": 5.679263279942127, - "y": 2.772532511936588 - }, - "isLocked": false, - "linkedName": "LB" - }, - { - "anchor": { - "x": 7.564944129185973, - "y": 2.961412209299814 - }, - "prevControl": { - "x": 7.100443430294522, - "y": 2.891199060884476 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "M3" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.2, - "rotationDegrees": 0.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "IntakeOn", - "waypointRelativePos": 0.6092233009708738, - "endWaypointRelativePos": 1.0, - "command": null - } - ], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 0.0 - }, - "reversed": false, - "folder": "Initial✔️", - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LBump-Main4.path b/src/main/deploy/pathplanner/paths/LBump-Main4.path deleted file mode 100644 index 73857e7..0000000 --- a/src/main/deploy/pathplanner/paths/LBump-Main4.path +++ /dev/null @@ -1,61 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.6, - "y": 2.51 - }, - "prevControl": null, - "nextControl": { - "x": 5.325279331824971, - "y": 2.4556189300493476 - }, - "isLocked": false, - "linkedName": "LB" - }, - { - "anchor": { - "x": 7.756784652889603, - "y": 2.1754616603872785 - }, - "prevControl": { - "x": 7.485616952764362, - "y": 2.1642888942636884 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "M4" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "IntakeOn", - "waypointRelativePos": 0.4, - "endWaypointRelativePos": 2.0, - "command": null - } - ], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 0.0 - }, - "reversed": false, - "folder": "Initial✔️", - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LBump-Shoot2.path b/src/main/deploy/pathplanner/paths/LBump-Shoot2.path deleted file mode 100644 index 18d30a8..0000000 --- a/src/main/deploy/pathplanner/paths/LBump-Shoot2.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.6, - "y": 2.51 - }, - "prevControl": null, - "nextControl": { - "x": 3.5364718065372305, - "y": 2.806352471449683 - }, - "isLocked": false, - "linkedName": "LB" - }, - { - "anchor": { - "x": 3.495168269230769, - "y": 3.976858974358975 - }, - "prevControl": { - "x": 2.819435106635128, - "y": 3.9536111912930467 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "S2" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 1.101706115206352 - }, - "reversed": false, - "folder": "Initial✔️", - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LBump-Shoot3.path b/src/main/deploy/pathplanner/paths/LBump-Shoot3.path deleted file mode 100644 index 714bdd1..0000000 --- a/src/main/deploy/pathplanner/paths/LBump-Shoot3.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.6, - "y": 2.51 - }, - "prevControl": null, - "nextControl": { - "x": 2.94361753445769, - "y": 1.8908647591613876 - }, - "isLocked": false, - "linkedName": "LB" - }, - { - "anchor": { - "x": 2.6085176282051274, - "y": 1.4477243589743591 - }, - "prevControl": { - "x": 2.861623824615099, - "y": 1.6826579820201002 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "S3" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 39.09385888622944 - }, - "reversed": false, - "folder": "Initial✔️", - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LBump-SweepUp-Shoot1.path b/src/main/deploy/pathplanner/paths/LBump-SweepUp-Shoot1.path deleted file mode 100644 index 4cba3c5..0000000 --- a/src/main/deploy/pathplanner/paths/LBump-SweepUp-Shoot1.path +++ /dev/null @@ -1,150 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.6, - "y": 2.51 - }, - "prevControl": null, - "nextControl": { - "x": 4.905948644793153, - "y": 2.889928673323824 - }, - "isLocked": false, - "linkedName": "LB" - }, - { - "anchor": { - "x": 8.280927710843374, - "y": 1.2921445783132537 - }, - "prevControl": { - "x": 7.302419933866362, - "y": 0.32575592718023927 - }, - "nextControl": { - "x": 8.9666541086164, - "y": 1.969378039489662 - }, - "isLocked": false, - "linkedName": "StartUpSweep" - }, - { - "anchor": { - "x": 7.625265060240963, - "y": 6.635795180722892 - }, - "prevControl": { - "x": 8.461847673846595, - "y": 6.064003392442868 - }, - "nextControl": { - "x": 7.088615698927714, - "y": 7.002587046815015 - }, - "isLocked": false, - "linkedName": "EndUpSweep" - }, - { - "anchor": { - "x": 5.125905848787446, - "y": 5.76231098430813 - }, - "prevControl": { - "x": 5.372812067950615, - "y": 5.723102325334002 - }, - "nextControl": { - "x": 4.85480368231628, - "y": 5.805361954949298 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.971899038461538, - "y": 5.9245833333333335 - }, - "prevControl": { - "x": 4.090164995643859, - "y": 5.935196071253095 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "S1" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.3905411994149175, - "rotationDegrees": 0.0 - }, - { - "waypointRelativePos": 0.9054119941491925, - "rotationDegrees": 110.0 - }, - { - "waypointRelativePos": 1.9878108239882952, - "rotationDegrees": 110.0 - }, - { - "waypointRelativePos": 2.8142369575816693, - "rotationDegrees": -45.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "IntakeDown", - "waypointRelativePos": 0.46135265700482614, - "endWaypointRelativePos": null, - "command": null - }, - { - "name": "IntakeOn", - "waypointRelativePos": 0.5772946859903278, - "endWaypointRelativePos": null, - "command": null - }, - { - "name": "IntakeOff", - "waypointRelativePos": 2.760869565217383, - "endWaypointRelativePos": null, - "command": null - }, - { - "name": "ReadyUp", - "waypointRelativePos": 2.770531400966177, - "endWaypointRelativePos": null, - "command": null - }, - { - "name": "Shoot", - "waypointRelativePos": 4.0, - "endWaypointRelativePos": null, - "command": null - } - ], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -45.0 - }, - "reversed": false, - "folder": "Compound Paths", - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LBump-SweepUp.path b/src/main/deploy/pathplanner/paths/LBump-SweepUp.path deleted file mode 100644 index 6819b10..0000000 --- a/src/main/deploy/pathplanner/paths/LBump-SweepUp.path +++ /dev/null @@ -1,59 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.6, - "y": 2.51 - }, - "prevControl": null, - "nextControl": { - "x": 5.964253012048193, - "y": 2.330277108433736 - }, - "isLocked": false, - "linkedName": "LB" - }, - { - "anchor": { - "x": 8.280927710843374, - "y": 1.2921445783132537 - }, - "prevControl": { - "x": 8.055933950086057, - "y": 0.20620331325301322 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "StartUpSweep" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.5443809523809531, - "rotationDegrees": 0.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 100.0 - }, - "reversed": false, - "folder": "Initial✔️", - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LTrench-Climb2.path b/src/main/deploy/pathplanner/paths/LTrench-Climb2.path deleted file mode 100644 index 0188f32..0000000 --- a/src/main/deploy/pathplanner/paths/LTrench-Climb2.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.6, - "y": 0.6394 - }, - "prevControl": null, - "nextControl": { - "x": 3.8496143483334184, - "y": 0.6532808179183692 - }, - "isLocked": false, - "linkedName": "LT" - }, - { - "anchor": { - "x": 1.0422131290243875, - "y": 2.8801012661030256 - }, - "prevControl": { - "x": -1.0463855421686747, - "y": 2.854807228915663 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "Climb2" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -90.0 - }, - "reversed": false, - "folder": "Climb Paths ✔", - "idealStartingState": { - "velocity": 0, - "rotation": -90.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LTrench-Shoot3.path b/src/main/deploy/pathplanner/paths/LTrench-Shoot3.path deleted file mode 100644 index 3fa73b3..0000000 --- a/src/main/deploy/pathplanner/paths/LTrench-Shoot3.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.6, - "y": 0.6394 - }, - "prevControl": null, - "nextControl": { - "x": 3.823079226713893, - "y": 0.5265476247093948 - }, - "isLocked": false, - "linkedName": "LT" - }, - { - "anchor": { - "x": 2.6085176282051274, - "y": 1.4477243589743591 - }, - "prevControl": { - "x": 2.3982456481731176, - "y": 1.5829489662520955 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "S3" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 39.09385888622944 - }, - "reversed": false, - "folder": "Initial✔️", - "idealStartingState": { - "velocity": 0, - "rotation": -90.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Main1-Climb1.path b/src/main/deploy/pathplanner/paths/Main1-Climb1.path deleted file mode 100644 index 4ac9697..0000000 --- a/src/main/deploy/pathplanner/paths/Main1-Climb1.path +++ /dev/null @@ -1,75 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 7.675728952774968, - "y": 5.9918925393593625 - }, - "prevControl": null, - "nextControl": { - "x": 7.915149014427164, - "y": 6.063851097422032 - }, - "isLocked": false, - "linkedName": "M1" - }, - { - "anchor": { - "x": 4.425424679487179, - "y": 5.357708333333334 - }, - "prevControl": { - "x": 5.131910656932007, - "y": 5.710334081532174 - }, - "nextControl": { - "x": 2.8554320587616338, - "y": 4.574083689356252 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.0716738049959909, - "y": 4.599583975068654 - }, - "prevControl": { - "x": 2.395304022609584, - "y": 4.649365787499372 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "Climb1" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.49142857142857144, - "rotationDegrees": 90.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 90.0 - }, - "reversed": false, - "folder": "Climb Paths ✔", - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Main1-Shoot1.path b/src/main/deploy/pathplanner/paths/Main1-Shoot1.path deleted file mode 100644 index e2959bf..0000000 --- a/src/main/deploy/pathplanner/paths/Main1-Shoot1.path +++ /dev/null @@ -1,59 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 7.675728952774968, - "y": 5.9918925393593625 - }, - "prevControl": null, - "nextControl": { - "x": 8.675728952774968, - "y": 5.9918925393593625 - }, - "isLocked": false, - "linkedName": "M1" - }, - { - "anchor": { - "x": 2.971899038461538, - "y": 5.9245833333333335 - }, - "prevControl": { - "x": 1.971899038461538, - "y": 5.9245833333333335 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "S1" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.4560000000000002, - "rotationDegrees": -45.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -45.0 - }, - "reversed": false, - "folder": "Gather✔️", - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Main1-Shoot2.path b/src/main/deploy/pathplanner/paths/Main1-Shoot2.path deleted file mode 100644 index 521a288..0000000 --- a/src/main/deploy/pathplanner/paths/Main1-Shoot2.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 7.675728952774968, - "y": 5.9918925393593625 - }, - "prevControl": null, - "nextControl": { - "x": 3.997265060240964, - "y": 5.455602409638555 - }, - "isLocked": false, - "linkedName": "M1" - }, - { - "anchor": { - "x": 3.495168269230769, - "y": 3.976858974358975 - }, - "prevControl": { - "x": 3.2323253012048188, - "y": 5.521168674698795 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "S2" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 1.101706115206352 - }, - "reversed": false, - "folder": "Gather✔️", - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Main2-Climb1.path b/src/main/deploy/pathplanner/paths/Main2-Climb1.path deleted file mode 100644 index 1c03ea5..0000000 --- a/src/main/deploy/pathplanner/paths/Main2-Climb1.path +++ /dev/null @@ -1,75 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 7.645743952021233, - "y": 5.00357851618481 - }, - "prevControl": null, - "nextControl": { - "x": 7.885164013673429, - "y": 5.075537074247479 - }, - "isLocked": false, - "linkedName": "M2" - }, - { - "anchor": { - "x": 4.270457831325301, - "y": 5.193337349397591 - }, - "prevControl": { - "x": 5.068180722891567, - "y": 5.237048192771084 - }, - "nextControl": { - "x": 3.0541576635264303, - "y": 5.126690764860669 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.0716738049959909, - "y": 4.599583975068654 - }, - "prevControl": { - "x": 2.4499425768264507, - "y": 4.638438076655999 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "Climb1" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.6468571428571428, - "rotationDegrees": 90.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 90.0 - }, - "reversed": false, - "folder": "Climb Paths ✔", - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Main2-Shoot1.path b/src/main/deploy/pathplanner/paths/Main2-Shoot1.path deleted file mode 100644 index d62b805..0000000 --- a/src/main/deploy/pathplanner/paths/Main2-Shoot1.path +++ /dev/null @@ -1,59 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 7.645743952021233, - "y": 5.00357851618481 - }, - "prevControl": null, - "nextControl": { - "x": 1.4841743960285756, - "y": 6.206729968328998 - }, - "isLocked": false, - "linkedName": "M2" - }, - { - "anchor": { - "x": 2.971899038461538, - "y": 5.9245833333333335 - }, - "prevControl": { - "x": 3.2964254130634165, - "y": 5.8629628486565135 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "S1" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.1177142857142854, - "rotationDegrees": -45.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -45.0 - }, - "reversed": false, - "folder": "Gather✔️", - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Main2-Shoot2.path b/src/main/deploy/pathplanner/paths/Main2-Shoot2.path deleted file mode 100644 index 3807627..0000000 --- a/src/main/deploy/pathplanner/paths/Main2-Shoot2.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 7.645743952021233, - "y": 5.00357851618481 - }, - "prevControl": null, - "nextControl": { - "x": 3.9426265060240966, - "y": 5.521168674698794 - }, - "isLocked": false, - "linkedName": "M2" - }, - { - "anchor": { - "x": 3.495168269230769, - "y": 3.976858974358975 - }, - "prevControl": { - "x": 3.276036144578313, - "y": 5.390036144578313 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "S2" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 1.101706115206352 - }, - "reversed": false, - "folder": "Gather✔️", - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Main3-Climb2.path b/src/main/deploy/pathplanner/paths/Main3-Climb2.path deleted file mode 100644 index ae6f2e8..0000000 --- a/src/main/deploy/pathplanner/paths/Main3-Climb2.path +++ /dev/null @@ -1,75 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 7.564944129185973, - "y": 2.961412209299814 - }, - "prevControl": null, - "nextControl": { - "x": 7.769430200353409, - "y": 3.1052365698876714 - }, - "isLocked": false, - "linkedName": "M3" - }, - { - "anchor": { - "x": 0.8282289156580382, - "y": 2.297493975907743 - }, - "prevControl": { - "x": 1.4598657270832405, - "y": 2.297493975907743 - }, - "nextControl": { - "x": 0.36926506023635153, - "y": 2.297493975907743 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.0422131290243875, - "y": 2.8801012661030256 - }, - "prevControl": { - "x": 0.23813253011586966, - "y": 2.8985180722932853 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "Climb2" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.2841904761904751, - "rotationDegrees": -90.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -90.0 - }, - "reversed": false, - "folder": "Climb Paths ✔", - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Main3-Shoot2.path b/src/main/deploy/pathplanner/paths/Main3-Shoot2.path deleted file mode 100644 index 77b6b93..0000000 --- a/src/main/deploy/pathplanner/paths/Main3-Shoot2.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 7.564944129185973, - "y": 2.961412209299814 - }, - "prevControl": null, - "nextControl": { - "x": 4.121292863752008, - "y": 2.482892801927034 - }, - "isLocked": false, - "linkedName": "M3" - }, - { - "anchor": { - "x": 3.495168269230769, - "y": 3.976858974358975 - }, - "prevControl": { - "x": 2.9396934550367324, - "y": 2.709996866554761 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "S2" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 1.101706115206352 - }, - "reversed": false, - "folder": "Gather✔️", - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Main3-Shoot3.path b/src/main/deploy/pathplanner/paths/Main3-Shoot3.path deleted file mode 100644 index 59577a1..0000000 --- a/src/main/deploy/pathplanner/paths/Main3-Shoot3.path +++ /dev/null @@ -1,59 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 7.564944129185973, - "y": 2.961412209299814 - }, - "prevControl": null, - "nextControl": { - "x": 4.434373493975903, - "y": 2.4723373493975904 - }, - "isLocked": false, - "linkedName": "M3" - }, - { - "anchor": { - "x": 2.6085176282051274, - "y": 1.4477243589743591 - }, - "prevControl": { - "x": 3.4945903614457823, - "y": 2.384915662650603 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "S3" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.23047619047618934, - "rotationDegrees": 45.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 39.09385888622944 - }, - "reversed": false, - "folder": "Gather✔️", - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Main4-Climb2.path b/src/main/deploy/pathplanner/paths/Main4-Climb2.path deleted file mode 100644 index b44f772..0000000 --- a/src/main/deploy/pathplanner/paths/Main4-Climb2.path +++ /dev/null @@ -1,75 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 7.756784652889603, - "y": 2.1754616603872785 - }, - "prevControl": null, - "nextControl": { - "x": 7.986817169589537, - "y": 2.2733648843418273 - }, - "isLocked": false, - "linkedName": "M4" - }, - { - "anchor": { - "x": 1.0422131290243875, - "y": 2.0006304661832486 - }, - "prevControl": { - "x": 1.5528673763406438, - "y": 1.7030458471435814 - }, - "nextControl": { - "x": 0.8262137425870961, - "y": 2.1265044708812546 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.0422131290243875, - "y": 2.8801012661030256 - }, - "prevControl": { - "x": 0.3474096385542168, - "y": 2.701819277108433 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "Climb2" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.31771428571428795, - "rotationDegrees": -90.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -90.0 - }, - "reversed": false, - "folder": "Climb Paths ✔", - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Main4-Shoot2.path b/src/main/deploy/pathplanner/paths/Main4-Shoot2.path deleted file mode 100644 index 2ee2cae..0000000 --- a/src/main/deploy/pathplanner/paths/Main4-Shoot2.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 7.756784652889603, - "y": 2.1754616603872785 - }, - "prevControl": null, - "nextControl": { - "x": 4.587361445783133, - "y": 2.3739879518072295 - }, - "isLocked": false, - "linkedName": "M4" - }, - { - "anchor": { - "x": 3.495168269230769, - "y": 3.976858974358975 - }, - "prevControl": { - "x": 2.9396934550367324, - "y": 2.709996866554761 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "S2" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 1.101706115206352 - }, - "reversed": false, - "folder": "Gather✔️", - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Main4-Shoot3.path b/src/main/deploy/pathplanner/paths/Main4-Shoot3.path deleted file mode 100644 index bc59ae1..0000000 --- a/src/main/deploy/pathplanner/paths/Main4-Shoot3.path +++ /dev/null @@ -1,59 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 7.756784652889603, - "y": 2.1754616603872785 - }, - "prevControl": null, - "nextControl": { - "x": 4.784060240963855, - "y": 2.505120481927711 - }, - "isLocked": false, - "linkedName": "M4" - }, - { - "anchor": { - "x": 2.6085176282051274, - "y": 1.4477243589743591 - }, - "prevControl": { - "x": 3.4621700088486667, - "y": 2.4314240470156303 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "S3" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.24723809523808998, - "rotationDegrees": 45.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 39.09385888622944 - }, - "reversed": false, - "folder": "Gather✔️", - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Shoot1-Center.path b/src/main/deploy/pathplanner/paths/Shoot1-Center.path deleted file mode 100644 index a382388..0000000 --- a/src/main/deploy/pathplanner/paths/Shoot1-Center.path +++ /dev/null @@ -1,59 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 2.971899038461538, - "y": 5.9245833333333335 - }, - "prevControl": null, - "nextControl": { - "x": 3.971899038461538, - "y": 5.9245833333333335 - }, - "isLocked": false, - "linkedName": "S1" - }, - { - "anchor": { - "x": 7.986562499999998, - "y": 4.020464743589744 - }, - "prevControl": { - "x": 6.620248397435898, - "y": 4.798100961538462 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "Center" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.5, - "rotationDegrees": -45.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 0.0 - }, - "reversed": false, - "folder": "Shoot✔️ ", - "idealStartingState": { - "velocity": 0, - "rotation": -45.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Shoot1-Climb1.path b/src/main/deploy/pathplanner/paths/Shoot1-Climb1.path deleted file mode 100644 index 73eb355..0000000 --- a/src/main/deploy/pathplanner/paths/Shoot1-Climb1.path +++ /dev/null @@ -1,59 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 2.971899038461538, - "y": 5.9245833333333335 - }, - "prevControl": null, - "nextControl": { - "x": 2.2379036144578306, - "y": 4.931072289156628 - }, - "isLocked": false, - "linkedName": "S1" - }, - { - "anchor": { - "x": 1.0716738049959909, - "y": 4.599583975068654 - }, - "prevControl": { - "x": 2.13303896236862, - "y": 4.671221209186119 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "Climb1" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.3295238095238099, - "rotationDegrees": 90.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 90.0 - }, - "reversed": false, - "folder": "Climb Paths ✔", - "idealStartingState": { - "velocity": 0, - "rotation": -45.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Shoot1-Depot.path b/src/main/deploy/pathplanner/paths/Shoot1-Depot.path deleted file mode 100644 index 6cb664b..0000000 --- a/src/main/deploy/pathplanner/paths/Shoot1-Depot.path +++ /dev/null @@ -1,66 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 2.971899038461538, - "y": 5.9245833333333335 - }, - "prevControl": null, - "nextControl": { - "x": 1.7243012048192772, - "y": 5.958277108433736 - }, - "isLocked": false, - "linkedName": "S1" - }, - { - "anchor": { - "x": 0.5375257072239912, - "y": 5.9245833333333335 - }, - "prevControl": { - "x": 1.233204093847408, - "y": 5.914671339202966 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "Depot" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.8201904761904751, - "rotationDegrees": 180.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "IntakeOn", - "waypointRelativePos": 0.31434977578475565, - "endWaypointRelativePos": 1.0, - "command": null - } - ], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 180.0 - }, - "reversed": false, - "folder": "Shoot✔️ ", - "idealStartingState": { - "velocity": 0, - "rotation": -45.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Shoot1-Main1.path b/src/main/deploy/pathplanner/paths/Shoot1-Main1.path deleted file mode 100644 index 9a0207f..0000000 --- a/src/main/deploy/pathplanner/paths/Shoot1-Main1.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 2.971899038461538, - "y": 5.9245833333333335 - }, - "prevControl": null, - "nextControl": { - "x": 3.971899038461538, - "y": 5.9245833333333335 - }, - "isLocked": false, - "linkedName": "S1" - }, - { - "anchor": { - "x": 7.675728952774968, - "y": 5.9918925393593625 - }, - "prevControl": { - "x": 6.675728952774968, - "y": 5.9918925393593625 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "M1" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 0.0 - }, - "reversed": false, - "folder": "Shoot✔️ ", - "idealStartingState": { - "velocity": 0, - "rotation": -45.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Shoot1-Main2.path b/src/main/deploy/pathplanner/paths/Shoot1-Main2.path deleted file mode 100644 index f362f54..0000000 --- a/src/main/deploy/pathplanner/paths/Shoot1-Main2.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 2.971899038461538, - "y": 5.9245833333333335 - }, - "prevControl": null, - "nextControl": { - "x": 3.971899038461538, - "y": 5.9245833333333335 - }, - "isLocked": false, - "linkedName": "S1" - }, - { - "anchor": { - "x": 7.645743952021233, - "y": 5.00357851618481 - }, - "prevControl": { - "x": 6.645743952021233, - "y": 5.00357851618481 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "M2" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 0.0 - }, - "reversed": false, - "folder": "Shoot✔️ ", - "idealStartingState": { - "velocity": 0, - "rotation": -45.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Shoot1-SweepDown.path b/src/main/deploy/pathplanner/paths/Shoot1-SweepDown.path deleted file mode 100644 index 7b461b7..0000000 --- a/src/main/deploy/pathplanner/paths/Shoot1-SweepDown.path +++ /dev/null @@ -1,75 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 2.971899038461538, - "y": 5.9245833333333335 - }, - "prevControl": null, - "nextControl": { - "x": 5.413822115384614, - "y": 5.902780448717949 - }, - "isLocked": false, - "linkedName": "S1" - }, - { - "anchor": { - "x": 6.227796474358973, - "y": 6.244358974358975 - }, - "prevControl": { - "x": 5.5225100323736624, - "y": 5.749344266630155 - }, - "nextControl": { - "x": 7.397884615384615, - "y": 7.065600961538462 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 8.279311758588921, - "y": 6.670571423499865 - }, - "prevControl": { - "x": 7.67609861756328, - "y": 7.193840654269096 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "StartDownSweep" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.03139013452914799, - "rotationDegrees": -90.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -100.0 - }, - "reversed": false, - "folder": "Shoot✔️ ", - "idealStartingState": { - "velocity": 0, - "rotation": -45.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Shoot2-Climb1.path b/src/main/deploy/pathplanner/paths/Shoot2-Climb1.path deleted file mode 100644 index 4b5f922..0000000 --- a/src/main/deploy/pathplanner/paths/Shoot2-Climb1.path +++ /dev/null @@ -1,59 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.495168269230769, - "y": 3.976858974358975 - }, - "prevControl": null, - "nextControl": { - "x": 2.2269759036144574, - "y": 4.701590361445783 - }, - "isLocked": false, - "linkedName": "S2" - }, - { - "anchor": { - "x": 1.0716738049959909, - "y": 4.599583975068654 - }, - "prevControl": { - "x": 2.0893281189951267, - "y": 4.6275103658126255 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "Climb1" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.6769523809523813, - "rotationDegrees": 90.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 90.0 - }, - "reversed": false, - "folder": "Climb Paths ✔", - "idealStartingState": { - "velocity": 0, - "rotation": 1.101706115206352 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Shoot2-Depot.path b/src/main/deploy/pathplanner/paths/Shoot2-Depot.path deleted file mode 100644 index 128b7e0..0000000 --- a/src/main/deploy/pathplanner/paths/Shoot2-Depot.path +++ /dev/null @@ -1,66 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.495168269230769, - "y": 3.976858974358975 - }, - "prevControl": null, - "nextControl": { - "x": 3.4618072289156623, - "y": 5.433746987951806 - }, - "isLocked": false, - "linkedName": "S2" - }, - { - "anchor": { - "x": 0.5375257072239912, - "y": 5.9245833333333335 - }, - "prevControl": { - "x": 2.718722891566265, - "y": 5.848999999999998 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "Depot" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.8064761904761906, - "rotationDegrees": 180.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "IntakeOn", - "waypointRelativePos": 0.787892376681611, - "endWaypointRelativePos": 1.0, - "command": null - } - ], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 180.0 - }, - "reversed": false, - "folder": "Shoot✔️ ", - "idealStartingState": { - "velocity": 0, - "rotation": 1.101706115206352 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Shoot2-Main2.path b/src/main/deploy/pathplanner/paths/Shoot2-Main2.path deleted file mode 100644 index c91ee58..0000000 --- a/src/main/deploy/pathplanner/paths/Shoot2-Main2.path +++ /dev/null @@ -1,70 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.495168269230769, - "y": 3.976858974358975 - }, - "prevControl": null, - "nextControl": { - "x": 3.562498037081225, - "y": 4.685423967526528 - }, - "isLocked": false, - "linkedName": "S2" - }, - { - "anchor": { - "x": 3.70221686746988, - "y": 5.302614457831326 - }, - "prevControl": { - "x": 3.315394304995904, - "y": 4.9551819731083935 - }, - "nextControl": { - "x": 4.311156428667846, - "y": 5.8495458039244985 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 7.645743952021233, - "y": 5.00357851618481 - }, - "prevControl": { - "x": 6.50986388720549, - "y": 5.104680754437479 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "M2" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 0.0 - }, - "reversed": false, - "folder": "Shoot✔️ ", - "idealStartingState": { - "velocity": 0, - "rotation": 1.101706115206352 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Shoot2-Main3.path b/src/main/deploy/pathplanner/paths/Shoot2-Main3.path deleted file mode 100644 index 13d8742..0000000 --- a/src/main/deploy/pathplanner/paths/Shoot2-Main3.path +++ /dev/null @@ -1,70 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.495168269230769, - "y": 3.976858974358975 - }, - "prevControl": null, - "nextControl": { - "x": 3.5379576543029922, - "y": 3.4214658537757705 - }, - "isLocked": false, - "linkedName": "S2" - }, - { - "anchor": { - "x": 3.8879879518072293, - "y": 2.5925421686746986 - }, - "prevControl": { - "x": 3.527217944605494, - "y": 2.8690974530897413 - }, - "nextControl": { - "x": 4.547933868109539, - "y": 2.0866477568937474 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 7.564944129185973, - "y": 2.961412209299814 - }, - "prevControl": { - "x": 6.532493975903614, - "y": 2.7783132530120485 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "M3" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 0.0 - }, - "reversed": false, - "folder": "Shoot✔️ ", - "idealStartingState": { - "velocity": 0, - "rotation": 1.101706115206352 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Shoot2-Main4.path b/src/main/deploy/pathplanner/paths/Shoot2-Main4.path deleted file mode 100644 index c422f4f..0000000 --- a/src/main/deploy/pathplanner/paths/Shoot2-Main4.path +++ /dev/null @@ -1,70 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.495168269230769, - "y": 3.976858974358975 - }, - "prevControl": null, - "nextControl": { - "x": 3.5273734939759036, - "y": 3.0733614457831333 - }, - "isLocked": false, - "linkedName": "S2" - }, - { - "anchor": { - "x": 3.8879879518072293, - "y": 2.5925421686746986 - }, - "prevControl": { - "x": 3.5160704712795057, - "y": 2.911713236928375 - }, - "nextControl": { - "x": 4.361618552629521, - "y": 2.1860832288916496 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 7.756784652889603, - "y": 2.1754616603872785 - }, - "prevControl": { - "x": 6.694320639307958, - "y": 2.3380283657725442 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "M4" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 0.0 - }, - "reversed": false, - "folder": "Shoot✔️ ", - "idealStartingState": { - "velocity": 0, - "rotation": 1.101706115206352 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Shoot2-SweepDown.path b/src/main/deploy/pathplanner/paths/Shoot2-SweepDown.path deleted file mode 100644 index ab8992c..0000000 --- a/src/main/deploy/pathplanner/paths/Shoot2-SweepDown.path +++ /dev/null @@ -1,91 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.495168269230769, - "y": 3.976858974358975 - }, - "prevControl": null, - "nextControl": { - "x": 3.4006891025641024, - "y": 5.270496794871795 - }, - "isLocked": false, - "linkedName": "S2" - }, - { - "anchor": { - "x": 4.38181891025641, - "y": 5.437652243589744 - }, - "prevControl": { - "x": 4.15843147257766, - "y": 5.325411196341767 - }, - "nextControl": { - "x": 5.98392258715268, - "y": 6.24262944536629 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 7.259799679487179, - "y": 6.716754807692308 - }, - "prevControl": { - "x": 5.872824634121077, - "y": 6.178766659335234 - }, - "nextControl": { - "x": 7.518566691486418, - "y": 6.817126898817146 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 8.279311758588921, - "y": 6.670571423499865 - }, - "prevControl": { - "x": 8.199367848332512, - "y": 7.5499544363203785 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "StartDownSweep" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 1.365470852017938, - "rotationDegrees": 0.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -100.0 - }, - "reversed": false, - "folder": "Shoot✔️ ", - "idealStartingState": { - "velocity": 0, - "rotation": 1.101706115206352 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Shoot2-SweepUp.path b/src/main/deploy/pathplanner/paths/Shoot2-SweepUp.path deleted file mode 100644 index ab50426..0000000 --- a/src/main/deploy/pathplanner/paths/Shoot2-SweepUp.path +++ /dev/null @@ -1,91 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.495168269230769, - "y": 3.976858974358975 - }, - "prevControl": null, - "nextControl": { - "x": 3.3062099358974355, - "y": 2.770432692307693 - }, - "isLocked": false, - "linkedName": "S2" - }, - { - "anchor": { - "x": 4.323677884615384, - "y": 2.6250801282051284 - }, - "prevControl": { - "x": 3.8024300684205867, - "y": 2.7241906284675195 - }, - "nextControl": { - "x": 5.355681089743589, - "y": 2.4288541666666665 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 7.572307692307692, - "y": 1.2951041666666674 - }, - "prevControl": { - "x": 6.9182211538461535, - "y": 1.353245192307693 - }, - "nextControl": { - "x": 8.249385873452773, - "y": 1.2349194394537715 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 8.280927710843374, - "y": 1.2921445783132537 - }, - "prevControl": { - "x": 8.159661685202346, - "y": 0.7293881680568439 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "StartUpSweep" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 1.365470852017938, - "rotationDegrees": 0.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 100.0 - }, - "reversed": false, - "folder": "Shoot✔️ ", - "idealStartingState": { - "velocity": 0, - "rotation": 1.101706115206352 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Shoot3-Climb2.path b/src/main/deploy/pathplanner/paths/Shoot3-Climb2.path deleted file mode 100644 index 6854f92..0000000 --- a/src/main/deploy/pathplanner/paths/Shoot3-Climb2.path +++ /dev/null @@ -1,75 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 2.6085176282051274, - "y": 1.4477243589743591 - }, - "prevControl": null, - "nextControl": { - "x": 1.2544096385542172, - "y": 2.0243012048192766 - }, - "isLocked": false, - "linkedName": "S3" - }, - { - "anchor": { - "x": 0.8828674698795178, - "y": 2.2428554216867465 - }, - "prevControl": { - "x": 1.0841649533235005, - "y": 2.0946011699463005 - }, - "nextControl": { - "x": 0.48719889838116087, - "y": 2.534262681236635 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.0422131290243875, - "y": 2.8801012661030256 - }, - "prevControl": { - "x": 0.44575903614457835, - "y": 2.811096385542169 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "Climb2" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 1.6647619047619044, - "rotationDegrees": -90.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -90.0 - }, - "reversed": false, - "folder": "Climb Paths ✔", - "idealStartingState": { - "velocity": 0, - "rotation": 39.09385888622944 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Shoot3-Depot.path b/src/main/deploy/pathplanner/paths/Shoot3-Depot.path deleted file mode 100644 index 8286d2c..0000000 --- a/src/main/deploy/pathplanner/paths/Shoot3-Depot.path +++ /dev/null @@ -1,66 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 2.6085176282051274, - "y": 1.4477243589743591 - }, - "prevControl": null, - "nextControl": { - "x": 2.575156587890021, - "y": 2.90461237256719 - }, - "isLocked": false, - "linkedName": "S3" - }, - { - "anchor": { - "x": 0.5375257072239912, - "y": 5.9245833333333335 - }, - "prevControl": { - "x": 2.9044939759036135, - "y": 6.154975903614457 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "Depot" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.8049523809523805, - "rotationDegrees": 180.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "IntakeOn", - "waypointRelativePos": 0.787892376681611, - "endWaypointRelativePos": 1.0, - "command": null - } - ], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 180.0 - }, - "reversed": false, - "folder": "Shoot✔️ ", - "idealStartingState": { - "velocity": 0, - "rotation": 39.09385888622944 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Shoot3-Main3.path b/src/main/deploy/pathplanner/paths/Shoot3-Main3.path deleted file mode 100644 index 294af81..0000000 --- a/src/main/deploy/pathplanner/paths/Shoot3-Main3.path +++ /dev/null @@ -1,59 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 2.6085176282051274, - "y": 1.4477243589743591 - }, - "prevControl": null, - "nextControl": { - "x": 2.789357442886346, - "y": 1.6203423227732132 - }, - "isLocked": false, - "linkedName": "S3" - }, - { - "anchor": { - "x": 7.564944129185973, - "y": 2.961412209299814 - }, - "prevControl": { - "x": 5.1774578313253015, - "y": 3.1498554216867465 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "M3" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.7333333333333323, - "rotationDegrees": 45.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 0.0 - }, - "reversed": false, - "folder": "Shoot✔️ ", - "idealStartingState": { - "velocity": 0, - "rotation": 39.09385888622944 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Shoot3-Main4.path b/src/main/deploy/pathplanner/paths/Shoot3-Main4.path deleted file mode 100644 index 4068130..0000000 --- a/src/main/deploy/pathplanner/paths/Shoot3-Main4.path +++ /dev/null @@ -1,59 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 2.6085176282051274, - "y": 1.4477243589743591 - }, - "prevControl": null, - "nextControl": { - "x": 4.816843373493977, - "y": 2.723674698795181 - }, - "isLocked": false, - "linkedName": "S3" - }, - { - "anchor": { - "x": 7.756784652889603, - "y": 2.1754616603872785 - }, - "prevControl": { - "x": 7.512832942647112, - "y": 2.12080305566887 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "M4" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.459047619047612, - "rotationDegrees": 45.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 0.0 - }, - "reversed": false, - "folder": "Shoot✔️ ", - "idealStartingState": { - "velocity": 0, - "rotation": 39.09385888622944 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Shoot3-SweepUp-Shoot1.path b/src/main/deploy/pathplanner/paths/Shoot3-SweepUp-Shoot1.path deleted file mode 100644 index 5884144..0000000 --- a/src/main/deploy/pathplanner/paths/Shoot3-SweepUp-Shoot1.path +++ /dev/null @@ -1,160 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 2.6085176282051274, - "y": 1.4477243589743591 - }, - "prevControl": null, - "nextControl": { - "x": 3.3306746987951805, - "y": 2.472337349397591 - }, - "isLocked": false, - "linkedName": "S3" - }, - { - "anchor": { - "x": 7.1444457831325305, - "y": 1.4273749999999996 - }, - "prevControl": { - "x": 6.084457831325301, - "y": 2.4941927710843377 - }, - "nextControl": { - "x": 7.7917009937618165, - "y": 0.7759493305393397 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 8.280927710843374, - "y": 1.2921445783132537 - }, - "prevControl": { - "x": 8.308344335215812, - "y": 1.0297570801859328 - }, - "nextControl": { - "x": 8.121500215148155, - "y": 2.817926204815189 - }, - "isLocked": false, - "linkedName": "StartUpSweep" - }, - { - "anchor": { - "x": 7.625265060240963, - "y": 6.635795180722892 - }, - "prevControl": { - "x": 7.794864177403886, - "y": 5.667030113663325 - }, - "nextControl": { - "x": 7.53647740964041, - "y": 7.1429580464675135 - }, - "isLocked": false, - "linkedName": "EndUpSweep" - }, - { - "anchor": { - "x": 5.876831325303061, - "y": 5.9245833333333335 - }, - "prevControl": { - "x": 7.687645370709767, - "y": 6.214313580605675 - }, - "nextControl": { - "x": 4.784060240967568, - "y": 5.749739959835268 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.971899038461538, - "y": 5.9245833333333335 - }, - "prevControl": { - "x": 4.5655060240982435, - "y": 5.761578313248924 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "S1" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.6419047619047696, - "rotationDegrees": 0.0 - }, - { - "waypointRelativePos": 1.7999999999999952, - "rotationDegrees": 100.0 - }, - { - "waypointRelativePos": 2.9809523809523726, - "rotationDegrees": 100.0 - }, - { - "waypointRelativePos": 3.95, - "rotationDegrees": -45.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "", - "waypointRelativePos": 0.0, - "endWaypointRelativePos": null, - "command": null - }, - { - "name": "IntakeOn", - "waypointRelativePos": 1.7690582959641157, - "endWaypointRelativePos": null, - "command": null - }, - { - "name": "ReadyUp", - "waypointRelativePos": 3.2040358744394712, - "endWaypointRelativePos": null, - "command": null - }, - { - "name": "IntakeOff", - "waypointRelativePos": 3.85874439461875, - "endWaypointRelativePos": null, - "command": null - } - ], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -45.0 - }, - "reversed": false, - "folder": "Compound Paths", - "idealStartingState": { - "velocity": 0, - "rotation": 39.09385888622944 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Shoot3-SweepUp.path b/src/main/deploy/pathplanner/paths/Shoot3-SweepUp.path deleted file mode 100644 index 00c6255..0000000 --- a/src/main/deploy/pathplanner/paths/Shoot3-SweepUp.path +++ /dev/null @@ -1,75 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 2.6085176282051274, - "y": 1.4477243589743591 - }, - "prevControl": null, - "nextControl": { - "x": 4.38181891025641, - "y": 2.5087980769230773 - }, - "isLocked": false, - "linkedName": "S3" - }, - { - "anchor": { - "x": 5.777203525641025, - "y": 2.0654727564102564 - }, - "prevControl": { - "x": 4.512636217948717, - "y": 2.276233974358975 - }, - "nextControl": { - "x": 6.580390865748494, - "y": 1.931608199725678 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 8.280927710843374, - "y": 1.2921445783132537 - }, - "prevControl": { - "x": 8.06518251853568, - "y": 0.736655796261972 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "StartUpSweep" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.9865470852017963, - "rotationDegrees": 45.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 100.0 - }, - "reversed": false, - "folder": "Shoot✔️ ", - "idealStartingState": { - "velocity": 0, - "rotation": 39.09385888622944 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SweepDown-Climb2.path b/src/main/deploy/pathplanner/paths/SweepDown-Climb2.path deleted file mode 100644 index f7a1e62..0000000 --- a/src/main/deploy/pathplanner/paths/SweepDown-Climb2.path +++ /dev/null @@ -1,75 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 7.735517857150679, - "y": 1.3139999999959786 - }, - "prevControl": null, - "nextControl": { - "x": 2.9157243685140113, - "y": 2.7792527943438943 - }, - "isLocked": false, - "linkedName": "EndDownSweep" - }, - { - "anchor": { - "x": 0.751734939759036, - "y": 2.330277108433736 - }, - "prevControl": { - "x": 1.341831325301205, - "y": 2.2865662650602405 - }, - "nextControl": { - "x": 0.28312723263173406, - "y": 2.364988790443167 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.0422131290243875, - "y": 2.8801012661030256 - }, - "prevControl": { - "x": 0.3692650602409637, - "y": 2.854807228915663 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "Climb2" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.1, - "rotationDegrees": -90.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -90.0 - }, - "reversed": false, - "folder": "Climb Paths ✔", - "idealStartingState": { - "velocity": 0, - "rotation": -100.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SweepDown-LBump.path b/src/main/deploy/pathplanner/paths/SweepDown-LBump.path deleted file mode 100644 index 6085d3b..0000000 --- a/src/main/deploy/pathplanner/paths/SweepDown-LBump.path +++ /dev/null @@ -1,59 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 7.735517857150679, - "y": 1.3139999999959786 - }, - "prevControl": null, - "nextControl": { - "x": 4.947032280227601, - "y": 1.8154663461498246 - }, - "isLocked": false, - "linkedName": "EndDownSweep" - }, - { - "anchor": { - "x": 3.6, - "y": 2.51 - }, - "prevControl": { - "x": 5.624583333333333, - "y": 2.4244060897435897 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "LB" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.5818385650224271, - "rotationDegrees": 0.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 0.0 - }, - "reversed": false, - "folder": "Sweep Paths ✔", - "idealStartingState": { - "velocity": 0, - "rotation": -100.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SweepDown-Shoot2.path b/src/main/deploy/pathplanner/paths/SweepDown-Shoot2.path deleted file mode 100644 index 039ea3f..0000000 --- a/src/main/deploy/pathplanner/paths/SweepDown-Shoot2.path +++ /dev/null @@ -1,75 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 7.735517857150679, - "y": 1.3139999999959786 - }, - "prevControl": null, - "nextControl": { - "x": 6.313346382791704, - "y": 1.459352564098542 - }, - "isLocked": false, - "linkedName": "EndDownSweep" - }, - { - "anchor": { - "x": 4.643453525641025, - "y": 2.3925160256410254 - }, - "prevControl": { - "x": 6.603471399310875, - "y": 1.746127152409478 - }, - "nextControl": { - "x": 3.277139423076923, - "y": 2.843108974358975 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.495168269230769, - "y": 3.976858974358975 - }, - "prevControl": { - "x": 3.0809134615384615, - "y": 3.2064903846153845 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "S2" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.04035874439461712, - "rotationDegrees": 0.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 1.101706115206352 - }, - "reversed": false, - "folder": "Sweep Paths ✔", - "idealStartingState": { - "velocity": 0, - "rotation": -100.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SweepDown-Shoot3.path b/src/main/deploy/pathplanner/paths/SweepDown-Shoot3.path deleted file mode 100644 index d9e34f4..0000000 --- a/src/main/deploy/pathplanner/paths/SweepDown-Shoot3.path +++ /dev/null @@ -1,75 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 7.735517857150679, - "y": 1.3139999999959786 - }, - "prevControl": null, - "nextControl": { - "x": 5.731936126381448, - "y": 1.6991842948677731 - }, - "isLocked": false, - "linkedName": "EndDownSweep" - }, - { - "anchor": { - "x": 4.534439102564102, - "y": 2.4288541666666674 - }, - "prevControl": { - "x": 5.348413461538461, - "y": 2.6250801282051293 - }, - "nextControl": { - "x": 3.7422023964459266, - "y": 2.2378685321560354 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.6085176282051274, - "y": 1.4477243589743591 - }, - "prevControl": { - "x": 3.0954487179487176, - "y": 1.876514423076924 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "S3" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.5721925133689835, - "rotationDegrees": 45.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 39.09385888622944 - }, - "reversed": false, - "folder": "Sweep Paths ✔", - "idealStartingState": { - "velocity": 0, - "rotation": -100.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SweepDown.path b/src/main/deploy/pathplanner/paths/SweepDown.path deleted file mode 100644 index 03b935d..0000000 --- a/src/main/deploy/pathplanner/paths/SweepDown.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 8.279311758588921, - "y": 6.670571423499865 - }, - "prevControl": null, - "nextControl": { - "x": 8.179612937480199, - "y": 6.10515131190865 - }, - "isLocked": true, - "linkedName": "StartDownSweep" - }, - { - "anchor": { - "x": 7.735517857150679, - "y": 1.3139999999959786 - }, - "prevControl": { - "x": 7.8898105807861985, - "y": 2.1890375184494006 - }, - "nextControl": null, - "isLocked": true, - "linkedName": "EndDownSweep" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -100.0 - }, - "reversed": false, - "folder": "Sweep Paths ✔", - "idealStartingState": { - "velocity": 0, - "rotation": -100.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SweepUp-Climb.path b/src/main/deploy/pathplanner/paths/SweepUp-Climb.path deleted file mode 100644 index 0a0dfd0..0000000 --- a/src/main/deploy/pathplanner/paths/SweepUp-Climb.path +++ /dev/null @@ -1,59 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 7.625265060240963, - "y": 6.635795180722892 - }, - "prevControl": null, - "nextControl": { - "x": 3.941260542168674, - "y": 5.722355636833047 - }, - "isLocked": false, - "linkedName": "EndUpSweep" - }, - { - "anchor": { - "x": 1.0716738049959909, - "y": 4.599583975068654 - }, - "prevControl": { - "x": 3.280448600922838, - "y": 4.638438076655999 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "Climb1" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.3158095238095237, - "rotationDegrees": 90.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 90.0 - }, - "reversed": false, - "folder": "Climb Paths ✔", - "idealStartingState": { - "velocity": 0, - "rotation": 100.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SweepUp-Shoot1.path b/src/main/deploy/pathplanner/paths/SweepUp-Shoot1.path deleted file mode 100644 index 2b5578d..0000000 --- a/src/main/deploy/pathplanner/paths/SweepUp-Shoot1.path +++ /dev/null @@ -1,59 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 7.625265060240963, - "y": 6.635795180722892 - }, - "prevControl": null, - "nextControl": { - "x": 5.116318990364502, - "y": 5.313439573860769 - }, - "isLocked": false, - "linkedName": "EndUpSweep" - }, - { - "anchor": { - "x": 2.971899038461538, - "y": 5.9245833333333335 - }, - "prevControl": { - "x": 3.2152851374812865, - "y": 5.98170781848593 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "S1" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.3706666666666657, - "rotationDegrees": -45.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -45.0 - }, - "reversed": false, - "folder": "Sweep Paths ✔", - "idealStartingState": { - "velocity": 0, - "rotation": 100.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SweepUp-Shoot2.path b/src/main/deploy/pathplanner/paths/SweepUp-Shoot2.path deleted file mode 100644 index be6b606..0000000 --- a/src/main/deploy/pathplanner/paths/SweepUp-Shoot2.path +++ /dev/null @@ -1,70 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 7.625265060240963, - "y": 6.635795180722892 - }, - "prevControl": null, - "nextControl": { - "x": 5.091146470497373, - "y": 5.860378514056224 - }, - "isLocked": false, - "linkedName": "EndUpSweep" - }, - { - "anchor": { - "x": 4.141987179487179, - "y": 5.597540064102564 - }, - "prevControl": { - "x": 5.993195112179487, - "y": 6.27832532051282 - }, - "nextControl": { - "x": 2.2907792467948713, - "y": 4.916754807692307 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.495168269230769, - "y": 3.976858974358975 - }, - "prevControl": { - "x": 2.971899038461538, - "y": 4.754495192307693 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "S2" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 1.101706115206352 - }, - "reversed": false, - "folder": "Sweep Paths ✔", - "idealStartingState": { - "velocity": 0, - "rotation": 100.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SweepUp-UBump.path b/src/main/deploy/pathplanner/paths/SweepUp-UBump.path deleted file mode 100644 index 6a677f5..0000000 --- a/src/main/deploy/pathplanner/paths/SweepUp-UBump.path +++ /dev/null @@ -1,59 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 7.625265060240963, - "y": 6.635795180722892 - }, - "prevControl": null, - "nextControl": { - "x": 5.9341913422922445, - "y": 6.703423385851097 - }, - "isLocked": false, - "linkedName": "EndUpSweep" - }, - { - "anchor": { - "x": 3.6, - "y": 5.590272435897436 - }, - "prevControl": { - "x": 5.014102564102563, - "y": 5.408581730769231 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "UB" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.5, - "rotationDegrees": 0.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 0.0 - }, - "reversed": false, - "folder": "Sweep Paths ✔", - "idealStartingState": { - "velocity": 0, - "rotation": 100.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SweepUp.path b/src/main/deploy/pathplanner/paths/SweepUp.path deleted file mode 100644 index 71d8f7b..0000000 --- a/src/main/deploy/pathplanner/paths/SweepUp.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 8.280927710843374, - "y": 1.2921445783132537 - }, - "prevControl": null, - "nextControl": { - "x": 8.147658276155333, - "y": 2.70010142418829 - }, - "isLocked": true, - "linkedName": "StartUpSweep" - }, - { - "anchor": { - "x": 7.625265060240963, - "y": 6.635795180722892 - }, - "prevControl": { - "x": 7.745112220650984, - "y": 5.302535542250024 - }, - "nextControl": null, - "isLocked": true, - "linkedName": "EndUpSweep" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 100.0 - }, - "reversed": false, - "folder": "Sweep Paths ✔", - "idealStartingState": { - "velocity": 0, - "rotation": 100.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/UBump-Climb.path b/src/main/deploy/pathplanner/paths/UBump-Climb.path deleted file mode 100644 index 30b22d0..0000000 --- a/src/main/deploy/pathplanner/paths/UBump-Climb.path +++ /dev/null @@ -1,59 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.6, - "y": 5.590272435897436 - }, - "prevControl": null, - "nextControl": { - "x": 3.185745192307692, - "y": 4.987059294871795 - }, - "isLocked": false, - "linkedName": "UB" - }, - { - "anchor": { - "x": 1.0716738049959909, - "y": 4.599583975068654 - }, - "prevControl": { - "x": 2.559219685260187, - "y": 4.583799522439131 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "Climb1" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.7531428571428569, - "rotationDegrees": 90.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 90.0 - }, - "reversed": false, - "folder": "Climb Paths ✔", - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/UBump-Main1.path b/src/main/deploy/pathplanner/paths/UBump-Main1.path deleted file mode 100644 index 4a0d7ca..0000000 --- a/src/main/deploy/pathplanner/paths/UBump-Main1.path +++ /dev/null @@ -1,61 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.6, - "y": 5.590272435897436 - }, - "prevControl": null, - "nextControl": { - "x": 5.5808211878116625, - "y": 5.791586626032064 - }, - "isLocked": false, - "linkedName": "UB" - }, - { - "anchor": { - "x": 7.675728952774968, - "y": 5.9918925393593625 - }, - "prevControl": { - "x": 7.136407521308552, - "y": 5.938295480976266 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "M1" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "IntakeOn", - "waypointRelativePos": 0.55, - "endWaypointRelativePos": 2.0, - "command": null - } - ], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 0.0 - }, - "reversed": false, - "folder": "Initial✔️", - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/UBump-Main2.path b/src/main/deploy/pathplanner/paths/UBump-Main2.path deleted file mode 100644 index 5e718cf..0000000 --- a/src/main/deploy/pathplanner/paths/UBump-Main2.path +++ /dev/null @@ -1,61 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.6, - "y": 5.590272435897436 - }, - "prevControl": null, - "nextControl": { - "x": 4.672552460510815, - "y": 5.426709321687979 - }, - "isLocked": false, - "linkedName": "UB" - }, - { - "anchor": { - "x": 7.645743952021233, - "y": 5.00357851618481 - }, - "prevControl": { - "x": 6.571965991274344, - "y": 5.166054074779463 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "M2" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "IntakeOn", - "waypointRelativePos": 0.6674757281553398, - "endWaypointRelativePos": 1.0, - "command": null - } - ], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 0.0 - }, - "reversed": false, - "folder": "Initial✔️", - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/UBump-Shoot1.path b/src/main/deploy/pathplanner/paths/UBump-Shoot1.path deleted file mode 100644 index b795d94..0000000 --- a/src/main/deploy/pathplanner/paths/UBump-Shoot1.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.6, - "y": 5.590272435897436 - }, - "prevControl": null, - "nextControl": { - "x": 3.3207114702438494, - "y": 5.694421508069544 - }, - "isLocked": false, - "linkedName": "UB" - }, - { - "anchor": { - "x": 2.971899038461538, - "y": 5.9245833333333335 - }, - "prevControl": { - "x": 3.2993273505587726, - "y": 5.541589285792651 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "S1" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -45.0 - }, - "reversed": false, - "folder": "Initial✔️", - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/UBump-Shoot2.path b/src/main/deploy/pathplanner/paths/UBump-Shoot2.path deleted file mode 100644 index 13d3536..0000000 --- a/src/main/deploy/pathplanner/paths/UBump-Shoot2.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.6, - "y": 5.590272435897436 - }, - "prevControl": null, - "nextControl": { - "x": 3.510693072115397, - "y": 5.356768104820228 - }, - "isLocked": false, - "linkedName": "UB" - }, - { - "anchor": { - "x": 3.495168269230769, - "y": 3.976858974358975 - }, - "prevControl": { - "x": 2.8585646321115363, - "y": 4.014768303888544 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "S2" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 1.101706115206352 - }, - "reversed": false, - "folder": "Initial✔️", - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/UBump-SweepDown-Shoot3.path b/src/main/deploy/pathplanner/paths/UBump-SweepDown-Shoot3.path deleted file mode 100644 index aff3f32..0000000 --- a/src/main/deploy/pathplanner/paths/UBump-SweepDown-Shoot3.path +++ /dev/null @@ -1,160 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.6, - "y": 5.590272435897436 - }, - "prevControl": null, - "nextControl": { - "x": 4.1725122228676925, - "y": 5.750543887023177 - }, - "isLocked": false, - "linkedName": "UB" - }, - { - "anchor": { - "x": 6.882180722888714, - "y": 6.482807228916967 - }, - "prevControl": { - "x": 5.723843373491124, - "y": 5.8817831325314245 - }, - "nextControl": { - "x": 7.883784999660117, - "y": 7.002507561204016 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 8.279311758588921, - "y": 6.670571423499865 - }, - "prevControl": { - "x": 8.307081411118329, - "y": 7.352898309838755 - }, - "nextControl": { - "x": 8.251092528858518, - "y": 5.977197995926804 - }, - "isLocked": false, - "linkedName": "StartDownSweep" - }, - { - "anchor": { - "x": 7.735517857150679, - "y": 1.3139999999959786 - }, - "prevControl": { - "x": 7.891073413953844, - "y": 2.283940806712021 - }, - "nextControl": { - "x": 7.5709291877882645, - "y": 0.28773472205604667 - }, - "isLocked": false, - "linkedName": "EndDownSweep" - }, - { - "anchor": { - "x": 5.559927710840523, - "y": 2.144506024097693 - }, - "prevControl": { - "x": 6.715737176628737, - "y": 1.5081614867539286 - }, - "nextControl": { - "x": 4.587361445777435, - "y": 2.6799638554243024 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.6085176282051274, - "y": 1.4477243589743591 - }, - "prevControl": { - "x": 4.150253012045344, - "y": 2.253783132531429 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "S3" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.7, - "rotationDegrees": 0.0 - }, - { - "waypointRelativePos": 1.9295238095238036, - "rotationDegrees": -100.0 - }, - { - "waypointRelativePos": 3.0114285714285707, - "rotationDegrees": -100.0 - }, - { - "waypointRelativePos": 3.9028571428571404, - "rotationDegrees": 45.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "", - "waypointRelativePos": 0.7914798206277904, - "endWaypointRelativePos": null, - "command": null - }, - { - "name": "IntakeOn", - "waypointRelativePos": 1.930493273542592, - "endWaypointRelativePos": null, - "command": null - }, - { - "name": "ReadyUp", - "waypointRelativePos": 2.9977578475336344, - "endWaypointRelativePos": null, - "command": null - }, - { - "name": "IntakeOff", - "waypointRelativePos": 3.517937219730874, - "endWaypointRelativePos": null, - "command": null - } - ], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 39.09385888622944 - }, - "reversed": false, - "folder": "Compound Paths", - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/UBump-SweepDown.path b/src/main/deploy/pathplanner/paths/UBump-SweepDown.path deleted file mode 100644 index 8fffdd7..0000000 --- a/src/main/deploy/pathplanner/paths/UBump-SweepDown.path +++ /dev/null @@ -1,59 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.6, - "y": 5.590272435897436 - }, - "prevControl": null, - "nextControl": { - "x": 6.95867469879518, - "y": 6.06755421686747 - }, - "isLocked": false, - "linkedName": "UB" - }, - { - "anchor": { - "x": 8.279311758588921, - "y": 6.670571423499865 - }, - "prevControl": { - "x": 8.010682890307153, - "y": 8.053463056166462 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "StartDownSweep" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.27771428571428713, - "rotationDegrees": 0.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -100.0 - }, - "reversed": false, - "folder": "Initial✔️", - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/UTrench-Climb1.path b/src/main/deploy/pathplanner/paths/UTrench-Climb1.path deleted file mode 100644 index 97c258d..0000000 --- a/src/main/deploy/pathplanner/paths/UTrench-Climb1.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.6, - "y": 7.4298 - }, - "prevControl": null, - "nextControl": { - "x": 2.966224917367802, - "y": 5.808254396375313 - }, - "isLocked": false, - "linkedName": "UT" - }, - { - "anchor": { - "x": 1.0716738049959909, - "y": 4.599583975068654 - }, - "prevControl": { - "x": 2.2437626499694, - "y": 4.597890853696445 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "Climb1" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 90.0 - }, - "reversed": false, - "folder": "Climb Paths ✔", - "idealStartingState": { - "velocity": 0, - "rotation": 90.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/UTrench-Shoot1.path b/src/main/deploy/pathplanner/paths/UTrench-Shoot1.path deleted file mode 100644 index 4a0fcee..0000000 --- a/src/main/deploy/pathplanner/paths/UTrench-Shoot1.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.6, - "y": 7.4298 - }, - "prevControl": null, - "nextControl": { - "x": 3.784613565375717, - "y": 7.598375892343081 - }, - "isLocked": false, - "linkedName": "UT" - }, - { - "anchor": { - "x": 2.971899038461538, - "y": 5.9245833333333335 - }, - "prevControl": { - "x": 2.9448849566443043, - "y": 6.173119526640408 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "S1" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -45.0 - }, - "reversed": false, - "folder": "Initial✔️", - "idealStartingState": { - "velocity": 0, - "rotation": 90.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 127a228..f981b63 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -1,44 +1,21 @@ { - "robotWidth": 0.69, - "robotLength": 0.7, + "robotWidth": 0.9, + "robotLength": 0.9, "holonomicMode": true, - "pathFolders": [ - "Climb Paths ✔", - "Compound Paths", - "Depot✔", - "Gather✔️", - "Initial✔️", - "Shoot✔️ ", - "Sweep Paths ✔" - ], - "autoFolders": [ - "Sweep First", - "Only Climb", - "Shoot First" - ], + "pathFolders": [], + "autoFolders": [], "defaultMaxVel": 3.0, "defaultMaxAccel": 3.0, "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, - "defaultNominalVoltage": 12.0, - "robotMass": 52.0, + "robotMass": 74.088, "robotMOI": 6.883, + "robotWheelbase": 0.546, "robotTrackwidth": 0.546, "driveWheelRadius": 0.048, "driveGearing": 5.143, "maxDriveSpeed": 5.45, "driveMotorType": "krakenX60FOC", "driveCurrentLimit": 60.0, - "wheelCOF": 1.2, - "flModuleX": 0.273, - "flModuleY": 0.273, - "frModuleX": 0.273, - "frModuleY": -0.273, - "blModuleX": -0.273, - "blModuleY": 0.273, - "brModuleX": -0.273, - "brModuleY": -0.273, - "bumperOffsetX": 0.0, - "bumperOffsetY": 0.0, - "robotFeatures": [] -} \ No newline at end of file + "wheelCOF": 1.2 +} diff --git a/src/main/java/frc/lib/W8/io/absoluteencoder/AbsoluteEncoderIOCANCoder.java b/src/main/java/frc/lib/W8/io/absoluteencoder/AbsoluteEncoderIOCANCoder.java index e915ac2..d6a3641 100644 --- a/src/main/java/frc/lib/W8/io/absoluteencoder/AbsoluteEncoderIOCANCoder.java +++ b/src/main/java/frc/lib/W8/io/absoluteencoder/AbsoluteEncoderIOCANCoder.java @@ -41,13 +41,7 @@ public AbsoluteEncoderIOCANCoder( angle = CANCoder.getAbsolutePosition(); - // Further reduce update frequency for absolute encoder since it's mainly used for - // initialization - updateThread.CTRECheckErrorAndRetry( - () -> angle.setUpdateFrequency(10.0)); // Further reduced from 25.0 - - // Optimize bus utilization - CANCoder.optimizeBusUtilization(1.0); + updateThread.CTRECheckErrorAndRetry(() -> angle.setUpdateFrequency(200)); } @Override diff --git a/src/main/java/frc/lib/W8/io/distancesensor/DistanceSensorIOCANRange.java b/src/main/java/frc/lib/W8/io/distancesensor/DistanceSensorIOCANRange.java index 02a2d54..90c66a2 100644 --- a/src/main/java/frc/lib/W8/io/distancesensor/DistanceSensorIOCANRange.java +++ b/src/main/java/frc/lib/W8/io/distancesensor/DistanceSensorIOCANRange.java @@ -51,13 +51,6 @@ public DistanceSensorIOCANRange(Device.CAN id, String name, CANrangeConfiguratio ambientSignal = CANRange.getAmbientSignal(); distance = CANRange.getDistance(); - - // Set very low update frequencies for distance sensors (not critical for control) - updateThread.CTRECheckErrorAndRetry( - () -> BaseStatusSignal.setUpdateFrequencyForAll(10.0, ambientSignal, distance)); - - // Optimize bus utilization - CANRange.optimizeBusUtilization(1.0); } @Override diff --git a/src/main/java/frc/lib/W8/io/motor/MotorIOTalonFX.java b/src/main/java/frc/lib/W8/io/motor/MotorIOTalonFX.java index ac6d766..2bbd047 100644 --- a/src/main/java/frc/lib/W8/io/motor/MotorIOTalonFX.java +++ b/src/main/java/frc/lib/W8/io/motor/MotorIOTalonFX.java @@ -105,7 +105,6 @@ public MotorIOTalonFX( this.name = name; motor = new TalonFX(main.id(), main.bus()); - motor.optimizeBusUtilization(); updateThread.CTRECheckErrorAndRetry(() -> motor.getConfigurator().apply(config)); // Initialize lists @@ -141,38 +140,17 @@ public MotorIOTalonFX( closedLoopReference = motor.getClosedLoopReference(); closedLoopReferenceSlope = motor.getClosedLoopReferenceSlope(); - // Set different update frequencies based on signal importance - // Critical signals for control (reduced frequency) updateThread.CTRECheckErrorAndRetry( () -> BaseStatusSignal.setUpdateFrequencyForAll( - 50.0, position, velocity)); // Critical for control loops + 100, position, velocity, supplyCurrent, supplyCurrent, torqueCurrent, temperature)); - // Important telemetry (lower frequency) updateThread.CTRECheckErrorAndRetry( () -> BaseStatusSignal.setUpdateFrequencyForAll( - 20.0, supplyCurrent, torqueCurrent)); // Important for monitoring + 200, closedLoopError, closedLoopReference, closedLoopReferenceSlope)); - // Non-critical telemetry (very low frequency) - updateThread.CTRECheckErrorAndRetry( - () -> - BaseStatusSignal.setUpdateFrequencyForAll( - 10.0, temperature, supplyVoltage)); // Less critical - - // Control loop feedback (reduced frequency when in closed-loop) - updateThread.CTRECheckErrorAndRetry( - () -> - BaseStatusSignal.setUpdateFrequencyForAll( - 100.0, closedLoopError, closedLoopReference, closedLoopReferenceSlope)); - - // Optimize bus utilization with longer timeout for better optimization - motor.optimizeBusUtilization(1.0, 1.0); // Increased timeout for better optimization - - // Optimize follower bus utilization - for (TalonFX follower : followers) { - follower.optimizeBusUtilization(1.0, 1.0); - } + motor.optimizeBusUtilization(0, 1.0); } /** @@ -263,17 +241,19 @@ protected ControlType getCurrentControlType() { */ @Override public void updateInputs(MotorInputs inputs) { - // Refresh signals in groups based on priority - // Refresh critical signals first - boolean criticalSignalsOk = BaseStatusSignal.refreshAll(position, velocity).isOK(); - - // Refresh telemetry signals - boolean telemetrySignalsOk = - BaseStatusSignal.refreshAll(supplyCurrent, torqueCurrent, temperature, supplyVoltage) + inputs.connected = + BaseStatusSignal.refreshAll( + position, + velocity, + supplyVoltage, + supplyCurrent, + torqueCurrent, + temperature, + closedLoopError, + closedLoopReference, + closedLoopReferenceSlope) .isOK(); - inputs.connected = criticalSignalsOk && telemetrySignalsOk; - inputs.position = position.getValue(); inputs.velocity = velocity.getValue(); inputs.appliedVoltage = supplyVoltage.getValue(); @@ -281,47 +261,34 @@ public void updateInputs(MotorInputs inputs) { inputs.torqueCurrent = torqueCurrent.getValue(); inputs.temperature = temperature.getValue(); - // Check current control modes + // Interpret control-loop status signals conditionally based on current mode + Double closedLoopErrorValue = closedLoopError.getValue(); + Double closedLoopTargetValue = closedLoopReference.getValue(); + boolean isRunningPositionControl = isRunningPositionControl(); boolean isRunningMotionMagic = isRunningMotionMagic(); boolean isRunningVelocityControl = isRunningVelocityControl(); - // Only update closed-loop signals when in closed-loop modes - if (isRunningPositionControl || isRunningVelocityControl) { - BaseStatusSignal.refreshAll(closedLoopError, closedLoopReference, closedLoopReferenceSlope); - - // Interpret control-loop status signals conditionally based on current mode - Double closedLoopErrorValue = closedLoopError.getValue(); - Double closedLoopTargetValue = closedLoopReference.getValue(); - - inputs.positionError = isRunningPositionControl ? Rotations.of(closedLoopErrorValue) : null; - - inputs.activeTrajectoryPosition = - isRunningPositionControl && isRunningMotionMagic - ? Rotations.of(closedLoopTargetValue) - : null; - - inputs.goalPosition = isRunningPositionControl ? goalPosition : null; - - if (isRunningVelocityControl) { - inputs.velocityError = RotationsPerSecond.of(closedLoopErrorValue); - inputs.activeTrajectoryVelocity = RotationsPerSecond.of(closedLoopTargetValue); - } else if (isRunningPositionControl && isRunningMotionMagic) { - var targetVelocity = closedLoopReferenceSlope.getValue(); - inputs.velocityError = - RotationsPerSecond.of(targetVelocity - inputs.velocity.in(RotationsPerSecond)); - inputs.activeTrajectoryVelocity = RotationsPerSecond.of(targetVelocity); - } else { - inputs.velocityError = null; - inputs.activeTrajectoryVelocity = null; - } + inputs.positionError = isRunningPositionControl ? Rotations.of(closedLoopErrorValue) : null; + + inputs.activeTrajectoryPosition = + isRunningPositionControl && isRunningMotionMagic + ? Rotations.of(closedLoopTargetValue) + : null; + + inputs.goalPosition = isRunningPositionControl ? goalPosition : null; + + if (isRunningVelocityControl) { + inputs.velocityError = RotationsPerSecond.of(closedLoopErrorValue); + inputs.activeTrajectoryVelocity = RotationsPerSecond.of(closedLoopTargetValue); + } else if (isRunningPositionControl && isRunningMotionMagic) { + var targetVelocity = closedLoopReferenceSlope.getValue(); + inputs.velocityError = + RotationsPerSecond.of(targetVelocity - inputs.velocity.in(RotationsPerSecond)); + inputs.activeTrajectoryVelocity = RotationsPerSecond.of(targetVelocity); } else { - // Not in closed-loop mode, set control loop values to null - inputs.positionError = null; inputs.velocityError = null; inputs.activeTrajectoryVelocity = null; - inputs.activeTrajectoryPosition = null; - inputs.goalPosition = null; } inputs.controlType = getCurrentControlType(); diff --git a/src/main/java/frc/lib/W8/mechanisms/rotary/RotaryMechanismReal.java b/src/main/java/frc/lib/W8/mechanisms/rotary/RotaryMechanismReal.java index d83d8df..b8b7b62 100644 --- a/src/main/java/frc/lib/W8/mechanisms/rotary/RotaryMechanismReal.java +++ b/src/main/java/frc/lib/W8/mechanisms/rotary/RotaryMechanismReal.java @@ -107,9 +107,4 @@ public Angle getPosition() { public AngularVelocity getVelocity() { return inputs.velocity; } - - @Override - public Current getSupplyCurrent() { - return inputs.supplyCurrent; - } } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 06bdd14..ca1ba8f 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -13,29 +13,15 @@ package frc.robot; -import static edu.wpi.first.units.Units.Degrees; -import static edu.wpi.first.units.Units.Foot; -import static edu.wpi.first.units.Units.Inches; -import static edu.wpi.first.units.Units.KilogramSquareMeters; -import static edu.wpi.first.units.Units.Kilograms; -import static edu.wpi.first.units.Units.Meters; -import static edu.wpi.first.units.Units.Radians; -import static edu.wpi.first.units.Units.RadiansPerSecond; -import static edu.wpi.first.units.Units.RadiansPerSecondPerSecond; -import static edu.wpi.first.units.Units.Rotations; -import static edu.wpi.first.units.Units.RotationsPerSecond; -import static edu.wpi.first.units.Units.RotationsPerSecondPerSecond; -import static edu.wpi.first.units.Units.Second; -import static edu.wpi.first.units.Units.Volts; +import static edu.wpi.first.units.Units.*; import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.CANdleConfiguration; import com.ctre.phoenix6.configs.CANdleFeaturesConfigs; import com.ctre.phoenix6.configs.LEDConfigs; import com.ctre.phoenix6.configs.Slot0Configs; -import com.ctre.phoenix6.configs.Slot1Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.RainbowAnimation; +import com.ctre.phoenix6.controls.*; import com.ctre.phoenix6.signals.Enable5VRailValue; import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; import com.ctre.phoenix6.signals.InvertedValue; @@ -62,9 +48,6 @@ import edu.wpi.first.units.measure.MomentOfInertia; import edu.wpi.first.units.measure.Velocity; import edu.wpi.first.units.measure.Voltage; -import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj.DutyCycle; -import edu.wpi.first.wpilibj.DutyCycleEncoder; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; import frc.lib.W8.mechanisms.linear.LinearMechanism.LinearMechCharacteristics; @@ -201,7 +184,7 @@ public class HopperConstants { public static final AngularAcceleration ACCELERATION = CRUISE_VELOCITY.div(0.1).per(Second); public static final Velocity JERK = ACCELERATION.per(Second); - public static final double GEARING = (1.0 / 1.0); + public static final double GEARING = (5.0 / 1.0); public static final Distance MIN_DISTANCE = Inches.of(0.0); public static final Distance MAX_DISTANCE = Inches.of(10.0); public static final Distance STARTING_DISTANCE = Inches.of(0.0); @@ -234,21 +217,21 @@ public static TalonFXConfiguration getFXConfig() { config.Voltage.PeakReverseVoltage = -12.0; config.MotorOutput.NeutralMode = NeutralModeValue.Brake; - config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; - config.SoftwareLimitSwitch.ForwardSoftLimitEnable = false; - // config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = - // CONVERTER.toAngle(MAX_DISTANCE).in(Rotations); + config.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; + config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = + CONVERTER.toAngle(MAX_DISTANCE).in(Rotations); - config.SoftwareLimitSwitch.ReverseSoftLimitEnable = false; - // config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = - // CONVERTER.toAngle(MIN_DISTANCE).in(Rotations); + config.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; + config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = + CONVERTER.toAngle(MIN_DISTANCE).in(Rotations); config.Feedback.RotorToSensorRatio = 1.0; config.Feedback.SensorToMechanismRatio = GEARING; - config.Slot0 = new Slot0Configs().withKP(10).withKI(0.0).withKD(0.0).withKS(2.5).withKV(0.05); + config.Slot0 = new Slot0Configs().withKP(0.75).withKI(0.0).withKD(0.0); config.MotionMagic.MotionMagicCruiseVelocity = CRUISE_VELOCITY.in(RotationsPerSecond); config.MotionMagic.MotionMagicAcceleration = ACCELERATION.in(RotationsPerSecondPerSecond); @@ -283,16 +266,14 @@ public final class ShooterConstants { public static final double HEIGHT_DIFFERENCE = 1.295; // Meters between flywheel center and top of hub opening public static final double EXIT_VELOCITY = 7.4; // m/s from ReCalc Flywheel Calculator - public static final AngularVelocity HOOD_VELOCITY = RotationsPerSecond.of(-0.0005); - public static final AngularAcceleration HOOD_ACCELERATION = RotationsPerSecondPerSecond.of(.2); + public static final AngularVelocity HOOD_VELOCITY = RotationsPerSecond.of(1.0); + public static final AngularAcceleration HOOD_ACCELERATION = RotationsPerSecondPerSecond.of(1.0); public static final Velocity HOOD_JERK = HOOD_ACCELERATION.per(Second); public static final double HOOD_TOLERANCE = 1.0; // In degrees public static final double GRAVITY = 9.81; // m/s^2 public static final double IDLE_HOOD_ANGLE = 25.0; // degrees public static final RotaryMechCharacteristics CONSTANTS = new RotaryMechCharacteristics(OFFSET, WHEEL_RADIUS, MIN_ANGLE, MAX_ANGLE, STARTING_ANGLE); - - public static final double HARD_STOP_CURRENT_LIMIT = 2.5; } // Needs tuned @@ -311,7 +292,7 @@ public class ShooterFeederConstants { // Velocity PID private static Slot0Configs SLOT0CONFIG = - new Slot0Configs().withKP(18.0).withKI(0.0).withKD(0.01).withKV(0.05).withKS(8.0); + new Slot0Configs().withKP(0.0).withKI(0.0).withKD(0.0).withKS(10.0); public static TalonFXConfiguration getFXConfig(boolean invert) { TalonFXConfiguration config = new TalonFXConfiguration(); @@ -452,19 +433,19 @@ public class ShooterRotaryConstants { public static final Angle TOLERANCE = Degrees.of(2.0); - public static final AngularVelocity CRUISE_VELOCITY = RadiansPerSecond.of(100); - public static final AngularAcceleration ACCELERATION = RadiansPerSecondPerSecond.of(200); - public static final Velocity JERK = ACCELERATION.per(Second); + // public static final AngularVelocity CRUISE_VELOCITY = Units.RadiansPerSecond.of(1); + // public static final AngularAcceleration ACCELERATION = + // CRUISE_VELOCITY.div(0.1).per(Units.Second); + // public static final Velocity JERK = ACCELERATION.per(Second); private static final double ROTOR_TO_SENSOR = (1.0 / 1.0); - //private static final double SENSOR_TO_MECHANISM = (50.0 / 1.0); - private static final double SENSOR_TO_MECHANISM = 1 / 50; + private static final double SENSOR_TO_MECHANISM = (50.0 / 1.0); public static final Translation3d OFFSET = Translation3d.kZero; public static final Angle MIN_ANGLE = Degrees.of(0.0); - public static final Angle MAX_ANGLE = Degrees.of(25); - public static final Angle STARTING_ANGLE = Degrees.of(0.0); + public static final Angle MAX_ANGLE = Degrees.of(45.0); + public static final Angle STARTING_ANGLE = Rotations.of(0.0); public static final Distance ARM_LENGTH = Meters.of(1.0); public static final RotaryMechCharacteristics CONSTANTS = @@ -480,10 +461,7 @@ public class ShooterRotaryConstants { // Positional PID private static Slot0Configs SLOT0CONFIG = - new Slot0Configs().withKP(250.0).withKI(0.0).withKD(0.0).withKS(10.0).withKV(0.0); - // Velocity PID - private static Slot1Configs SLOT1CONFIG = - new Slot1Configs().withKP(30.0).withKI(0.0).withKD(0.0).withKS(40.0).withKV(0.0); + new Slot0Configs().withKP(30.0).withKI(0.0).withKD(5.0); /** * Creates and returns the TalonFX motor controller configuration for the rotary mechanism. @@ -519,20 +497,16 @@ public static TalonFXConfiguration getFXConfig() { config.MotorOutput.NeutralMode = NeutralModeValue.Brake; config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; - config.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; - config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = MAX_ANGLE.in(Degrees); + config.SoftwareLimitSwitch.ForwardSoftLimitEnable = false; + // config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = MAX_ANGLE.in(Units.Rotaitons); - config.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; - config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = MIN_ANGLE.in(Degrees); + config.SoftwareLimitSwitch.ReverseSoftLimitEnable = false; + // config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = MIN_ANGLE.in(Units.Rotations); config.Feedback.RotorToSensorRatio = ROTOR_TO_SENSOR; config.Feedback.SensorToMechanismRatio = SENSOR_TO_MECHANISM; config.Slot0 = SLOT0CONFIG; - config.Slot1 = SLOT1CONFIG; - - config.MotionMagic.MotionMagicCruiseVelocity = CRUISE_VELOCITY.in(RotationsPerSecond); - config.MotionMagic.MotionMagicAcceleration = ACCELERATION.in(RotationsPerSecondPerSecond); return config; } @@ -654,10 +628,6 @@ public static VisionSystemSim getSystemSim() { } } - // config.Slot0 = new Slot0Configs().withKP(0.75).withKI(0.0).withKD(0.0); - private static Slot0Configs SLOT0CONFIG = - new Slot0Configs().withKP(30).withKI(0.0).withKD(0.0).withKV(0.05).withKS(8.0); - public static TalonFXConfiguration getFXConfig() { TalonFXConfiguration config = new TalonFXConfiguration(); @@ -675,19 +645,19 @@ public static TalonFXConfiguration getFXConfig() { config.MotorOutput.NeutralMode = NeutralModeValue.Brake; config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; - config.SoftwareLimitSwitch.ForwardSoftLimitEnable = false; - // config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = - // CONVERTER.toAngle(MAX_DISTANCE).in(Rotations); + config.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; + config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = + CONVERTER.toAngle(MAX_DISTANCE).in(Rotations); - config.SoftwareLimitSwitch.ReverseSoftLimitEnable = false; - // config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = - // CONVERTER.toAngle(MIN_DISTANCE).in(Rotations); + config.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; + config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = + CONVERTER.toAngle(MIN_DISTANCE).in(Rotations); config.Feedback.RotorToSensorRatio = 1.0; config.Feedback.SensorToMechanismRatio = GEARING; - config.Slot0 = SLOT0CONFIG; + config.Slot0 = new Slot0Configs().withKP(0.75).withKI(0.0).withKD(0.0); config.MotionMagic.MotionMagicCruiseVelocity = CRUISE_VELOCITY.in(RotationsPerSecond); config.MotionMagic.MotionMagicAcceleration = ACCELERATION.in(RotationsPerSecondPerSecond); @@ -703,8 +673,8 @@ public class IntakePivotConstants { public static final Angle MIN_ANGLE = Degrees.of(0.0); public static final Angle MAX_ANGLE = Degrees.of(130.0); - public static final Angle PICKUP_ANGLE = Degrees.of(120.0); - public static final Angle STOW_ANGLE = Degrees.of(0.0); + public static final Angle PICKUP_ANGLE = MAX_ANGLE; + public static final Angle STOW_ANGLE = Rotations.of(0.0); public static final Angle TOLERANCE = Degrees.of(1.0); @@ -725,14 +695,9 @@ public class IntakePivotConstants { public static final DCMotor DCMOTOR = DCMotor.getKrakenX60(1); public static final MomentOfInertia MOI = KilogramSquareMeters.of(1); - public static final int ENCODER_CHANNEL = 9; - public static final DutyCycleEncoder ENCODER1 = - new DutyCycleEncoder(new DutyCycle(new DigitalInput(ENCODER_CHANNEL))); - // public static final Encoder ENCODER = new Encoder(ENCODER_CHANNEL, ENCODER_CHANNEL); - // Positional PID public static final Slot0Configs SLOT_0_CONFIG = - new Slot0Configs().withKP(.0).withKI(0.0).withKD(0).withKS(0.00).withKV(0.0); + new Slot0Configs().withKP(100.0).withKI(0.0).withKD(0).withKS(0.07).withKV(0.1); // ^^^ CHANGE @@ -773,91 +738,25 @@ public static TalonFXConfiguration getFXConfig() { } } - public class FeederConstants { - public static final AngularVelocity FEED_SPEED = RotationsPerSecond.of(20.0); - public static final AngularAcceleration FEED_ACCELERATION = RotationsPerSecondPerSecond.of(50); - - public static String NAME = "ShooterTower"; - - public static final AngularVelocity MAX_VELOCITY = RadiansPerSecond.of(2 * Math.PI); - public static final AngularAcceleration MAX_ACCELERATION = MAX_VELOCITY.per(Second); - - private static final double GEARING = (15.0 / 30.0); - - public static final AngularVelocity TOLERANCE = MAX_VELOCITY.times(0.1); - - public static final DCMotor DCMOTOR = DCMotor.getKrakenX60(1); - public static final MomentOfInertia MOI = KilogramSquareMeters.of(1.0); - - // Velocity PID - private static Slot0Configs SLOT0CONFIG = - new Slot0Configs().withKP(5.0).withKI(0.0).withKD(0.0).withKV(0.05).withKS(12.0); - - public static TalonFXConfiguration getFXConfig(boolean invert) { - TalonFXConfiguration config = new TalonFXConfiguration(); - - config.CurrentLimits.SupplyCurrentLimitEnable = Robot.isReal(); - config.CurrentLimits.SupplyCurrentLimit = 40.0; - config.CurrentLimits.SupplyCurrentLowerLimit = 40.0; - config.CurrentLimits.SupplyCurrentLowerTime = 0.1; - - config.CurrentLimits.StatorCurrentLimitEnable = Robot.isReal(); - config.CurrentLimits.StatorCurrentLimit = 80.0; - - config.Voltage.PeakForwardVoltage = 12.0; - config.Voltage.PeakReverseVoltage = -12.0; - - config.MotorOutput.NeutralMode = NeutralModeValue.Brake; - // config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; - if (invert == true) { - config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; - } else { - config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; - } - - config.SoftwareLimitSwitch.ForwardSoftLimitEnable = false; - - config.SoftwareLimitSwitch.ReverseSoftLimitEnable = false; - - config.Feedback.RotorToSensorRatio = 1.0; - - config.Feedback.SensorToMechanismRatio = GEARING; - - config.Slot0 = SLOT0CONFIG; - - return config; - } - } - public class ClimberConstants { public static final DCMotor DCMOTOR = DCMotor.getKrakenX60(1); public static final Mass CARRIAGE_MASS = Kilograms.of(2.5); public static final String MOTOR_NAME = "Climber motor"; - public static final String NAME = "Climber"; public static final Distance TOLERANCE = Inches.of(0.1); - public static final double GEARING = 1; + public static final double GEARING = (5.0 / 1.0); public static final Distance MIN_DISTANCE = Inches.of(0.0); public static final Distance MAX_DISTANCE = Inches.of(10.0); public static final Distance STARTING_DISTANCE = Inches.of(0.0); public static final Distance DRUM_RADIUS = Inches.of(2.0); public static final DistanceAngleConverter CONVERTER = new DistanceAngleConverter(DRUM_RADIUS); - public static final AngularVelocity CRUISE_VELOCITY = - RadiansPerSecond.of(2 * Math.PI).times(8000.0); - public static final AngularVelocity LOWER_VELOCITY = - RadiansPerSecond.of(-2 * Math.PI).times(8000.0); - public static final AngularVelocity CALIBRATE_VELOCITY = - RadiansPerSecond.of(-2 * Math.PI).times(1000.0); - + RadiansPerSecond.of(2 * Math.PI).times(10.0); public static final AngularAcceleration ACCELERATION = CRUISE_VELOCITY.div(0.1).per(Second); public static final Velocity JERK = ACCELERATION.per(Second); public static final Distance BOTTOM = Inches.of(0.0); public static final Distance MIDDLE = Inches.of(15.0); public static final Distance TOP = Inches.of(30.0); - public static final double L1 = 10; - public static final double STOW = 0; - public static final LinearMechCharacteristics CHARACTERISTICS = new LinearMechCharacteristics( new Translation3d(0.0, 0.0, 0.0), @@ -883,20 +782,19 @@ public static TalonFXConfiguration getFXConfig() { config.MotorOutput.NeutralMode = NeutralModeValue.Brake; config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; - config.SoftwareLimitSwitch.ForwardSoftLimitEnable = false; - // config.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; - // config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = - // CONVERTER.toAngle(MAX_DISTANCE).in(Rotations); - config.SoftwareLimitSwitch.ReverseSoftLimitEnable = false; - // config.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; - // config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = - // CONVERTER.toAngle(MIN_DISTANCE).in(Rotations); + config.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; + config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = + CONVERTER.toAngle(MAX_DISTANCE).in(Rotations); + + config.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; + config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = + CONVERTER.toAngle(MIN_DISTANCE).in(Rotations); config.Feedback.RotorToSensorRatio = 1.0; config.Feedback.SensorToMechanismRatio = GEARING; - config.Slot0 = new Slot0Configs().withKP(10).withKI(0.0).withKD(0.0); + config.Slot0 = new Slot0Configs().withKP(0.75).withKI(0.0).withKD(0.0); config.MotionMagic.MotionMagicCruiseVelocity = CRUISE_VELOCITY.in(RotationsPerSecond); config.MotionMagic.MotionMagicAcceleration = ACCELERATION.in(RotationsPerSecondPerSecond); @@ -910,6 +808,6 @@ public static TalonFXConfiguration getFXConfig() { public static final AngularAcceleration ANGULAR_ACCELERATION = RotationsPerSecondPerSecond.of(1); public static final double CLIMB_SPEED = 1.0; - public static final double HARD_STOP_CURRENT_LIMIT = 20.0; + public static final double HARD_STOP_CURRENT_LIMIT = 50.0; } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index cd9b2ec..4ed57d2 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -13,11 +13,10 @@ package frc.robot; -import static edu.wpi.first.units.Units.RotationsPerSecond; - -import com.ctre.phoenix6.BaseStatusSignal; +import au.grapplerobotics.LaserCan; import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.auto.NamedCommands; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; @@ -36,7 +35,6 @@ import frc.lib.W8.mechanisms.rotary.RotaryMechanismReal; import frc.lib.W8.mechanisms.rotary.RotaryMechanismSim; import frc.robot.Constants.ClimberConstants; -import frc.robot.Constants.FeederConstants; import frc.robot.Constants.HopperConstants; import frc.robot.Constants.IntakeFlywheelConstants; import frc.robot.Constants.IntakeFlywheelConstants.VisionConstants; @@ -78,9 +76,8 @@ public class RobotContainer { private final Climber climber; private final Shooter shooter; public final Intake intake; - // private final BallCounter ballCounter; + private final BallCounter ballCounter; private final Vision vision; - private final Climber climber; // Controller private final CommandXboxController controller = new CommandXboxController(0); @@ -91,10 +88,8 @@ public class RobotContainer { /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { // Check if the robot is real before using the ball counter! - // if (Robot.isReal()) ballCounter = new BallCounter(new LaserCan(1)); - // else ballCounter = null; - - BaseStatusSignal.setUpdateFrequencyForAll(50.0); + if (Robot.isReal()) ballCounter = new BallCounter(new LaserCan(1)); + else ballCounter = null; switch (Constants.currentMode) { case REAL: @@ -117,17 +112,24 @@ public RobotContainer() { new Shooter( new FlywheelMechanismReal( new MotorIOTalonFX( - "ShooterLeftFlywheel", - ShooterFlywheelConstants.getFXConfig(true), + ShooterFlywheelConstants.NAME_L, + ShooterFlywheelConstants.getFXConfig(), Ports.LeftFlywheel)), new FlywheelMechanismReal( new MotorIOTalonFX( - "ShooterRightFlywheel", - ShooterFlywheelConstants.getFXConfig(false), + ShooterFlywheelConstants.NAME_R, + ShooterFlywheelConstants.getFXConfig(), Ports.RightFlywheel)), new FlywheelMechanismReal( new MotorIOTalonFX( - "ShooterTower", FeederConstants.getFXConfig(false), Ports.TowerRoller)), + ShooterFeederConstants.NAME, + ShooterFeederConstants.getFXConfig(false), + Ports.Spindexer)), + new FlywheelMechanismReal( + new MotorIOTalonFX( + ShooterTowerConstants.NAME, + ShooterTowerConstants.getFXConfig(), + Ports.TowerRoller)), new RotaryMechanismReal( new MotorIOTalonFX( ShooterRotaryConstants.NAME, @@ -150,14 +152,6 @@ public RobotContainer() { Ports.IntakePivot), IntakePivotConstants.CONSTANTS, Optional.empty())); - climber = - new Climber( - new LinearMechanismReal( - new MotorIOTalonFX( - ClimberConstants.MOTOR_NAME, - ClimberConstants.getFXConfig(), - Ports.ClimberMotor), - ClimberConstants.CHARACTERISTICS)); vision = new Vision( new VisionIOPhotonVision( @@ -206,8 +200,8 @@ public RobotContainer() { ShooterFlywheelConstants.TOLERANCE), new FlywheelMechanismSim( new MotorIOTalonFXSim( - ShooterFlywheelConstants.NAME, - ShooterFlywheelConstants.getFXConfig(true), + ShooterFlywheelConstants.NAME_R, + ShooterFlywheelConstants.getFXConfig(), Ports.RightFlywheel), ShooterFlywheelConstants.DCMOTOR, ShooterFlywheelConstants.MOI, @@ -268,6 +262,7 @@ public RobotContainer() { VisionConstants.aprilTagLayout, PoseStrategy.CONSTRAINED_SOLVEPNP, VisionConstants.getSystemSim())); + climber = new Climber( new LinearMechanismSim( @@ -305,31 +300,14 @@ public RobotContainer() { new Intake( new FlywheelMechanism() {}, new RotaryMechanism(IntakePivotConstants.NAME, IntakePivotConstants.CONSTANTS) {}); - vision = new Vision(null); - + vision = new Vision(new VisionIO() {}); climber = new Climber( - new LinearMechanism(ClimberConstants.NAME, ClimberConstants.CHARACTERISTICS) {}); + new LinearMechanism( + ClimberConstants.MOTOR_NAME, ClimberConstants.CHARACTERISTICS) {}); break; } - // Extends climber arm - NamedCommands.registerCommand("ExtendClimber", getAutonomousCommand()); - // Retracts climber arm - NamedCommands.registerCommand("Climb", getAutonomousCommand()); - - // Bring flywheel up to speed + hood to position for known locations? - NamedCommands.registerCommand("ReadyUp", getAutonomousCommand()); - // Shoots - NamedCommands.registerCommand("Shoot", getAutonomousCommand()); - - // Runs Intake Rollers - NamedCommands.registerCommand("IntakeOn", getAutonomousCommand()); - // Stops Intake Rollers - NamedCommands.registerCommand("IntakeOff", getAutonomousCommand()); - // Extends the intake - NamedCommands.registerCommand("IntakeDown", getAutonomousCommand()); - // Set up auto routines autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser()); @@ -382,59 +360,24 @@ private void configureButtonBindings() { // controller.x().onTrue(Commands.runOnce(drive::stopWithX, drive)); // Reset gyro to 0° when B button is pressed - // controller - // .b() - // .onTrue( - // Commands.runOnce( - // () -> - // drive.setPose( - // new Pose2d(drive.getPose().getTranslation(), new Rotation2d())), - // drive) - // .ignoringDisable(true)); - - // controller - // .x() - // .onTrue(Commands.runOnce(() -> hopper.setGoal(HopperConstants.HOPPER_POSITION), hopper)); - - // controller.rightTrigger().onTrue(Commands.runOnce(() -> shooter.simShoot())); - - // controller - // .leftTrigger() - // .onTrue( - // Commands.runOnce( - // () -> { - // Robot.fuelSim.clearFuel(); - // Robot.fuelSim.spawnStartingFuel(); - // intake.simBalls = 0; - // })); - // controller.y().onTrue(Commands.runOnce(() -> intake.setVelocity(RotationsPerSecond.of(10)))); - // controller.a().onTrue(intake.intake()); - // controller.x().onTrue(intake.stowAndStopRollers()); - - // Flywheel - controller.leftBumper().whileTrue(shooter.runFlywheel(RotationsPerSecond.of(25))); - controller.leftBumper().onFalse(shooter.runFlywheel(RotationsPerSecond.of(0))); - - // Intake + Spindexer + Tower controller - .rightBumper() - .whileTrue( - Commands.parallel( - // intake.runRollers(RotationsPerSecond.of(30)), - hopper.runSpindexer(15), shooter.runTower(RotationsPerSecond.of(70)))); + .b() + .onTrue( + Commands.runOnce( + () -> + drive.setPose( + new Pose2d(drive.getPose().getTranslation(), new Rotation2d())), + drive) + .ignoringDisable(true)); controller - .rightBumper() - .onFalse( - Commands.parallel( - intake.runRollers(RotationsPerSecond.of(0)), - hopper.runSpindexer(0), - shooter.runTower(RotationsPerSecond.of(0)))); + .x() + .onTrue(Commands.runOnce(() -> hopper.setGoal(HopperConstants.HOPPER_POSITION), hopper)); // Commands for sim testing! - //controller.rightTrigger().onTrue(Commands.runOnce(() -> shooter.simShoot())); + controller.rightTrigger().onTrue(Commands.runOnce(() -> shooter.simShoot())); - //controller + controller .leftTrigger() .onTrue( Commands.runOnce( @@ -447,31 +390,6 @@ private void configureButtonBindings() { // controller.leftBumper().onTrue(intake.intake()); // controller.rightBumper().onTrue(intake.stowAndStopRollers()); // controller.a().onTrue(shooter.runFlywheel()); - // Intake Rollers 11 Motor: 9 Intake - controller.a().whileTrue((intake.runRollers(RotationsPerSecond.of(15)))); - controller.a().onFalse((intake.runRollers(RotationsPerSecond.of(0)))); - - // Spindexer 1:1 - // controller.x().whileTrue(hopper.runSpindexer(18)); - // controller.x().onFalse(hopper.runSpindexer(0)); - - // Tower - 15 Motor:7 Tower - - controller.y().whileTrue(shooter.runTower(RotationsPerSecond.of(70))); - controller.y().onFalse(shooter.runTower(RotationsPerSecond.of(0))); - - // controller.b().onFalse(shooter.setHoodAngle(ShooterRotaryConstants.STARTING_ANGLE.magnitude())); - controller.b().whileTrue(shooter.setHoodAngle(5)); - - // controller.x().onFalse(intake.setPivotAngle(IntakePivotConstants.STOW_ANGLE)); - controller.x().whileTrue(intake.setPivotAngle(IntakePivotConstants.PICKUP_ANGLE)); - - controller.povRight().onTrue(shooter.calibrateHood()); - - // controller.povUp().whileTrue(climber.raiseClimber()); - // controller.povUp().onFalse(climber.stopClimber()); - // controller.povDown().whileTrue(climber.lowerClimber()); - // controller.povDown().onFalse(climber.stopClimber()); } /** diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java index c891c3c..fc20735 100644 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -13,10 +13,9 @@ import edu.wpi.first.math.numbers.N3; import edu.wpi.first.units.measure.*; -// // import frc.robot.subsystems.CommandSwerveDrivetrain; -// Generated by the 2026 Tuner X Swerve Project Generator +// Generated by the Tuner X Swerve Project Generator // https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html public class TunerConstants { // Both sets of gains need to be tuned to your individual robot. @@ -57,7 +56,7 @@ public class TunerConstants { // The stator current at which the wheels start to slip; // This needs to be tuned to your individual robot - private static final Current kSlipCurrent = Amps.of(120); + private static final Current kSlipCurrent = Amps.of(120.0); // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. @@ -77,17 +76,17 @@ public class TunerConstants { // CAN bus that the devices are located on; // All swerve devices must share the same CAN bus - public static final CANBus kCANBus = new CANBus("rio", "./logs/example.hoot"); + public static final CANBus kCANBus = new CANBus("", "./logs/example.hoot"); // Theoretical free speed (m/s) at 12 V applied output; // This needs to be tuned to your individual robot - public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(5.04); + public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(4.73); // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; // This may need to be tuned to your individual robot private static final double kCoupleRatio = 3.5714285714285716; - private static final double kDriveGearRatio = 6.122448979591837; + private static final double kDriveGearRatio = 6.746031746031747; private static final double kSteerGearRatio = 21.428571428571427; private static final Distance kWheelRadius = Inches.of(2); @@ -97,8 +96,8 @@ public class TunerConstants { private static final int kPigeonId = 0; // These are only used for simulation - private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01); - private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.01); + private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.004); + private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.025); // Simulated voltage necessary to overcome friction private static final Voltage kSteerFrictionVoltage = Volts.of(0.2); private static final Voltage kDriveFrictionVoltage = Volts.of(0.2); @@ -139,45 +138,45 @@ public class TunerConstants { private static final int kFrontLeftDriveMotorId = 35; private static final int kFrontLeftSteerMotorId = 12; private static final int kFrontLeftEncoderId = 1; - private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.06396484375); + private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.1416015625); private static final boolean kFrontLeftSteerMotorInverted = true; private static final boolean kFrontLeftEncoderInverted = false; - private static final Distance kFrontLeftXPos = Inches.of(10.75); - private static final Distance kFrontLeftYPos = Inches.of(10.75); + private static final Distance kFrontLeftXPos = Inches.of(12.25); + private static final Distance kFrontLeftYPos = Inches.of(10.25); // Front Right private static final int kFrontRightDriveMotorId = 41; private static final int kFrontRightSteerMotorId = 42; private static final int kFrontRightEncoderId = 4; - private static final Angle kFrontRightEncoderOffset = Rotations.of(0.27783203125); + private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.1748046875); private static final boolean kFrontRightSteerMotorInverted = true; private static final boolean kFrontRightEncoderInverted = false; - private static final Distance kFrontRightXPos = Inches.of(10.75); - private static final Distance kFrontRightYPos = Inches.of(-10.75); + private static final Distance kFrontRightXPos = Inches.of(12.25); + private static final Distance kFrontRightYPos = Inches.of(-10.25); // Back Left private static final int kBackLeftDriveMotorId = 21; private static final int kBackLeftSteerMotorId = 25; private static final int kBackLeftEncoderId = 2; - private static final Angle kBackLeftEncoderOffset = Rotations.of(0.380859375); + private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.369384765625); private static final boolean kBackLeftSteerMotorInverted = true; private static final boolean kBackLeftEncoderInverted = false; - private static final Distance kBackLeftXPos = Inches.of(-10.75); - private static final Distance kBackLeftYPos = Inches.of(10.75); + private static final Distance kBackLeftXPos = Inches.of(-12.25); + private static final Distance kBackLeftYPos = Inches.of(10.25); // Back Right private static final int kBackRightDriveMotorId = 31; private static final int kBackRightSteerMotorId = 32; private static final int kBackRightEncoderId = 3; - private static final Angle kBackRightEncoderOffset = Rotations.of(0.11279296875); + private static final Angle kBackRightEncoderOffset = Rotations.of(-0.388916015625); private static final boolean kBackRightSteerMotorInverted = true; private static final boolean kBackRightEncoderInverted = false; - private static final Distance kBackRightXPos = Inches.of(-10.75); - private static final Distance kBackRightYPos = Inches.of(-10.75); + private static final Distance kBackRightXPos = Inches.of(-12.25); + private static final Distance kBackRightYPos = Inches.of(-10.25); public static final SwerveModuleConstants< TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> @@ -292,9 +291,9 @@ public TunerSwerveDrivetrain( * @param odometryUpdateFrequency The frequency to run the odometry loop. If unspecified or set * to 0 Hz, this is 250 Hz on CAN FD, and 100 Hz on CAN 2.0. * @param odometryStandardDeviation The standard deviation for odometry calculation in the form - * [x, y, theta]áµ€, with units in meters and radians + * [x, y, theta]ᵀ, with units in meters and radians * @param visionStandardDeviation The standard deviation for vision calculation in the form [x, - * y, theta]áµ€, with units in meters and radians + * y, theta]ᵀ, with units in meters and radians * @param modules Constants for each specific module */ public TunerSwerveDrivetrain( diff --git a/src/main/java/frc/robot/subsystems/Climber.java b/src/main/java/frc/robot/subsystems/Climber.java index 1a22939..7354a3a 100644 --- a/src/main/java/frc/robot/subsystems/Climber.java +++ b/src/main/java/frc/robot/subsystems/Climber.java @@ -1,15 +1,18 @@ package frc.robot.subsystems; import static edu.wpi.first.units.Units.Amps; -import static edu.wpi.first.units.Units.DegreesPerSecond; import static edu.wpi.first.units.Units.Meters; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.lib.W8.io.motor.MotorIO.PIDSlot; import frc.lib.W8.mechanisms.linear.LinearMechanism; import frc.robot.Constants.ClimberConstants; +import org.littletonrobotics.junction.Logger; public class Climber extends SubsystemBase { private LinearMechanism _io; @@ -38,28 +41,6 @@ public boolean isAboveCurrentLimit() { } } - // public Command homeCommand() - // { - // return Commands.sequence( - // runOnce(() -> _io.runVoltage(Volts.of(-2))), - // Commands.waitUntil(() -> isAboveCurrentLimit())); - // } - - @Override - public void periodic() { - _io.periodic(); - - // double z = Math.abs(Math.sin(Timer.getFPGATimestamp()) * 0.33); // Placeholder for position - - // // The z of the Translation3D should be - // // 'ClimberConstants.CONVERTER.toDistance(_io.getPosition()).in(Meters)', change after fixing - // // motor configs. - // Logger.recordOutput( - // "3DField/4_Climber", new Pose3d(new Translation3d(0, 0, z), new Rotation3d(0, 0, 0))); - - // _io.runVoltage(Volts.of(Math.sin(Timer.getFPGATimestamp()) * 0.25)); - } - public Command runClimber() { return this.runOnce( () -> @@ -71,47 +52,6 @@ public Command runClimber() { PIDSlot.SLOT_0)); } - public Command calibrateClimber() { - System.out.println(ClimberConstants.CALIBRATE_VELOCITY); - System.out.println(ClimberConstants.ACCELERATION); - return this.run( - () -> - _io.runVelocity( - ClimberConstants.CALIBRATE_VELOCITY, - ClimberConstants.ACCELERATION, - PIDSlot.SLOT_0)) - .until(() -> isAboveCurrentLimit()); - } - - public Command stopClimber() { - return this.run( - () -> - _io.runVelocity( - DegreesPerSecond.of(0.0), ClimberConstants.ACCELERATION, PIDSlot.SLOT_0)); - } - - public Command raiseClimber() { - System.out.println(ClimberConstants.CRUISE_VELOCITY); - System.out.println(ClimberConstants.ACCELERATION); - return this.run( - () -> - _io.runVelocity( - ClimberConstants.CRUISE_VELOCITY, - ClimberConstants.ACCELERATION, - PIDSlot.SLOT_0)) - .until(() -> isAboveCurrentLimit()); - } - - public Command lowerClimber() { - System.out.println(ClimberConstants.LOWER_VELOCITY); - System.out.println(ClimberConstants.ACCELERATION); - return this.run( - () -> - _io.runVelocity( - ClimberConstants.LOWER_VELOCITY, ClimberConstants.ACCELERATION, PIDSlot.SLOT_0)) - .until(() -> isAboveCurrentLimit()); - } - public boolean nearGoalposition() { if (Math.abs( goalDistance.in(Meters) diff --git a/src/main/java/frc/robot/subsystems/Hopper.java b/src/main/java/frc/robot/subsystems/Hopper.java index 46c4b82..43cc31e 100644 --- a/src/main/java/frc/robot/subsystems/Hopper.java +++ b/src/main/java/frc/robot/subsystems/Hopper.java @@ -1,21 +1,18 @@ package frc.robot.subsystems; import static edu.wpi.first.units.Units.*; +import static edu.wpi.first.units.Units.Inches; import edu.wpi.first.units.measure.*; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.units.measure.Distance; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.lib.W8.io.motor.MotorIO.PIDSlot; import frc.lib.W8.mechanisms.flywheel.FlywheelMechanism; import frc.robot.Constants.HopperConstants; -import org.littletonrobotics.junction.Logger; public class Hopper extends SubsystemBase { private FlywheelMechanism _io; - public AngularVelocity targetVelocity = RotationsPerSecond.of(0.0); - public Hopper(FlywheelMechanism io) { _io = io; } @@ -30,21 +27,6 @@ public void setGoal(double position) { PIDSlot.SLOT_0); } - // Velocity of Rollers - public void setVelocity(double velocity) { - AngularVelocity angVelo = RotationsPerSecond.of(velocity); - - _io.runVelocity(angVelo, HopperConstants.ACCELERATION, PIDSlot.SLOT_0); - targetVelocity = angVelo; - } - - public Command runSpindexer(double velocity) { - return Commands.runOnce(() -> setVelocity(velocity), this); - } - @Override - public void periodic() { - _io.periodic(); - Logger.recordOutput("Hopper/TargetVelocity", targetVelocity); - } + public void periodic() {} } diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 5911822..1961f6c 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -2,7 +2,6 @@ import static edu.wpi.first.units.Units.Degree; import static edu.wpi.first.units.Units.Radians; -import static edu.wpi.first.units.Units.Rotations; import static edu.wpi.first.units.Units.RotationsPerSecond; import edu.wpi.first.math.geometry.Pose3d; @@ -19,7 +18,6 @@ import frc.lib.W8.util.LoggerHelper; import frc.robot.Constants.IntakeFlywheelConstants; import frc.robot.Constants.IntakePivotConstants; -import frc.robot.Robot; import org.littletonrobotics.junction.Logger; public class Intake extends SubsystemBase { @@ -29,23 +27,19 @@ public class Intake extends SubsystemBase { double pivotAngle; public double desiredAngle; public int simBalls; - public AngularVelocity targetSpeed = RotationsPerSecond.of(0); public Intake(FlywheelMechanism rollerIO, RotaryMechanism pivotIO) { _rollerIO = rollerIO; _pivotIO = pivotIO; - if (Robot.isReal()) tunePivotPosition(); - simBalls = 0; } // Velocity of Rollers - public void setVelocity(AngularVelocity velocity) { - // AngularVelocity angVelo = RotationsPerSecond.of(velocity); + public void setVelocity(double velocity) { + AngularVelocity angVelo = RotationsPerSecond.of(velocity); - _rollerIO.runVelocity(velocity, IntakeFlywheelConstants.ACCELERATION, PIDSlot.SLOT_0); - targetSpeed = velocity; + _rollerIO.runVelocity(angVelo, IntakeFlywheelConstants.ACCELERATION, PIDSlot.SLOT_0); } public void setPivotAngle(Angle pivotAngle) { @@ -66,11 +60,7 @@ public Angle getPosition() { } public void stop() { - setVelocity(RotationsPerSecond.of(0)); - } - - public Command runRollers(AngularVelocity velocity) { - return Commands.run(() -> setVelocity(velocity), this); + setVelocity(0); } public Command intake() { @@ -92,8 +82,7 @@ public boolean canIntake() { public Command stowAndStopRollers() { return Commands.sequence( - Commands.run( - () -> setVelocity(RotationsPerSecond.of(IntakeFlywheelConstants.PICKUP_SPEED))), + Commands.run(() -> setVelocity(IntakeFlywheelConstants.PICKUP_SPEED)), setStowAngle(IntakePivotConstants.STOW_ANGLE)); } @@ -108,20 +97,13 @@ private Command setStowAngle(Angle stowAngle) { PIDSlot.SLOT_0)); } - public void tunePivotPosition() { - System.out.println(IntakePivotConstants.ENCODER1.get()); - _pivotIO.setEncoderPosition(Rotations.of(IntakePivotConstants.ENCODER1.get())); - } - @Override public void periodic() { - // if (_pivotIO.getPosition().in(Degree) < IntakePivotConstants.MAX_ANGLE.in(Degree)) - // _pivotIO.runVoltage(Volts.of(0.25)); - _rollerIO.periodic(); - Logger.recordOutput("Intake/TargetSpeed", targetSpeed); + LoggerHelper.recordCurrentCommand("1_Intake", this); _pivotIO.periodic(); - // Logger.recordOutput("Intake/TargetPivot", null); + _rollerIO.periodic(); + Logger.recordOutput( "3DField/1_Intake", new Pose3d( diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index a9780ed..eb85b4e 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -1,12 +1,9 @@ package frc.robot.subsystems; -import static edu.wpi.first.units.Units.Amps; import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.Radians; import static edu.wpi.first.units.Units.RotationsPerSecond; -import edu.wpi.first.math.filter.Debouncer; -import edu.wpi.first.math.filter.Debouncer.DebounceType; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; @@ -15,11 +12,9 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.lib.W8.io.motor.MotorIO.PIDSlot; import frc.lib.W8.mechanisms.flywheel.FlywheelMechanism; import frc.lib.W8.mechanisms.rotary.RotaryMechanism; @@ -27,7 +22,6 @@ import frc.robot.Constants.ShooterConstants; import frc.robot.Constants.ShooterFeederConstants; import frc.robot.Constants.ShooterTowerConstants; -import frc.robot.Constants.ShooterRotaryConstants; import frc.robot.Robot; import org.littletonrobotics.junction.Logger; @@ -42,13 +36,6 @@ public class Shooter extends SubsystemBase { // desired target values private double desiredVelo; private double hoodAngle; - private double desiredHoodAngle; - - public AngularVelocity targetVelocity = RotationsPerSecond.of(0.0); - public AngularVelocity feederTargetVelocity = RotationsPerSecond.of(0.0); - - private Debouncer homeDebouncer = new Debouncer(0.1, DebounceType.kRising); - private Trigger homedTrigger; public Shooter( FlywheelMechanism lflywheel, @@ -61,13 +48,6 @@ public Shooter( _feeder = feeder; _tower = tower; _hood = hood; - homedTrigger = - new Trigger( - () -> - homeDebouncer.calculate( - _hood - .getSupplyCurrent() - .gte(Amps.of(ShooterConstants.HARD_STOP_CURRENT_LIMIT)))); } // Sets feeder motor speed @@ -82,21 +62,16 @@ public void runFeeder() { public void runTower() { _feeder.runVelocity( ShooterTowerConstants.MAX_VELOCITY, ShooterTowerConstants.MAX_ACCELERATION, PIDSlot.SLOT_2); - - public void runFeeder(AngularVelocity velocity) { - _feeder.runVelocity(velocity, FeederConstants.FEED_ACCELERATION, PIDSlot.SLOT_0); - feederTargetVelocity = velocity; } // Sets the flywheel velocity based on an input. - public void setFlywheelVelocity(AngularVelocity velocity) { + public void setFlywheelVelocity(double velocity) { // store the desired velocity then send converted velocity to the mechanism - // this.desiredVelo = velocity; - // AngularVelocity angVelo = RotationsPerSecond.of(velocity); - // AngularVelocity negangVelo = RotationsPerSecond.of(velocity); - _lflywheel.runVelocity(velocity, ShooterConstants.ACCELERATION, PIDSlot.SLOT_0); - _rflywheel.runVelocity(velocity, ShooterConstants.ACCELERATION, PIDSlot.SLOT_0); - targetVelocity = velocity; + this.desiredVelo = velocity; + AngularVelocity angVelo = RotationsPerSecond.of(velocity); + AngularVelocity negangVelo = RotationsPerSecond.of(velocity); + _lflywheel.runVelocity(angVelo, ShooterConstants.ACCELERATION, PIDSlot.SLOT_0); + _rflywheel.runVelocity(negangVelo, ShooterConstants.ACCELERATION, PIDSlot.SLOT_0); } // // Broken aha !! @@ -145,22 +120,14 @@ public double getHoodAngleDegrees(Translation2d robotPos) { } // Sets hood angle - public Command setHoodAngle(double angleDegrees) { + public void setHoodAngle(double angleDegrees) { hoodAngle = angleDegrees; - desiredHoodAngle = angleDegrees; - return this.runOnce( - () -> { - System.out.println("Command"); - _hood.runPosition( - Angle.ofBaseUnits(angleDegrees, Degrees), - ShooterRotaryConstants.CRUISE_VELOCITY, - ShooterRotaryConstants.ACCELERATION, - ShooterRotaryConstants.JERK, - PIDSlot.SLOT_0); - // _hood.runVelocity(ShooterConstants.HOOD_VELOCITY, - // ShooterConstants.HOOD_ACCELERATION, PIDSlot.SLOT_0); - }) - .andThen(() -> System.out.println("Command ran")); + _hood.runPosition( + Angle.ofBaseUnits(angleDegrees, Degrees), + ShooterConstants.HOOD_VELOCITY, + ShooterConstants.HOOD_ACCELERATION, + ShooterConstants.HOOD_JERK, + PIDSlot.SLOT_0); } // Checks if hood is at angle @@ -168,34 +135,7 @@ public boolean hoodAtAngle() { return Math.abs(hoodAngle - _hood.getPosition().in(Degrees)) < ShooterConstants.HOOD_TOLERANCE; } - public boolean isAboveCurrentLimit() { - if (Math.abs(_hood.getSupplyCurrent().in(Amps)) > ShooterConstants.HARD_STOP_CURRENT_LIMIT) { - return true; - } else { - return false; - } - } - - public Command calibrateHood() { - return Commands.sequence( - runOnce(() -> _hood.runVoltage(Voltage.ofBaseUnits(-1, Volts))), - Commands.waitUntil(homedTrigger), - runOnce(() -> _hood.setEncoderPosition(Angle.ofBaseUnits(0, Degrees))), - runOnce(() -> _hood.runVoltage(Voltage.ofBaseUnits(0, Volts)))); - } - - // public Command calibrateHood() { - // return this.run( - // () -> - // _hood.runVelocity( - // ShooterConstants.HOOD_VELOCITY, - // ShooterConstants.HOOD_ACCELERATION, - // PIDSlot.SLOT_1)) - // .until(() -> isAboveCurrentLimit()).andThen(this.run( () -> - // _hood.setEncoderPosition(Angle.ofBaseUnits(0, Degrees)))); - // } - - public Command shoot(AngularVelocity velocity) { + public Command shoot(double velocity) { // Prepare targets return Commands.sequence( // Set and wait in parallel for both hood and flywheel @@ -206,18 +146,11 @@ public Command shoot(AngularVelocity velocity) { Commands.runOnce(() -> runFeeder()), Commands.runOnce(() -> runTower()), // stop flywheel when finished - Commands.runOnce(() -> setFlywheelVelocity(RotationsPerSecond.of(0.0)))); - } - - public Command runFlywheel(AngularVelocity velocity) { - System.out.println("Flywheel"); - - return Commands.run(() -> setFlywheelVelocity(velocity), this); + Commands.runOnce(() -> setFlywheelVelocity(0))); } - public Command runTower(AngularVelocity velocity) { - System.out.println("Tower"); - return Commands.run(() -> runFeeder(velocity), this); + public Command runFlywheel() { + return Commands.runOnce(() -> setFlywheelVelocity(1)); } public void simShoot() { diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java index 39878e4..bb6a6d6 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java @@ -172,38 +172,28 @@ public ModuleIOTalonFX( turnAppliedVolts = turnTalon.getMotorVoltage(); turnCurrent = turnTalon.getStatorCurrent(); - // Configure periodic frames with optimized frequencies - // Odometry signals need high frequency for accurate pose estimation + // Configure periodic frames BaseStatusSignal.setUpdateFrequencyForAll( Drive.ODOMETRY_FREQUENCY, drivePosition, turnPosition); - - // Control signals need reduced frequency - BaseStatusSignal.setUpdateFrequencyForAll(50.0, driveVelocity, turnVelocity); - - // Telemetry signals can use very low frequency BaseStatusSignal.setUpdateFrequencyForAll( - 10.0, driveAppliedVolts, driveCurrent, turnAppliedVolts, turnCurrent); - - // Absolute encoder only needs very low updates since it's mainly used for initialization - BaseStatusSignal.setUpdateFrequencyForAll(20.0, turnAbsolutePosition); - - // Optimize bus utilization for each motor with longer timeout for better optimization + 50.0, + driveVelocity, + driveAppliedVolts, + driveCurrent, + turnAbsolutePosition, + turnVelocity, + turnAppliedVolts, + turnCurrent); ParentDevice.optimizeBusUtilizationForAll(driveTalon, turnTalon); - cancoder.optimizeBusUtilization(1.0); } @Override public void updateInputs(ModuleIOInputs inputs) { - // Refresh signals in priority groups - // Refresh critical odometry signals first - var driveStatus = BaseStatusSignal.refreshAll(drivePosition, driveVelocity); - var turnStatus = BaseStatusSignal.refreshAll(turnPosition, turnVelocity); - - // Refresh telemetry signals - BaseStatusSignal.refreshAll(driveAppliedVolts, driveCurrent); - BaseStatusSignal.refreshAll(turnAppliedVolts, turnCurrent); - - // Refresh absolute encoder with lower priority + // Refresh all signals + var driveStatus = + BaseStatusSignal.refreshAll(drivePosition, driveVelocity, driveAppliedVolts, driveCurrent); + var turnStatus = + BaseStatusSignal.refreshAll(turnPosition, turnVelocity, turnAppliedVolts, turnCurrent); var turnEncoderStatus = BaseStatusSignal.refreshAll(turnAbsolutePosition); // Update drive inputs diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json index 868ae9d..162ad66 100644 --- a/vendordeps/AdvantageKit.json +++ b/vendordeps/AdvantageKit.json @@ -1,7 +1,7 @@ { "fileName": "AdvantageKit.json", "name": "AdvantageKit", - "version": "26.0.1", + "version": "26.0.0", "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", "frcYear": "2026", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "org.littletonrobotics.akit", "artifactId": "akit-java", - "version": "26.0.1" + "version": "26.0.0" } ], "jniDependencies": [ { "groupId": "org.littletonrobotics.akit", "artifactId": "akit-wpilibio", - "version": "26.0.1", + "version": "26.0.0", "skipInvalidPlatforms": false, "isJar": false, "validPlatforms": [ diff --git a/vendordeps/Phoenix6-26.1.1.json b/vendordeps/Phoenix6-26.1.0.json similarity index 92% rename from vendordeps/Phoenix6-26.1.1.json rename to vendordeps/Phoenix6-26.1.0.json index 7a0eca0..dc5dc62 100644 --- a/vendordeps/Phoenix6-26.1.1.json +++ b/vendordeps/Phoenix6-26.1.0.json @@ -1,7 +1,7 @@ { - "fileName": "Phoenix6-26.1.1.json", + "fileName": "Phoenix6-26.1.0.json", "name": "CTRE-Phoenix (v6)", - "version": "26.1.1", + "version": "26.1.0", "frcYear": "2026", "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "26.1.1" + "version": "26.1.0" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "api-cpp", - "version": "26.1.1", + "version": "26.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -40,7 +40,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "26.1.1", + "version": "26.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -54,7 +54,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "api-cpp-sim", - "version": "26.1.1", + "version": "26.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -68,7 +68,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "26.1.1", + "version": "26.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -82,7 +82,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "26.1.1", + "version": "26.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -96,7 +96,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "26.1.1", + "version": "26.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -110,7 +110,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "26.1.1", + "version": "26.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -124,7 +124,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "26.1.1", + "version": "26.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -138,7 +138,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "26.1.1", + "version": "26.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -152,7 +152,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "26.1.1", + "version": "26.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -166,7 +166,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "26.1.1", + "version": "26.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -180,7 +180,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "26.1.1", + "version": "26.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -194,7 +194,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdi", - "version": "26.1.1", + "version": "26.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -208,7 +208,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdle", - "version": "26.1.1", + "version": "26.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -224,7 +224,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "26.1.1", + "version": "26.1.0", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -240,7 +240,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "26.1.1", + "version": "26.1.0", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -256,7 +256,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "26.1.1", + "version": "26.1.0", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -272,7 +272,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "26.1.1", + "version": "26.1.0", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -288,7 +288,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "26.1.1", + "version": "26.1.0", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -304,7 +304,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "26.1.1", + "version": "26.1.0", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -320,7 +320,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "26.1.1", + "version": "26.1.0", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -336,7 +336,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "26.1.1", + "version": "26.1.0", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -352,7 +352,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "26.1.1", + "version": "26.1.0", "libName": "CTRE_SimProTalonFXS", "headerClassifier": "headers", "sharedLibrary": true, @@ -368,7 +368,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "26.1.1", + "version": "26.1.0", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -384,7 +384,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "26.1.1", + "version": "26.1.0", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -400,7 +400,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "26.1.1", + "version": "26.1.0", "libName": "CTRE_SimProCANrange", "headerClassifier": "headers", "sharedLibrary": true, @@ -416,7 +416,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdi", - "version": "26.1.1", + "version": "26.1.0", "libName": "CTRE_SimProCANdi", "headerClassifier": "headers", "sharedLibrary": true, @@ -432,7 +432,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdle", - "version": "26.1.1", + "version": "26.1.0", "libName": "CTRE_SimProCANdle", "headerClassifier": "headers", "sharedLibrary": true, diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json index e8196c3..d35e593 100644 --- a/vendordeps/REVLib.json +++ b/vendordeps/REVLib.json @@ -1,7 +1,7 @@ { "fileName": "REVLib.json", "name": "REVLib", - "version": "2026.0.4", + "version": "2026.0.1", "frcYear": "2026", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2026.0.4" + "version": "2026.0.1" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2026.0.4", + "version": "2026.0.1", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -34,7 +34,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "RevLibBackendDriver", - "version": "2026.0.4", + "version": "2026.0.1", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -49,7 +49,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "RevLibWpiBackendDriver", - "version": "2026.0.4", + "version": "2026.0.1", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -66,7 +66,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2026.0.4", + "version": "2026.0.1", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -83,7 +83,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2026.0.4", + "version": "2026.0.1", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false, @@ -100,7 +100,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "RevLibBackendDriver", - "version": "2026.0.4", + "version": "2026.0.1", "libName": "BackendDriver", "sharedLibrary": true, "skipInvalidPlatforms": true, @@ -116,7 +116,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "RevLibWpiBackendDriver", - "version": "2026.0.4", + "version": "2026.0.1", "libName": "REVLibWpi", "sharedLibrary": true, "skipInvalidPlatforms": true, diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json index 62f135c..0a948bd 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -1,7 +1,7 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "v2026.3.1", + "version": "v2026.1.1-rc-2", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", "frcYear": "2026", "mavenUrls": [ @@ -13,7 +13,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2026.3.1", + "version": "v2026.1.1-rc-2", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -28,7 +28,7 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-cpp", - "version": "v2026.3.1", + "version": "v2026.1.1-rc-2", "libName": "photonlib", "headerClassifier": "headers", "sharedLibrary": true, @@ -43,7 +43,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2026.3.1", + "version": "v2026.1.1-rc-2", "libName": "photontargeting", "headerClassifier": "headers", "sharedLibrary": true, @@ -60,12 +60,12 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-java", - "version": "v2026.3.1" + "version": "v2026.1.1-rc-2" }, { "groupId": "org.photonvision", "artifactId": "photontargeting-java", - "version": "v2026.3.1" + "version": "v2026.1.1-rc-2" } ] } \ No newline at end of file From bb883c6687b480106272955552f714e970475dd9 Mon Sep 17 00:00:00 2001 From: rhit-collinkn Date: Thu, 19 Mar 2026 03:30:17 -0400 Subject: [PATCH 7/8] spotless --- src/main/java/frc/robot/RobotContainer.java | 4 ++-- src/main/java/frc/robot/subsystems/Shooter.java | 1 + 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 46a3cae..7a0edc1 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -421,8 +421,8 @@ private void configureButtonBindings() { // controller.povLeft().onTrue(shooter.setHoodAngle(6.8)); // controller.povDown().onTrue(shooter.setHoodAngle(12.1)); // controller.povRight().onTrue(shooter.setHoodAngle(18.6)); - - //controller.povLeft().onTrue(shooter.setAngleForDistance(2.0)); + + // controller.povLeft().onTrue(shooter.setAngleForDistance(2.0)); } /** diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 1d2de27..78d0aa3 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -15,6 +15,7 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; +import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.units.Units; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; From 8fd547b4f746d3b21e5b61ce080d174325351136 Mon Sep 17 00:00:00 2001 From: rhit-collinkn Date: Thu, 19 Mar 2026 03:56:44 -0400 Subject: [PATCH 8/8] readding autos --- .../deploy/pathplanner/autos/Blah Blah.auto | 31 ++++ .../autos/Cal being indecisive smh .auto | 45 +++++ .../pathplanner/paths/Depot-Climb1.path | 59 +++++++ .../pathplanner/paths/Depot-Shoot1.path | 54 ++++++ .../pathplanner/paths/Depot-Shoot2.path | 54 ++++++ .../pathplanner/paths/Depot-Shoot3.path | 54 ++++++ .../pathplanner/paths/Example Path.path | 4 +- .../deploy/pathplanner/paths/JustClimb1.path | 54 ++++++ .../deploy/pathplanner/paths/JustClimb2.path | 54 ++++++ .../deploy/pathplanner/paths/LBump-Climb.path | 59 +++++++ .../deploy/pathplanner/paths/LBump-Main3.path | 66 ++++++++ .../deploy/pathplanner/paths/LBump-Main4.path | 61 +++++++ .../pathplanner/paths/LBump-Shoot2.path | 54 ++++++ .../pathplanner/paths/LBump-Shoot3.path | 54 ++++++ .../paths/LBump-SweepUp-Shoot1.path | 150 ++++++++++++++++ .../pathplanner/paths/LBump-SweepUp.path | 59 +++++++ .../pathplanner/paths/LTrench-Climb2.path | 54 ++++++ .../pathplanner/paths/LTrench-Shoot3.path | 54 ++++++ .../pathplanner/paths/Main1-Climb1.path | 75 ++++++++ .../pathplanner/paths/Main1-Shoot1.path | 59 +++++++ .../pathplanner/paths/Main1-Shoot2.path | 54 ++++++ .../pathplanner/paths/Main2-Climb1.path | 75 ++++++++ .../pathplanner/paths/Main2-Shoot1.path | 59 +++++++ .../pathplanner/paths/Main2-Shoot2.path | 54 ++++++ .../pathplanner/paths/Main3-Climb2.path | 75 ++++++++ .../pathplanner/paths/Main3-Shoot2.path | 54 ++++++ .../pathplanner/paths/Main3-Shoot3.path | 59 +++++++ .../pathplanner/paths/Main4-Climb2.path | 75 ++++++++ .../pathplanner/paths/Main4-Shoot2.path | 54 ++++++ .../pathplanner/paths/Main4-Shoot3.path | 59 +++++++ .../deploy/pathplanner/paths/Move Down .path | 24 +-- .../deploy/pathplanner/paths/Move Up .path | 20 +-- .../paths/Rotating Cal rows his boat .path | 54 ++++++ .../paths/SWERVE BEND THAT CORNER WHOAAA.path | 59 +++++++ .../pathplanner/paths/Shoot1-Center.path | 59 +++++++ .../pathplanner/paths/Shoot1-Climb1.path | 59 +++++++ .../pathplanner/paths/Shoot1-Depot.path | 66 ++++++++ .../pathplanner/paths/Shoot1-Main1.path | 54 ++++++ .../pathplanner/paths/Shoot1-Main2.path | 54 ++++++ .../pathplanner/paths/Shoot1-SweepDown.path | 75 ++++++++ .../pathplanner/paths/Shoot2-Climb1.path | 59 +++++++ .../pathplanner/paths/Shoot2-Depot.path | 66 ++++++++ .../pathplanner/paths/Shoot2-Main2.path | 70 ++++++++ .../pathplanner/paths/Shoot2-Main3.path | 70 ++++++++ .../pathplanner/paths/Shoot2-Main4.path | 70 ++++++++ .../pathplanner/paths/Shoot2-SweepDown.path | 91 ++++++++++ .../pathplanner/paths/Shoot2-SweepUp.path | 91 ++++++++++ .../pathplanner/paths/Shoot3-Climb2.path | 75 ++++++++ .../pathplanner/paths/Shoot3-Depot.path | 66 ++++++++ .../pathplanner/paths/Shoot3-Main3.path | 59 +++++++ .../pathplanner/paths/Shoot3-Main4.path | 59 +++++++ .../paths/Shoot3-SweepUp-Shoot1.path | 160 ++++++++++++++++++ .../pathplanner/paths/Shoot3-SweepUp.path | 75 ++++++++ .../pathplanner/paths/SweepDown-Climb2.path | 75 ++++++++ .../pathplanner/paths/SweepDown-LBump.path | 59 +++++++ .../pathplanner/paths/SweepDown-Shoot2.path | 75 ++++++++ .../pathplanner/paths/SweepDown-Shoot3.path | 75 ++++++++ .../deploy/pathplanner/paths/SweepDown.path | 54 ++++++ .../pathplanner/paths/SweepUp-Climb.path | 59 +++++++ .../pathplanner/paths/SweepUp-Shoot1.path | 59 +++++++ .../pathplanner/paths/SweepUp-Shoot2.path | 70 ++++++++ .../pathplanner/paths/SweepUp-UBump.path | 59 +++++++ .../deploy/pathplanner/paths/SweepUp.path | 54 ++++++ .../deploy/pathplanner/paths/UBump-Climb.path | 59 +++++++ .../deploy/pathplanner/paths/UBump-Main1.path | 61 +++++++ .../deploy/pathplanner/paths/UBump-Main2.path | 61 +++++++ .../pathplanner/paths/UBump-Shoot1.path | 54 ++++++ .../pathplanner/paths/UBump-Shoot2.path | 54 ++++++ .../paths/UBump-SweepDown-Shoot3.path | 160 ++++++++++++++++++ .../pathplanner/paths/UBump-SweepDown.path | 59 +++++++ .../pathplanner/paths/UTrench-Climb1.path | 54 ++++++ .../pathplanner/paths/UTrench-Shoot1.path | 54 ++++++ src/main/deploy/pathplanner/settings.json | 6 +- 73 files changed, 4541 insertions(+), 27 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/Blah Blah.auto create mode 100644 src/main/deploy/pathplanner/autos/Cal being indecisive smh .auto create mode 100644 src/main/deploy/pathplanner/paths/Depot-Climb1.path create mode 100644 src/main/deploy/pathplanner/paths/Depot-Shoot1.path create mode 100644 src/main/deploy/pathplanner/paths/Depot-Shoot2.path create mode 100644 src/main/deploy/pathplanner/paths/Depot-Shoot3.path create mode 100644 src/main/deploy/pathplanner/paths/JustClimb1.path create mode 100644 src/main/deploy/pathplanner/paths/JustClimb2.path create mode 100644 src/main/deploy/pathplanner/paths/LBump-Climb.path create mode 100644 src/main/deploy/pathplanner/paths/LBump-Main3.path create mode 100644 src/main/deploy/pathplanner/paths/LBump-Main4.path create mode 100644 src/main/deploy/pathplanner/paths/LBump-Shoot2.path create mode 100644 src/main/deploy/pathplanner/paths/LBump-Shoot3.path create mode 100644 src/main/deploy/pathplanner/paths/LBump-SweepUp-Shoot1.path create mode 100644 src/main/deploy/pathplanner/paths/LBump-SweepUp.path create mode 100644 src/main/deploy/pathplanner/paths/LTrench-Climb2.path create mode 100644 src/main/deploy/pathplanner/paths/LTrench-Shoot3.path create mode 100644 src/main/deploy/pathplanner/paths/Main1-Climb1.path create mode 100644 src/main/deploy/pathplanner/paths/Main1-Shoot1.path create mode 100644 src/main/deploy/pathplanner/paths/Main1-Shoot2.path create mode 100644 src/main/deploy/pathplanner/paths/Main2-Climb1.path create mode 100644 src/main/deploy/pathplanner/paths/Main2-Shoot1.path create mode 100644 src/main/deploy/pathplanner/paths/Main2-Shoot2.path create mode 100644 src/main/deploy/pathplanner/paths/Main3-Climb2.path create mode 100644 src/main/deploy/pathplanner/paths/Main3-Shoot2.path create mode 100644 src/main/deploy/pathplanner/paths/Main3-Shoot3.path create mode 100644 src/main/deploy/pathplanner/paths/Main4-Climb2.path create mode 100644 src/main/deploy/pathplanner/paths/Main4-Shoot2.path create mode 100644 src/main/deploy/pathplanner/paths/Main4-Shoot3.path create mode 100644 src/main/deploy/pathplanner/paths/Rotating Cal rows his boat .path create mode 100644 src/main/deploy/pathplanner/paths/SWERVE BEND THAT CORNER WHOAAA.path create mode 100644 src/main/deploy/pathplanner/paths/Shoot1-Center.path create mode 100644 src/main/deploy/pathplanner/paths/Shoot1-Climb1.path create mode 100644 src/main/deploy/pathplanner/paths/Shoot1-Depot.path create mode 100644 src/main/deploy/pathplanner/paths/Shoot1-Main1.path create mode 100644 src/main/deploy/pathplanner/paths/Shoot1-Main2.path create mode 100644 src/main/deploy/pathplanner/paths/Shoot1-SweepDown.path create mode 100644 src/main/deploy/pathplanner/paths/Shoot2-Climb1.path create mode 100644 src/main/deploy/pathplanner/paths/Shoot2-Depot.path create mode 100644 src/main/deploy/pathplanner/paths/Shoot2-Main2.path create mode 100644 src/main/deploy/pathplanner/paths/Shoot2-Main3.path create mode 100644 src/main/deploy/pathplanner/paths/Shoot2-Main4.path create mode 100644 src/main/deploy/pathplanner/paths/Shoot2-SweepDown.path create mode 100644 src/main/deploy/pathplanner/paths/Shoot2-SweepUp.path create mode 100644 src/main/deploy/pathplanner/paths/Shoot3-Climb2.path create mode 100644 src/main/deploy/pathplanner/paths/Shoot3-Depot.path create mode 100644 src/main/deploy/pathplanner/paths/Shoot3-Main3.path create mode 100644 src/main/deploy/pathplanner/paths/Shoot3-Main4.path create mode 100644 src/main/deploy/pathplanner/paths/Shoot3-SweepUp-Shoot1.path create mode 100644 src/main/deploy/pathplanner/paths/Shoot3-SweepUp.path create mode 100644 src/main/deploy/pathplanner/paths/SweepDown-Climb2.path create mode 100644 src/main/deploy/pathplanner/paths/SweepDown-LBump.path create mode 100644 src/main/deploy/pathplanner/paths/SweepDown-Shoot2.path create mode 100644 src/main/deploy/pathplanner/paths/SweepDown-Shoot3.path create mode 100644 src/main/deploy/pathplanner/paths/SweepDown.path create mode 100644 src/main/deploy/pathplanner/paths/SweepUp-Climb.path create mode 100644 src/main/deploy/pathplanner/paths/SweepUp-Shoot1.path create mode 100644 src/main/deploy/pathplanner/paths/SweepUp-Shoot2.path create mode 100644 src/main/deploy/pathplanner/paths/SweepUp-UBump.path create mode 100644 src/main/deploy/pathplanner/paths/SweepUp.path create mode 100644 src/main/deploy/pathplanner/paths/UBump-Climb.path create mode 100644 src/main/deploy/pathplanner/paths/UBump-Main1.path create mode 100644 src/main/deploy/pathplanner/paths/UBump-Main2.path create mode 100644 src/main/deploy/pathplanner/paths/UBump-Shoot1.path create mode 100644 src/main/deploy/pathplanner/paths/UBump-Shoot2.path create mode 100644 src/main/deploy/pathplanner/paths/UBump-SweepDown-Shoot3.path create mode 100644 src/main/deploy/pathplanner/paths/UBump-SweepDown.path create mode 100644 src/main/deploy/pathplanner/paths/UTrench-Climb1.path create mode 100644 src/main/deploy/pathplanner/paths/UTrench-Shoot1.path diff --git a/src/main/deploy/pathplanner/autos/Blah Blah.auto b/src/main/deploy/pathplanner/autos/Blah Blah.auto new file mode 100644 index 0000000..81a12fe --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Blah Blah.auto @@ -0,0 +1,31 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Move Down " + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Olivia's Test Autos ", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Cal being indecisive smh .auto b/src/main/deploy/pathplanner/autos/Cal being indecisive smh .auto new file mode 100644 index 0000000..06dff9e --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Cal being indecisive smh .auto @@ -0,0 +1,45 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Move Down " + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "named", + "data": { + "name": "ReadyUp" + } + } + ] + } + } + ] + } + } + ] + } + }, + "resetOdom": true, + "folder": "Olivia's Test Autos ", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Depot-Climb1.path b/src/main/deploy/pathplanner/paths/Depot-Climb1.path new file mode 100644 index 0000000..482fa46 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Depot-Climb1.path @@ -0,0 +1,59 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 0.5375257072239912, + "y": 5.9245833333333335 + }, + "prevControl": null, + "nextControl": { + "x": 0.7576434822031876, + "y": 6.04310828909459 + }, + "isLocked": false, + "linkedName": "Depot" + }, + { + "anchor": { + "x": 1.0716738049959909, + "y": 4.599583975068654 + }, + "prevControl": { + "x": 2.4268716151814416, + "y": 4.535752194154123 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Climb1" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.643428571428569, + "rotationDegrees": 90.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 0.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.0 + }, + "reversed": false, + "folder": "Climb Paths ✔", + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Depot-Shoot1.path b/src/main/deploy/pathplanner/paths/Depot-Shoot1.path new file mode 100644 index 0000000..81a6195 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Depot-Shoot1.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 0.5375257072239912, + "y": 5.9245833333333335 + }, + "prevControl": null, + "nextControl": { + "x": 1.5375257072239912, + "y": 5.9245833333333335 + }, + "isLocked": false, + "linkedName": "Depot" + }, + { + "anchor": { + "x": 2.6085176282051274, + "y": 1.4477243589743591 + }, + "prevControl": { + "x": 2.637588141025641, + "y": 2.610544871794872 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "S3" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 0.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 39.09385888622944 + }, + "reversed": false, + "folder": "Depot✔", + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Depot-Shoot2.path b/src/main/deploy/pathplanner/paths/Depot-Shoot2.path new file mode 100644 index 0000000..1994377 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Depot-Shoot2.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 0.5375257072239912, + "y": 5.9245833333333335 + }, + "prevControl": null, + "nextControl": { + "x": 1.5375257072239912, + "y": 5.9245833333333335 + }, + "isLocked": false, + "linkedName": "Depot" + }, + { + "anchor": { + "x": 3.495168269230769, + "y": 3.976858974358975 + }, + "prevControl": { + "x": 2.495168269230769, + "y": 3.976858974358975 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "S2" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 0.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 1.101706115206352 + }, + "reversed": false, + "folder": "Depot✔", + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Depot-Shoot3.path b/src/main/deploy/pathplanner/paths/Depot-Shoot3.path new file mode 100644 index 0000000..c7decbd --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Depot-Shoot3.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 0.5375257072239912, + "y": 5.9245833333333335 + }, + "prevControl": null, + "nextControl": { + "x": 1.5375257072239912, + "y": 5.9245833333333335 + }, + "isLocked": false, + "linkedName": "Depot" + }, + { + "anchor": { + "x": 2.971899038461538, + "y": 5.9245833333333335 + }, + "prevControl": { + "x": 1.971899038461538, + "y": 5.924583333333333 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "S1" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 0.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -45.0 + }, + "reversed": false, + "folder": "Depot✔", + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/Example Path.path index 303dbb2..fd61ab1 100644 --- a/src/main/deploy/pathplanner/paths/Example Path.path +++ b/src/main/deploy/pathplanner/paths/Example Path.path @@ -49,8 +49,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 0.5, + "maxAcceleration": 0.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/JustClimb1.path b/src/main/deploy/pathplanner/paths/JustClimb1.path new file mode 100644 index 0000000..9b384c2 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/JustClimb1.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.4627440246710683, + "y": 4.57531457311145 + }, + "prevControl": null, + "nextControl": { + "x": 1.7127430750220372, + "y": 4.576003647573122 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.0716738049959909, + "y": 4.599583975068654 + }, + "prevControl": { + "x": 0.7923425408940383, + "y": 4.599031203468609 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Climb1" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 0.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.0 + }, + "reversed": false, + "folder": "Climb Paths ✔", + "idealStartingState": { + "velocity": 0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/JustClimb2.path b/src/main/deploy/pathplanner/paths/JustClimb2.path new file mode 100644 index 0000000..0912397 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/JustClimb2.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 0.6732340615750425, + "y": 2.8801012661030256 + }, + "prevControl": null, + "nextControl": { + "x": 0.9232331119260113, + "y": 2.8807903405646975 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.0422131290243875, + "y": 2.8801012661030256 + }, + "prevControl": { + "x": 0.7628818649224349, + "y": 2.8795484945029806 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Climb2" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 0.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0 + }, + "reversed": false, + "folder": "Climb Paths ✔", + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LBump-Climb.path b/src/main/deploy/pathplanner/paths/LBump-Climb.path new file mode 100644 index 0000000..4761cc3 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/LBump-Climb.path @@ -0,0 +1,59 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.6, + "y": 2.51 + }, + "prevControl": null, + "nextControl": { + "x": 3.0456045805726717, + "y": 1.8737948147497536 + }, + "isLocked": false, + "linkedName": "LB" + }, + { + "anchor": { + "x": 1.0422131290243875, + "y": 2.8801012661030256 + }, + "prevControl": { + "x": -0.1993986099955909, + "y": 2.81140296010582 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Climb2" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": -90.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 0.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0 + }, + "reversed": false, + "folder": "Climb Paths ✔", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LBump-Main3.path b/src/main/deploy/pathplanner/paths/LBump-Main3.path new file mode 100644 index 0000000..fb84f24 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/LBump-Main3.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.6, + "y": 2.51 + }, + "prevControl": null, + "nextControl": { + "x": 5.679263279942127, + "y": 2.772532511936588 + }, + "isLocked": false, + "linkedName": "LB" + }, + { + "anchor": { + "x": 7.564944129185973, + "y": 2.961412209299814 + }, + "prevControl": { + "x": 7.100443430294522, + "y": 2.891199060884476 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "M3" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.2, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "IntakeOn", + "waypointRelativePos": 0.6092233009708738, + "endWaypointRelativePos": 1.0, + "command": null + } + ], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 0.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Initial✔️", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LBump-Main4.path b/src/main/deploy/pathplanner/paths/LBump-Main4.path new file mode 100644 index 0000000..91b0f21 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/LBump-Main4.path @@ -0,0 +1,61 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.6, + "y": 2.51 + }, + "prevControl": null, + "nextControl": { + "x": 5.325279331824971, + "y": 2.4556189300493476 + }, + "isLocked": false, + "linkedName": "LB" + }, + { + "anchor": { + "x": 7.756784652889603, + "y": 2.1754616603872785 + }, + "prevControl": { + "x": 7.485616952764362, + "y": 2.1642888942636884 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "M4" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "IntakeOn", + "waypointRelativePos": 0.4, + "endWaypointRelativePos": 2.0, + "command": null + } + ], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 0.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Initial✔️", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LBump-Shoot2.path b/src/main/deploy/pathplanner/paths/LBump-Shoot2.path new file mode 100644 index 0000000..75761f4 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/LBump-Shoot2.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.6, + "y": 2.51 + }, + "prevControl": null, + "nextControl": { + "x": 3.5364718065372305, + "y": 2.806352471449683 + }, + "isLocked": false, + "linkedName": "LB" + }, + { + "anchor": { + "x": 3.495168269230769, + "y": 3.976858974358975 + }, + "prevControl": { + "x": 2.819435106635128, + "y": 3.9536111912930467 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "S2" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 0.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 1.101706115206352 + }, + "reversed": false, + "folder": "Initial✔️", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LBump-Shoot3.path b/src/main/deploy/pathplanner/paths/LBump-Shoot3.path new file mode 100644 index 0000000..79c65a3 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/LBump-Shoot3.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.6, + "y": 2.51 + }, + "prevControl": null, + "nextControl": { + "x": 2.94361753445769, + "y": 1.8908647591613876 + }, + "isLocked": false, + "linkedName": "LB" + }, + { + "anchor": { + "x": 2.6085176282051274, + "y": 1.4477243589743591 + }, + "prevControl": { + "x": 2.861623824615099, + "y": 1.6826579820201002 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "S3" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 0.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 39.09385888622944 + }, + "reversed": false, + "folder": "Initial✔️", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LBump-SweepUp-Shoot1.path b/src/main/deploy/pathplanner/paths/LBump-SweepUp-Shoot1.path new file mode 100644 index 0000000..cacd00f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/LBump-SweepUp-Shoot1.path @@ -0,0 +1,150 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.6, + "y": 2.51 + }, + "prevControl": null, + "nextControl": { + "x": 4.905948644793153, + "y": 2.889928673323824 + }, + "isLocked": false, + "linkedName": "LB" + }, + { + "anchor": { + "x": 8.280927710843374, + "y": 1.2921445783132537 + }, + "prevControl": { + "x": 7.302419933866362, + "y": 0.32575592718023927 + }, + "nextControl": { + "x": 8.9666541086164, + "y": 1.969378039489662 + }, + "isLocked": false, + "linkedName": "StartUpSweep" + }, + { + "anchor": { + "x": 7.625265060240963, + "y": 6.635795180722892 + }, + "prevControl": { + "x": 8.461847673846595, + "y": 6.064003392442868 + }, + "nextControl": { + "x": 7.088615698927714, + "y": 7.002587046815015 + }, + "isLocked": false, + "linkedName": "EndUpSweep" + }, + { + "anchor": { + "x": 5.125905848787446, + "y": 5.76231098430813 + }, + "prevControl": { + "x": 5.372812067950615, + "y": 5.723102325334002 + }, + "nextControl": { + "x": 4.85480368231628, + "y": 5.805361954949298 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.971899038461538, + "y": 5.9245833333333335 + }, + "prevControl": { + "x": 4.090164995643859, + "y": 5.935196071253095 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "S1" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.3905411994149175, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 0.9054119941491925, + "rotationDegrees": 110.0 + }, + { + "waypointRelativePos": 1.9878108239882952, + "rotationDegrees": 110.0 + }, + { + "waypointRelativePos": 2.8142369575816693, + "rotationDegrees": -45.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "IntakeDown", + "waypointRelativePos": 0.46135265700482614, + "endWaypointRelativePos": null, + "command": null + }, + { + "name": "IntakeOn", + "waypointRelativePos": 0.5772946859903278, + "endWaypointRelativePos": null, + "command": null + }, + { + "name": "IntakeOff", + "waypointRelativePos": 2.760869565217383, + "endWaypointRelativePos": null, + "command": null + }, + { + "name": "ReadyUp", + "waypointRelativePos": 2.770531400966177, + "endWaypointRelativePos": null, + "command": null + }, + { + "name": "Shoot", + "waypointRelativePos": 4.0, + "endWaypointRelativePos": null, + "command": null + } + ], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 0.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -45.0 + }, + "reversed": false, + "folder": "Compound Paths", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LBump-SweepUp.path b/src/main/deploy/pathplanner/paths/LBump-SweepUp.path new file mode 100644 index 0000000..ca8d19f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/LBump-SweepUp.path @@ -0,0 +1,59 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.6, + "y": 2.51 + }, + "prevControl": null, + "nextControl": { + "x": 5.964253012048193, + "y": 2.330277108433736 + }, + "isLocked": false, + "linkedName": "LB" + }, + { + "anchor": { + "x": 8.280927710843374, + "y": 1.2921445783132537 + }, + "prevControl": { + "x": 8.055933950086057, + "y": 0.20620331325301322 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "StartUpSweep" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5443809523809531, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 0.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 100.0 + }, + "reversed": false, + "folder": "Initial✔️", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LTrench-Climb2.path b/src/main/deploy/pathplanner/paths/LTrench-Climb2.path new file mode 100644 index 0000000..369f579 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/LTrench-Climb2.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.6, + "y": 0.6394 + }, + "prevControl": null, + "nextControl": { + "x": 3.8496143483334184, + "y": 0.6532808179183692 + }, + "isLocked": false, + "linkedName": "LT" + }, + { + "anchor": { + "x": 1.0422131290243875, + "y": 2.8801012661030256 + }, + "prevControl": { + "x": -1.0463855421686747, + "y": 2.854807228915663 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Climb2" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 0.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0 + }, + "reversed": false, + "folder": "Climb Paths ✔", + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LTrench-Shoot3.path b/src/main/deploy/pathplanner/paths/LTrench-Shoot3.path new file mode 100644 index 0000000..8f18008 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/LTrench-Shoot3.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.6, + "y": 0.6394 + }, + "prevControl": null, + "nextControl": { + "x": 3.823079226713893, + "y": 0.5265476247093948 + }, + "isLocked": false, + "linkedName": "LT" + }, + { + "anchor": { + "x": 2.6085176282051274, + "y": 1.4477243589743591 + }, + "prevControl": { + "x": 2.3982456481731176, + "y": 1.5829489662520955 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "S3" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 0.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 39.09385888622944 + }, + "reversed": false, + "folder": "Initial✔️", + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Main1-Climb1.path b/src/main/deploy/pathplanner/paths/Main1-Climb1.path new file mode 100644 index 0000000..35a1e4a --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Main1-Climb1.path @@ -0,0 +1,75 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.675728952774968, + "y": 5.9918925393593625 + }, + "prevControl": null, + "nextControl": { + "x": 7.915149014427164, + "y": 6.063851097422032 + }, + "isLocked": false, + "linkedName": "M1" + }, + { + "anchor": { + "x": 4.425424679487179, + "y": 5.357708333333334 + }, + "prevControl": { + "x": 5.131910656932007, + "y": 5.710334081532174 + }, + "nextControl": { + "x": 2.8554320587616338, + "y": 4.574083689356252 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.0716738049959909, + "y": 4.599583975068654 + }, + "prevControl": { + "x": 2.395304022609584, + "y": 4.649365787499372 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Climb1" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.49142857142857144, + "rotationDegrees": 90.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 0.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.0 + }, + "reversed": false, + "folder": "Climb Paths ✔", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Main1-Shoot1.path b/src/main/deploy/pathplanner/paths/Main1-Shoot1.path new file mode 100644 index 0000000..364f8be --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Main1-Shoot1.path @@ -0,0 +1,59 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.675728952774968, + "y": 5.9918925393593625 + }, + "prevControl": null, + "nextControl": { + "x": 8.675728952774968, + "y": 5.9918925393593625 + }, + "isLocked": false, + "linkedName": "M1" + }, + { + "anchor": { + "x": 2.971899038461538, + "y": 5.9245833333333335 + }, + "prevControl": { + "x": 1.971899038461538, + "y": 5.9245833333333335 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "S1" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.4560000000000002, + "rotationDegrees": -45.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 0.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -45.0 + }, + "reversed": false, + "folder": "Gather✔️", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Main1-Shoot2.path b/src/main/deploy/pathplanner/paths/Main1-Shoot2.path new file mode 100644 index 0000000..8431473 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Main1-Shoot2.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.675728952774968, + "y": 5.9918925393593625 + }, + "prevControl": null, + "nextControl": { + "x": 3.997265060240964, + "y": 5.455602409638555 + }, + "isLocked": false, + "linkedName": "M1" + }, + { + "anchor": { + "x": 3.495168269230769, + "y": 3.976858974358975 + }, + "prevControl": { + "x": 3.2323253012048188, + "y": 5.521168674698795 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "S2" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 0.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 1.101706115206352 + }, + "reversed": false, + "folder": "Gather✔️", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Main2-Climb1.path b/src/main/deploy/pathplanner/paths/Main2-Climb1.path new file mode 100644 index 0000000..b3db7d4 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Main2-Climb1.path @@ -0,0 +1,75 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.645743952021233, + "y": 5.00357851618481 + }, + "prevControl": null, + "nextControl": { + "x": 7.885164013673429, + "y": 5.075537074247479 + }, + "isLocked": false, + "linkedName": "M2" + }, + { + "anchor": { + "x": 4.270457831325301, + "y": 5.193337349397591 + }, + "prevControl": { + "x": 5.068180722891567, + "y": 5.237048192771084 + }, + "nextControl": { + "x": 3.0541576635264303, + "y": 5.126690764860669 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.0716738049959909, + "y": 4.599583975068654 + }, + "prevControl": { + "x": 2.4499425768264507, + "y": 4.638438076655999 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Climb1" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.6468571428571428, + "rotationDegrees": 90.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 0.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.0 + }, + "reversed": false, + "folder": "Climb Paths ✔", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Main2-Shoot1.path b/src/main/deploy/pathplanner/paths/Main2-Shoot1.path new file mode 100644 index 0000000..e765140 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Main2-Shoot1.path @@ -0,0 +1,59 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.645743952021233, + "y": 5.00357851618481 + }, + "prevControl": null, + "nextControl": { + "x": 1.4841743960285756, + "y": 6.206729968328998 + }, + "isLocked": false, + "linkedName": "M2" + }, + { + "anchor": { + "x": 2.971899038461538, + "y": 5.9245833333333335 + }, + "prevControl": { + "x": 3.2964254130634165, + "y": 5.8629628486565135 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "S1" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.1177142857142854, + "rotationDegrees": -45.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 0.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -45.0 + }, + "reversed": false, + "folder": "Gather✔️", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Main2-Shoot2.path b/src/main/deploy/pathplanner/paths/Main2-Shoot2.path new file mode 100644 index 0000000..cff8acd --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Main2-Shoot2.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.645743952021233, + "y": 5.00357851618481 + }, + "prevControl": null, + "nextControl": { + "x": 3.942626506024097, + "y": 5.521168674698794 + }, + "isLocked": false, + "linkedName": "M2" + }, + { + "anchor": { + "x": 3.495168269230769, + "y": 3.976858974358975 + }, + "prevControl": { + "x": 3.276036144578313, + "y": 5.390036144578313 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "S2" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 0.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 1.101706115206352 + }, + "reversed": false, + "folder": "Gather✔️", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Main3-Climb2.path b/src/main/deploy/pathplanner/paths/Main3-Climb2.path new file mode 100644 index 0000000..7f778d0 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Main3-Climb2.path @@ -0,0 +1,75 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.564944129185973, + "y": 2.961412209299814 + }, + "prevControl": null, + "nextControl": { + "x": 7.769430200353409, + "y": 3.1052365698876714 + }, + "isLocked": false, + "linkedName": "M3" + }, + { + "anchor": { + "x": 0.8282289156580382, + "y": 2.297493975907743 + }, + "prevControl": { + "x": 1.4598657270832405, + "y": 2.297493975907743 + }, + "nextControl": { + "x": 0.36926506023635153, + "y": 2.297493975907743 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.0422131290243875, + "y": 2.8801012661030256 + }, + "prevControl": { + "x": 0.23813253011586966, + "y": 2.8985180722932853 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Climb2" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.2841904761904751, + "rotationDegrees": -90.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 0.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0 + }, + "reversed": false, + "folder": "Climb Paths ✔", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Main3-Shoot2.path b/src/main/deploy/pathplanner/paths/Main3-Shoot2.path new file mode 100644 index 0000000..7627d06 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Main3-Shoot2.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.564944129185973, + "y": 2.961412209299814 + }, + "prevControl": null, + "nextControl": { + "x": 4.121292863752008, + "y": 2.482892801927034 + }, + "isLocked": false, + "linkedName": "M3" + }, + { + "anchor": { + "x": 3.495168269230769, + "y": 3.976858974358975 + }, + "prevControl": { + "x": 2.9396934550367324, + "y": 2.709996866554761 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "S2" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 0.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 1.101706115206352 + }, + "reversed": false, + "folder": "Gather✔️", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Main3-Shoot3.path b/src/main/deploy/pathplanner/paths/Main3-Shoot3.path new file mode 100644 index 0000000..53aa1e4 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Main3-Shoot3.path @@ -0,0 +1,59 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.564944129185973, + "y": 2.961412209299814 + }, + "prevControl": null, + "nextControl": { + "x": 4.434373493975903, + "y": 2.4723373493975904 + }, + "isLocked": false, + "linkedName": "M3" + }, + { + "anchor": { + "x": 2.6085176282051274, + "y": 1.4477243589743591 + }, + "prevControl": { + "x": 3.4945903614457823, + "y": 2.384915662650603 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "S3" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.23047619047618934, + "rotationDegrees": 45.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 0.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 39.09385888622944 + }, + "reversed": false, + "folder": "Gather✔️", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Main4-Climb2.path b/src/main/deploy/pathplanner/paths/Main4-Climb2.path new file mode 100644 index 0000000..5369cff --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Main4-Climb2.path @@ -0,0 +1,75 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.756784652889603, + "y": 2.1754616603872785 + }, + "prevControl": null, + "nextControl": { + "x": 7.986817169589537, + "y": 2.2733648843418273 + }, + "isLocked": false, + "linkedName": "M4" + }, + { + "anchor": { + "x": 1.0422131290243875, + "y": 2.0006304661832486 + }, + "prevControl": { + "x": 1.5528673763406438, + "y": 1.7030458471435814 + }, + "nextControl": { + "x": 0.8262137425870961, + "y": 2.1265044708812546 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.0422131290243875, + "y": 2.8801012661030256 + }, + "prevControl": { + "x": 0.3474096385542168, + "y": 2.701819277108433 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Climb2" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.31771428571428795, + "rotationDegrees": -90.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 0.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0 + }, + "reversed": false, + "folder": "Climb Paths ✔", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Main4-Shoot2.path b/src/main/deploy/pathplanner/paths/Main4-Shoot2.path new file mode 100644 index 0000000..e2cafa1 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Main4-Shoot2.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.756784652889603, + "y": 2.1754616603872785 + }, + "prevControl": null, + "nextControl": { + "x": 4.587361445783133, + "y": 2.3739879518072295 + }, + "isLocked": false, + "linkedName": "M4" + }, + { + "anchor": { + "x": 3.495168269230769, + "y": 3.976858974358975 + }, + "prevControl": { + "x": 2.9396934550367324, + "y": 2.709996866554761 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "S2" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 0.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 1.101706115206352 + }, + "reversed": false, + "folder": "Gather✔️", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Main4-Shoot3.path b/src/main/deploy/pathplanner/paths/Main4-Shoot3.path new file mode 100644 index 0000000..5fce4f9 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Main4-Shoot3.path @@ -0,0 +1,59 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.756784652889603, + "y": 2.1754616603872785 + }, + "prevControl": null, + "nextControl": { + "x": 4.784060240963855, + "y": 2.505120481927711 + }, + "isLocked": false, + "linkedName": "M4" + }, + { + "anchor": { + "x": 2.6085176282051274, + "y": 1.4477243589743591 + }, + "prevControl": { + "x": 3.4621700088486667, + "y": 2.4314240470156303 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "S3" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.24723809523808998, + "rotationDegrees": 45.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 0.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 39.09385888622944 + }, + "reversed": false, + "folder": "Gather✔️", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Move Down .path b/src/main/deploy/pathplanner/paths/Move Down .path index dd02f81..4e3566e 100644 --- a/src/main/deploy/pathplanner/paths/Move Down .path +++ b/src/main/deploy/pathplanner/paths/Move Down .path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 2.344094151212553, - "y": 5.089500713266761 + "x": 2.5262075134168156, + "y": 5.786 }, "prevControl": null, "nextControl": { - "x": 2.344094151212553, - "y": 5.339500713266761 + "x": 2.5262075134168156, + "y": 5.451165697708541 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.344094151212553, - "y": 4.59 + "x": 2.5262075134168156, + "y": 3.786 }, "prevControl": { - "x": 2.344094151212553, - "y": 4.84 + "x": 2.5262075134168156, + "y": 4.036 }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "Top Position " } ], "rotationTargets": [], @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 0.5, + "maxAcceleration": 0.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -47,7 +47,7 @@ "reversed": false, "folder": "New Folder", "idealStartingState": { - "velocity": 0, + "velocity": 0.0, "rotation": 0.0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/Move Up .path b/src/main/deploy/pathplanner/paths/Move Up .path index ae4b778..b190ad7 100644 --- a/src/main/deploy/pathplanner/paths/Move Up .path +++ b/src/main/deploy/pathplanner/paths/Move Up .path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 2.033566333808844, - "y": 2.838174037089873 + "x": 2.639785330948121, + "y": 4.28649373881932 }, "prevControl": null, "nextControl": { - "x": 2.004356021916995, - "y": 2.589886386009154 + "x": 2.639542529386918, + "y": 4.816394912583497 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.033566333808844, - "y": 3.388 + "x": 2.639785330948121, + "y": 6.286 }, "prevControl": { - "x": 2.019698828903213, - "y": 3.637615088301353 + "x": 2.639472847563966, + "y": 5.762694956403431 }, "nextControl": null, "isLocked": false, @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 0.5, + "maxAcceleration": 0.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/Rotating Cal rows his boat .path b/src/main/deploy/pathplanner/paths/Rotating Cal rows his boat .path new file mode 100644 index 0000000..a2457af --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Rotating Cal rows his boat .path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.6782110912343464, + "y": 6.1686404293381045 + }, + "prevControl": null, + "nextControl": { + "x": 3.6781162799222873, + "y": 5.371914863241866 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.678, + "y": 4.169 + }, + "prevControl": { + "x": 3.705489649159504, + "y": 4.807769668377498 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 0.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "New Folder", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SWERVE BEND THAT CORNER WHOAAA.path b/src/main/deploy/pathplanner/paths/SWERVE BEND THAT CORNER WHOAAA.path new file mode 100644 index 0000000..40f2716 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/SWERVE BEND THAT CORNER WHOAAA.path @@ -0,0 +1,59 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.62953488372093, + "y": 5.260017889087655 + }, + "prevControl": null, + "nextControl": { + "x": 2.1205724508050086, + "y": 6.233542039355992 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.029194991055456, + "y": 3.994436493738819 + }, + "prevControl": { + "x": 2.866940966010733, + "y": 4.8868336314847935 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.7192513368983966, + "rotationDegrees": -50.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 0.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "New Folder", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Shoot1-Center.path b/src/main/deploy/pathplanner/paths/Shoot1-Center.path new file mode 100644 index 0000000..e107b75 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Shoot1-Center.path @@ -0,0 +1,59 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.971899038461538, + "y": 5.9245833333333335 + }, + "prevControl": null, + "nextControl": { + "x": 3.971899038461538, + "y": 5.9245833333333335 + }, + "isLocked": false, + "linkedName": "S1" + }, + { + "anchor": { + "x": 7.986562499999998, + "y": 4.020464743589744 + }, + "prevControl": { + "x": 6.620248397435898, + "y": 4.798100961538462 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Center" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": -45.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 0.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Shoot✔️ ", + "idealStartingState": { + "velocity": 0, + "rotation": -45.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Shoot1-Climb1.path b/src/main/deploy/pathplanner/paths/Shoot1-Climb1.path new file mode 100644 index 0000000..b9d3c37 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Shoot1-Climb1.path @@ -0,0 +1,59 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.971899038461538, + "y": 5.9245833333333335 + }, + "prevControl": null, + "nextControl": { + "x": 2.2379036144578306, + "y": 4.931072289156628 + }, + "isLocked": false, + "linkedName": "S1" + }, + { + "anchor": { + "x": 1.0716738049959909, + "y": 4.599583975068654 + }, + "prevControl": { + "x": 2.13303896236862, + "y": 4.671221209186119 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Climb1" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.3295238095238099, + "rotationDegrees": 90.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 0.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.0 + }, + "reversed": false, + "folder": "Climb Paths ✔", + "idealStartingState": { + "velocity": 0, + "rotation": -45.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Shoot1-Depot.path b/src/main/deploy/pathplanner/paths/Shoot1-Depot.path new file mode 100644 index 0000000..d9774fa --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Shoot1-Depot.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.971899038461538, + "y": 5.9245833333333335 + }, + "prevControl": null, + "nextControl": { + "x": 1.7243012048192772, + "y": 5.958277108433736 + }, + "isLocked": false, + "linkedName": "S1" + }, + { + "anchor": { + "x": 0.5375257072239912, + "y": 5.9245833333333335 + }, + "prevControl": { + "x": 1.233204093847408, + "y": 5.914671339202966 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Depot" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.8201904761904751, + "rotationDegrees": 180.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "IntakeOn", + "waypointRelativePos": 0.31434977578475565, + "endWaypointRelativePos": 1.0, + "command": null + } + ], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 0.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "Shoot✔️ ", + "idealStartingState": { + "velocity": 0, + "rotation": -45.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Shoot1-Main1.path b/src/main/deploy/pathplanner/paths/Shoot1-Main1.path new file mode 100644 index 0000000..1750626 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Shoot1-Main1.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.971899038461538, + "y": 5.9245833333333335 + }, + "prevControl": null, + "nextControl": { + "x": 3.971899038461538, + "y": 5.9245833333333335 + }, + "isLocked": false, + "linkedName": "S1" + }, + { + "anchor": { + "x": 7.675728952774968, + "y": 5.9918925393593625 + }, + "prevControl": { + "x": 6.675728952774968, + "y": 5.9918925393593625 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "M1" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 0.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Shoot✔️ ", + "idealStartingState": { + "velocity": 0, + "rotation": -45.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Shoot1-Main2.path b/src/main/deploy/pathplanner/paths/Shoot1-Main2.path new file mode 100644 index 0000000..6b6de52 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Shoot1-Main2.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.971899038461538, + "y": 5.9245833333333335 + }, + "prevControl": null, + "nextControl": { + "x": 3.971899038461538, + "y": 5.9245833333333335 + }, + "isLocked": false, + "linkedName": "S1" + }, + { + "anchor": { + "x": 7.645743952021233, + "y": 5.00357851618481 + }, + "prevControl": { + "x": 6.645743952021233, + "y": 5.00357851618481 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "M2" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 0.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Shoot✔️ ", + "idealStartingState": { + "velocity": 0, + "rotation": -45.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Shoot1-SweepDown.path b/src/main/deploy/pathplanner/paths/Shoot1-SweepDown.path new file mode 100644 index 0000000..5faa902 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Shoot1-SweepDown.path @@ -0,0 +1,75 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.971899038461538, + "y": 5.9245833333333335 + }, + "prevControl": null, + "nextControl": { + "x": 5.413822115384614, + "y": 5.902780448717949 + }, + "isLocked": false, + "linkedName": "S1" + }, + { + "anchor": { + "x": 6.227796474358973, + "y": 6.244358974358975 + }, + "prevControl": { + "x": 5.5225100323736624, + "y": 5.749344266630155 + }, + "nextControl": { + "x": 7.397884615384615, + "y": 7.065600961538462 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.279311758588921, + "y": 6.670571423499865 + }, + "prevControl": { + "x": 7.67609861756328, + "y": 7.193840654269096 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "StartDownSweep" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.03139013452914799, + "rotationDegrees": -90.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 0.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -100.0 + }, + "reversed": false, + "folder": "Shoot✔️ ", + "idealStartingState": { + "velocity": 0, + "rotation": -45.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Shoot2-Climb1.path b/src/main/deploy/pathplanner/paths/Shoot2-Climb1.path new file mode 100644 index 0000000..7a68c7b --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Shoot2-Climb1.path @@ -0,0 +1,59 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.495168269230769, + "y": 3.976858974358975 + }, + "prevControl": null, + "nextControl": { + "x": 2.2269759036144574, + "y": 4.701590361445783 + }, + "isLocked": false, + "linkedName": "S2" + }, + { + "anchor": { + "x": 1.0716738049959909, + "y": 4.599583975068654 + }, + "prevControl": { + "x": 2.0893281189951267, + "y": 4.6275103658126255 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Climb1" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.6769523809523813, + "rotationDegrees": 90.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 0.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.0 + }, + "reversed": false, + "folder": "Climb Paths ✔", + "idealStartingState": { + "velocity": 0, + "rotation": 1.101706115206352 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Shoot2-Depot.path b/src/main/deploy/pathplanner/paths/Shoot2-Depot.path new file mode 100644 index 0000000..93b9c6f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Shoot2-Depot.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.495168269230769, + "y": 3.976858974358975 + }, + "prevControl": null, + "nextControl": { + "x": 3.4618072289156623, + "y": 5.433746987951806 + }, + "isLocked": false, + "linkedName": "S2" + }, + { + "anchor": { + "x": 0.5375257072239912, + "y": 5.9245833333333335 + }, + "prevControl": { + "x": 2.718722891566265, + "y": 5.848999999999998 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Depot" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.8064761904761906, + "rotationDegrees": 180.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "IntakeOn", + "waypointRelativePos": 0.787892376681611, + "endWaypointRelativePos": 1.0, + "command": null + } + ], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 0.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "Shoot✔️ ", + "idealStartingState": { + "velocity": 0, + "rotation": 1.101706115206352 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Shoot2-Main2.path b/src/main/deploy/pathplanner/paths/Shoot2-Main2.path new file mode 100644 index 0000000..bdd1c7f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Shoot2-Main2.path @@ -0,0 +1,70 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.495168269230769, + "y": 3.976858974358975 + }, + "prevControl": null, + "nextControl": { + "x": 3.562498037081225, + "y": 4.685423967526528 + }, + "isLocked": false, + "linkedName": "S2" + }, + { + "anchor": { + "x": 3.70221686746988, + "y": 5.302614457831326 + }, + "prevControl": { + "x": 3.315394304995904, + "y": 4.9551819731083935 + }, + "nextControl": { + "x": 4.311156428667846, + "y": 5.8495458039244985 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.645743952021233, + "y": 5.00357851618481 + }, + "prevControl": { + "x": 6.50986388720549, + "y": 5.104680754437479 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "M2" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 0.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Shoot✔️ ", + "idealStartingState": { + "velocity": 0, + "rotation": 1.101706115206352 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Shoot2-Main3.path b/src/main/deploy/pathplanner/paths/Shoot2-Main3.path new file mode 100644 index 0000000..b2c9149 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Shoot2-Main3.path @@ -0,0 +1,70 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.495168269230769, + "y": 3.976858974358975 + }, + "prevControl": null, + "nextControl": { + "x": 3.5379576543029922, + "y": 3.4214658537757705 + }, + "isLocked": false, + "linkedName": "S2" + }, + { + "anchor": { + "x": 3.8879879518072293, + "y": 2.5925421686746986 + }, + "prevControl": { + "x": 3.527217944605494, + "y": 2.8690974530897413 + }, + "nextControl": { + "x": 4.547933868109539, + "y": 2.0866477568937474 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.564944129185973, + "y": 2.961412209299814 + }, + "prevControl": { + "x": 6.532493975903614, + "y": 2.7783132530120485 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "M3" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 0.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Shoot✔️ ", + "idealStartingState": { + "velocity": 0, + "rotation": 1.101706115206352 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Shoot2-Main4.path b/src/main/deploy/pathplanner/paths/Shoot2-Main4.path new file mode 100644 index 0000000..fc917d4 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Shoot2-Main4.path @@ -0,0 +1,70 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.495168269230769, + "y": 3.976858974358975 + }, + "prevControl": null, + "nextControl": { + "x": 3.5273734939759036, + "y": 3.0733614457831333 + }, + "isLocked": false, + "linkedName": "S2" + }, + { + "anchor": { + "x": 3.8879879518072293, + "y": 2.5925421686746986 + }, + "prevControl": { + "x": 3.5160704712795057, + "y": 2.911713236928375 + }, + "nextControl": { + "x": 4.361618552629521, + "y": 2.1860832288916496 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.756784652889603, + "y": 2.1754616603872785 + }, + "prevControl": { + "x": 6.694320639307958, + "y": 2.3380283657725442 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "M4" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 0.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Shoot✔️ ", + "idealStartingState": { + "velocity": 0, + "rotation": 1.101706115206352 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Shoot2-SweepDown.path b/src/main/deploy/pathplanner/paths/Shoot2-SweepDown.path new file mode 100644 index 0000000..7a02ab3 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Shoot2-SweepDown.path @@ -0,0 +1,91 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.495168269230769, + "y": 3.976858974358975 + }, + "prevControl": null, + "nextControl": { + "x": 3.4006891025641024, + "y": 5.270496794871795 + }, + "isLocked": false, + "linkedName": "S2" + }, + { + "anchor": { + "x": 4.38181891025641, + "y": 5.437652243589744 + }, + "prevControl": { + "x": 4.15843147257766, + "y": 5.325411196341767 + }, + "nextControl": { + "x": 5.98392258715268, + "y": 6.24262944536629 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.259799679487179, + "y": 6.716754807692308 + }, + "prevControl": { + "x": 5.872824634121077, + "y": 6.178766659335234 + }, + "nextControl": { + "x": 7.518566691486418, + "y": 6.817126898817146 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.279311758588921, + "y": 6.670571423499865 + }, + "prevControl": { + "x": 8.199367848332512, + "y": 7.5499544363203785 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "StartDownSweep" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.365470852017938, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 0.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -100.0 + }, + "reversed": false, + "folder": "Shoot✔️ ", + "idealStartingState": { + "velocity": 0, + "rotation": 1.101706115206352 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Shoot2-SweepUp.path b/src/main/deploy/pathplanner/paths/Shoot2-SweepUp.path new file mode 100644 index 0000000..3451533 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Shoot2-SweepUp.path @@ -0,0 +1,91 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.495168269230769, + "y": 3.976858974358975 + }, + "prevControl": null, + "nextControl": { + "x": 3.3062099358974355, + "y": 2.770432692307693 + }, + "isLocked": false, + "linkedName": "S2" + }, + { + "anchor": { + "x": 4.323677884615384, + "y": 2.6250801282051284 + }, + "prevControl": { + "x": 3.8024300684205867, + "y": 2.7241906284675195 + }, + "nextControl": { + "x": 5.355681089743589, + "y": 2.4288541666666665 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.572307692307692, + "y": 1.2951041666666674 + }, + "prevControl": { + "x": 6.9182211538461535, + "y": 1.353245192307693 + }, + "nextControl": { + "x": 8.249385873452773, + "y": 1.2349194394537715 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.280927710843374, + "y": 1.2921445783132537 + }, + "prevControl": { + "x": 8.159661685202346, + "y": 0.7293881680568439 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "StartUpSweep" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.365470852017938, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 0.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 100.0 + }, + "reversed": false, + "folder": "Shoot✔️ ", + "idealStartingState": { + "velocity": 0, + "rotation": 1.101706115206352 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Shoot3-Climb2.path b/src/main/deploy/pathplanner/paths/Shoot3-Climb2.path new file mode 100644 index 0000000..a1fb8c2 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Shoot3-Climb2.path @@ -0,0 +1,75 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.6085176282051274, + "y": 1.4477243589743591 + }, + "prevControl": null, + "nextControl": { + "x": 1.2544096385542172, + "y": 2.0243012048192766 + }, + "isLocked": false, + "linkedName": "S3" + }, + { + "anchor": { + "x": 0.8828674698795178, + "y": 2.2428554216867465 + }, + "prevControl": { + "x": 1.0841649533235005, + "y": 2.0946011699463005 + }, + "nextControl": { + "x": 0.4871988983811609, + "y": 2.534262681236635 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.0422131290243875, + "y": 2.8801012661030256 + }, + "prevControl": { + "x": 0.44575903614457846, + "y": 2.811096385542169 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Climb2" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.6647619047619044, + "rotationDegrees": -90.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 0.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0 + }, + "reversed": false, + "folder": "Climb Paths ✔", + "idealStartingState": { + "velocity": 0, + "rotation": 39.09385888622944 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Shoot3-Depot.path b/src/main/deploy/pathplanner/paths/Shoot3-Depot.path new file mode 100644 index 0000000..7c09d37 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Shoot3-Depot.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.6085176282051274, + "y": 1.4477243589743591 + }, + "prevControl": null, + "nextControl": { + "x": 2.575156587890021, + "y": 2.90461237256719 + }, + "isLocked": false, + "linkedName": "S3" + }, + { + "anchor": { + "x": 0.5375257072239912, + "y": 5.9245833333333335 + }, + "prevControl": { + "x": 2.9044939759036135, + "y": 6.154975903614457 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Depot" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.8049523809523805, + "rotationDegrees": 180.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "IntakeOn", + "waypointRelativePos": 0.787892376681611, + "endWaypointRelativePos": 1.0, + "command": null + } + ], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 0.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "Shoot✔️ ", + "idealStartingState": { + "velocity": 0, + "rotation": 39.09385888622944 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Shoot3-Main3.path b/src/main/deploy/pathplanner/paths/Shoot3-Main3.path new file mode 100644 index 0000000..c8d260a --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Shoot3-Main3.path @@ -0,0 +1,59 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.6085176282051274, + "y": 1.4477243589743591 + }, + "prevControl": null, + "nextControl": { + "x": 2.789357442886346, + "y": 1.6203423227732132 + }, + "isLocked": false, + "linkedName": "S3" + }, + { + "anchor": { + "x": 7.564944129185973, + "y": 2.961412209299814 + }, + "prevControl": { + "x": 5.1774578313253015, + "y": 3.1498554216867465 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "M3" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.7333333333333323, + "rotationDegrees": 45.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 0.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Shoot✔️ ", + "idealStartingState": { + "velocity": 0, + "rotation": 39.09385888622944 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Shoot3-Main4.path b/src/main/deploy/pathplanner/paths/Shoot3-Main4.path new file mode 100644 index 0000000..4c6d738 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Shoot3-Main4.path @@ -0,0 +1,59 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.6085176282051274, + "y": 1.4477243589743591 + }, + "prevControl": null, + "nextControl": { + "x": 4.816843373493977, + "y": 2.723674698795181 + }, + "isLocked": false, + "linkedName": "S3" + }, + { + "anchor": { + "x": 7.756784652889603, + "y": 2.1754616603872785 + }, + "prevControl": { + "x": 7.512832942647112, + "y": 2.12080305566887 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "M4" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.459047619047612, + "rotationDegrees": 45.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 0.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Shoot✔️ ", + "idealStartingState": { + "velocity": 0, + "rotation": 39.09385888622944 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Shoot3-SweepUp-Shoot1.path b/src/main/deploy/pathplanner/paths/Shoot3-SweepUp-Shoot1.path new file mode 100644 index 0000000..d045f0f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Shoot3-SweepUp-Shoot1.path @@ -0,0 +1,160 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.6085176282051274, + "y": 1.4477243589743591 + }, + "prevControl": null, + "nextControl": { + "x": 3.3306746987951805, + "y": 2.472337349397591 + }, + "isLocked": false, + "linkedName": "S3" + }, + { + "anchor": { + "x": 7.1444457831325305, + "y": 1.4273749999999996 + }, + "prevControl": { + "x": 6.084457831325301, + "y": 2.4941927710843377 + }, + "nextControl": { + "x": 7.7917009937618165, + "y": 0.7759493305393397 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.280927710843374, + "y": 1.2921445783132537 + }, + "prevControl": { + "x": 8.308344335215812, + "y": 1.0297570801859328 + }, + "nextControl": { + "x": 8.121500215148155, + "y": 2.817926204815189 + }, + "isLocked": false, + "linkedName": "StartUpSweep" + }, + { + "anchor": { + "x": 7.625265060240963, + "y": 6.635795180722892 + }, + "prevControl": { + "x": 7.794864177403886, + "y": 5.667030113663325 + }, + "nextControl": { + "x": 7.53647740964041, + "y": 7.1429580464675135 + }, + "isLocked": false, + "linkedName": "EndUpSweep" + }, + { + "anchor": { + "x": 5.876831325303061, + "y": 5.9245833333333335 + }, + "prevControl": { + "x": 7.687645370709767, + "y": 6.214313580605675 + }, + "nextControl": { + "x": 4.784060240967568, + "y": 5.749739959835268 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.971899038461538, + "y": 5.9245833333333335 + }, + "prevControl": { + "x": 4.5655060240982435, + "y": 5.761578313248924 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "S1" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.6419047619047696, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 1.7999999999999952, + "rotationDegrees": 100.0 + }, + { + "waypointRelativePos": 2.9809523809523726, + "rotationDegrees": 100.0 + }, + { + "waypointRelativePos": 3.95, + "rotationDegrees": -45.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "", + "waypointRelativePos": 0.0, + "endWaypointRelativePos": null, + "command": null + }, + { + "name": "IntakeOn", + "waypointRelativePos": 1.7690582959641157, + "endWaypointRelativePos": null, + "command": null + }, + { + "name": "ReadyUp", + "waypointRelativePos": 3.2040358744394712, + "endWaypointRelativePos": null, + "command": null + }, + { + "name": "IntakeOff", + "waypointRelativePos": 3.85874439461875, + "endWaypointRelativePos": null, + "command": null + } + ], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 0.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -45.0 + }, + "reversed": false, + "folder": "Compound Paths", + "idealStartingState": { + "velocity": 0, + "rotation": 39.09385888622944 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Shoot3-SweepUp.path b/src/main/deploy/pathplanner/paths/Shoot3-SweepUp.path new file mode 100644 index 0000000..e27ce7f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Shoot3-SweepUp.path @@ -0,0 +1,75 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.6085176282051274, + "y": 1.4477243589743591 + }, + "prevControl": null, + "nextControl": { + "x": 4.38181891025641, + "y": 2.5087980769230773 + }, + "isLocked": false, + "linkedName": "S3" + }, + { + "anchor": { + "x": 5.777203525641025, + "y": 2.0654727564102564 + }, + "prevControl": { + "x": 4.512636217948717, + "y": 2.276233974358975 + }, + "nextControl": { + "x": 6.580390865748494, + "y": 1.931608199725678 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.280927710843374, + "y": 1.2921445783132537 + }, + "prevControl": { + "x": 8.06518251853568, + "y": 0.736655796261972 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "StartUpSweep" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.9865470852017963, + "rotationDegrees": 45.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 0.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 100.0 + }, + "reversed": false, + "folder": "Shoot✔️ ", + "idealStartingState": { + "velocity": 0, + "rotation": 39.09385888622944 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SweepDown-Climb2.path b/src/main/deploy/pathplanner/paths/SweepDown-Climb2.path new file mode 100644 index 0000000..5b2f138 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/SweepDown-Climb2.path @@ -0,0 +1,75 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.735517857150679, + "y": 1.3139999999959786 + }, + "prevControl": null, + "nextControl": { + "x": 2.9157243685140113, + "y": 2.7792527943438943 + }, + "isLocked": false, + "linkedName": "EndDownSweep" + }, + { + "anchor": { + "x": 0.751734939759036, + "y": 2.330277108433736 + }, + "prevControl": { + "x": 1.341831325301205, + "y": 2.2865662650602405 + }, + "nextControl": { + "x": 0.2831272326317341, + "y": 2.364988790443167 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.0422131290243875, + "y": 2.8801012661030256 + }, + "prevControl": { + "x": 0.3692650602409637, + "y": 2.854807228915663 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Climb2" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.1, + "rotationDegrees": -90.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 0.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0 + }, + "reversed": false, + "folder": "Climb Paths ✔", + "idealStartingState": { + "velocity": 0, + "rotation": -100.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SweepDown-LBump.path b/src/main/deploy/pathplanner/paths/SweepDown-LBump.path new file mode 100644 index 0000000..13caef7 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/SweepDown-LBump.path @@ -0,0 +1,59 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.735517857150679, + "y": 1.3139999999959786 + }, + "prevControl": null, + "nextControl": { + "x": 4.947032280227601, + "y": 1.8154663461498246 + }, + "isLocked": false, + "linkedName": "EndDownSweep" + }, + { + "anchor": { + "x": 3.6, + "y": 2.51 + }, + "prevControl": { + "x": 5.624583333333333, + "y": 2.4244060897435897 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "LB" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5818385650224271, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 0.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Sweep Paths ✔", + "idealStartingState": { + "velocity": 0, + "rotation": -100.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SweepDown-Shoot2.path b/src/main/deploy/pathplanner/paths/SweepDown-Shoot2.path new file mode 100644 index 0000000..86b6e97 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/SweepDown-Shoot2.path @@ -0,0 +1,75 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.735517857150679, + "y": 1.3139999999959786 + }, + "prevControl": null, + "nextControl": { + "x": 6.313346382791704, + "y": 1.459352564098542 + }, + "isLocked": false, + "linkedName": "EndDownSweep" + }, + { + "anchor": { + "x": 4.643453525641025, + "y": 2.3925160256410254 + }, + "prevControl": { + "x": 6.603471399310875, + "y": 1.746127152409478 + }, + "nextControl": { + "x": 3.277139423076923, + "y": 2.843108974358975 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.495168269230769, + "y": 3.976858974358975 + }, + "prevControl": { + "x": 3.0809134615384615, + "y": 3.2064903846153845 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "S2" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.04035874439461712, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 0.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 1.101706115206352 + }, + "reversed": false, + "folder": "Sweep Paths ✔", + "idealStartingState": { + "velocity": 0, + "rotation": -100.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SweepDown-Shoot3.path b/src/main/deploy/pathplanner/paths/SweepDown-Shoot3.path new file mode 100644 index 0000000..6c38dc9 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/SweepDown-Shoot3.path @@ -0,0 +1,75 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.735517857150679, + "y": 1.3139999999959786 + }, + "prevControl": null, + "nextControl": { + "x": 5.731936126381448, + "y": 1.6991842948677731 + }, + "isLocked": false, + "linkedName": "EndDownSweep" + }, + { + "anchor": { + "x": 4.534439102564102, + "y": 2.4288541666666674 + }, + "prevControl": { + "x": 5.348413461538461, + "y": 2.6250801282051293 + }, + "nextControl": { + "x": 3.7422023964459266, + "y": 2.2378685321560354 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.6085176282051274, + "y": 1.4477243589743591 + }, + "prevControl": { + "x": 3.0954487179487176, + "y": 1.876514423076924 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "S3" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5721925133689835, + "rotationDegrees": 45.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 0.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 39.09385888622944 + }, + "reversed": false, + "folder": "Sweep Paths ✔", + "idealStartingState": { + "velocity": 0, + "rotation": -100.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SweepDown.path b/src/main/deploy/pathplanner/paths/SweepDown.path new file mode 100644 index 0000000..4fbd5b1 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/SweepDown.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 8.279311758588921, + "y": 6.670571423499865 + }, + "prevControl": null, + "nextControl": { + "x": 8.179612937480199, + "y": 6.10515131190865 + }, + "isLocked": true, + "linkedName": "StartDownSweep" + }, + { + "anchor": { + "x": 7.735517857150679, + "y": 1.3139999999959786 + }, + "prevControl": { + "x": 7.8898105807861985, + "y": 2.1890375184494006 + }, + "nextControl": null, + "isLocked": true, + "linkedName": "EndDownSweep" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 0.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -100.0 + }, + "reversed": false, + "folder": "Sweep Paths ✔", + "idealStartingState": { + "velocity": 0, + "rotation": -100.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SweepUp-Climb.path b/src/main/deploy/pathplanner/paths/SweepUp-Climb.path new file mode 100644 index 0000000..0816927 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/SweepUp-Climb.path @@ -0,0 +1,59 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.625265060240963, + "y": 6.635795180722892 + }, + "prevControl": null, + "nextControl": { + "x": 3.941260542168674, + "y": 5.722355636833047 + }, + "isLocked": false, + "linkedName": "EndUpSweep" + }, + { + "anchor": { + "x": 1.0716738049959909, + "y": 4.599583975068654 + }, + "prevControl": { + "x": 3.2804486009228384, + "y": 4.638438076655999 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Climb1" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.3158095238095237, + "rotationDegrees": 90.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 0.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.0 + }, + "reversed": false, + "folder": "Climb Paths ✔", + "idealStartingState": { + "velocity": 0, + "rotation": 100.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SweepUp-Shoot1.path b/src/main/deploy/pathplanner/paths/SweepUp-Shoot1.path new file mode 100644 index 0000000..98888b5 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/SweepUp-Shoot1.path @@ -0,0 +1,59 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.625265060240963, + "y": 6.635795180722892 + }, + "prevControl": null, + "nextControl": { + "x": 5.116318990364502, + "y": 5.313439573860769 + }, + "isLocked": false, + "linkedName": "EndUpSweep" + }, + { + "anchor": { + "x": 2.971899038461538, + "y": 5.9245833333333335 + }, + "prevControl": { + "x": 3.2152851374812865, + "y": 5.98170781848593 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "S1" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.3706666666666657, + "rotationDegrees": -45.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 0.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -45.0 + }, + "reversed": false, + "folder": "Sweep Paths ✔", + "idealStartingState": { + "velocity": 0, + "rotation": 100.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SweepUp-Shoot2.path b/src/main/deploy/pathplanner/paths/SweepUp-Shoot2.path new file mode 100644 index 0000000..c273453 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/SweepUp-Shoot2.path @@ -0,0 +1,70 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.625265060240963, + "y": 6.635795180722892 + }, + "prevControl": null, + "nextControl": { + "x": 5.091146470497373, + "y": 5.860378514056224 + }, + "isLocked": false, + "linkedName": "EndUpSweep" + }, + { + "anchor": { + "x": 4.141987179487179, + "y": 5.597540064102564 + }, + "prevControl": { + "x": 5.993195112179487, + "y": 6.27832532051282 + }, + "nextControl": { + "x": 2.2907792467948713, + "y": 4.916754807692307 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.495168269230769, + "y": 3.976858974358975 + }, + "prevControl": { + "x": 2.971899038461538, + "y": 4.754495192307693 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "S2" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 0.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 1.101706115206352 + }, + "reversed": false, + "folder": "Sweep Paths ✔", + "idealStartingState": { + "velocity": 0, + "rotation": 100.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SweepUp-UBump.path b/src/main/deploy/pathplanner/paths/SweepUp-UBump.path new file mode 100644 index 0000000..3c0fc3c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/SweepUp-UBump.path @@ -0,0 +1,59 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.625265060240963, + "y": 6.635795180722892 + }, + "prevControl": null, + "nextControl": { + "x": 5.9341913422922445, + "y": 6.703423385851097 + }, + "isLocked": false, + "linkedName": "EndUpSweep" + }, + { + "anchor": { + "x": 3.6, + "y": 5.590272435897436 + }, + "prevControl": { + "x": 5.014102564102563, + "y": 5.408581730769231 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "UB" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 0.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Sweep Paths ✔", + "idealStartingState": { + "velocity": 0, + "rotation": 100.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SweepUp.path b/src/main/deploy/pathplanner/paths/SweepUp.path new file mode 100644 index 0000000..5920899 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/SweepUp.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 8.280927710843374, + "y": 1.2921445783132537 + }, + "prevControl": null, + "nextControl": { + "x": 8.147658276155333, + "y": 2.70010142418829 + }, + "isLocked": true, + "linkedName": "StartUpSweep" + }, + { + "anchor": { + "x": 7.625265060240963, + "y": 6.635795180722892 + }, + "prevControl": { + "x": 7.745112220650984, + "y": 5.302535542250024 + }, + "nextControl": null, + "isLocked": true, + "linkedName": "EndUpSweep" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 0.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 100.0 + }, + "reversed": false, + "folder": "Sweep Paths ✔", + "idealStartingState": { + "velocity": 0, + "rotation": 100.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/UBump-Climb.path b/src/main/deploy/pathplanner/paths/UBump-Climb.path new file mode 100644 index 0000000..fe2d6d0 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/UBump-Climb.path @@ -0,0 +1,59 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.6, + "y": 5.590272435897436 + }, + "prevControl": null, + "nextControl": { + "x": 3.185745192307692, + "y": 4.987059294871795 + }, + "isLocked": false, + "linkedName": "UB" + }, + { + "anchor": { + "x": 1.0716738049959909, + "y": 4.599583975068654 + }, + "prevControl": { + "x": 2.559219685260187, + "y": 4.583799522439131 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Climb1" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.7531428571428569, + "rotationDegrees": 90.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 0.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.0 + }, + "reversed": false, + "folder": "Climb Paths ✔", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/UBump-Main1.path b/src/main/deploy/pathplanner/paths/UBump-Main1.path new file mode 100644 index 0000000..36c940b --- /dev/null +++ b/src/main/deploy/pathplanner/paths/UBump-Main1.path @@ -0,0 +1,61 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.6, + "y": 5.590272435897436 + }, + "prevControl": null, + "nextControl": { + "x": 5.5808211878116625, + "y": 5.791586626032064 + }, + "isLocked": false, + "linkedName": "UB" + }, + { + "anchor": { + "x": 7.675728952774968, + "y": 5.9918925393593625 + }, + "prevControl": { + "x": 7.136407521308552, + "y": 5.938295480976266 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "M1" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "IntakeOn", + "waypointRelativePos": 0.55, + "endWaypointRelativePos": 2.0, + "command": null + } + ], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 0.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Initial✔️", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/UBump-Main2.path b/src/main/deploy/pathplanner/paths/UBump-Main2.path new file mode 100644 index 0000000..0a66dcd --- /dev/null +++ b/src/main/deploy/pathplanner/paths/UBump-Main2.path @@ -0,0 +1,61 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.6, + "y": 5.590272435897436 + }, + "prevControl": null, + "nextControl": { + "x": 4.672552460510815, + "y": 5.426709321687979 + }, + "isLocked": false, + "linkedName": "UB" + }, + { + "anchor": { + "x": 7.645743952021233, + "y": 5.00357851618481 + }, + "prevControl": { + "x": 6.571965991274344, + "y": 5.166054074779463 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "M2" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "IntakeOn", + "waypointRelativePos": 0.6674757281553398, + "endWaypointRelativePos": 1.0, + "command": null + } + ], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 0.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Initial✔️", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/UBump-Shoot1.path b/src/main/deploy/pathplanner/paths/UBump-Shoot1.path new file mode 100644 index 0000000..3a1eb2d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/UBump-Shoot1.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.6, + "y": 5.590272435897436 + }, + "prevControl": null, + "nextControl": { + "x": 3.3207114702438494, + "y": 5.694421508069544 + }, + "isLocked": false, + "linkedName": "UB" + }, + { + "anchor": { + "x": 2.971899038461538, + "y": 5.9245833333333335 + }, + "prevControl": { + "x": 3.2993273505587726, + "y": 5.541589285792651 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "S1" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 0.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -45.0 + }, + "reversed": false, + "folder": "Initial✔️", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/UBump-Shoot2.path b/src/main/deploy/pathplanner/paths/UBump-Shoot2.path new file mode 100644 index 0000000..29a9187 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/UBump-Shoot2.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.6, + "y": 5.590272435897436 + }, + "prevControl": null, + "nextControl": { + "x": 3.510693072115397, + "y": 5.356768104820228 + }, + "isLocked": false, + "linkedName": "UB" + }, + { + "anchor": { + "x": 3.495168269230769, + "y": 3.976858974358975 + }, + "prevControl": { + "x": 2.8585646321115363, + "y": 4.014768303888544 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "S2" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 0.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 1.101706115206352 + }, + "reversed": false, + "folder": "Initial✔️", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/UBump-SweepDown-Shoot3.path b/src/main/deploy/pathplanner/paths/UBump-SweepDown-Shoot3.path new file mode 100644 index 0000000..640e7d7 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/UBump-SweepDown-Shoot3.path @@ -0,0 +1,160 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.6, + "y": 5.590272435897436 + }, + "prevControl": null, + "nextControl": { + "x": 4.1725122228676925, + "y": 5.750543887023177 + }, + "isLocked": false, + "linkedName": "UB" + }, + { + "anchor": { + "x": 6.882180722888714, + "y": 6.482807228916967 + }, + "prevControl": { + "x": 5.723843373491124, + "y": 5.8817831325314245 + }, + "nextControl": { + "x": 7.883784999660117, + "y": 7.002507561204016 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.279311758588921, + "y": 6.670571423499865 + }, + "prevControl": { + "x": 8.307081411118329, + "y": 7.352898309838755 + }, + "nextControl": { + "x": 8.251092528858518, + "y": 5.977197995926804 + }, + "isLocked": false, + "linkedName": "StartDownSweep" + }, + { + "anchor": { + "x": 7.735517857150679, + "y": 1.3139999999959786 + }, + "prevControl": { + "x": 7.891073413953844, + "y": 2.283940806712021 + }, + "nextControl": { + "x": 7.5709291877882645, + "y": 0.28773472205604667 + }, + "isLocked": false, + "linkedName": "EndDownSweep" + }, + { + "anchor": { + "x": 5.559927710840523, + "y": 2.144506024097693 + }, + "prevControl": { + "x": 6.715737176628737, + "y": 1.5081614867539286 + }, + "nextControl": { + "x": 4.587361445777435, + "y": 2.6799638554243024 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.6085176282051274, + "y": 1.4477243589743591 + }, + "prevControl": { + "x": 4.150253012045344, + "y": 2.253783132531429 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "S3" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.7, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 1.9295238095238036, + "rotationDegrees": -100.0 + }, + { + "waypointRelativePos": 3.0114285714285707, + "rotationDegrees": -100.0 + }, + { + "waypointRelativePos": 3.9028571428571404, + "rotationDegrees": 45.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "", + "waypointRelativePos": 0.7914798206277904, + "endWaypointRelativePos": null, + "command": null + }, + { + "name": "IntakeOn", + "waypointRelativePos": 1.930493273542592, + "endWaypointRelativePos": null, + "command": null + }, + { + "name": "ReadyUp", + "waypointRelativePos": 2.9977578475336344, + "endWaypointRelativePos": null, + "command": null + }, + { + "name": "IntakeOff", + "waypointRelativePos": 3.517937219730874, + "endWaypointRelativePos": null, + "command": null + } + ], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 0.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 39.09385888622944 + }, + "reversed": false, + "folder": "Compound Paths", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/UBump-SweepDown.path b/src/main/deploy/pathplanner/paths/UBump-SweepDown.path new file mode 100644 index 0000000..1145fca --- /dev/null +++ b/src/main/deploy/pathplanner/paths/UBump-SweepDown.path @@ -0,0 +1,59 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.6, + "y": 5.590272435897436 + }, + "prevControl": null, + "nextControl": { + "x": 6.95867469879518, + "y": 6.06755421686747 + }, + "isLocked": false, + "linkedName": "UB" + }, + { + "anchor": { + "x": 8.279311758588921, + "y": 6.670571423499865 + }, + "prevControl": { + "x": 8.010682890307153, + "y": 8.053463056166462 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "StartDownSweep" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.27771428571428713, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 0.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -100.0 + }, + "reversed": false, + "folder": "Initial✔️", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/UTrench-Climb1.path b/src/main/deploy/pathplanner/paths/UTrench-Climb1.path new file mode 100644 index 0000000..474bc98 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/UTrench-Climb1.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.6, + "y": 7.4298 + }, + "prevControl": null, + "nextControl": { + "x": 2.966224917367802, + "y": 5.808254396375313 + }, + "isLocked": false, + "linkedName": "UT" + }, + { + "anchor": { + "x": 1.0716738049959909, + "y": 4.599583975068654 + }, + "prevControl": { + "x": 2.2437626499694, + "y": 4.597890853696445 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Climb1" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 0.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.0 + }, + "reversed": false, + "folder": "Climb Paths ✔", + "idealStartingState": { + "velocity": 0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/UTrench-Shoot1.path b/src/main/deploy/pathplanner/paths/UTrench-Shoot1.path new file mode 100644 index 0000000..220fbd8 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/UTrench-Shoot1.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.6, + "y": 7.4298 + }, + "prevControl": null, + "nextControl": { + "x": 3.784613565375717, + "y": 7.598375892343081 + }, + "isLocked": false, + "linkedName": "UT" + }, + { + "anchor": { + "x": 2.971899038461538, + "y": 5.9245833333333335 + }, + "prevControl": { + "x": 2.9448849566443043, + "y": 6.173119526640408 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "S1" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 0.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -45.0 + }, + "reversed": false, + "folder": "Initial✔️", + "idealStartingState": { + "velocity": 0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index cc91021..bf0674a 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -13,13 +13,13 @@ "Sweep Paths ✔" ], "autoFolders": [ - "New Folder", + "Olivia's Test Autos ", "Only Climb", "Shoot First", "Sweep First" ], - "defaultMaxVel": 3.0, - "defaultMaxAccel": 3.0, + "defaultMaxVel": 0.5, + "defaultMaxAccel": 0.5, "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, "robotMass": 74.088,