From ebf6128b9905db4f249510afb83d79b7a11bc922 Mon Sep 17 00:00:00 2001 From: Niosocket11 <107586975+Niosocket11@users.noreply.github.com> Date: Fri, 17 Jan 2025 17:29:44 -0800 Subject: [PATCH 01/51] Pathplannerstuff --- src/main/deploy/pathplanner/navgrid.json | 1 + .../pathplanner/paths/CoralConfigs.path | 135 ++++++++++++++++++ .../pathplanner/paths/StartingConfigs.path | 115 +++++++++++++++ src/main/deploy/pathplanner/settings.json | 32 +++++ 4 files changed, 283 insertions(+) create mode 100644 src/main/deploy/pathplanner/navgrid.json create mode 100644 src/main/deploy/pathplanner/paths/CoralConfigs.path create mode 100644 src/main/deploy/pathplanner/paths/StartingConfigs.path create mode 100644 src/main/deploy/pathplanner/settings.json diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json new file mode 100644 index 0000000..23e0db9 --- /dev/null +++ b/src/main/deploy/pathplanner/navgrid.json @@ -0,0 +1 @@ +{"field_size":{"x":17.548,"y":8.052},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/CoralConfigs.path b/src/main/deploy/pathplanner/paths/CoralConfigs.path new file mode 100644 index 0000000..cd412d7 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/CoralConfigs.path @@ -0,0 +1,135 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.2448049779275094, + "y": 7.447722322258365 + }, + "prevControl": null, + "nextControl": { + "x": 3.244804977927509, + "y": 7.447722322258365 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.004174292696725, + "y": 5.244474697559479 + }, + "prevControl": { + "x": 4.7064615000910095, + "y": 5.244474697559479 + }, + "nextControl": { + "x": 5.30188708530244, + "y": 5.244474697559479 + }, + "isLocked": false, + "linkedName": "C11" + }, + { + "anchor": { + "x": 5.289208179311857, + "y": 5.080085654005255 + }, + "prevControl": { + "x": 5.040367278685427, + "y": 5.056039735943611 + }, + "nextControl": { + "x": 5.538049079938287, + "y": 5.1041315720668985 + }, + "isLocked": false, + "linkedName": "C12" + }, + { + "anchor": { + "x": 5.805, + "y": 4.189142416305975 + }, + "prevControl": { + "x": 5.5324143394454595, + "y": 5.159338758385921 + }, + "nextControl": { + "x": 6.07758566055454, + "y": 3.218946074226031 + }, + "isLocked": false, + "linkedName": "C21" + }, + { + "anchor": { + "x": 5.803912909888542, + "y": 3.8582569473798314 + }, + "prevControl": { + "x": 5.667794856321749, + "y": 4.295016773740704 + }, + "nextControl": { + "x": 5.940030963455334, + "y": 3.4214971210189535 + }, + "isLocked": false, + "linkedName": "C22" + }, + { + "anchor": { + "x": 6.364839756618353, + "y": 0.5451487714944108 + }, + "prevControl": { + "x": 6.799189344211289, + "y": 1.457427320518574 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1, + "rotationDegrees": -119.99999999999999 + }, + { + "waypointRelativePos": 2, + "rotationDegrees": -119.99999999999999 + }, + { + "waypointRelativePos": 3, + "rotationDegrees": 180.0 + }, + { + "waypointRelativePos": 4, + "rotationDegrees": 180.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": null, + "idealStartingState": { + "velocity": 0, + "rotation": -117.26958303209868 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/StartingConfigs.path b/src/main/deploy/pathplanner/paths/StartingConfigs.path new file mode 100644 index 0000000..ce1c8d0 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/StartingConfigs.path @@ -0,0 +1,115 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 8.0, + "y": 7.4 + }, + "prevControl": null, + "nextControl": { + "x": 8.062264438838344, + "y": 7.644490337970196 + }, + "isLocked": false, + "linkedName": "S1F" + }, + { + "anchor": { + "x": 8.0, + "y": 6.5 + }, + "prevControl": { + "x": 7.971636103762701, + "y": 7.116609069648897 + }, + "nextControl": { + "x": 8.020395624409266, + "y": 6.056614969724504 + }, + "isLocked": false, + "linkedName": "S2F" + }, + { + "anchor": { + "x": 8.0, + "y": 5.5 + }, + "prevControl": { + "x": 7.987043611353682, + "y": 5.749664038245891 + }, + "nextControl": { + "x": 8.012956388646318, + "y": 5.250335961754109 + }, + "isLocked": false, + "linkedName": "S3F" + }, + { + "anchor": { + "x": 8.0, + "y": 4.5 + }, + "prevControl": { + "x": 8.122297824367529, + "y": 4.846496610775691 + }, + "nextControl": { + "x": 7.894024420976902, + "y": 4.199747897017828 + }, + "isLocked": false, + "linkedName": "S4F" + }, + { + "anchor": { + "x": 6.3982675999074194, + "y": 2.8808078241188046 + }, + "prevControl": { + "x": 6.5963632343599095, + "y": 2.728301694370307 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1, + "rotationDegrees": 180.0 + }, + { + "waypointRelativePos": 2, + "rotationDegrees": 180.0 + }, + { + "waypointRelativePos": 3, + "rotationDegrees": 180.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": 180.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0.0, + "rotation": 180.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json new file mode 100644 index 0000000..2ca3c4c --- /dev/null +++ b/src/main/deploy/pathplanner/settings.json @@ -0,0 +1,32 @@ +{ + "robotWidth": 0.9, + "robotLength": 0.9, + "holonomicMode": true, + "pathFolders": [], + "autoFolders": [], + "defaultMaxVel": 3.0, + "defaultMaxAccel": 3.0, + "defaultMaxAngVel": 540.0, + "defaultMaxAngAccel": 720.0, + "defaultNominalVoltage": 12.0, + "robotMass": 74.088, + "robotMOI": 6.883, + "robotTrackwidth": 0.546, + "driveWheelRadius": 0.048, + "driveGearing": 5.143, + "maxDriveSpeed": 5.45, + "driveMotorType": "NEO", + "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 From e745c86bb06ab0ff3213f787d116633ae7d48cc3 Mon Sep 17 00:00:00 2001 From: Niosocket11 <107586975+Niosocket11@users.noreply.github.com> Date: Mon, 20 Jan 2025 13:42:33 -0800 Subject: [PATCH 02/51] Create AlgaeEffectors subsystem --- src/main/java/AlgaeEffector.java | 0 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 src/main/java/AlgaeEffector.java diff --git a/src/main/java/AlgaeEffector.java b/src/main/java/AlgaeEffector.java new file mode 100644 index 0000000..e69de29 From 7cf195dd1f775d10efc70799702086be1b20ac6c Mon Sep 17 00:00:00 2001 From: Niosocket11 <107586975+Niosocket11@users.noreply.github.com> Date: Mon, 20 Jan 2025 13:43:54 -0800 Subject: [PATCH 03/51] More algae stuff --- src/main/java/AlgaeEffector.java | 59 +++++++++++++++++++ .../java/org/carlmontrobotics/Constants.java | 4 ++ 2 files changed, 63 insertions(+) diff --git a/src/main/java/AlgaeEffector.java b/src/main/java/AlgaeEffector.java index e69de29..c6fc6e3 100644 --- a/src/main/java/AlgaeEffector.java +++ b/src/main/java/AlgaeEffector.java @@ -0,0 +1,59 @@ +package org.carlmontrobotics.subsystems; + +import static edu.wpi.first.units.MutableMeasure.mutable; +import static edu.wpi.first.units.Units.Radians; +import static edu.wpi.first.units.Units.RadiansPerSecond; +import static edu.wpi.first.units.Units.Seconds; +import static edu.wpi.first.units.Units.Volts; +import static org.carlmontrobotics.Constants.Armc.*; +import static org.carlmontrobotics.Constants.Armc.NEO_BUILTIN_ENCODER_CPR; // (42) explicit import + +import org.carlmontrobotics.commands.TeleopArm; +import org.carlmontrobotics.lib199.MotorConfig; +import org.carlmontrobotics.lib199.MotorControllerFactory; +import org.carlmontrobotics.lib199.sim.MockedEncoder; + +import static org.carlmontrobotics.RobotContainer.*; + +import org.carlmontrobotics.RobotContainer; + +import com.revrobotics.CANSparkBase; +import com.revrobotics.CANSparkBase.IdleMode; +import com.revrobotics.CANSparkBase.SoftLimitDirection; +import com.revrobotics.CANSparkMax; +import com.revrobotics.SparkAbsoluteEncoder; +import com.revrobotics.SparkPIDController; + +import edu.wpi.first.hal.SimDouble; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.Angle; +import edu.wpi.first.units.Measure; +import edu.wpi.first.units.MutableMeasure; +import edu.wpi.first.units.Velocity; +import edu.wpi.first.units.Voltage; +import edu.wpi.first.util.sendable.SendableBuilder; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj.simulation.SimDeviceSim; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; + +public class AlgaeEffector extends SubsystemBase { + private CANSparkMax upperMotor = new CANSparkMax(Constants.AlgaeEffectorc.upperMotorID, MotorType.kBrushless); + private CANSparkMax lowerMotor = newCANSparkMax(Constatns.AlgaeEffectorc.lowerMotorID, MotorType.kBrushless); + public RunAlgaeMotors(double upperMotorSpeed, double lowerMotorSpeed) { + upperMotor.set(upperMotorSpeed); + lowerMotor.set(lowerMotorSpeed); + } +} diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 6d6986d..58ef954 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -12,4 +12,8 @@ public static final class Manipulator { public static final int port = 1; } } + public static final class AlgaeEffectorc { + public static int upperMotorID = 1; + public static int lowerMotorID = 2; + } } From d12fdb3d39e5056cd0b2682d1c47dd446d954ab6 Mon Sep 17 00:00:00 2001 From: Niosocket11 <107586975+Niosocket11@users.noreply.github.com> Date: Mon, 20 Jan 2025 13:44:56 -0800 Subject: [PATCH 04/51] Moved --- src/main/java/{ => org/carlmontrobotics}/AlgaeEffector.java | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename src/main/java/{ => org/carlmontrobotics}/AlgaeEffector.java (100%) diff --git a/src/main/java/AlgaeEffector.java b/src/main/java/org/carlmontrobotics/AlgaeEffector.java similarity index 100% rename from src/main/java/AlgaeEffector.java rename to src/main/java/org/carlmontrobotics/AlgaeEffector.java From ddb9ff8924361dbe222b87120fe82581bb3a4b8b Mon Sep 17 00:00:00 2001 From: KeremOlgun <144630037+KeremOlgun@users.noreply.github.com> Date: Mon, 20 Jan 2025 13:52:12 -0800 Subject: [PATCH 05/51] Made command --- .../java/org/carlmontrobotics/RunAlgae.java | 39 +++++++++++++++++++ 1 file changed, 39 insertions(+) create mode 100644 src/main/java/org/carlmontrobotics/RunAlgae.java diff --git a/src/main/java/org/carlmontrobotics/RunAlgae.java b/src/main/java/org/carlmontrobotics/RunAlgae.java new file mode 100644 index 0000000..6005987 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/RunAlgae.java @@ -0,0 +1,39 @@ +package org.carlmontrobotics.commands; +import org.carlmontrobotics.subsystems.IntakeShooter; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; + +public class RunAlgae extends Command { + + public RunAlgae(AlgaeEffector algaeEffector, boolean inverted) { + addRequirements(this.algaeEffector = algaeEffector); + this.inverted = inverted; + } + + @Override + public void initialize() { + + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + if (inverted) { + algaeEffector.RunAlgaeMotors(0.5, -0.5) + } + else { + algaeEffector.RunAlgaeMotors(0.5, 0.5) + } + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false} \ No newline at end of file From 5b6d40ab74c89ddceb1a375b3e52e6183b3c8e50 Mon Sep 17 00:00:00 2001 From: Sofie Budman Date: Mon, 20 Jan 2025 13:52:36 -0800 Subject: [PATCH 06/51] update algae effector --- src/main/java/org/carlmontrobotics/AlgaeEffector.java | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/AlgaeEffector.java b/src/main/java/org/carlmontrobotics/AlgaeEffector.java index c6fc6e3..4055fb2 100644 --- a/src/main/java/org/carlmontrobotics/AlgaeEffector.java +++ b/src/main/java/org/carlmontrobotics/AlgaeEffector.java @@ -20,16 +20,18 @@ import com.revrobotics.CANSparkBase; import com.revrobotics.CANSparkBase.IdleMode; import com.revrobotics.CANSparkBase.SoftLimitDirection; +import com.revrobotics.spark.SparkFlex; import com.revrobotics.CANSparkMax; import com.revrobotics.SparkAbsoluteEncoder; import com.revrobotics.SparkPIDController; +import com.revrobotics.SparkLowLevel; import edu.wpi.first.hal.SimDouble; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; -import edu.wpi.first.units.Angle; +import edu.wpi.first.unitss.Angle; import edu.wpi.first.units.Measure; import edu.wpi.first.units.MutableMeasure; import edu.wpi.first.units.Velocity; @@ -50,9 +52,9 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; public class AlgaeEffector extends SubsystemBase { - private CANSparkMax upperMotor = new CANSparkMax(Constants.AlgaeEffectorc.upperMotorID, MotorType.kBrushless); - private CANSparkMax lowerMotor = newCANSparkMax(Constatns.AlgaeEffectorc.lowerMotorID, MotorType.kBrushless); - public RunAlgaeMotors(double upperMotorSpeed, double lowerMotorSpeed) { + private SparkFlex upperMotor = new SparkFlex(Constants.AlgaeEffectorc.upperMotorID, MotorType kBrushless); + private SparkFlex lowerMotor = new SparkFlex(Constants.AlgaeEffectorc.lowerMotorID, MotorType kBrushless); + public void RunAlgaeMotors(double upperMotorSpeed, double lowerMotorSpeed) { upperMotor.set(upperMotorSpeed); lowerMotor.set(lowerMotorSpeed); } From d9bcafd8bc6a26c32adf548ddb5e65e18f4fe261 Mon Sep 17 00:00:00 2001 From: Sofie Budman Date: Mon, 20 Jan 2025 13:56:45 -0800 Subject: [PATCH 07/51] l --- src/main/java/org/carlmontrobotics/AlgaeEffector.java | 1 + src/main/java/org/carlmontrobotics/RunAlgae.java | 1 + 2 files changed, 2 insertions(+) diff --git a/src/main/java/org/carlmontrobotics/AlgaeEffector.java b/src/main/java/org/carlmontrobotics/AlgaeEffector.java index 4055fb2..3d5f260 100644 --- a/src/main/java/org/carlmontrobotics/AlgaeEffector.java +++ b/src/main/java/org/carlmontrobotics/AlgaeEffector.java @@ -55,6 +55,7 @@ public class AlgaeEffector extends SubsystemBase { private SparkFlex upperMotor = new SparkFlex(Constants.AlgaeEffectorc.upperMotorID, MotorType kBrushless); private SparkFlex lowerMotor = new SparkFlex(Constants.AlgaeEffectorc.lowerMotorID, MotorType kBrushless); public void RunAlgaeMotors(double upperMotorSpeed, double lowerMotorSpeed) { + upperMotor.set(upperMotorSpeed); lowerMotor.set(lowerMotorSpeed); } diff --git a/src/main/java/org/carlmontrobotics/RunAlgae.java b/src/main/java/org/carlmontrobotics/RunAlgae.java index 6005987..8189ea1 100644 --- a/src/main/java/org/carlmontrobotics/RunAlgae.java +++ b/src/main/java/org/carlmontrobotics/RunAlgae.java @@ -9,6 +9,7 @@ public class RunAlgae extends Command { public RunAlgae(AlgaeEffector algaeEffector, boolean inverted) { addRequirements(this.algaeEffector = algaeEffector); this.inverted = inverted; + } @Override From 5bf7b098f5c7727b159b42b97b4a7b24f66f697d Mon Sep 17 00:00:00 2001 From: Sofie Budman Date: Mon, 20 Jan 2025 13:57:05 -0800 Subject: [PATCH 08/51] semicolons --- src/main/java/org/carlmontrobotics/RunAlgae.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/RunAlgae.java b/src/main/java/org/carlmontrobotics/RunAlgae.java index 8189ea1..ef817e7 100644 --- a/src/main/java/org/carlmontrobotics/RunAlgae.java +++ b/src/main/java/org/carlmontrobotics/RunAlgae.java @@ -21,10 +21,10 @@ public void initialize() { @Override public void execute() { if (inverted) { - algaeEffector.RunAlgaeMotors(0.5, -0.5) + algaeEffector.RunAlgaeMotors(0.5, -0.5); } else { - algaeEffector.RunAlgaeMotors(0.5, 0.5) + algaeEffector.RunAlgaeMotors(0.5, 0.5); } } From 295dda69efd76eb9c28867bd3e53bdd1f8e53100 Mon Sep 17 00:00:00 2001 From: Niosocket11 <107586975+Niosocket11@users.noreply.github.com> Date: Mon, 20 Jan 2025 13:57:15 -0800 Subject: [PATCH 09/51] Stuff --- .../java/org/carlmontrobotics/AlgaeEffector.java | 12 ++---------- src/main/java/org/carlmontrobotics/Constants.java | 10 ++++++++-- .../java/org/carlmontrobotics/RobotContainer.java | 10 ++++------ 3 files changed, 14 insertions(+), 18 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/AlgaeEffector.java b/src/main/java/org/carlmontrobotics/AlgaeEffector.java index 3d5f260..c0c9f39 100644 --- a/src/main/java/org/carlmontrobotics/AlgaeEffector.java +++ b/src/main/java/org/carlmontrobotics/AlgaeEffector.java @@ -1,17 +1,9 @@ package org.carlmontrobotics.subsystems; -import static edu.wpi.first.units.MutableMeasure.mutable; -import static edu.wpi.first.units.Units.Radians; -import static edu.wpi.first.units.Units.RadiansPerSecond; -import static edu.wpi.first.units.Units.Seconds; -import static edu.wpi.first.units.Units.Volts; -import static org.carlmontrobotics.Constants.Armc.*; -import static org.carlmontrobotics.Constants.Armc.NEO_BUILTIN_ENCODER_CPR; // (42) explicit import - -import org.carlmontrobotics.commands.TeleopArm; + import org.carlmontrobotics.lib199.MotorConfig; import org.carlmontrobotics.lib199.MotorControllerFactory; -import org.carlmontrobotics.lib199.sim.MockedEncoder; + import static org.carlmontrobotics.RobotContainer.*; diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 58ef954..6fea057 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -7,13 +7,19 @@ public final class Constants { public static final class OI { public static final class Driver { public static final int port = 0; + public static final int A = 1; + public static final int B = 2; + public static final int X = 3; + public static final int Y = 4; + public static final int leftBumper = 5; + public static final int rightBumper = 6; } public static final class Manipulator { public static final int port = 1; } } public static final class AlgaeEffectorc { - public static int upperMotorID = 1; - public static int lowerMotorID = 2; + public static final int upperMotorID = 1; + public static final int lowerMotorID = 2; } } diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index 836869c..47cb7d9 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -30,13 +30,9 @@ public class RobotContainer { //1. using GenericHID allows us to use different kinds of controllers //2. Use absolute paths from constants to reduce confusion public final GenericHID driverController = new GenericHID(OI.Driver.port); - public final GenericHID manipulatorController = new GenericHID(OI.Manipulator.port); - + private final AlgaeEffector algaeEffector = new AlgaeEffector(); public RobotContainer() { - - setDefaultCommands(); setBindingsDriver(); - setBindingsManipulator(); } private void setDefaultCommands() { @@ -48,7 +44,9 @@ private void setDefaultCommands() { // () -> driverController.getRawButton(OI.Driver.slowDriveButton) // )); } - private void setBindingsDriver() {} + private void setBindingsDriver() { + + } private void setBindingsManipulator() {} public Command getAutonomousCommand() { From 489785e3d11510de3c358c748e1c006db91f1178 Mon Sep 17 00:00:00 2001 From: Sofie Budman Date: Mon, 20 Jan 2025 13:58:05 -0800 Subject: [PATCH 10/51] move stuff --- .../java/org/carlmontrobotics/{ => commands}/RunAlgae.java | 5 +++-- .../org/carlmontrobotics/{ => subsystems}/AlgaeEffector.java | 0 2 files changed, 3 insertions(+), 2 deletions(-) rename src/main/java/org/carlmontrobotics/{ => commands}/RunAlgae.java (91%) rename src/main/java/org/carlmontrobotics/{ => subsystems}/AlgaeEffector.java (100%) diff --git a/src/main/java/org/carlmontrobotics/RunAlgae.java b/src/main/java/org/carlmontrobotics/commands/RunAlgae.java similarity index 91% rename from src/main/java/org/carlmontrobotics/RunAlgae.java rename to src/main/java/org/carlmontrobotics/commands/RunAlgae.java index ef817e7..d1e0eb5 100644 --- a/src/main/java/org/carlmontrobotics/RunAlgae.java +++ b/src/main/java/org/carlmontrobotics/commands/RunAlgae.java @@ -1,5 +1,5 @@ package org.carlmontrobotics.commands; -import org.carlmontrobotics.subsystems.IntakeShooter; +import org.carlmontrobotics.subsystems.AlgaeEffector; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; @@ -37,4 +37,5 @@ public void end(boolean interrupted) { // Returns true when the command should end. @Override public boolean isFinished() { - return false} \ No newline at end of file + return false; + } \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/AlgaeEffector.java b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java similarity index 100% rename from src/main/java/org/carlmontrobotics/AlgaeEffector.java rename to src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java From 02c34a8006f89eacac44a78c065f582bde98a6a9 Mon Sep 17 00:00:00 2001 From: Sofie Budman Date: Mon, 20 Jan 2025 14:00:48 -0800 Subject: [PATCH 11/51] push push! --- src/main/java/org/carlmontrobotics/commands/RunAlgae.java | 3 ++- .../java/org/carlmontrobotics/subsystems/AlgaeEffector.java | 6 ++++-- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/commands/RunAlgae.java b/src/main/java/org/carlmontrobotics/commands/RunAlgae.java index d1e0eb5..be0740f 100644 --- a/src/main/java/org/carlmontrobotics/commands/RunAlgae.java +++ b/src/main/java/org/carlmontrobotics/commands/RunAlgae.java @@ -38,4 +38,5 @@ public void end(boolean interrupted) { @Override public boolean isFinished() { return false; - } \ No newline at end of file + } +} \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java index c0c9f39..b17ecb5 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java +++ b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java @@ -7,12 +7,14 @@ import static org.carlmontrobotics.RobotContainer.*; +import org.carlmontrobotics.Constants; import org.carlmontrobotics.RobotContainer; import com.revrobotics.CANSparkBase; import com.revrobotics.CANSparkBase.IdleMode; import com.revrobotics.CANSparkBase.SoftLimitDirection; import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.CANSparkMax; import com.revrobotics.SparkAbsoluteEncoder; import com.revrobotics.SparkPIDController; @@ -44,8 +46,8 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; public class AlgaeEffector extends SubsystemBase { - private SparkFlex upperMotor = new SparkFlex(Constants.AlgaeEffectorc.upperMotorID, MotorType kBrushless); - private SparkFlex lowerMotor = new SparkFlex(Constants.AlgaeEffectorc.lowerMotorID, MotorType kBrushless); + private SparkFlex upperMotor = new SparkFlex(Constants.AlgaeEffectorc.upperMotorID, MotorType.kBrushless); + private SparkFlex lowerMotor = new SparkFlex(Constants.AlgaeEffectorc.lowerMotorID, MotorType.kBrushless); public void RunAlgaeMotors(double upperMotorSpeed, double lowerMotorSpeed) { upperMotor.set(upperMotorSpeed); From f47ef0600ae7dbb712a66e4ed8774ad5a3f2e96a Mon Sep 17 00:00:00 2001 From: Sofie Budman Date: Mon, 20 Jan 2025 14:01:53 -0800 Subject: [PATCH 12/51] import yay! --- src/main/java/org/carlmontrobotics/RobotContainer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index 47cb7d9..d65e2e8 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -8,7 +8,7 @@ // import org.carlmontrobotics.subsystems.*; // import org.carlmontrobotics.commands.*; import static org.carlmontrobotics.Constants.OI; - +import org.carlmontrobotics.subsystems.AlgaeEffector; //controllers import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.XboxController.Axis; From 05d75ff3fba4887fa2c95d366b9100dbcf7fd8f5 Mon Sep 17 00:00:00 2001 From: Niosocket11 <107586975+Niosocket11@users.noreply.github.com> Date: Mon, 20 Jan 2025 14:03:58 -0800 Subject: [PATCH 13/51] CookingStuff --- .../java/org/carlmontrobotics/RobotContainer.java | 3 +++ .../org/carlmontrobotics/commands/RunAlgae.java | 13 +++++++++---- 2 files changed, 12 insertions(+), 4 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index d65e2e8..12752cc 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -45,6 +45,9 @@ private void setDefaultCommands() { // )); } private void setBindingsDriver() { + new Trigger(() -> driverController.getRawButton(OI.X)).onTrue(new RunAlgae(algaeEffector, false, false)); + new Trigger(() -> driverController.getRawButton(OI.Y)).onTrue(new RunAlgae(algaeEffector, true, false)); + new Trigger(() -> driverController.getRawButton(OI.A)).onTrue(new RunAlgae(algaeEffector, false, true)); } private void setBindingsManipulator() {} diff --git a/src/main/java/org/carlmontrobotics/commands/RunAlgae.java b/src/main/java/org/carlmontrobotics/commands/RunAlgae.java index be0740f..6305770 100644 --- a/src/main/java/org/carlmontrobotics/commands/RunAlgae.java +++ b/src/main/java/org/carlmontrobotics/commands/RunAlgae.java @@ -6,7 +6,7 @@ public class RunAlgae extends Command { - public RunAlgae(AlgaeEffector algaeEffector, boolean inverted) { + public RunAlgae(AlgaeEffector algaeEffector, boolean inverted, boolean stop) { addRequirements(this.algaeEffector = algaeEffector); this.inverted = inverted; @@ -20,11 +20,16 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if (inverted) { - algaeEffector.RunAlgaeMotors(0.5, -0.5); + if (stop) { + algaeEffector.RunAlgaeMotors(0,0); } else { - algaeEffector.RunAlgaeMotors(0.5, 0.5); + if (inverted) { + algaeEffector.RunAlgaeMotors(0.5, -0.5); + } + else { + algaeEffector.RunAlgaeMotors(0.5, 0.5); + } } } From 195fb80ca3cf5504586bf3ad57738ceb59a9acff Mon Sep 17 00:00:00 2001 From: Sofie Budman Date: Mon, 20 Jan 2025 14:04:37 -0800 Subject: [PATCH 14/51] l --- src/main/java/org/carlmontrobotics/commands/RunAlgae.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/java/org/carlmontrobotics/commands/RunAlgae.java b/src/main/java/org/carlmontrobotics/commands/RunAlgae.java index 6305770..ad32788 100644 --- a/src/main/java/org/carlmontrobotics/commands/RunAlgae.java +++ b/src/main/java/org/carlmontrobotics/commands/RunAlgae.java @@ -5,6 +5,8 @@ import edu.wpi.first.wpilibj2.command.Command; public class RunAlgae extends Command { + AlgaeEffector algaeEffector; + Boolean inverted; public RunAlgae(AlgaeEffector algaeEffector, boolean inverted, boolean stop) { addRequirements(this.algaeEffector = algaeEffector); From 27e3eaa6c57a42e110acb23afd8baf7e4261e4fa Mon Sep 17 00:00:00 2001 From: Sofie Budman Date: Mon, 20 Jan 2025 14:07:02 -0800 Subject: [PATCH 15/51] o --- src/main/java/org/carlmontrobotics/Constants.java | 1 + src/main/java/org/carlmontrobotics/commands/RunAlgae.java | 1 + 2 files changed, 2 insertions(+) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 6fea057..527d683 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -17,6 +17,7 @@ public static final class Driver { public static final class Manipulator { public static final int port = 1; } + public static final int X = 0; } public static final class AlgaeEffectorc { public static final int upperMotorID = 1; diff --git a/src/main/java/org/carlmontrobotics/commands/RunAlgae.java b/src/main/java/org/carlmontrobotics/commands/RunAlgae.java index ad32788..6dc2732 100644 --- a/src/main/java/org/carlmontrobotics/commands/RunAlgae.java +++ b/src/main/java/org/carlmontrobotics/commands/RunAlgae.java @@ -8,6 +8,7 @@ public class RunAlgae extends Command { AlgaeEffector algaeEffector; Boolean inverted; + public RunAlgae(AlgaeEffector algaeEffector, boolean inverted, boolean stop) { addRequirements(this.algaeEffector = algaeEffector); this.inverted = inverted; From 29b307ef608bdb23cf6dc6074c2f7c2e0b81599c Mon Sep 17 00:00:00 2001 From: Niosocket11 <107586975+Niosocket11@users.noreply.github.com> Date: Mon, 20 Jan 2025 14:10:05 -0800 Subject: [PATCH 16/51] RobotContainer --- src/main/java/org/carlmontrobotics/RobotContainer.java | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index 12752cc..30b34e7 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -45,8 +45,9 @@ private void setDefaultCommands() { // )); } private void setBindingsDriver() { - new Trigger(() -> driverController.getRawButton(OI.X)).onTrue(new RunAlgae(algaeEffector, false, false)); - new Trigger(() -> driverController.getRawButton(OI.Y)).onTrue(new RunAlgae(algaeEffector, true, false)); + new Trigger(() -> driverController.getRawButton(OI.X)).onTrue(new RunAlgae(algaeEffector, 1, false)); + new Trigger(() -> driverController.getRawButton(OI.Y)).onTrue(new RunAlgae(algaeEffector, 2, false)); + new Trigger(() -> driverController.getRawButton(OI.B)).onTrue(new RunAlgae(algaeEffector, 3, false)); new Trigger(() -> driverController.getRawButton(OI.A)).onTrue(new RunAlgae(algaeEffector, false, true)); } From aa0356ecb82c06a644bd030e0954a82d6a1e9f46 Mon Sep 17 00:00:00 2001 From: Niosocket11 <107586975+Niosocket11@users.noreply.github.com> Date: Mon, 20 Jan 2025 14:11:14 -0800 Subject: [PATCH 17/51] Update RunAlgae.java --- .../carlmontrobotics/commands/RunAlgae.java | 20 ++++++++++++------- 1 file changed, 13 insertions(+), 7 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/commands/RunAlgae.java b/src/main/java/org/carlmontrobotics/commands/RunAlgae.java index 6dc2732..e8d3c0c 100644 --- a/src/main/java/org/carlmontrobotics/commands/RunAlgae.java +++ b/src/main/java/org/carlmontrobotics/commands/RunAlgae.java @@ -6,12 +6,15 @@ public class RunAlgae extends Command { AlgaeEffector algaeEffector; - Boolean inverted; + boolean stop; + int direction; - public RunAlgae(AlgaeEffector algaeEffector, boolean inverted, boolean stop) { + + public RunAlgae(AlgaeEffector algaeEffector, int direction, boolean stop) { addRequirements(this.algaeEffector = algaeEffector); - this.inverted = inverted; + this.direction = direction; + this.stop = stop; } @@ -27,11 +30,14 @@ public void execute() { algaeEffector.RunAlgaeMotors(0,0); } else { - if (inverted) { - algaeEffector.RunAlgaeMotors(0.5, -0.5); + if (direction == 1) { + algaeEffector.RunAlgaeMotors(0.5, 0.5); + } + else if (direction == 2) { + algaeEffector.RunAlgaeMotors(-0.5, -0.5); } - else { - algaeEffector.RunAlgaeMotors(0.5, 0.5); + else if (direction == 3) { + algaeEffector.RunAlgaeMotors(-0.5, 0.5); } } } From a365b59f5ed5e8c74ca78ddc99a041aa827ce172 Mon Sep 17 00:00:00 2001 From: Sofie Budman Date: Mon, 20 Jan 2025 14:21:17 -0800 Subject: [PATCH 18/51] k --- build.gradle | 2 +- .../subsystems/AlgaeEffector.java | 15 ++++----------- 2 files changed, 5 insertions(+), 12 deletions(-) diff --git a/build.gradle b/build.gradle index 8d59a2e..35c0c0f 100644 --- a/build.gradle +++ b/build.gradle @@ -21,7 +21,7 @@ deploy { // or from command line. If not found an exception will be thrown. // You can use getTeamOrDefault(team) instead of getTeamNumber if you // want to store a team number in this file. - team = project.frc.getTeamNumber() + team = project.frc.getTeamOrDefault(199) debug = project.frc.getDebugOrDefault(false) artifacts { diff --git a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java index b17ecb5..9f202c1 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java +++ b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java @@ -10,26 +10,19 @@ import org.carlmontrobotics.Constants; import org.carlmontrobotics.RobotContainer; -import com.revrobotics.CANSparkBase; -import com.revrobotics.CANSparkBase.IdleMode; -import com.revrobotics.CANSparkBase.SoftLimitDirection; import com.revrobotics.spark.SparkFlex; import com.revrobotics.spark.SparkLowLevel.MotorType; -import com.revrobotics.CANSparkMax; -import com.revrobotics.SparkAbsoluteEncoder; -import com.revrobotics.SparkPIDController; -import com.revrobotics.SparkLowLevel; + import edu.wpi.first.hal.SimDouble; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; -import edu.wpi.first.unitss.Angle; + import edu.wpi.first.units.Measure; import edu.wpi.first.units.MutableMeasure; -import edu.wpi.first.units.Velocity; -import edu.wpi.first.units.Voltage; + import edu.wpi.first.util.sendable.SendableBuilder; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.RobotBase; @@ -43,7 +36,7 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; + public class AlgaeEffector extends SubsystemBase { private SparkFlex upperMotor = new SparkFlex(Constants.AlgaeEffectorc.upperMotorID, MotorType.kBrushless); From c44fc1ecefafe1d56f5f3f4d407d315072690f20 Mon Sep 17 00:00:00 2001 From: Sofie Budman Date: Mon, 20 Jan 2025 14:34:17 -0800 Subject: [PATCH 19/51] yay it builds hooray!! --- src/main/java/org/carlmontrobotics/RobotContainer.java | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index 30b34e7..c5e9f54 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -8,6 +8,8 @@ // import org.carlmontrobotics.subsystems.*; // import org.carlmontrobotics.commands.*; import static org.carlmontrobotics.Constants.OI; + +import org.carlmontrobotics.commands.RunAlgae; import org.carlmontrobotics.subsystems.AlgaeEffector; //controllers import edu.wpi.first.wpilibj.GenericHID; @@ -45,10 +47,10 @@ private void setDefaultCommands() { // )); } private void setBindingsDriver() { - new Trigger(() -> driverController.getRawButton(OI.X)).onTrue(new RunAlgae(algaeEffector, 1, false)); - new Trigger(() -> driverController.getRawButton(OI.Y)).onTrue(new RunAlgae(algaeEffector, 2, false)); - new Trigger(() -> driverController.getRawButton(OI.B)).onTrue(new RunAlgae(algaeEffector, 3, false)); - new Trigger(() -> driverController.getRawButton(OI.A)).onTrue(new RunAlgae(algaeEffector, false, true)); + new Trigger(() -> driverController.getRawButton(OI.Driver.X)).onTrue(new RunAlgae(algaeEffector, 1, false)); //wrong + new Trigger(() -> driverController.getRawButton(OI.Driver.Y)).onTrue(new RunAlgae(algaeEffector, 2, false)); + new Trigger(() -> driverController.getRawButton(OI.Driver.B)).onTrue(new RunAlgae(algaeEffector, 3, false)); + new Trigger(() -> driverController.getRawButton(OI.Driver.A)).onTrue(new RunAlgae(algaeEffector, 0, true)); } private void setBindingsManipulator() {} From 4d0dba2da111ccfc5e2418ec59a522049b6dd070 Mon Sep 17 00:00:00 2001 From: Rand0mAsianKid <144396869+Rand0mAsianKid@users.noreply.github.com> Date: Fri, 24 Jan 2025 12:49:24 -0800 Subject: [PATCH 20/51] did some of the coral effector --- .../java/org/carlmontrobotics/Constants.java | 11 +++ .../carlmontrobotics/commands/RunCoral.java | 44 ++++++++++++ .../subsystems/CoralEffector.java | 67 +++++++++++++++++++ 3 files changed, 122 insertions(+) create mode 100644 src/main/java/org/carlmontrobotics/commands/RunCoral.java create mode 100644 src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 527d683..6da292f 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -23,4 +23,15 @@ public static final class AlgaeEffectorc { public static final int upperMotorID = 1; public static final int lowerMotorID = 2; } + public static final class CoralEffectorc { + public static final int effectorMotorID = 3; + public static final int effectorDistanceSensorID = 4; + public static final double kP = 0.0; + public static final double kI = 0.0; + public static final double kD = 0.0; + public static final double kS = 0.0; + + public static final double feedforward[] = {0.0}; + + } } diff --git a/src/main/java/org/carlmontrobotics/commands/RunCoral.java b/src/main/java/org/carlmontrobotics/commands/RunCoral.java new file mode 100644 index 0000000..78ff52a --- /dev/null +++ b/src/main/java/org/carlmontrobotics/commands/RunCoral.java @@ -0,0 +1,44 @@ +package org.carlmontrobotics.commands; + +import static org.carlmontrobotics.Constants.CoralEffectorc.*; + +import org.carlmontrobotics.subsystems.CoralEffector; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; + +public class RunCoral extends Command { + // intake until sees game peice or 4sec has passed + private final CoralEffector Coral; + private double initSpeed = 2100; + + public RunCoral(CoralEffector Coral) { + addRequirements(this.Coral = Coral); + } + + @Override + public void initialize() { + Coral.setRPMEffector(initSpeed); + } + + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + increaseSpeed = SmartDashboard.getNumber("Increase speed", 0); + slowSpeed = SmartDashboard.getNumber("Slow intake speed", 0); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + Coral.stopEffector(); + } + + @Override + public boolean isFinished() { + //distance sensor doesn't detect coral + return Coral.outtakeDetectsNote(); + } +} \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java b/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java new file mode 100644 index 0000000..f9e76ca --- /dev/null +++ b/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java @@ -0,0 +1,67 @@ +package org.carlmontrobotics.subsystems; + +import org.carlmontrobotics.Constants; +import org.carlmontrobotics.lib199.MotorConfig; +import org.carlmontrobotics.lib199.MotorControllerFactory; + +import com.playingwithfusion.TimeOfFlight; +import com.playingwithfusion.TimeOfFlight.RangingMode; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.SparkFlex; + +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.util.datalog.DataLogEntry; +import edu.wpi.first.util.datalog.StringLogEntry; +import edu.wpi.first.util.sendable.SendableBuilder; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkFlex; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; + + + +public class CoralEffector extends SubsystemBase { + private SparkFlex effectorMotor = new SparkFlex(Constants.CoralEffectorc.effectorMotorID, MotorType.kBrushless); + private final RelativeEncoder effectorEncoder = effectorMotor.getEncoder(); + private final SparkPIDController pidControllerEffector = effectorMotor.getPIDController(); + private final SimpleMotorFeedforward effectorFeedforward = new SimpleMotorFeedforward(kS[OUTTAKE], kV[OUTTAKE], kA[OUTTAKE]); + private double goalOutakeRPM = effectorEncoder.getVelocity(); + + private Timer effectorTOFTimer = new Timer(); + private TimeOfFlight effectorDistanceSensor = new TimeOfFlight(Constants.CoralEffectorc.effectorDistanceSensorID); + + private double lastValidDistanceIntake = Double.POSITIVE_INFINITY; + private double lastValidDistanceOuttake = Double.POSITIVE_INFINITY; + + public CoralEffector() { + pidControllerEffector.setP(Constants.CoralEffectorc.kP); + pidControllerEffector.setI(Constants.CoralEffectorc.kI); + pidControllerEffector.setD(Constants.CoralEffectorc.kD); + //Will add distance sensor later + + } + + public void motorSetEffector(double speed) { + effectorMotor.set(speed); + } + + public void shootIntake() { + motorSetEffector(0.5); + } + + public void setRPMEffector(double rpm) { + pidControllerEffector.setReference(rpm, CANSparkBase.ControlType.kVelocity, 0, + effectorFeedforward.calculate(rpm / 60.0)); + } + + public void stopEffector() { + setRPMEffector(0); + } + + public void intakeShootEffector() { + setRPMEffector(2100); + } +} From c36bef4187e0899ad4967fef98675e15da315a60 Mon Sep 17 00:00:00 2001 From: Rand0mAsianKid <144396869+Rand0mAsianKid@users.noreply.github.com> Date: Fri, 24 Jan 2025 19:26:44 -0800 Subject: [PATCH 21/51] Updated commands and subsytems --- .../java/org/carlmontrobotics/Constants.java | 12 ++- .../org/carlmontrobotics/RobotContainer.java | 30 +++++++- .../{RunCoral.java => IntakeCoral.java} | 20 ++--- .../commands/OuttakeCoral.java | 42 +++++++++++ .../subsystems/CoralEffector.java | 74 ++++++++++++++----- 5 files changed, 143 insertions(+), 35 deletions(-) rename src/main/java/org/carlmontrobotics/commands/{RunCoral.java => IntakeCoral.java} (64%) create mode 100644 src/main/java/org/carlmontrobotics/commands/OuttakeCoral.java diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 6da292f..27bdb81 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -1,5 +1,7 @@ package org.carlmontrobotics; +import edu.wpi.first.wpilibj.XboxController.Axis; + public final class Constants { // public static final class Drivetrain { // public static final double MAX_SPEED_MPS = 2; @@ -16,6 +18,7 @@ public static final class Driver { } public static final class Manipulator { public static final int port = 1; + public static final double MIN_AXIS_TRIGGER_VALUE = 0.2; } public static final int X = 0; } @@ -26,12 +29,13 @@ public static final class AlgaeEffectorc { public static final class CoralEffectorc { public static final int effectorMotorID = 3; public static final int effectorDistanceSensorID = 4; + public static final int ManipulatorControllerPort = 5; + public static final Axis OuttakeTrigger = Axis.kRightTrigger; + public static final Axis IntakeTrigger = Axis.kLeftTrigger; public static final double kP = 0.0; public static final double kI = 0.0; public static final double kD = 0.0; - public static final double kS = 0.0; - - public static final double feedforward[] = {0.0}; - + //TODO: Change after testing + public static final int DETECT_DISTANCE_INCHES = 3; } } diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index c5e9f54..025793a 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -9,8 +9,15 @@ // import org.carlmontrobotics.commands.*; import static org.carlmontrobotics.Constants.OI; +import org.carlmontrobotics.Constants.OI.Manipulator.*; +import org.carlmontrobotics.Constants.OI.Manipulator; import org.carlmontrobotics.commands.RunAlgae; import org.carlmontrobotics.subsystems.AlgaeEffector; +import org.carlmontrobotics.subsystems.CoralEffector; +import org.carlmontrobotics.commands.IntakeCoral; +import org.carlmontrobotics.commands.OuttakeCoral; + + //controllers import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.XboxController.Axis; @@ -18,11 +25,13 @@ //commands import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.PrintCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; //control bindings +import static org.carlmontrobotics.Constants.CoralEffectorc.*; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.POVButton; import edu.wpi.first.wpilibj2.command.button.Trigger; @@ -32,9 +41,12 @@ public class RobotContainer { //1. using GenericHID allows us to use different kinds of controllers //2. Use absolute paths from constants to reduce confusion public final GenericHID driverController = new GenericHID(OI.Driver.port); + public final GenericHID manipulatorController = new GenericHID(OI.Driver.port); private final AlgaeEffector algaeEffector = new AlgaeEffector(); + private final CoralEffector coralEffector = new CoralEffector(); public RobotContainer() { setBindingsDriver(); + setBindingsManipulator(); } private void setDefaultCommands() { @@ -53,7 +65,23 @@ private void setBindingsDriver() { new Trigger(() -> driverController.getRawButton(OI.Driver.A)).onTrue(new RunAlgae(algaeEffector, 0, true)); } - private void setBindingsManipulator() {} + + + private void setBindingsManipulator() { + axisTrigger(manipulatorController, OuttakeTrigger) + .whileTrue(new IntakeCoral(coralEffector)); + + axisTrigger(manipulatorController, IntakeTrigger) + .whileTrue(new OuttakeCoral(coralEffector)); + } + + + + private Trigger axisTrigger(GenericHID controller, Axis axis) { + return new Trigger(() -> Math + .abs(getStickValue(controller, axis)) > Constants.OI.Manipulator.MIN_AXIS_TRIGGER_VALUE); + } + public Command getAutonomousCommand() { return Commands.print("No autonomous command configured"); diff --git a/src/main/java/org/carlmontrobotics/commands/RunCoral.java b/src/main/java/org/carlmontrobotics/commands/IntakeCoral.java similarity index 64% rename from src/main/java/org/carlmontrobotics/commands/RunCoral.java rename to src/main/java/org/carlmontrobotics/commands/IntakeCoral.java index 78ff52a..c5a37fc 100644 --- a/src/main/java/org/carlmontrobotics/commands/RunCoral.java +++ b/src/main/java/org/carlmontrobotics/commands/IntakeCoral.java @@ -8,27 +8,25 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; -public class RunCoral extends Command { - // intake until sees game peice or 4sec has passed +public class IntakeCoral extends Command { private final CoralEffector Coral; - private double initSpeed = 2100; + int increaseSpeed; - public RunCoral(CoralEffector Coral) { + private double Speed = 1000; + + public IntakeCoral(CoralEffector Coral) { addRequirements(this.Coral = Coral); } @Override public void initialize() { - Coral.setRPMEffector(initSpeed); + Coral.setRPM(Speed); } // Called every time the scheduler runs while the command is scheduled. @Override - public void execute() { - increaseSpeed = SmartDashboard.getNumber("Increase speed", 0); - slowSpeed = SmartDashboard.getNumber("Slow intake speed", 0); - } + public void execute() {} // Called once the command ends or is interrupted. @Override @@ -39,6 +37,8 @@ public void end(boolean interrupted) { @Override public boolean isFinished() { //distance sensor doesn't detect coral - return Coral.outtakeDetectsNote(); + //TODO: make distance sensor stuff + //TODO: add smartdashboard + return Coral.coralDetects() == true; } } \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/commands/OuttakeCoral.java b/src/main/java/org/carlmontrobotics/commands/OuttakeCoral.java new file mode 100644 index 0000000..d1c037f --- /dev/null +++ b/src/main/java/org/carlmontrobotics/commands/OuttakeCoral.java @@ -0,0 +1,42 @@ +package org.carlmontrobotics.commands; + +import static org.carlmontrobotics.Constants.CoralEffectorc.*; + +import org.carlmontrobotics.subsystems.CoralEffector; + +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; + +public class OuttakeCoral extends Command { + private final CoralEffector Coral; + int increaseSpeed; private double Speed = 2100; + public OuttakeCoral(CoralEffector Coral) { + addRequirements(this.Coral = Coral); + } + + @Override + public void initialize() { + Coral.setRPM(Speed); + } + + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + Coral.stopEffector(); + } + + @Override + public boolean isFinished() { + //distance sensor doesn't detect coral + //TODO: make distance sensor stuff + //TODO: add smartdashboard + return Coral.coralDetects() == false; + } +} \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java b/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java index f9e76ca..f170aa7 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java +++ b/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java @@ -8,60 +8,94 @@ import com.playingwithfusion.TimeOfFlight.RangingMode; import com.revrobotics.RelativeEncoder; import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.SparkBase.ControlType; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.ClosedLoopSlot; +import com.revrobotics.spark.SparkClosedLoopController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.util.Units; import edu.wpi.first.util.datalog.DataLogEntry; import edu.wpi.first.util.datalog.StringLogEntry; import edu.wpi.first.util.sendable.SendableBuilder; +import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.config.SparkFlexConfig; +import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; import com.revrobotics.spark.SparkFlex; import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import static org.carlmontrobotics.Constants.CoralEffectorc.*; + public class CoralEffector extends SubsystemBase { private SparkFlex effectorMotor = new SparkFlex(Constants.CoralEffectorc.effectorMotorID, MotorType.kBrushless); private final RelativeEncoder effectorEncoder = effectorMotor.getEncoder(); - private final SparkPIDController pidControllerEffector = effectorMotor.getPIDController(); - private final SimpleMotorFeedforward effectorFeedforward = new SimpleMotorFeedforward(kS[OUTTAKE], kV[OUTTAKE], kA[OUTTAKE]); + private final SparkClosedLoopController pidControllerEffector = effectorMotor.getClosedLoopController(); + // private final SimpleMotorFeedforward effectorFeedforward = new SimpleMotorFeedforward(kS[OUTTAKE], kV[OUTTAKE], kA[OUTTAKE]); private double goalOutakeRPM = effectorEncoder.getVelocity(); - + DigitalInput limitSwitch = new DigitalInput(0); //TODO: change channel after wired private Timer effectorTOFTimer = new Timer(); private TimeOfFlight effectorDistanceSensor = new TimeOfFlight(Constants.CoralEffectorc.effectorDistanceSensorID); - - private double lastValidDistanceIntake = Double.POSITIVE_INFINITY; - private double lastValidDistanceOuttake = Double.POSITIVE_INFINITY; + + private double lastValidDistance = Double.POSITIVE_INFINITY; public CoralEffector() { - pidControllerEffector.setP(Constants.CoralEffectorc.kP); - pidControllerEffector.setI(Constants.CoralEffectorc.kI); - pidControllerEffector.setD(Constants.CoralEffectorc.kD); + SparkFlexConfig c = new SparkFlexConfig(); + c.closedLoop.pid( + Constants.CoralEffectorc.kP, + Constants.CoralEffectorc.kI, + Constants.CoralEffectorc.kD + ).feedbackSensor(FeedbackSensor.kPrimaryEncoder); + effectorMotor.configure(c, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); //Will add distance sensor later } - public void motorSetEffector(double speed) { + public void setSpeed(double speed) { effectorMotor.set(speed); } - - public void shootIntake() { - motorSetEffector(0.5); - } - public void setRPMEffector(double rpm) { - pidControllerEffector.setReference(rpm, CANSparkBase.ControlType.kVelocity, 0, - effectorFeedforward.calculate(rpm / 60.0)); + public void setRPM(double rpm) { + pidControllerEffector.setReference(rpm, ControlType.kVelocity, ClosedLoopSlot.kSlot0); } public void stopEffector() { - setRPMEffector(0); + setRPM(0); + } + + public void setRPM() { + setRPM(2100); } - public void intakeShootEffector() { - setRPMEffector(2100); + + public boolean coralDetects() { + return lastValidDistance < DETECT_DISTANCE_INCHES; + } + + public boolean limitDetects() { + return limitSwitch.get(); + } + + public void updateValues() { + if (effectorDistanceSensor.isRangeValid()) { + if (lastValidDistance != 5.75) { + effectorTOFTimer.reset(); + } else + effectorTOFTimer.start(); + lastValidDistance = Units.metersToInches(effectorDistanceSensor.getRange()); + } + } + + + @Override + public void periodic() { + updateValues(); + limitDetects(); } } From c583b7492c692c745fe3e4bc464081822a09f0fa Mon Sep 17 00:00:00 2001 From: Niosocket11 <107586975+Niosocket11@users.noreply.github.com> Date: Sat, 25 Jan 2025 10:11:47 -0800 Subject: [PATCH 22/51] Comment --- .../java/org/carlmontrobotics/subsystems/CoralEffector.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java b/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java index f170aa7..9c4e936 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java +++ b/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java @@ -34,7 +34,7 @@ public class CoralEffector extends SubsystemBase { - private SparkFlex effectorMotor = new SparkFlex(Constants.CoralEffectorc.effectorMotorID, MotorType.kBrushless); + private SparkFlex effectorMotor = new SparkFlex(Constants.CoralEffectorc.effectorMotorID, MotorType.kBrushless); //why is there a vortex motor on coral effectors? private final RelativeEncoder effectorEncoder = effectorMotor.getEncoder(); private final SparkClosedLoopController pidControllerEffector = effectorMotor.getClosedLoopController(); // private final SimpleMotorFeedforward effectorFeedforward = new SimpleMotorFeedforward(kS[OUTTAKE], kV[OUTTAKE], kA[OUTTAKE]); From fd16a132c19e90fcb6eb4584a4aeb47e76d60438 Mon Sep 17 00:00:00 2001 From: Rand0mAsianKid <144396869+Rand0mAsianKid@users.noreply.github.com> Date: Fri, 31 Jan 2025 19:23:58 -0800 Subject: [PATCH 23/51] added limit switch annd fixed alex's issues --- .../java/org/carlmontrobotics/Constants.java | 21 +++++++++++-------- .../org/carlmontrobotics/RobotContainer.java | 21 ++++++++++--------- .../commands/IntakeCoral.java | 9 ++++---- .../commands/OuttakeCoral.java | 6 +++--- .../subsystems/CoralEffector.java | 21 +++++++------------ 5 files changed, 38 insertions(+), 40 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 27bdb81..7e3ab54 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -7,20 +7,24 @@ public final class Constants { // public static final double MAX_SPEED_MPS = 2; // } public static final class OI { + public static final double MIN_AXIS_TRIGGER_VALUE = 0.2; + public static final class Driver { - public static final int port = 0; - public static final int A = 1; + public static final int driverPort = 0; + /*public static final int A = 1; public static final int B = 2; public static final int X = 3; - public static final int Y = 4; + public static final int Y = 4;*/ + //Not neccesary public static final int leftBumper = 5; public static final int rightBumper = 6; } public static final class Manipulator { - public static final int port = 1; - public static final double MIN_AXIS_TRIGGER_VALUE = 0.2; + public static final int manipulatorPort = 5; + //public static final int X = 0; + public static final Axis OuttakeTrigger = Axis.kRightTrigger; + public static final Axis IntakeTrigger = Axis.kLeftTrigger; } - public static final int X = 0; } public static final class AlgaeEffectorc { public static final int upperMotorID = 1; @@ -29,12 +33,11 @@ public static final class AlgaeEffectorc { public static final class CoralEffectorc { public static final int effectorMotorID = 3; public static final int effectorDistanceSensorID = 4; - public static final int ManipulatorControllerPort = 5; - public static final Axis OuttakeTrigger = Axis.kRightTrigger; - public static final Axis IntakeTrigger = Axis.kLeftTrigger; public static final double kP = 0.0; public static final double kI = 0.0; public static final double kD = 0.0; + public static double intakeRPM = 1000; + public static double outtakeRPM = 2100; //TODO: Change after testing public static final int DETECT_DISTANCE_INCHES = 3; } diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index 025793a..a411569 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -7,8 +7,9 @@ //199 files // import org.carlmontrobotics.subsystems.*; // import org.carlmontrobotics.commands.*; -import static org.carlmontrobotics.Constants.OI; +import static org.carlmontrobotics.Constants.OI.*; +import org.carlmontrobotics.Constants.OI; import org.carlmontrobotics.Constants.OI.Manipulator.*; import org.carlmontrobotics.Constants.OI.Manipulator; import org.carlmontrobotics.commands.RunAlgae; @@ -40,8 +41,8 @@ public class RobotContainer { //1. using GenericHID allows us to use different kinds of controllers //2. Use absolute paths from constants to reduce confusion - public final GenericHID driverController = new GenericHID(OI.Driver.port); - public final GenericHID manipulatorController = new GenericHID(OI.Driver.port); + public final GenericHID driverController = new GenericHID(OI.Driver.driverPort); + public final GenericHID manipulatorController = new GenericHID(OI.Manipulator.manipulatorPort); private final AlgaeEffector algaeEffector = new AlgaeEffector(); private final CoralEffector coralEffector = new CoralEffector(); public RobotContainer() { @@ -59,19 +60,19 @@ private void setDefaultCommands() { // )); } private void setBindingsDriver() { - new Trigger(() -> driverController.getRawButton(OI.Driver.X)).onTrue(new RunAlgae(algaeEffector, 1, false)); //wrong - new Trigger(() -> driverController.getRawButton(OI.Driver.Y)).onTrue(new RunAlgae(algaeEffector, 2, false)); - new Trigger(() -> driverController.getRawButton(OI.Driver.B)).onTrue(new RunAlgae(algaeEffector, 3, false)); - new Trigger(() -> driverController.getRawButton(OI.Driver.A)).onTrue(new RunAlgae(algaeEffector, 0, true)); + // new Trigger(() -> driverController.getRawButton(OI.Driver.X)).onTrue(new RunAlgae(algaeEffector, 1, false)); //wrong + // new Trigger(() -> driverController.getRawButton(OI.Driver.Y)).onTrue(new RunAlgae(algaeEffector, 2, false)); + // new Trigger(() -> driverController.getRawButton(OI.Driver.B)).onTrue(new RunAlgae(algaeEffector, 3, false)); + // new Trigger(() -> driverController.getRawButton(OI.Driver.A)).onTrue(new RunAlgae(algaeEffector, 0, true)); } private void setBindingsManipulator() { - axisTrigger(manipulatorController, OuttakeTrigger) + axisTrigger(manipulatorController, OI.Manipulator.OuttakeTrigger) .whileTrue(new IntakeCoral(coralEffector)); - axisTrigger(manipulatorController, IntakeTrigger) + axisTrigger(manipulatorController, OI.Manipulator.IntakeTrigger) .whileTrue(new OuttakeCoral(coralEffector)); } @@ -79,7 +80,7 @@ private void setBindingsManipulator() { private Trigger axisTrigger(GenericHID controller, Axis axis) { return new Trigger(() -> Math - .abs(getStickValue(controller, axis)) > Constants.OI.Manipulator.MIN_AXIS_TRIGGER_VALUE); + .abs(getStickValue(controller, axis)) > Constants.OI.MIN_AXIS_TRIGGER_VALUE); } diff --git a/src/main/java/org/carlmontrobotics/commands/IntakeCoral.java b/src/main/java/org/carlmontrobotics/commands/IntakeCoral.java index c5a37fc..f0a8ba8 100644 --- a/src/main/java/org/carlmontrobotics/commands/IntakeCoral.java +++ b/src/main/java/org/carlmontrobotics/commands/IntakeCoral.java @@ -2,6 +2,7 @@ import static org.carlmontrobotics.Constants.CoralEffectorc.*; +import org.carlmontrobotics.Constants; import org.carlmontrobotics.subsystems.CoralEffector; import edu.wpi.first.wpilibj.Timer; @@ -10,9 +11,6 @@ public class IntakeCoral extends Command { private final CoralEffector Coral; - int increaseSpeed; - - private double Speed = 1000; public IntakeCoral(CoralEffector Coral) { addRequirements(this.Coral = Coral); @@ -20,7 +18,7 @@ public IntakeCoral(CoralEffector Coral) { @Override public void initialize() { - Coral.setRPM(Speed); + Coral.setRPM(Constants.CoralEffectorc.intakeRPM); } @@ -32,6 +30,7 @@ public void execute() {} @Override public void end(boolean interrupted) { Coral.stopEffector(); + //TODO: Test different times } @Override @@ -39,6 +38,6 @@ public boolean isFinished() { //distance sensor doesn't detect coral //TODO: make distance sensor stuff //TODO: add smartdashboard - return Coral.coralDetects() == true; + return !Coral.coralDetects() && !Coral.limitDetects(); } } \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/commands/OuttakeCoral.java b/src/main/java/org/carlmontrobotics/commands/OuttakeCoral.java index d1c037f..c23ce61 100644 --- a/src/main/java/org/carlmontrobotics/commands/OuttakeCoral.java +++ b/src/main/java/org/carlmontrobotics/commands/OuttakeCoral.java @@ -2,6 +2,7 @@ import static org.carlmontrobotics.Constants.CoralEffectorc.*; +import org.carlmontrobotics.Constants; import org.carlmontrobotics.subsystems.CoralEffector; import edu.wpi.first.wpilibj.DigitalInput; @@ -11,14 +12,13 @@ public class OuttakeCoral extends Command { private final CoralEffector Coral; - int increaseSpeed; private double Speed = 2100; public OuttakeCoral(CoralEffector Coral) { addRequirements(this.Coral = Coral); } @Override public void initialize() { - Coral.setRPM(Speed); + Coral.setRPM(Constants.CoralEffectorc.outtakeRPM); } @@ -37,6 +37,6 @@ public boolean isFinished() { //distance sensor doesn't detect coral //TODO: make distance sensor stuff //TODO: add smartdashboard - return Coral.coralDetects() == false; + return !Coral.coralDetects() && !Coral.limitDetects(); } } \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java b/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java index f170aa7..ae02644 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java +++ b/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java @@ -30,9 +30,6 @@ import edu.wpi.first.math.controller.SimpleMotorFeedforward; import static org.carlmontrobotics.Constants.CoralEffectorc.*; - - - public class CoralEffector extends SubsystemBase { private SparkFlex effectorMotor = new SparkFlex(Constants.CoralEffectorc.effectorMotorID, MotorType.kBrushless); private final RelativeEncoder effectorEncoder = effectorMotor.getEncoder(); @@ -45,6 +42,14 @@ public class CoralEffector extends SubsystemBase { private double lastValidDistance = Double.POSITIVE_INFINITY; + public boolean coralDetects() { + return lastValidDistance < DETECT_DISTANCE_INCHES; + } + + public boolean limitDetects() { + return limitSwitch.get(); + } + public CoralEffector() { SparkFlexConfig c = new SparkFlexConfig(); c.closedLoop.pid( @@ -73,15 +78,6 @@ public void setRPM() { setRPM(2100); } - - public boolean coralDetects() { - return lastValidDistance < DETECT_DISTANCE_INCHES; - } - - public boolean limitDetects() { - return limitSwitch.get(); - } - public void updateValues() { if (effectorDistanceSensor.isRangeValid()) { if (lastValidDistance != 5.75) { @@ -92,7 +88,6 @@ public void updateValues() { } } - @Override public void periodic() { updateValues(); From d657af08018c6a7c41c9d8c29c1604f0d6b9ee30 Mon Sep 17 00:00:00 2001 From: Niosocket11 <107586975+Niosocket11@users.noreply.github.com> Date: Sat, 1 Feb 2025 14:43:26 -0800 Subject: [PATCH 24/51] Sparkflex to Sparkmax update --- .../java/org/carlmontrobotics/subsystems/CoralEffector.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java b/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java index 8ed97e2..75d71bf 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java +++ b/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java @@ -8,6 +8,7 @@ import com.playingwithfusion.TimeOfFlight.RangingMode; import com.revrobotics.RelativeEncoder; import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.SparkBase.PersistMode; import com.revrobotics.spark.SparkBase.ResetMode; @@ -31,7 +32,7 @@ import static org.carlmontrobotics.Constants.CoralEffectorc.*; public class CoralEffector extends SubsystemBase { - private SparkFlex effectorMotor = new SparkFlex(Constants.CoralEffectorc.effectorMotorID, MotorType.kBrushless); //why is there a vortex motor on coral effectors? + private SparkMax effectorMotor = new SparkMax(Constants.CoralEffectorc.effectorMotorID, MotorType.kBrushless); //why is there a vortex motor on coral effectors? private final RelativeEncoder effectorEncoder = effectorMotor.getEncoder(); private final SparkClosedLoopController pidControllerEffector = effectorMotor.getClosedLoopController(); // private final SimpleMotorFeedforward effectorFeedforward = new SimpleMotorFeedforward(kS[OUTTAKE], kV[OUTTAKE], kA[OUTTAKE]); From d69aa38ff35bd8be1bfa401507a58a0f408e8892 Mon Sep 17 00:00:00 2001 From: Rand0mAsianKid <144396869+Rand0mAsianKid@users.noreply.github.com> Date: Sun, 2 Feb 2025 15:36:50 -0800 Subject: [PATCH 25/51] finished algae subsystem --- .../java/org/carlmontrobotics/Constants.java | 13 +++ .../org/carlmontrobotics/RobotContainer.java | 2 +- .../{RunAlgae.java => IntakeAlgae.java} | 23 +--- .../commands/OuttakeAlgae.java | 39 +++++++ .../subsystems/AlgaeEffector.java | 101 ++++++++++++++++-- 5 files changed, 150 insertions(+), 28 deletions(-) rename src/main/java/org/carlmontrobotics/commands/{RunAlgae.java => IntakeAlgae.java} (59%) create mode 100644 src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 7e3ab54..c708cbd 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -6,6 +6,19 @@ public final class Constants { // public static final class Drivetrain { // public static final double MAX_SPEED_MPS = 2; // } + + public static final int top = 1; + public static final int bottom = 1; + public static final int pincher = 1; + + public static final double[] kP = {/*/Top/*/0.0, /*/Bottom/*/0.0, /*/Pincher/*/0.0}; + public static final double[] kI = {/*/Top/*/0.0, /*/Bottom/*/0.0, /*/Pincher/*/0.0}; + public static final double[] kD = {/*/Top/*/0.0, /*/Bottom/*/0.0, /*/Pincher/*/0.0}; + + public static final double[] kS = {/*/Top/*/0.0, /*/Bottom/*/0.0, /*/Pincher/*/0.0}; + public static final double[] kV = {/*/Top/*/0.0, /*/Bottom/*/0.0, /*/Pincher/*/0.0}; + public static final double[] kA = {/*/Top/*/0.0, /*/Bottom/*/0.0, /*/Pincher/*/0.0}; + public static final class OI { public static final double MIN_AXIS_TRIGGER_VALUE = 0.2; diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index a411569..62a8e0f 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -12,7 +12,7 @@ import org.carlmontrobotics.Constants.OI; import org.carlmontrobotics.Constants.OI.Manipulator.*; import org.carlmontrobotics.Constants.OI.Manipulator; -import org.carlmontrobotics.commands.RunAlgae; +import org.carlmontrobotics.commands.OuttakeAlgae; import org.carlmontrobotics.subsystems.AlgaeEffector; import org.carlmontrobotics.subsystems.CoralEffector; import org.carlmontrobotics.commands.IntakeCoral; diff --git a/src/main/java/org/carlmontrobotics/commands/RunAlgae.java b/src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java similarity index 59% rename from src/main/java/org/carlmontrobotics/commands/RunAlgae.java rename to src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java index e8d3c0c..64ac272 100644 --- a/src/main/java/org/carlmontrobotics/commands/RunAlgae.java +++ b/src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java @@ -4,18 +4,15 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; -public class RunAlgae extends Command { +public class IntakeAlgae extends Command { AlgaeEffector algaeEffector; boolean stop; int direction; - - - public RunAlgae(AlgaeEffector algaeEffector, int direction, boolean stop) { + public IntakeAlgae(AlgaeEffector algaeEffector) { addRequirements(this.algaeEffector = algaeEffector); this.direction = direction; - this.stop = stop; - + this.stop = stop; } @Override @@ -26,20 +23,6 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if (stop) { - algaeEffector.RunAlgaeMotors(0,0); - } - else { - if (direction == 1) { - algaeEffector.RunAlgaeMotors(0.5, 0.5); - } - else if (direction == 2) { - algaeEffector.RunAlgaeMotors(-0.5, -0.5); - } - else if (direction == 3) { - algaeEffector.RunAlgaeMotors(-0.5, 0.5); - } - } } // Called once the command ends or is interrupted. diff --git a/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java b/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java new file mode 100644 index 0000000..27788b2 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java @@ -0,0 +1,39 @@ +package org.carlmontrobotics.commands; +import org.carlmontrobotics.subsystems.AlgaeEffector; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; + +public class OuttakeAlgae extends Command { + AlgaeEffector algaeEffector; + boolean stop; + int direction; + + public OuttakeAlgae(AlgaeEffector algaeEffector, int direction, boolean stop) { + addRequirements(this.algaeEffector = algaeEffector); + this.direction = direction; + this.stop = stop; + } + + @Override + public void initialize() { + + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java index 9f202c1..a9d9718 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java +++ b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java @@ -10,13 +10,22 @@ import org.carlmontrobotics.Constants; import org.carlmontrobotics.RobotContainer; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.ClosedLoopSlot; +import com.revrobotics.spark.SparkBase.ControlType; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkClosedLoopController; import com.revrobotics.spark.SparkFlex; import com.revrobotics.spark.SparkLowLevel.MotorType; - +import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; +import com.revrobotics.spark.config.SparkFlexConfig; +import com.revrobotics.spark.SparkMax; import edu.wpi.first.hal.SimDouble; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; @@ -24,6 +33,7 @@ import edu.wpi.first.units.MutableMeasure; import edu.wpi.first.util.sendable.SendableBuilder; +import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.Timer; @@ -39,11 +49,88 @@ public class AlgaeEffector extends SubsystemBase { - private SparkFlex upperMotor = new SparkFlex(Constants.AlgaeEffectorc.upperMotorID, MotorType.kBrushless); - private SparkFlex lowerMotor = new SparkFlex(Constants.AlgaeEffectorc.lowerMotorID, MotorType.kBrushless); - public void RunAlgaeMotors(double upperMotorSpeed, double lowerMotorSpeed) { - - upperMotor.set(upperMotorSpeed); - lowerMotor.set(lowerMotorSpeed); + private SparkFlex topMotor = new SparkFlex(Constants.CoralEffectorc.effectorMotorID, MotorType.kBrushless); //why is there a vortex motor on coral effectors? + private SparkMax bottomMotor = new SparkMax(Constants.CoralEffectorc.effectorMotorID, MotorType.kBrushless); //why is there a vortex motor on coral effectors? + private SparkFlex pincherMotor = new SparkFlex(Constants.CoralEffectorc.effectorMotorID, MotorType.kBrushless); //why is there a vortex motor on coral effectors? + + private final RelativeEncoder topEncoder = topMotor.getEncoder(); + private final RelativeEncoder bottomEncoder = bottomMotor.getEncoder(); + private final RelativeEncoder pincherEncoder = pincherMotor.getEncoder(); + + private final SparkClosedLoopController pidControllerTop = topMotor.getClosedLoopController(); + private final SparkClosedLoopController pidControllerBottom = bottomMotor.getClosedLoopController(); + private final SparkClosedLoopController pidControllerPincher = pincherMotor.getClosedLoopController(); + + private final SimpleMotorFeedforward topFeedforward = new SimpleMotorFeedforward(kS[top], kV[top], kA[top]); + private final SimpleMotorFeedforward bottomFeedforward = new SimpleMotorFeedforward(kS[bottom], kV[bottom], kA[bottom]); + private final SimpleMotorFeedforward pincherFeedforward = new SimpleMotorFeedforward(kS[pincher], kV[pincher], kA[pincher]); + //TODO: add feedforward + + DigitalInput limitSwitch = new DigitalInput(1); + + public boolean limitDetects() { + return limitSwitch.get(); + } + //-------------------------------------------------------------------------------------------- + public AlgaeEffector() { + SparkFlexConfig a = new SparkFlexConfig(); + SparkFlexConfig b = new SparkFlexConfig(); + SparkFlexConfig c = new SparkFlexConfig(); + + a.closedLoop.pid( + Constants.kP[top], + Constants.kI[top], + Constants.kD[top] + ).feedbackSensor(FeedbackSensor.kPrimaryEncoder); + topMotor.configure(a, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + + b.closedLoop.pid( + Constants.kP[bottom], + Constants.kI[bottom], + Constants.kD[bottom] + ).feedbackSensor(FeedbackSensor.kPrimaryEncoder); + bottomMotor.configure(b, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + + a.closedLoop.pid( + Constants.AlgaeEffectorc.kP[pincher], + Constants.AlgaeEffectorc.kI[pincher], + Constants.AlgaeEffectorc.kD[pincher] + ).feedbackSensor(FeedbackSensor.kPrimaryEncoder); + pincherMotor.configure(a, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + } + //---------------------------------------------------------------------------------------- + + public void setTopRPM(double toprpm) { + pidControllerTop.setReference(toprpm, ControlType.kVelocity, ClosedLoopSlot.kSlot0); + topFeedforward.calculate(toprpm); + } + + public void setBottomRPM(double bottomrpm) { + pidControllerBottom.setReference(bottomrpm, ControlType.kVelocity, ClosedLoopSlot.kSlot0); + bottomFeedforward.calculate(bottomrpm); + } + + public void setPincherRPM(double pincherrpm) { + pidControllerPincher.setReference(pincherrpm, ControlType.kVelocity, ClosedLoopSlot.kSlot0); + pincherFeedforward.calculate(pincherrpm); + } + + public void runRPM() { + //TODO: Change RPM according to design + setTopRPM(1000); + setBottomRPM(2100); + setPincherRPM(2100); + } + + public void stopMotors() { + setTopRPM(0); + setBottomRPM(0); + setPincherRPM(0); + } + + public void setSpeed(double speed) { + topMotor.set(speed); + bottomMotor.set(speed); + pincherMotor.set(speed); } } From 1d0127a7c88b9aaf33e57732be5628936ecdbaa7 Mon Sep 17 00:00:00 2001 From: Rand0mAsianKid <144396869+Rand0mAsianKid@users.noreply.github.com> Date: Sun, 2 Feb 2025 15:41:53 -0800 Subject: [PATCH 26/51] finished up some last things --- .../subsystems/AlgaeEffector.java | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java index a9d9718..e0239cc 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java +++ b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java @@ -78,23 +78,23 @@ public AlgaeEffector() { SparkFlexConfig c = new SparkFlexConfig(); a.closedLoop.pid( - Constants.kP[top], - Constants.kI[top], - Constants.kD[top] + Constants.kP[Constants.top], + Constants.kI[Constants.top], + Constants.kD[Constants.top] ).feedbackSensor(FeedbackSensor.kPrimaryEncoder); topMotor.configure(a, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); b.closedLoop.pid( - Constants.kP[bottom], - Constants.kI[bottom], - Constants.kD[bottom] + Constants.kP[Constants.bottom], + Constants.kI[Constants.bottom], + Constants.kD[Constants.bottom] ).feedbackSensor(FeedbackSensor.kPrimaryEncoder); bottomMotor.configure(b, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); a.closedLoop.pid( - Constants.AlgaeEffectorc.kP[pincher], - Constants.AlgaeEffectorc.kI[pincher], - Constants.AlgaeEffectorc.kD[pincher] + Constants.kP[Constants.pincher], + Constants.kI[Constants.pincher], + Constants.kD[Constants.pincher] ).feedbackSensor(FeedbackSensor.kPrimaryEncoder); pincherMotor.configure(a, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); } From 73b9766df706dd76368fdc16cfdd7bd4dfc90879 Mon Sep 17 00:00:00 2001 From: Matthew Date: Sun, 2 Feb 2025 20:55:51 -0800 Subject: [PATCH 27/51] finished algae commands without bindings --- .../java/org/carlmontrobotics/Constants.java | 9 +++++ .../commands/IntakeAlgae.java | 35 +++++++++++-------- .../commands/OuttakeAlgae.java | 19 +++++----- .../subsystems/AlgaeEffector.java | 6 ++-- 4 files changed, 42 insertions(+), 27 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index c708cbd..810137b 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -42,6 +42,15 @@ public static final class Manipulator { public static final class AlgaeEffectorc { public static final int upperMotorID = 1; public static final int lowerMotorID = 2; + + public static double intakeTopRPM = 1000; + public static double intakeBottomRPM = 1000; + public static double intakePincherRPM = 1000; + + public static double outtakeTopRPM = 2100; + public static double outtakeBottomRPM = 2100; + public static double outtakePincherRPM = 2100; + } public static final class CoralEffectorc { public static final int effectorMotorID = 3; diff --git a/src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java b/src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java index 64ac272..9b44931 100644 --- a/src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java +++ b/src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java @@ -1,39 +1,46 @@ package org.carlmontrobotics.commands; + +import static org.carlmontrobotics.Constants.CoralEffectorc.*; + +import org.carlmontrobotics.Constants; import org.carlmontrobotics.subsystems.AlgaeEffector; +import org.carlmontrobotics.subsystems.CoralEffector; import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; public class IntakeAlgae extends Command { - AlgaeEffector algaeEffector; - boolean stop; - int direction; - - public IntakeAlgae(AlgaeEffector algaeEffector) { - addRequirements(this.algaeEffector = algaeEffector); - this.direction = direction; - this.stop = stop; + private final AlgaeEffector Algae; + + public IntakeAlgae(AlgaeEffector Algae) { + addRequirements(this.Algae = Algae); } @Override public void initialize() { - + Algae.setTopRPM(Constants.AlgaeEffectorc.intakeTopRPM); + Algae.setBottomRPM(Constants.AlgaeEffectorc.intakeBottomRPM); + Algae.setPincherRPM(Constants.AlgaeEffectorc.intakePincherRPM); } + // Called every time the scheduler runs while the command is scheduled. @Override - public void execute() { - } + public void execute() {} // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - + Algae.stopMotors(); + //TODO: Test different times } - // Returns true when the command should end. @Override public boolean isFinished() { - return false; + //distance sensor doesn't detect coral + //TODO: make distance sensor stuff + //TODO: add smartdashboard + return Algae.limitDetects(); } } \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java b/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java index 27788b2..fcee808 100644 --- a/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java +++ b/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java @@ -1,23 +1,22 @@ package org.carlmontrobotics.commands; +import org.carlmontrobotics.Constants; import org.carlmontrobotics.subsystems.AlgaeEffector; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; public class OuttakeAlgae extends Command { - AlgaeEffector algaeEffector; - boolean stop; - int direction; + AlgaeEffector Algae; - public OuttakeAlgae(AlgaeEffector algaeEffector, int direction, boolean stop) { - addRequirements(this.algaeEffector = algaeEffector); - this.direction = direction; - this.stop = stop; + public OuttakeAlgae(AlgaeEffector algaeEffector) { + addRequirements(this.Algae = algaeEffector); } @Override public void initialize() { - + Algae.setTopRPM(Constants.AlgaeEffectorc.outtakeTopRPM); + Algae.setBottomRPM(Constants.AlgaeEffectorc.outtakeBottomRPM); + Algae.setPincherRPM(Constants.AlgaeEffectorc.outtakePincherRPM); } // Called every time the scheduler runs while the command is scheduled. @@ -28,12 +27,12 @@ public void execute() { // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - + Algae.stopMotors(); } // Returns true when the command should end. @Override public boolean isFinished() { - return false; + return Algae.limitDetects(); } } \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java index e0239cc..273714f 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java +++ b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java @@ -61,9 +61,9 @@ public class AlgaeEffector extends SubsystemBase { private final SparkClosedLoopController pidControllerBottom = bottomMotor.getClosedLoopController(); private final SparkClosedLoopController pidControllerPincher = pincherMotor.getClosedLoopController(); - private final SimpleMotorFeedforward topFeedforward = new SimpleMotorFeedforward(kS[top], kV[top], kA[top]); - private final SimpleMotorFeedforward bottomFeedforward = new SimpleMotorFeedforward(kS[bottom], kV[bottom], kA[bottom]); - private final SimpleMotorFeedforward pincherFeedforward = new SimpleMotorFeedforward(kS[pincher], kV[pincher], kA[pincher]); + private final SimpleMotorFeedforward topFeedforward = new SimpleMotorFeedforward(Constants.kS[Constants.top], Constants.kV[Constants.top], Constants.kA[Constants.top]); + private final SimpleMotorFeedforward bottomFeedforward = new SimpleMotorFeedforward(Constants.kS[Constants.bottom], Constants.kV[Constants.bottom], Constants.kA[Constants.bottom]); + private final SimpleMotorFeedforward pincherFeedforward = new SimpleMotorFeedforward(Constants.kS[Constants.pincher], Constants.kV[Constants.pincher], Constants.kA[Constants.pincher]); //TODO: add feedforward DigitalInput limitSwitch = new DigitalInput(1); From 294ee6fd12950f8d1b5f5baf9d35d8c04fb61878 Mon Sep 17 00:00:00 2001 From: Matthew Date: Sun, 2 Feb 2025 21:08:39 -0800 Subject: [PATCH 28/51] BIndings(for now) --- src/main/java/org/carlmontrobotics/Constants.java | 3 +++ src/main/java/org/carlmontrobotics/RobotContainer.java | 9 ++++++++- 2 files changed, 11 insertions(+), 1 deletion(-) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 810137b..7ace265 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -1,6 +1,7 @@ package org.carlmontrobotics; import edu.wpi.first.wpilibj.XboxController.Axis; +import edu.wpi.first.wpilibj.XboxController.Button; public final class Constants { // public static final class Drivetrain { @@ -37,6 +38,8 @@ public static final class Manipulator { //public static final int X = 0; public static final Axis OuttakeTrigger = Axis.kRightTrigger; public static final Axis IntakeTrigger = Axis.kLeftTrigger; + public static final int OuttakeBumper = Button.kRightBumper.value; + public static final int IntakeBumper = Button.kLeftBumper.value; } } public static final class AlgaeEffectorc { diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index 62a8e0f..26d4977 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -15,12 +15,14 @@ import org.carlmontrobotics.commands.OuttakeAlgae; import org.carlmontrobotics.subsystems.AlgaeEffector; import org.carlmontrobotics.subsystems.CoralEffector; +import org.carlmontrobotics.commands.IntakeAlgae; import org.carlmontrobotics.commands.IntakeCoral; import org.carlmontrobotics.commands.OuttakeCoral; //controllers import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.PS5Controller.Button; import edu.wpi.first.wpilibj.XboxController.Axis; //commands @@ -74,8 +76,13 @@ private void setBindingsManipulator() { axisTrigger(manipulatorController, OI.Manipulator.IntakeTrigger) .whileTrue(new OuttakeCoral(coralEffector)); + + new JoystickButton(manipulatorController, OI.Manipulator.IntakeBumper) + .whileTrue(new IntakeAlgae(algaeEffector)); + + new JoystickButton(manipulatorController, OI.Manipulator.OuttakeBumper) + .whileTrue(new OuttakeAlgae(algaeEffector)); } - private Trigger axisTrigger(GenericHID controller, Axis axis) { From 47a5fed6d1eb002e74c3c930387d2ac0f4d8e693 Mon Sep 17 00:00:00 2001 From: Niosocket11 <107586975+Niosocket11@users.noreply.github.com> Date: Fri, 7 Feb 2025 19:03:44 -0800 Subject: [PATCH 29/51] badcode -> goodcode --- .../subsystems/AlgaeEffector.java | 18 +++++++++--------- .../subsystems/CoralEffector.java | 6 +++--- 2 files changed, 12 insertions(+), 12 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java index 273714f..769733f 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java +++ b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java @@ -73,30 +73,30 @@ public boolean limitDetects() { } //-------------------------------------------------------------------------------------------- public AlgaeEffector() { - SparkFlexConfig a = new SparkFlexConfig(); - SparkFlexConfig b = new SparkFlexConfig(); - SparkFlexConfig c = new SparkFlexConfig(); + SparkFlexConfig topConfig = new SparkFlexConfig(); + SparkFlexConfig bottomConfig = new SparkFlexConfig(); + SparkFlexConfig pincherConfig = new SparkFlexConfig(); - a.closedLoop.pid( + topConfig.closedLoop.pid( Constants.kP[Constants.top], Constants.kI[Constants.top], Constants.kD[Constants.top] ).feedbackSensor(FeedbackSensor.kPrimaryEncoder); - topMotor.configure(a, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + topMotor.configure(topConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); - b.closedLoop.pid( + bottomConfig.closedLoop.pid( Constants.kP[Constants.bottom], Constants.kI[Constants.bottom], Constants.kD[Constants.bottom] ).feedbackSensor(FeedbackSensor.kPrimaryEncoder); - bottomMotor.configure(b, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + bottomMotor.configure(bottomConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); - a.closedLoop.pid( + pincherConfig.closedLoop.pid( Constants.kP[Constants.pincher], Constants.kI[Constants.pincher], Constants.kD[Constants.pincher] ).feedbackSensor(FeedbackSensor.kPrimaryEncoder); - pincherMotor.configure(a, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + pincherMotor.configure(pincherConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); } //---------------------------------------------------------------------------------------- diff --git a/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java b/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java index 75d71bf..ba500c9 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java +++ b/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java @@ -52,13 +52,13 @@ public boolean limitDetects() { } public CoralEffector() { - SparkFlexConfig c = new SparkFlexConfig(); - c.closedLoop.pid( + SparkFlexConfig effectorConfig = new SparkFlexConfig(); + effectorConfig.closedLoop.pid( Constants.CoralEffectorc.kP, Constants.CoralEffectorc.kI, Constants.CoralEffectorc.kD ).feedbackSensor(FeedbackSensor.kPrimaryEncoder); - effectorMotor.configure(c, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + effectorMotor.configure(effectorConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); //Will add distance sensor later } From dde5ab25132b5ed342988e303b69d7adb572bf5e Mon Sep 17 00:00:00 2001 From: Kenneth-Choothakan <134165881+Kenneth-Choothakan@users.noreply.github.com> Date: Fri, 7 Feb 2025 19:46:41 -0800 Subject: [PATCH 30/51] Aiken fixed a bunch of prs --- .vscode/settings.json | 3 + .wpilib/wpilib_preferences.json | 6 ++ networktables.json | 1 + simgui-ds.json | 98 +++++++++++++++++++ simgui.json | 19 ++++ .../java/org/carlmontrobotics/Constants.java | 16 +-- .../org/carlmontrobotics/RobotContainer.java | 4 +- .../commands/IntakeCoral.java | 9 +- .../commands/OuttakeAlgae.java | 5 +- .../commands/OuttakeCoral.java | 5 +- .../subsystems/AlgaeEffector.java | 50 +++++----- 11 files changed, 175 insertions(+), 41 deletions(-) create mode 100644 .vscode/settings.json create mode 100644 .wpilib/wpilib_preferences.json create mode 100644 networktables.json create mode 100644 simgui-ds.json create mode 100644 simgui.json diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..c5f3f6b --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,3 @@ +{ + "java.configuration.updateBuildConfiguration": "interactive" +} \ No newline at end of file diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json new file mode 100644 index 0000000..f0ee648 --- /dev/null +++ b/.wpilib/wpilib_preferences.json @@ -0,0 +1,6 @@ +{ + "currentLanguage": "none", + "enableCppIntellisense": false, + "projectYear": "none", + "teamNumber": 199 +} \ No newline at end of file diff --git a/networktables.json b/networktables.json new file mode 100644 index 0000000..fe51488 --- /dev/null +++ b/networktables.json @@ -0,0 +1 @@ +[] diff --git a/simgui-ds.json b/simgui-ds.json new file mode 100644 index 0000000..c4b7efd --- /dev/null +++ b/simgui-ds.json @@ -0,0 +1,98 @@ +{ + "keyboardJoysticks": [ + { + "axisConfig": [ + { + "decKey": 65, + "incKey": 68 + }, + { + "decKey": 87, + "incKey": 83 + }, + { + "decKey": 69, + "decayRate": 0.0, + "incKey": 82, + "keyRate": 0.009999999776482582 + } + ], + "axisCount": 3, + "buttonCount": 4, + "buttonKeys": [ + 90, + 88, + 67, + 86 + ], + "povConfig": [ + { + "key0": 328, + "key135": 323, + "key180": 322, + "key225": 321, + "key270": 324, + "key315": 327, + "key45": 329, + "key90": 326 + } + ], + "povCount": 1 + }, + { + "axisConfig": [ + { + "decKey": 74, + "incKey": 76 + }, + { + "decKey": 73, + "incKey": 75 + } + ], + "axisCount": 2, + "buttonCount": 4, + "buttonKeys": [ + 77, + 44, + 46, + 47 + ], + "povCount": 0 + }, + { + "axisConfig": [ + { + "decKey": 263, + "incKey": 262 + }, + { + "decKey": 265, + "incKey": 264 + } + ], + "axisCount": 2, + "buttonCount": 6, + "buttonKeys": [ + 260, + 268, + 266, + 261, + 269, + 267 + ], + "povCount": 0 + }, + { + "axisCount": 0, + "buttonCount": 0, + "povCount": 0 + } + ], + "robotJoysticks": [ + { + "guid": "78696e70757401000000000000000000", + "useGamepad": true + } + ] +} diff --git a/simgui.json b/simgui.json new file mode 100644 index 0000000..4630a5c --- /dev/null +++ b/simgui.json @@ -0,0 +1,19 @@ +{ + "HALProvider": { + "Other Devices": { + "SPARK Flex [1] RELATIVE ENCODER": { + "header": { + "open": true + } + } + } + }, + "NTProvider": { + "types": { + "/FMSInfo": "FMSInfo" + } + }, + "NetworkTables Info": { + "visible": true + } +} diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 7ace265..b05fb75 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -8,9 +8,7 @@ public final class Constants { // public static final double MAX_SPEED_MPS = 2; // } - public static final int top = 1; - public static final int bottom = 1; - public static final int pincher = 1; + public static final double[] kP = {/*/Top/*/0.0, /*/Bottom/*/0.0, /*/Pincher/*/0.0}; public static final double[] kI = {/*/Top/*/0.0, /*/Bottom/*/0.0, /*/Pincher/*/0.0}; @@ -34,7 +32,7 @@ public static final class Driver { public static final int rightBumper = 6; } public static final class Manipulator { - public static final int manipulatorPort = 5; + public static final int manipulatorPort = 2; //public static final int X = 0; public static final Axis OuttakeTrigger = Axis.kRightTrigger; public static final Axis IntakeTrigger = Axis.kLeftTrigger; @@ -45,6 +43,10 @@ public static final class Manipulator { public static final class AlgaeEffectorc { public static final int upperMotorID = 1; public static final int lowerMotorID = 2; + public static final int pinchMotorID = 3; + public static final int TopkS = 1; + public static final int BottomkS = 1; + public static final int PincherkS = 1; public static double intakeTopRPM = 1000; public static double intakeBottomRPM = 1000; @@ -56,12 +58,12 @@ public static final class AlgaeEffectorc { } public static final class CoralEffectorc { - public static final int effectorMotorID = 3; - public static final int effectorDistanceSensorID = 4; + public static final int effectorMotorID = 4; + public static final int effectorDistanceSensorID = 5; public static final double kP = 0.0; public static final double kI = 0.0; public static final double kD = 0.0; - public static double intakeRPM = 1000; + public static double intakeRPM = -1000; public static double outtakeRPM = 2100; //TODO: Change after testing public static final int DETECT_DISTANCE_INCHES = 3; diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index 26d4977..8583810 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -71,10 +71,10 @@ private void setBindingsDriver() { private void setBindingsManipulator() { - axisTrigger(manipulatorController, OI.Manipulator.OuttakeTrigger) + axisTrigger(manipulatorController, OI.Manipulator.IntakeTrigger) .whileTrue(new IntakeCoral(coralEffector)); - axisTrigger(manipulatorController, OI.Manipulator.IntakeTrigger) + axisTrigger(manipulatorController, OI.Manipulator.OuttakeTrigger) .whileTrue(new OuttakeCoral(coralEffector)); new JoystickButton(manipulatorController, OI.Manipulator.IntakeBumper) diff --git a/src/main/java/org/carlmontrobotics/commands/IntakeCoral.java b/src/main/java/org/carlmontrobotics/commands/IntakeCoral.java index f0a8ba8..980387f 100644 --- a/src/main/java/org/carlmontrobotics/commands/IntakeCoral.java +++ b/src/main/java/org/carlmontrobotics/commands/IntakeCoral.java @@ -4,21 +4,22 @@ import org.carlmontrobotics.Constants; import org.carlmontrobotics.subsystems.CoralEffector; - import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; public class IntakeCoral extends Command { private final CoralEffector Coral; - + private final Timer timer = new Timer(); public IntakeCoral(CoralEffector Coral) { addRequirements(this.Coral = Coral); } @Override public void initialize() { - Coral.setRPM(Constants.CoralEffectorc.intakeRPM); + timer.reset(); + timer.start(); + Coral.setRPM(Constants.CoralEffectorc.intakeRPM); } @@ -38,6 +39,6 @@ public boolean isFinished() { //distance sensor doesn't detect coral //TODO: make distance sensor stuff //TODO: add smartdashboard - return !Coral.coralDetects() && !Coral.limitDetects(); + return (Coral.coralDetects() && Coral.limitDetects()) || timer.get() > 4; } } \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java b/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java index fcee808..f510d56 100644 --- a/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java +++ b/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java @@ -7,7 +7,7 @@ public class OuttakeAlgae extends Command { AlgaeEffector Algae; - + private final Timer timer = new Timer(); public OuttakeAlgae(AlgaeEffector algaeEffector) { addRequirements(this.Algae = algaeEffector); } @@ -17,6 +17,7 @@ public void initialize() { Algae.setTopRPM(Constants.AlgaeEffectorc.outtakeTopRPM); Algae.setBottomRPM(Constants.AlgaeEffectorc.outtakeBottomRPM); Algae.setPincherRPM(Constants.AlgaeEffectorc.outtakePincherRPM); + timer.start(); } // Called every time the scheduler runs while the command is scheduled. @@ -33,6 +34,6 @@ public void end(boolean interrupted) { // Returns true when the command should end. @Override public boolean isFinished() { - return Algae.limitDetects(); + return Algae.limitDetects() || timer.get() > 1; } } \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/commands/OuttakeCoral.java b/src/main/java/org/carlmontrobotics/commands/OuttakeCoral.java index c23ce61..40436ed 100644 --- a/src/main/java/org/carlmontrobotics/commands/OuttakeCoral.java +++ b/src/main/java/org/carlmontrobotics/commands/OuttakeCoral.java @@ -12,12 +12,15 @@ public class OuttakeCoral extends Command { private final CoralEffector Coral; + private final Timer timer = new Timer(); public OuttakeCoral(CoralEffector Coral) { addRequirements(this.Coral = Coral); } @Override public void initialize() { + timer.reset(); + timer.start(); Coral.setRPM(Constants.CoralEffectorc.outtakeRPM); } @@ -37,6 +40,6 @@ public boolean isFinished() { //distance sensor doesn't detect coral //TODO: make distance sensor stuff //TODO: add smartdashboard - return !Coral.coralDetects() && !Coral.limitDetects(); + return (!Coral.coralDetects() && !Coral.limitDetects()) || timer.get() > 4; } } \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java index 273714f..e0d97b3 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java +++ b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java @@ -49,9 +49,9 @@ public class AlgaeEffector extends SubsystemBase { - private SparkFlex topMotor = new SparkFlex(Constants.CoralEffectorc.effectorMotorID, MotorType.kBrushless); //why is there a vortex motor on coral effectors? - private SparkMax bottomMotor = new SparkMax(Constants.CoralEffectorc.effectorMotorID, MotorType.kBrushless); //why is there a vortex motor on coral effectors? - private SparkFlex pincherMotor = new SparkFlex(Constants.CoralEffectorc.effectorMotorID, MotorType.kBrushless); //why is there a vortex motor on coral effectors? + private SparkFlex topMotor = new SparkFlex(Constants.AlgaeEffectorc.upperMotorID, MotorType.kBrushless); //why is there a vortex motor on coral effectors? + private SparkMax bottomMotor = new SparkMax(Constants.AlgaeEffectorc.lowerMotorID, MotorType.kBrushless); //why is there a vortex motor on coral effectors? + private SparkFlex pincherMotor = new SparkFlex(Constants.AlgaeEffectorc.pinchMotorID, MotorType.kBrushless); //why is there a vortex motor on coral effectors? private final RelativeEncoder topEncoder = topMotor.getEncoder(); private final RelativeEncoder bottomEncoder = bottomMotor.getEncoder(); @@ -61,9 +61,9 @@ public class AlgaeEffector extends SubsystemBase { private final SparkClosedLoopController pidControllerBottom = bottomMotor.getClosedLoopController(); private final SparkClosedLoopController pidControllerPincher = pincherMotor.getClosedLoopController(); - private final SimpleMotorFeedforward topFeedforward = new SimpleMotorFeedforward(Constants.kS[Constants.top], Constants.kV[Constants.top], Constants.kA[Constants.top]); - private final SimpleMotorFeedforward bottomFeedforward = new SimpleMotorFeedforward(Constants.kS[Constants.bottom], Constants.kV[Constants.bottom], Constants.kA[Constants.bottom]); - private final SimpleMotorFeedforward pincherFeedforward = new SimpleMotorFeedforward(Constants.kS[Constants.pincher], Constants.kV[Constants.pincher], Constants.kA[Constants.pincher]); + private final SimpleMotorFeedforward topFeedforward = new SimpleMotorFeedforward(Constants.kS[Constants.AlgaeEffectorc.TopkS], Constants.kV[Constants.AlgaeEffectorc.TopkS], Constants.kA[Constants.AlgaeEffectorc.TopkS]); + private final SimpleMotorFeedforward bottomFeedforward = new SimpleMotorFeedforward(Constants.kS[Constants.AlgaeEffectorc.BottomkS], Constants.kV[Constants.AlgaeEffectorc.BottomkS], Constants.kA[Constants.AlgaeEffectorc.BottomkS]); + private final SimpleMotorFeedforward pincherFeedforward = new SimpleMotorFeedforward(Constants.kS[Constants.AlgaeEffectorc.PincherkS], Constants.kV[Constants.AlgaeEffectorc.PincherkS], Constants.kA[Constants.AlgaeEffectorc.PincherkS]); //TODO: add feedforward DigitalInput limitSwitch = new DigitalInput(1); @@ -73,30 +73,30 @@ public boolean limitDetects() { } //-------------------------------------------------------------------------------------------- public AlgaeEffector() { - SparkFlexConfig a = new SparkFlexConfig(); - SparkFlexConfig b = new SparkFlexConfig(); - SparkFlexConfig c = new SparkFlexConfig(); - - a.closedLoop.pid( - Constants.kP[Constants.top], - Constants.kI[Constants.top], - Constants.kD[Constants.top] + SparkFlexConfig pincherMotorConfig = new SparkFlexConfig(); + SparkFlexConfig bottomMotorConfig = new SparkFlexConfig(); + + + pincherMotorConfig.closedLoop.pid( + Constants.kP[Constants.AlgaeEffectorc.TopkS], + Constants.kI[Constants.AlgaeEffectorc.TopkS], + Constants.kD[Constants.AlgaeEffectorc.TopkS] ).feedbackSensor(FeedbackSensor.kPrimaryEncoder); - topMotor.configure(a, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + topMotor.configure(pincherMotorConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); - b.closedLoop.pid( - Constants.kP[Constants.bottom], - Constants.kI[Constants.bottom], - Constants.kD[Constants.bottom] + bottomMotorConfig.closedLoop.pid( + Constants.kP[Constants.AlgaeEffectorc.BottomkS], + Constants.kI[Constants.AlgaeEffectorc.BottomkS], + Constants.kD[Constants.AlgaeEffectorc.BottomkS] ).feedbackSensor(FeedbackSensor.kPrimaryEncoder); - bottomMotor.configure(b, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + bottomMotor.configure(bottomMotorConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); - a.closedLoop.pid( - Constants.kP[Constants.pincher], - Constants.kI[Constants.pincher], - Constants.kD[Constants.pincher] + pincherMotorConfig.closedLoop.pid( + Constants.kP[Constants.AlgaeEffectorc.PincherkS], + Constants.kI[Constants.AlgaeEffectorc.PincherkS], + Constants.kD[Constants.AlgaeEffectorc.PincherkS] ).feedbackSensor(FeedbackSensor.kPrimaryEncoder); - pincherMotor.configure(a, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + pincherMotor.configure(pincherMotorConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); } //---------------------------------------------------------------------------------------- From e2ae207ef5c0eeef2a0077c887cc0867f5d8dfd3 Mon Sep 17 00:00:00 2001 From: Kenneth-Choothakan <134165881+Kenneth-Choothakan@users.noreply.github.com> Date: Fri, 7 Feb 2025 20:16:30 -0800 Subject: [PATCH 31/51] =?UTF-8?q?Fixed=20some=20sim=20stuff=20found=20sum?= =?UTF-8?q?=20out=20Matthew=20is=20a=20sour=20trout=F0=9F=90=9F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/main/java/org/carlmontrobotics/RobotContainer.java | 2 +- .../java/org/carlmontrobotics/commands/IntakeAlgae.java | 6 ++++-- .../java/org/carlmontrobotics/commands/OuttakeAlgae.java | 2 +- 3 files changed, 6 insertions(+), 4 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index 8583810..e39e162 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -81,7 +81,7 @@ private void setBindingsManipulator() { .whileTrue(new IntakeAlgae(algaeEffector)); new JoystickButton(manipulatorController, OI.Manipulator.OuttakeBumper) - .whileTrue(new OuttakeAlgae(algaeEffector)); + .whileFalse(new OuttakeAlgae(algaeEffector)); } diff --git a/src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java b/src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java index 9b44931..ee38226 100644 --- a/src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java +++ b/src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java @@ -12,13 +12,15 @@ public class IntakeAlgae extends Command { private final AlgaeEffector Algae; - + private final Timer timer = new Timer(); public IntakeAlgae(AlgaeEffector Algae) { addRequirements(this.Algae = Algae); } @Override public void initialize() { + timer.reset(); + timer.start(); Algae.setTopRPM(Constants.AlgaeEffectorc.intakeTopRPM); Algae.setBottomRPM(Constants.AlgaeEffectorc.intakeBottomRPM); Algae.setPincherRPM(Constants.AlgaeEffectorc.intakePincherRPM); @@ -41,6 +43,6 @@ public boolean isFinished() { //distance sensor doesn't detect coral //TODO: make distance sensor stuff //TODO: add smartdashboard - return Algae.limitDetects(); + return Algae.limitDetects() || timer.get() > 1; //Simulator doesnt work propperly because limiswtich is non existant (only for simulator) } } \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java b/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java index f510d56..10d885e 100644 --- a/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java +++ b/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java @@ -34,6 +34,6 @@ public void end(boolean interrupted) { // Returns true when the command should end. @Override public boolean isFinished() { - return Algae.limitDetects() || timer.get() > 1; + return Algae.limitDetects() || timer.get() > 1; //Simulator doesnt work propperly because limiswtich is non existant (only for simulator) } } \ No newline at end of file From ca3859c6899bcc9bab5d252e9df1ecf481eb0609 Mon Sep 17 00:00:00 2001 From: Niosocket11 <107586975+Niosocket11@users.noreply.github.com> Date: Sat, 8 Feb 2025 11:14:25 -0800 Subject: [PATCH 32/51] Corrected configs and added methods to check if shooting motors are up to speed --- .../subsystems/AlgaeEffector.java | 16 +++++++++++++--- 1 file changed, 13 insertions(+), 3 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java index e0d97b3..75d4970 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java +++ b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java @@ -20,6 +20,7 @@ import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; import com.revrobotics.spark.config.SparkFlexConfig; +import com.revrobotics.spark.config.SparkMaxConfig; import com.revrobotics.spark.SparkMax; import edu.wpi.first.hal.SimDouble; @@ -74,15 +75,16 @@ public boolean limitDetects() { //-------------------------------------------------------------------------------------------- public AlgaeEffector() { SparkFlexConfig pincherMotorConfig = new SparkFlexConfig(); - SparkFlexConfig bottomMotorConfig = new SparkFlexConfig(); + SparkMaxConfig bottomMotorConfig = new SparkMaxConfig(); + SparkFlexConfig topMotorConfig = new SparkFlexConfig(); - pincherMotorConfig.closedLoop.pid( + topMotorConfig.closedLoop.pid( Constants.kP[Constants.AlgaeEffectorc.TopkS], Constants.kI[Constants.AlgaeEffectorc.TopkS], Constants.kD[Constants.AlgaeEffectorc.TopkS] ).feedbackSensor(FeedbackSensor.kPrimaryEncoder); - topMotor.configure(pincherMotorConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + topMotor.configure(topMotorConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); bottomMotorConfig.closedLoop.pid( Constants.kP[Constants.AlgaeEffectorc.BottomkS], @@ -128,6 +130,14 @@ public void stopMotors() { setPincherRPM(0); } + public boolean checkIfAtTopRPM(double rpm) { + return topEncoder.getVelocity() == rpm; + } + + public boolean checkIfAtBottomRPM() { + return bottomEncoder.getVelocity() == rpm; + } + public void setSpeed(double speed) { topMotor.set(speed); bottomMotor.set(speed); From ca9dabc12bf2255eafb048734e47535c3e867d4b Mon Sep 17 00:00:00 2001 From: Niosocket11 <107586975+Niosocket11@users.noreply.github.com> Date: Sat, 8 Feb 2025 11:19:11 -0800 Subject: [PATCH 33/51] forgor to add --- .../java/org/carlmontrobotics/subsystems/AlgaeEffector.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java index 75d4970..89ae98a 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java +++ b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java @@ -134,7 +134,7 @@ public boolean checkIfAtTopRPM(double rpm) { return topEncoder.getVelocity() == rpm; } - public boolean checkIfAtBottomRPM() { + public boolean checkIfAtBottomRPM(double rpm) { return bottomEncoder.getVelocity() == rpm; } From 908d91e85723e003ea39a80daddaa13e3644623f Mon Sep 17 00:00:00 2001 From: Sofie Budman Date: Sun, 9 Feb 2025 11:40:50 -0800 Subject: [PATCH 34/51] project year --- .wpilib/wpilib_preferences.json | 2 +- src/main/java/org/carlmontrobotics/Constants.java | 5 +++-- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json index f0ee648..96c51f6 100644 --- a/.wpilib/wpilib_preferences.json +++ b/.wpilib/wpilib_preferences.json @@ -1,6 +1,6 @@ { "currentLanguage": "none", "enableCppIntellisense": false, - "projectYear": "none", + "projectYear": "2025", "teamNumber": 199 } \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index b05fb75..854a5fc 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -23,6 +23,8 @@ public static final class OI { public static final class Driver { public static final int driverPort = 0; + public static final int effectorMotorID = 4; + public static final int effectorDistanceSensorID = 5; /*public static final int A = 1; public static final int B = 2; public static final int X = 3; @@ -58,8 +60,7 @@ public static final class AlgaeEffectorc { } public static final class CoralEffectorc { - public static final int effectorMotorID = 4; - public static final int effectorDistanceSensorID = 5; + public static final double kP = 0.0; public static final double kI = 0.0; public static final double kD = 0.0; From 97afeb5e88361dc7d97d200fc7cde99c4938768f Mon Sep 17 00:00:00 2001 From: Niosocket11 <107586975+Niosocket11@users.noreply.github.com> Date: Tue, 11 Feb 2025 20:15:28 -0800 Subject: [PATCH 35/51] Add constants --- src/main/java/org/carlmontrobotics/Constants.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 854a5fc..cd69add 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -68,5 +68,7 @@ public static final class CoralEffectorc { public static double outtakeRPM = 2100; //TODO: Change after testing public static final int DETECT_DISTANCE_INCHES = 3; + public static final int effectorMotorID = 0; + public static final int effectorDistanceSensorID = 0; } } From 589598cb3b3866f8e3a68de22b8f12997b1157f7 Mon Sep 17 00:00:00 2001 From: Rand0mAsianKid Date: Sat, 15 Feb 2025 18:10:24 -0800 Subject: [PATCH 36/51] New Paths and Autos(add commmands when ready, and settup when ready --- .../autos/Center 1 Piece L1 Auto.auto | 25 +++++++++ .../autos/Center 1 Piece L2 Auto.auto | 25 +++++++++ .../autos/Center 1 Piece L3 Auto.auto | 25 +++++++++ .../autos/Center 1 Piece L4 Auto.auto | 25 +++++++++ .../pathplanner/autos/Center Forward.auto | 19 +++++++ .../autos/Left 1 Piece L1 Auto.auto | 25 +++++++++ .../autos/Left 1 Piece L2 Auto.auto | 25 +++++++++ .../autos/Left 1 Piece L3 Auto.auto | 25 +++++++++ .../autos/Left 1 Piece L4 Auto.auto | 25 +++++++++ .../autos/Left 2 Piece L1 Auto.auto | 37 +++++++++++++ .../autos/Left 2 Piece L2 Auto.auto | 37 +++++++++++++ .../autos/Left 2 Piece L3 Auto.auto | 37 +++++++++++++ .../autos/Left 2 Piece L4 Auto.auto | 37 +++++++++++++ .../autos/Left 3 Piece L1 Auto.auto | 49 +++++++++++++++++ .../autos/Left 3 Piece L2 Auto.auto | 49 +++++++++++++++++ .../autos/Left 3 Piece L3 Auto.auto | 49 +++++++++++++++++ .../autos/Left 3 Piece L4 Auto.auto | 49 +++++++++++++++++ .../autos/Left 4 Piece L1 Auto.auto | 55 +++++++++++++++++++ .../autos/Left 4 Piece L2 Auto.auto | 55 +++++++++++++++++++ .../autos/Left 4 Piece L3 Auto.auto | 55 +++++++++++++++++++ .../autos/Left 4 Piece L4 Auto.auto | 55 +++++++++++++++++++ .../pathplanner/autos/Left Forward.auto | 19 +++++++ .../autos/Right 1 Piece L1 Auto.auto | 19 +++++++ .../autos/Right 1 Piece L2 Auto.auto | 19 +++++++ .../autos/Right 1 Piece L3 Auto.auto | 19 +++++++ .../autos/Right 1 Piece L4 Auto.auto | 19 +++++++ .../autos/Right 2 Piece L1 Auto.auto | 37 +++++++++++++ .../autos/Right 2 Piece L2 Auto.auto | 37 +++++++++++++ .../autos/Right 2 Piece L3 Auto.auto | 37 +++++++++++++ .../autos/Right 2 Piece L4 Auto.auto | 37 +++++++++++++ .../autos/Right 3 Piece L1 Auto.auto | 49 +++++++++++++++++ .../autos/Right 3 Piece L2 Auto.auto | 49 +++++++++++++++++ .../autos/Right 3 Piece L3 Auto.auto | 49 +++++++++++++++++ .../autos/Right 3 Piece L4 Auto.auto | 49 +++++++++++++++++ .../autos/Right 4 Piece L1 Auto.auto | 55 +++++++++++++++++++ .../autos/Right 4 Piece L2 Auto.auto | 55 +++++++++++++++++++ .../autos/Right 4 Piece L3 Auto.auto | 55 +++++++++++++++++++ .../autos/Right 4 Piece L4 Auto.auto | 55 +++++++++++++++++++ .../pathplanner/autos/Right Forward.auto | 19 +++++++ .../pathplanner/paths/Center Forward.path | 54 ++++++++++++++++++ .../pathplanner/paths/Center Piece(1).path | 54 ++++++++++++++++++ .../pathplanner/paths/Center Piece(2).path | 54 ++++++++++++++++++ .../pathplanner/paths/CoralConfigs.path | 2 +- .../pathplanner/paths/Left Forward.path | 54 ++++++++++++++++++ .../pathplanner/paths/Left Piece(1).path | 54 ++++++++++++++++++ .../pathplanner/paths/Left Piece(2).path | 54 ++++++++++++++++++ .../pathplanner/paths/Left Piece(3).path | 54 ++++++++++++++++++ .../pathplanner/paths/Left Piece(4).path | 54 ++++++++++++++++++ .../paths/Left Piece(Go-Back).path | 54 ++++++++++++++++++ .../pathplanner/paths/Right Forward.path | 54 ++++++++++++++++++ .../pathplanner/paths/Right Piece(1).path | 54 ++++++++++++++++++ .../pathplanner/paths/Right Piece(2).path | 54 ++++++++++++++++++ .../pathplanner/paths/Right Piece(3).path | 54 ++++++++++++++++++ .../pathplanner/paths/Right Piece(4).path | 54 ++++++++++++++++++ .../pathplanner/paths/Right Piece(5).path | 54 ++++++++++++++++++ .../pathplanner/paths/Right Piece(6).path | 54 ++++++++++++++++++ .../pathplanner/paths/Right Piece(7).path | 54 ++++++++++++++++++ .../pathplanner/paths/StartingConfigs.path | 2 +- src/main/deploy/pathplanner/settings.json | 29 +++++++++- 59 files changed, 2408 insertions(+), 4 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/Center 1 Piece L1 Auto.auto create mode 100644 src/main/deploy/pathplanner/autos/Center 1 Piece L2 Auto.auto create mode 100644 src/main/deploy/pathplanner/autos/Center 1 Piece L3 Auto.auto create mode 100644 src/main/deploy/pathplanner/autos/Center 1 Piece L4 Auto.auto create mode 100644 src/main/deploy/pathplanner/autos/Center Forward.auto create mode 100644 src/main/deploy/pathplanner/autos/Left 1 Piece L1 Auto.auto create mode 100644 src/main/deploy/pathplanner/autos/Left 1 Piece L2 Auto.auto create mode 100644 src/main/deploy/pathplanner/autos/Left 1 Piece L3 Auto.auto create mode 100644 src/main/deploy/pathplanner/autos/Left 1 Piece L4 Auto.auto create mode 100644 src/main/deploy/pathplanner/autos/Left 2 Piece L1 Auto.auto create mode 100644 src/main/deploy/pathplanner/autos/Left 2 Piece L2 Auto.auto create mode 100644 src/main/deploy/pathplanner/autos/Left 2 Piece L3 Auto.auto create mode 100644 src/main/deploy/pathplanner/autos/Left 2 Piece L4 Auto.auto create mode 100644 src/main/deploy/pathplanner/autos/Left 3 Piece L1 Auto.auto create mode 100644 src/main/deploy/pathplanner/autos/Left 3 Piece L2 Auto.auto create mode 100644 src/main/deploy/pathplanner/autos/Left 3 Piece L3 Auto.auto create mode 100644 src/main/deploy/pathplanner/autos/Left 3 Piece L4 Auto.auto create mode 100644 src/main/deploy/pathplanner/autos/Left 4 Piece L1 Auto.auto create mode 100644 src/main/deploy/pathplanner/autos/Left 4 Piece L2 Auto.auto create mode 100644 src/main/deploy/pathplanner/autos/Left 4 Piece L3 Auto.auto create mode 100644 src/main/deploy/pathplanner/autos/Left 4 Piece L4 Auto.auto create mode 100644 src/main/deploy/pathplanner/autos/Left Forward.auto create mode 100644 src/main/deploy/pathplanner/autos/Right 1 Piece L1 Auto.auto create mode 100644 src/main/deploy/pathplanner/autos/Right 1 Piece L2 Auto.auto create mode 100644 src/main/deploy/pathplanner/autos/Right 1 Piece L3 Auto.auto create mode 100644 src/main/deploy/pathplanner/autos/Right 1 Piece L4 Auto.auto create mode 100644 src/main/deploy/pathplanner/autos/Right 2 Piece L1 Auto.auto create mode 100644 src/main/deploy/pathplanner/autos/Right 2 Piece L2 Auto.auto create mode 100644 src/main/deploy/pathplanner/autos/Right 2 Piece L3 Auto.auto create mode 100644 src/main/deploy/pathplanner/autos/Right 2 Piece L4 Auto.auto create mode 100644 src/main/deploy/pathplanner/autos/Right 3 Piece L1 Auto.auto create mode 100644 src/main/deploy/pathplanner/autos/Right 3 Piece L2 Auto.auto create mode 100644 src/main/deploy/pathplanner/autos/Right 3 Piece L3 Auto.auto create mode 100644 src/main/deploy/pathplanner/autos/Right 3 Piece L4 Auto.auto create mode 100644 src/main/deploy/pathplanner/autos/Right 4 Piece L1 Auto.auto create mode 100644 src/main/deploy/pathplanner/autos/Right 4 Piece L2 Auto.auto create mode 100644 src/main/deploy/pathplanner/autos/Right 4 Piece L3 Auto.auto create mode 100644 src/main/deploy/pathplanner/autos/Right 4 Piece L4 Auto.auto create mode 100644 src/main/deploy/pathplanner/autos/Right Forward.auto create mode 100644 src/main/deploy/pathplanner/paths/Center Forward.path create mode 100644 src/main/deploy/pathplanner/paths/Center Piece(1).path create mode 100644 src/main/deploy/pathplanner/paths/Center Piece(2).path create mode 100644 src/main/deploy/pathplanner/paths/Left Forward.path create mode 100644 src/main/deploy/pathplanner/paths/Left Piece(1).path create mode 100644 src/main/deploy/pathplanner/paths/Left Piece(2).path create mode 100644 src/main/deploy/pathplanner/paths/Left Piece(3).path create mode 100644 src/main/deploy/pathplanner/paths/Left Piece(4).path create mode 100644 src/main/deploy/pathplanner/paths/Left Piece(Go-Back).path create mode 100644 src/main/deploy/pathplanner/paths/Right Forward.path create mode 100644 src/main/deploy/pathplanner/paths/Right Piece(1).path create mode 100644 src/main/deploy/pathplanner/paths/Right Piece(2).path create mode 100644 src/main/deploy/pathplanner/paths/Right Piece(3).path create mode 100644 src/main/deploy/pathplanner/paths/Right Piece(4).path create mode 100644 src/main/deploy/pathplanner/paths/Right Piece(5).path create mode 100644 src/main/deploy/pathplanner/paths/Right Piece(6).path create mode 100644 src/main/deploy/pathplanner/paths/Right Piece(7).path diff --git a/src/main/deploy/pathplanner/autos/Center 1 Piece L1 Auto.auto b/src/main/deploy/pathplanner/autos/Center 1 Piece L1 Auto.auto new file mode 100644 index 0000000..be9f8c0 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Center 1 Piece L1 Auto.auto @@ -0,0 +1,25 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Center Piece(1)" + } + }, + { + "type": "path", + "data": { + "pathName": "Center Piece(2)" + } + } + ] + } + }, + "resetOdom": true, + "folder": "1 Piece L1", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Center 1 Piece L2 Auto.auto b/src/main/deploy/pathplanner/autos/Center 1 Piece L2 Auto.auto new file mode 100644 index 0000000..e1b55e9 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Center 1 Piece L2 Auto.auto @@ -0,0 +1,25 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Center Piece(1)" + } + }, + { + "type": "path", + "data": { + "pathName": "Center Piece(2)" + } + } + ] + } + }, + "resetOdom": true, + "folder": "1 piece L2", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Center 1 Piece L3 Auto.auto b/src/main/deploy/pathplanner/autos/Center 1 Piece L3 Auto.auto new file mode 100644 index 0000000..f03d42f --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Center 1 Piece L3 Auto.auto @@ -0,0 +1,25 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Center Piece(1)" + } + }, + { + "type": "path", + "data": { + "pathName": "Center Piece(2)" + } + } + ] + } + }, + "resetOdom": true, + "folder": "1 piece L3", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Center 1 Piece L4 Auto.auto b/src/main/deploy/pathplanner/autos/Center 1 Piece L4 Auto.auto new file mode 100644 index 0000000..0a0293c --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Center 1 Piece L4 Auto.auto @@ -0,0 +1,25 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Center Piece(1)" + } + }, + { + "type": "path", + "data": { + "pathName": "Center Piece(2)" + } + } + ] + } + }, + "resetOdom": true, + "folder": "1 piece L4", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Center Forward.auto b/src/main/deploy/pathplanner/autos/Center Forward.auto new file mode 100644 index 0000000..d1af341 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Center Forward.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Center Forward" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Forward", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Left 1 Piece L1 Auto.auto b/src/main/deploy/pathplanner/autos/Left 1 Piece L1 Auto.auto new file mode 100644 index 0000000..3905c9d --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Left 1 Piece L1 Auto.auto @@ -0,0 +1,25 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Left Piece(1)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(Go-Back)" + } + } + ] + } + }, + "resetOdom": true, + "folder": "1 Piece L1", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Left 1 Piece L2 Auto.auto b/src/main/deploy/pathplanner/autos/Left 1 Piece L2 Auto.auto new file mode 100644 index 0000000..85c5c4c --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Left 1 Piece L2 Auto.auto @@ -0,0 +1,25 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Left Piece(1)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(Go-Back)" + } + } + ] + } + }, + "resetOdom": true, + "folder": "1 piece L2", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Left 1 Piece L3 Auto.auto b/src/main/deploy/pathplanner/autos/Left 1 Piece L3 Auto.auto new file mode 100644 index 0000000..9532d4c --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Left 1 Piece L3 Auto.auto @@ -0,0 +1,25 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Left Piece(1)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(Go-Back)" + } + } + ] + } + }, + "resetOdom": true, + "folder": "1 piece L3", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Left 1 Piece L4 Auto.auto b/src/main/deploy/pathplanner/autos/Left 1 Piece L4 Auto.auto new file mode 100644 index 0000000..adf5d22 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Left 1 Piece L4 Auto.auto @@ -0,0 +1,25 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Left Piece(1)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(Go-Back)" + } + } + ] + } + }, + "resetOdom": true, + "folder": "1 piece L4", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Left 2 Piece L1 Auto.auto b/src/main/deploy/pathplanner/autos/Left 2 Piece L1 Auto.auto new file mode 100644 index 0000000..613872e --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Left 2 Piece L1 Auto.auto @@ -0,0 +1,37 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Left Piece(1)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(2)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(3)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(4)" + } + } + ] + } + }, + "resetOdom": true, + "folder": "2 Piece L1", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Left 2 Piece L2 Auto.auto b/src/main/deploy/pathplanner/autos/Left 2 Piece L2 Auto.auto new file mode 100644 index 0000000..23c27dc --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Left 2 Piece L2 Auto.auto @@ -0,0 +1,37 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Left Piece(1)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(2)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(3)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(4)" + } + } + ] + } + }, + "resetOdom": true, + "folder": "2 Piece L2", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Left 2 Piece L3 Auto.auto b/src/main/deploy/pathplanner/autos/Left 2 Piece L3 Auto.auto new file mode 100644 index 0000000..9ef50b0 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Left 2 Piece L3 Auto.auto @@ -0,0 +1,37 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Left Piece(1)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(2)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(3)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(4)" + } + } + ] + } + }, + "resetOdom": true, + "folder": "2 Piece L3", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Left 2 Piece L4 Auto.auto b/src/main/deploy/pathplanner/autos/Left 2 Piece L4 Auto.auto new file mode 100644 index 0000000..6ab6103 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Left 2 Piece L4 Auto.auto @@ -0,0 +1,37 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Left Piece(1)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(2)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(3)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(4)" + } + } + ] + } + }, + "resetOdom": true, + "folder": "2 Piece L4", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Left 3 Piece L1 Auto.auto b/src/main/deploy/pathplanner/autos/Left 3 Piece L1 Auto.auto new file mode 100644 index 0000000..5212094 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Left 3 Piece L1 Auto.auto @@ -0,0 +1,49 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Left Piece(1)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(2)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(3)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(4)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(3)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(4)" + } + } + ] + } + }, + "resetOdom": true, + "folder": "3 Piece L1", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Left 3 Piece L2 Auto.auto b/src/main/deploy/pathplanner/autos/Left 3 Piece L2 Auto.auto new file mode 100644 index 0000000..6553275 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Left 3 Piece L2 Auto.auto @@ -0,0 +1,49 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Left Piece(1)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(2)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(3)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(4)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(3)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(4)" + } + } + ] + } + }, + "resetOdom": true, + "folder": "3 Piece L2", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Left 3 Piece L3 Auto.auto b/src/main/deploy/pathplanner/autos/Left 3 Piece L3 Auto.auto new file mode 100644 index 0000000..f0f718d --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Left 3 Piece L3 Auto.auto @@ -0,0 +1,49 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Left Piece(1)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(2)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(3)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(4)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(3)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(4)" + } + } + ] + } + }, + "resetOdom": true, + "folder": "3 Piece L3", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Left 3 Piece L4 Auto.auto b/src/main/deploy/pathplanner/autos/Left 3 Piece L4 Auto.auto new file mode 100644 index 0000000..df9d5a3 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Left 3 Piece L4 Auto.auto @@ -0,0 +1,49 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Left Piece(1)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(2)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(3)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(4)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(3)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(4)" + } + } + ] + } + }, + "resetOdom": true, + "folder": "3 Piece L4", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Left 4 Piece L1 Auto.auto b/src/main/deploy/pathplanner/autos/Left 4 Piece L1 Auto.auto new file mode 100644 index 0000000..543cf51 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Left 4 Piece L1 Auto.auto @@ -0,0 +1,55 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Left Piece(1)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(2)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(3)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(4)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(3)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(4)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(3)" + } + } + ] + } + }, + "resetOdom": true, + "folder": "4 Piece L1", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Left 4 Piece L2 Auto.auto b/src/main/deploy/pathplanner/autos/Left 4 Piece L2 Auto.auto new file mode 100644 index 0000000..c32a19a --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Left 4 Piece L2 Auto.auto @@ -0,0 +1,55 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Left Piece(1)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(2)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(3)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(4)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(3)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(4)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(3)" + } + } + ] + } + }, + "resetOdom": true, + "folder": "4 Piece L2", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Left 4 Piece L3 Auto.auto b/src/main/deploy/pathplanner/autos/Left 4 Piece L3 Auto.auto new file mode 100644 index 0000000..dc5f19b --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Left 4 Piece L3 Auto.auto @@ -0,0 +1,55 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Left Piece(1)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(2)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(3)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(4)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(3)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(4)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(3)" + } + } + ] + } + }, + "resetOdom": true, + "folder": "4 Piece L3", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Left 4 Piece L4 Auto.auto b/src/main/deploy/pathplanner/autos/Left 4 Piece L4 Auto.auto new file mode 100644 index 0000000..8febe09 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Left 4 Piece L4 Auto.auto @@ -0,0 +1,55 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Left Piece(1)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(2)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(3)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(4)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(3)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(4)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(3)" + } + } + ] + } + }, + "resetOdom": true, + "folder": "4 Piece L4", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Left Forward.auto b/src/main/deploy/pathplanner/autos/Left Forward.auto new file mode 100644 index 0000000..3bb1da7 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Left Forward.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Left Forward" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Forward", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right 1 Piece L1 Auto.auto b/src/main/deploy/pathplanner/autos/Right 1 Piece L1 Auto.auto new file mode 100644 index 0000000..2fa2b02 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Right 1 Piece L1 Auto.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Right Piece(1)" + } + } + ] + } + }, + "resetOdom": true, + "folder": "1 Piece L1", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right 1 Piece L2 Auto.auto b/src/main/deploy/pathplanner/autos/Right 1 Piece L2 Auto.auto new file mode 100644 index 0000000..969c8a3 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Right 1 Piece L2 Auto.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Right Piece(1)" + } + } + ] + } + }, + "resetOdom": true, + "folder": "1 piece L2", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right 1 Piece L3 Auto.auto b/src/main/deploy/pathplanner/autos/Right 1 Piece L3 Auto.auto new file mode 100644 index 0000000..240d3ce --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Right 1 Piece L3 Auto.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Right Piece(1)" + } + } + ] + } + }, + "resetOdom": true, + "folder": "1 piece L3", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right 1 Piece L4 Auto.auto b/src/main/deploy/pathplanner/autos/Right 1 Piece L4 Auto.auto new file mode 100644 index 0000000..20da153 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Right 1 Piece L4 Auto.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Right Piece(1)" + } + } + ] + } + }, + "resetOdom": true, + "folder": "1 piece L4", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right 2 Piece L1 Auto.auto b/src/main/deploy/pathplanner/autos/Right 2 Piece L1 Auto.auto new file mode 100644 index 0000000..08f211a --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Right 2 Piece L1 Auto.auto @@ -0,0 +1,37 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Right Piece(1)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(2)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(5)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(6)" + } + } + ] + } + }, + "resetOdom": true, + "folder": "2 Piece L1", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right 2 Piece L2 Auto.auto b/src/main/deploy/pathplanner/autos/Right 2 Piece L2 Auto.auto new file mode 100644 index 0000000..7c510ca --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Right 2 Piece L2 Auto.auto @@ -0,0 +1,37 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Right Piece(1)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(2)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(5)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(6)" + } + } + ] + } + }, + "resetOdom": true, + "folder": "2 Piece L2", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right 2 Piece L3 Auto.auto b/src/main/deploy/pathplanner/autos/Right 2 Piece L3 Auto.auto new file mode 100644 index 0000000..c46a95b --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Right 2 Piece L3 Auto.auto @@ -0,0 +1,37 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Right Piece(1)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(2)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(5)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(6)" + } + } + ] + } + }, + "resetOdom": true, + "folder": "2 Piece L3", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right 2 Piece L4 Auto.auto b/src/main/deploy/pathplanner/autos/Right 2 Piece L4 Auto.auto new file mode 100644 index 0000000..687fff2 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Right 2 Piece L4 Auto.auto @@ -0,0 +1,37 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Right Piece(1)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(2)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(5)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(6)" + } + } + ] + } + }, + "resetOdom": true, + "folder": "2 Piece L4", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right 3 Piece L1 Auto.auto b/src/main/deploy/pathplanner/autos/Right 3 Piece L1 Auto.auto new file mode 100644 index 0000000..083e375 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Right 3 Piece L1 Auto.auto @@ -0,0 +1,49 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Right Piece(1)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(2)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(5)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(6)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(5)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(6)" + } + } + ] + } + }, + "resetOdom": true, + "folder": "3 Piece L1", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right 3 Piece L2 Auto.auto b/src/main/deploy/pathplanner/autos/Right 3 Piece L2 Auto.auto new file mode 100644 index 0000000..36b2444 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Right 3 Piece L2 Auto.auto @@ -0,0 +1,49 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Right Piece(1)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(2)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(5)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(6)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(5)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(6)" + } + } + ] + } + }, + "resetOdom": true, + "folder": "3 Piece L2", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right 3 Piece L3 Auto.auto b/src/main/deploy/pathplanner/autos/Right 3 Piece L3 Auto.auto new file mode 100644 index 0000000..b44c443 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Right 3 Piece L3 Auto.auto @@ -0,0 +1,49 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Right Piece(1)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(2)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(5)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(6)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(5)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(6)" + } + } + ] + } + }, + "resetOdom": true, + "folder": "3 Piece L3", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right 3 Piece L4 Auto.auto b/src/main/deploy/pathplanner/autos/Right 3 Piece L4 Auto.auto new file mode 100644 index 0000000..39e2e31 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Right 3 Piece L4 Auto.auto @@ -0,0 +1,49 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Right Piece(1)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(2)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(5)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(6)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(5)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(6)" + } + } + ] + } + }, + "resetOdom": true, + "folder": "3 Piece L4", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right 4 Piece L1 Auto.auto b/src/main/deploy/pathplanner/autos/Right 4 Piece L1 Auto.auto new file mode 100644 index 0000000..54857e1 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Right 4 Piece L1 Auto.auto @@ -0,0 +1,55 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Right Piece(1)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(2)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(3)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(4)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(5)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(6)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(7)" + } + } + ] + } + }, + "resetOdom": true, + "folder": "4 Piece L1", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right 4 Piece L2 Auto.auto b/src/main/deploy/pathplanner/autos/Right 4 Piece L2 Auto.auto new file mode 100644 index 0000000..7e970dc --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Right 4 Piece L2 Auto.auto @@ -0,0 +1,55 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Right Piece(1)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(2)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(3)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(4)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(5)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(6)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(7)" + } + } + ] + } + }, + "resetOdom": true, + "folder": "4 Piece L2", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right 4 Piece L3 Auto.auto b/src/main/deploy/pathplanner/autos/Right 4 Piece L3 Auto.auto new file mode 100644 index 0000000..0c16c73 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Right 4 Piece L3 Auto.auto @@ -0,0 +1,55 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Right Piece(1)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(2)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(3)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(4)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(5)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(6)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(7)" + } + } + ] + } + }, + "resetOdom": true, + "folder": "4 Piece L3", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right 4 Piece L4 Auto.auto b/src/main/deploy/pathplanner/autos/Right 4 Piece L4 Auto.auto new file mode 100644 index 0000000..d003402 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Right 4 Piece L4 Auto.auto @@ -0,0 +1,55 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Right Piece(1)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(2)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(3)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(4)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(5)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(6)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(7)" + } + } + ] + } + }, + "resetOdom": true, + "folder": "4 Piece L4", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right Forward.auto b/src/main/deploy/pathplanner/autos/Right Forward.auto new file mode 100644 index 0000000..d8e00b3 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Right Forward.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Right Forward" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Forward", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Center Forward.path b/src/main/deploy/pathplanner/paths/Center Forward.path new file mode 100644 index 0000000..bbd1b4f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Center Forward.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 8.06637323943662, + "y": 3.741549295774647 + }, + "prevControl": null, + "nextControl": { + "x": 7.641197183098591, + "y": 3.755721830985915 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.790845070422535, + "y": 3.741549295774647 + }, + "prevControl": { + "x": 7.258538732394367, + "y": 3.7273767605633794 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "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": "Forward", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Center Piece(1).path b/src/main/deploy/pathplanner/paths/Center Piece(1).path new file mode 100644 index 0000000..0af9886 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Center Piece(1).path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 8.00968309859155, + "y": 4.039172535211267 + }, + "prevControl": null, + "nextControl": { + "x": 7.759683101292217, + "y": 4.039209282099408 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.770422535211268, + "y": 4.039172535211267 + }, + "prevControl": { + "x": 6.195598591549295, + "y": 4.010827464788732 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "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": "Center Paths", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Center Piece(2).path b/src/main/deploy/pathplanner/paths/Center Piece(2).path new file mode 100644 index 0000000..e3b5bd0 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Center Piece(2).path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.7987676056338024, + "y": 4.039172535211267 + }, + "prevControl": null, + "nextControl": { + "x": 6.464876760563381, + "y": 4.053345070422535 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.918397887323944, + "y": 4.039172535211267 + }, + "prevControl": { + "x": 6.450704225352113, + "y": 4.053345070422535 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "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": -38.99099404250553 + }, + "reversed": false, + "folder": "Center Paths", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/CoralConfigs.path b/src/main/deploy/pathplanner/paths/CoralConfigs.path index cd412d7..5a181b7 100644 --- a/src/main/deploy/pathplanner/paths/CoralConfigs.path +++ b/src/main/deploy/pathplanner/paths/CoralConfigs.path @@ -126,7 +126,7 @@ "rotation": 0.0 }, "reversed": false, - "folder": null, + "folder": "Daniel Stuff", "idealStartingState": { "velocity": 0, "rotation": -117.26958303209868 diff --git a/src/main/deploy/pathplanner/paths/Left Forward.path b/src/main/deploy/pathplanner/paths/Left Forward.path new file mode 100644 index 0000000..78eed49 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Forward.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 8.080545774647888, + "y": 7.0012323943661965 + }, + "prevControl": null, + "nextControl": { + "x": 7.414436619718311, + "y": 6.972887323943662 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.189348591549296, + "y": 7.0012323943661965 + }, + "prevControl": { + "x": 5.75625, + "y": 7.0012323943661965 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "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": "Forward", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Piece(1).path b/src/main/deploy/pathplanner/paths/Left Piece(1).path new file mode 100644 index 0000000..998ba82 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Piece(1).path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 8.023855633802818, + "y": 7.114612676056338 + }, + "prevControl": null, + "nextControl": { + "x": 7.47112676056338, + "y": 6.661091549295775 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.175176056338028, + "y": 5.14463028169014 + }, + "prevControl": { + "x": 5.727904929577464, + "y": 5.5131161971830975 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "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": -117.97947438848014 + }, + "reversed": false, + "folder": "Left Paths", + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Piece(2).path b/src/main/deploy/pathplanner/paths/Left Piece(2).path new file mode 100644 index 0000000..dbde03d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Piece(2).path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.175176056338028, + "y": 5.14463028169014 + }, + "prevControl": null, + "nextControl": { + "x": 3.871302816901408, + "y": 5.754049295774647 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.3344190140845058, + "y": 7.142957746478873 + }, + "prevControl": { + "x": 2.312323943661971, + "y": 6.6469190140845065 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "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": 126.2538377374448 + }, + "reversed": false, + "folder": "Left Paths", + "idealStartingState": { + "velocity": 0, + "rotation": -118.81079374297319 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Piece(3).path b/src/main/deploy/pathplanner/paths/Left Piece(3).path new file mode 100644 index 0000000..e9ccd66 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Piece(3).path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.3344190140845058, + "y": 7.142957746478873 + }, + "prevControl": null, + "nextControl": { + "x": 1.7737676056331966, + "y": 6.746126760569904 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.8146126760563375, + "y": 5.158802816901408 + }, + "prevControl": { + "x": 3.290228873239436, + "y": 5.654841549295774 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "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": 119.05460409907721 + }, + "reversed": false, + "folder": "Left Paths", + "idealStartingState": { + "velocity": 0, + "rotation": 126.46923439005198 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Piece(4).path b/src/main/deploy/pathplanner/paths/Left Piece(4).path new file mode 100644 index 0000000..a9f555d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Piece(4).path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.8287852112676046, + "y": 5.14463028169014 + }, + "prevControl": null, + "nextControl": { + "x": 4.268133802816295, + "y": 4.747799295781171 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.2352112676056324, + "y": 7.057922535211268 + }, + "prevControl": { + "x": 2.0288732394366185, + "y": 6.505193661971831 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "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": 123.6900675259798 + }, + "reversed": false, + "folder": "Left Paths", + "idealStartingState": { + "velocity": 0, + "rotation": 126.46923439005198 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Piece(Go-Back).path b/src/main/deploy/pathplanner/paths/Left Piece(Go-Back).path new file mode 100644 index 0000000..949ce82 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Piece(Go-Back).path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.146830985915492, + "y": 5.187147887323943 + }, + "prevControl": null, + "nextControl": { + "x": 5.416109154929577, + "y": 5.456426056338028 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.145158450704225, + "y": 7.256338028169014 + }, + "prevControl": { + "x": 6.861707746478873, + "y": 6.972887323943662 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "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": 180.0 + }, + "reversed": false, + "folder": "Left Paths", + "idealStartingState": { + "velocity": 0, + "rotation": -119.74488129694231 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Forward.path b/src/main/deploy/pathplanner/paths/Right Forward.path new file mode 100644 index 0000000..0ce8ac8 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Forward.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 8.06637323943662, + "y": 0.7653169014084494 + }, + "prevControl": null, + "nextControl": { + "x": 7.570334507042254, + "y": 0.7511443661971827 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.81294014084507, + "y": 0.7653169014084494 + }, + "prevControl": { + "x": 6.450704225352113, + "y": 0.7369718309859138 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "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": "Forward", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Piece(1).path b/src/main/deploy/pathplanner/paths/Right Piece(1).path new file mode 100644 index 0000000..dc3d7e9 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Piece(1).path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 8.038028169014085, + "y": 1.062940140845069 + }, + "prevControl": null, + "nextControl": { + "x": 7.386091549295775, + "y": 1.3747359154929564 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.189348591549296, + "y": 2.9053697183098577 + }, + "prevControl": { + "x": 5.841285211267606, + "y": 2.4518485915492945 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "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": -58.240519915187086 + }, + "reversed": false, + "folder": "Right Paths", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Piece(2).path b/src/main/deploy/pathplanner/paths/Right Piece(2).path new file mode 100644 index 0000000..86fad16 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Piece(2).path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.118485915492958, + "y": 2.8486795774647873 + }, + "prevControl": null, + "nextControl": { + "x": 4.6649647887323935, + "y": 2.5652288732394357 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.2068661971830972, + "y": 0.9495598591549276 + }, + "prevControl": { + "x": 1.9013204225352105, + "y": 1.3322183098591531 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "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": -126.46923439005185 + }, + "reversed": false, + "folder": "Right Paths", + "idealStartingState": { + "velocity": 0, + "rotation": -64.88516511385541 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Piece(3).path b/src/main/deploy/pathplanner/paths/Right Piece(3).path new file mode 100644 index 0000000..6c4db13 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Piece(3).path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.2068661971830972, + "y": 0.9495598591549276 + }, + "prevControl": null, + "nextControl": { + "x": 1.7879401408450692, + "y": 1.2188380281690128 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.189348591549296, + "y": 2.8345070422535197 + }, + "prevControl": { + "x": 5.4201178223185265, + "y": 2.930660888407366 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "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": -61.476881393687925 + }, + "reversed": false, + "folder": "Right Paths", + "idealStartingState": { + "velocity": 0, + "rotation": -125.31121343963326 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Piece(4).path b/src/main/deploy/pathplanner/paths/Right Piece(4).path new file mode 100644 index 0000000..9dd8363 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Piece(4).path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.1610035211267595, + "y": 2.8486795774647873 + }, + "prevControl": null, + "nextControl": { + "x": 4.126408450704225, + "y": 2.3101232394366185 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.2352112676056324, + "y": 0.992077464788731 + }, + "prevControl": { + "x": 1.4352112676056326, + "y": 1.1420774647887308 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "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": -123.02386755579657 + }, + "reversed": false, + "folder": "Right Paths", + "idealStartingState": { + "velocity": 0, + "rotation": -56.309932474020215 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Piece(5).path b/src/main/deploy/pathplanner/paths/Right Piece(5).path new file mode 100644 index 0000000..9a57cfc --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Piece(5).path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.0934859154929564, + "y": 1.0771126760563368 + }, + "prevControl": null, + "nextControl": { + "x": 3.276056338028168, + "y": 2.5085387323943653 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.7720950704225347, + "y": 2.891197183098591 + }, + "prevControl": { + "x": 3.554703599534407, + "y": 2.7490248488316853 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "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": -129.5596679689945 + }, + "reversed": false, + "folder": "Right Paths", + "idealStartingState": { + "velocity": 0, + "rotation": -125.31121343963326 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Piece(6).path b/src/main/deploy/pathplanner/paths/Right Piece(6).path new file mode 100644 index 0000000..f1553bf --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Piece(6).path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.8146126760563375, + "y": 2.9337147887323933 + }, + "prevControl": null, + "nextControl": { + "x": 2.90757042253521, + "y": 2.125880281690139 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.2068661971830972, + "y": 1.0345950704225333 + }, + "prevControl": { + "x": 1.4477992957746464, + "y": 1.261355633802815 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "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": -126.86989764584399 + }, + "reversed": false, + "folder": "Right Paths", + "idealStartingState": { + "velocity": 0, + "rotation": -125.31121343963326 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Piece(7).path b/src/main/deploy/pathplanner/paths/Right Piece(7).path new file mode 100644 index 0000000..7db80b9 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Piece(7).path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.3344190140845058, + "y": 0.9070422535211253 + }, + "prevControl": null, + "nextControl": { + "x": 2.7091549295774637, + "y": 1.8424295774647876 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.8571302816901403, + "y": 2.8345070422535197 + }, + "prevControl": { + "x": 2.978433098591548, + "y": 2.125880281690139 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "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": -126.86989764584399 + }, + "reversed": false, + "folder": "Right Paths", + "idealStartingState": { + "velocity": 0, + "rotation": -125.31121343963326 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/StartingConfigs.path b/src/main/deploy/pathplanner/paths/StartingConfigs.path index ce1c8d0..eee380c 100644 --- a/src/main/deploy/pathplanner/paths/StartingConfigs.path +++ b/src/main/deploy/pathplanner/paths/StartingConfigs.path @@ -106,7 +106,7 @@ "rotation": 180.0 }, "reversed": false, - "folder": null, + "folder": "Daniel Stuff", "idealStartingState": { "velocity": 0.0, "rotation": 180.0 diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 2ca3c4c..1d3b2c9 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -2,8 +2,33 @@ "robotWidth": 0.9, "robotLength": 0.9, "holonomicMode": true, - "pathFolders": [], - "autoFolders": [], + "pathFolders": [ + "Center Paths", + "Daniel Stuff", + "Forward", + "Left Paths", + "Other Stuff", + "Right Paths" + ], + "autoFolders": [ + "1 Piece L1", + "1 piece L2", + "1 piece L3", + "1 piece L4", + "2 Piece L1", + "2 Piece L2", + "2 Piece L3", + "2 Piece L4", + "3 Piece L1", + "3 Piece L2", + "3 Piece L3", + "3 Piece L4", + "4 Piece L1", + "4 Piece L2", + "4 Piece L3", + "4 Piece L4", + "Forward" + ], "defaultMaxVel": 3.0, "defaultMaxAccel": 3.0, "defaultMaxAngVel": 540.0, From a7edcafdd339a8de6643171f8ce6986446e6451a Mon Sep 17 00:00:00 2001 From: Rand0mAsianKid Date: Sat, 15 Feb 2025 18:10:41 -0800 Subject: [PATCH 37/51] new paths --- src/main/deploy/pathplanner/paths/Right Forward.path | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/deploy/pathplanner/paths/Right Forward.path b/src/main/deploy/pathplanner/paths/Right Forward.path index 0ce8ac8..a32fad7 100644 --- a/src/main/deploy/pathplanner/paths/Right Forward.path +++ b/src/main/deploy/pathplanner/paths/Right Forward.path @@ -9,7 +9,7 @@ "prevControl": null, "nextControl": { "x": 7.570334507042254, - "y": 0.7511443661971827 + "y": 0.7511443661971826 }, "isLocked": false, "linkedName": null From b69deb40ef0c062c5570a8542e2e2df541a5eac6 Mon Sep 17 00:00:00 2001 From: Rand0mAsianKid Date: Wed, 19 Feb 2025 03:44:52 -0800 Subject: [PATCH 38/51] added some setup stuff(will fix) --- .../org/carlmontrobotics/RobotContainer.java | 109 +++++++++++++++++- 1 file changed, 106 insertions(+), 3 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index e39e162..657a623 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -9,12 +9,21 @@ // import org.carlmontrobotics.commands.*; import static org.carlmontrobotics.Constants.OI.*; +import java.util.List; + import org.carlmontrobotics.Constants.OI; import org.carlmontrobotics.Constants.OI.Manipulator.*; import org.carlmontrobotics.Constants.OI.Manipulator; import org.carlmontrobotics.commands.OuttakeAlgae; import org.carlmontrobotics.subsystems.AlgaeEffector; import org.carlmontrobotics.subsystems.CoralEffector; + +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.auto.NamedCommands; +import com.pathplanner.lib.commands.PathPlannerAuto; +import com.pathplanner.lib.path.GoalEndState; +import com.pathplanner.lib.path.PathPlannerPath; + import org.carlmontrobotics.commands.IntakeAlgae; import org.carlmontrobotics.commands.IntakeCoral; import org.carlmontrobotics.commands.OuttakeCoral; @@ -24,14 +33,16 @@ import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.PS5Controller.Button; import edu.wpi.first.wpilibj.XboxController.Axis; - //commands import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; import edu.wpi.first.wpilibj2.command.PrintCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; //control bindings import static org.carlmontrobotics.Constants.CoralEffectorc.*; @@ -47,9 +58,35 @@ public class RobotContainer { public final GenericHID manipulatorController = new GenericHID(OI.Manipulator.manipulatorPort); private final AlgaeEffector algaeEffector = new AlgaeEffector(); private final CoralEffector coralEffector = new CoralEffector(); + + private List autoCommands = new ArrayList(); + private SendableChooser autoSelector = new SendableChooser(); + + private boolean hasSetupAutos = false; + private final String[] autoNames = new String[] { + /* These are assumed to be equal to the AUTO ames in pathplanner */ + "Left-Auto Ruiner", "Center-Auto Ruiner", "Right-Auto Ruiner", + }; + + DigitalInput[] autoSelectors = new DigitalInput[Math.min(autoNames.length, 10)]; + + public RobotContainer() { setBindingsDriver(); setBindingsManipulator(); + + registerAutoCommands(); + SmartDashboard.putData(autoSelector); + SmartDashboard.setPersistent("SendableChooser[0]"); + + int i = 3; + for (String n : autoNames) { + autoSelector.addOption(n, i); + i++; + } + + ShuffleboardTab autoSelectorTab = Shuffleboard.getTab("Auto Chooser Tab"); + autoSelectorTab.add(autoSelector).withSize(2, 1); } private void setDefaultCommands() { @@ -91,9 +128,9 @@ private Trigger axisTrigger(GenericHID controller, Axis axis) { } - public Command getAutonomousCommand() { + /*public Command getAutonomousCommand() { return Commands.print("No autonomous command configured"); - } + }*/ /** * Flips an axis' Y coordinates upside down, but only if the select axis is a joystick axis @@ -129,4 +166,70 @@ private double inputProcessing(double value) { private double ProcessedAxisValue(GenericHID hid, Axis axis){ return inputProcessing(getStickValue(hid, axis)); } + + + + private void registerAutoCommands() { + // NamedCommands.registerCommand("Intake", new Intake(intakeShooter)); + //NamedCommands.registerCommand("Eject", new Eject(intakeShooter)); + } + + private void setupAutos() { + { + for (int i = 0; i < autoNames.length; i++) { + String name = autoNames[i]; + + autoCommands.add(new PathPlannerAuto(name)); + } + } + + // AUTOGENERATED AUTO FOR SLOT 2 + { + Pose2d currPos = drivetrain.getPose(); + + List bezierPoints = PathPlannerPath.bezierFromPoses( + currPos, currPos.plus(new Transform2d(0, 1, new Rotation2d(0)))); + /** + * PATHPLANNER SETTINGS Robot Width (m): .91 Robot Length(m): .94 Max Module Spd + * (m/s): 4.30 + * Default Constraints Max Vel: 1.54, Max Accel: 6.86 Max Angvel: 360, Max + * AngAccel: 360 + * (guesses!) + */ + // Create the path using the bezier points created above + PathPlannerPath path = new PathPlannerPath(bezierPoints, + /* m/s, m/s^2, rad/s, rad/s^2 */ + Autoc.pathConstraints, new GoalEndState(0, currPos.getRotation())); + // Prevent the path from being flipped if the coordinates are already correct + path.preventFlipping = DriverStation.getAlliance().get() == DriverStation.Alliance.Blue; + if (DriverStation.getAlliance().get() == DriverStation.Alliance.Red) { + path.flipPath(); + } + + // NOTHING + autoCommands.add(0, new PrintCommand("Running NULL Auto!")); + // RAW FORWARD command + autoCommands.add(1, new LastResortAuto(drivetrain)); + // smart forward command + autoCommands.add(2, + new SequentialCommandGroup(AutoBuilder.followPath(path))); + // no events so just use path instead of auto + + } + } + + public Command getAutonomousCommand() { + if (!hasSetupAutos) { + setupAutos(); + hasSetupAutos = true; + } + Integer autoIndex = autoSelector.getSelected(); + + if (autoIndex != null && autoIndex != 0) { + new PrintCommand("Running selected auto: " + autoSelector.toString()); + return autoCommands.get(autoIndex.intValue()); + } + return new PrintCommand("No auto :("); + } } + From d2f4eebf5ca31cf16f96652929901abcf0028f91 Mon Sep 17 00:00:00 2001 From: Rand0mAsianKid Date: Wed, 19 Feb 2025 12:58:27 -0800 Subject: [PATCH 39/51] stuff --- build.gradle | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/build.gradle b/build.gradle index 35c0c0f..b5ad6c3 100644 --- a/build.gradle +++ b/build.gradle @@ -86,7 +86,7 @@ dependencies { testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1' testRuntimeOnly 'org.junit.platform:junit-platform-launcher' - implementation "com.github.deepbluerobotics:lib199:2025-SNAPSHOT" + implementation "com.github.DeepBlueRobotics:lib199:2025-SNAPSHOT" } test { From 793e044208ac04d2546f1175a9911ec5a8cb7e45 Mon Sep 17 00:00:00 2001 From: Rand0mAsianKid <144396869+Rand0mAsianKid@users.noreply.github.com> Date: Thu, 20 Feb 2025 01:33:12 -0800 Subject: [PATCH 40/51] Auto Settup --- .../org/carlmontrobotics/RobotContainer.java | 44 +++++-------------- 1 file changed, 10 insertions(+), 34 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index 657a623..a7e2971 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -9,6 +9,7 @@ // import org.carlmontrobotics.commands.*; import static org.carlmontrobotics.Constants.OI.*; +import java.util.ArrayList; import java.util.List; import org.carlmontrobotics.Constants.OI; @@ -28,11 +29,16 @@ import org.carlmontrobotics.commands.IntakeCoral; import org.carlmontrobotics.commands.OuttakeCoral; - +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj.DriverStation; //controllers import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.PS5Controller.Button; import edu.wpi.first.wpilibj.XboxController.Axis; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; //commands import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; @@ -68,23 +74,22 @@ public class RobotContainer { "Left-Auto Ruiner", "Center-Auto Ruiner", "Right-Auto Ruiner", }; - DigitalInput[] autoSelectors = new DigitalInput[Math.min(autoNames.length, 10)]; + //DigitalInput[] autoSelectors = new DigitalInput[Math.min(autoNames.length, 10)]; public RobotContainer() { setBindingsDriver(); setBindingsManipulator(); + registerAutoCommands(); SmartDashboard.putData(autoSelector); SmartDashboard.setPersistent("SendableChooser[0]"); - int i = 3; for (String n : autoNames) { autoSelector.addOption(n, i); i++; } - ShuffleboardTab autoSelectorTab = Shuffleboard.getTab("Auto Chooser Tab"); autoSelectorTab.add(autoSelector).withSize(2, 1); } @@ -183,38 +188,9 @@ private void setupAutos() { } } - // AUTOGENERATED AUTO FOR SLOT 2 { - Pose2d currPos = drivetrain.getPose(); - - List bezierPoints = PathPlannerPath.bezierFromPoses( - currPos, currPos.plus(new Transform2d(0, 1, new Rotation2d(0)))); - /** - * PATHPLANNER SETTINGS Robot Width (m): .91 Robot Length(m): .94 Max Module Spd - * (m/s): 4.30 - * Default Constraints Max Vel: 1.54, Max Accel: 6.86 Max Angvel: 360, Max - * AngAccel: 360 - * (guesses!) - */ - // Create the path using the bezier points created above - PathPlannerPath path = new PathPlannerPath(bezierPoints, - /* m/s, m/s^2, rad/s, rad/s^2 */ - Autoc.pathConstraints, new GoalEndState(0, currPos.getRotation())); - // Prevent the path from being flipped if the coordinates are already correct - path.preventFlipping = DriverStation.getAlliance().get() == DriverStation.Alliance.Blue; - if (DriverStation.getAlliance().get() == DriverStation.Alliance.Red) { - path.flipPath(); - } - - // NOTHING + //No Auto autoCommands.add(0, new PrintCommand("Running NULL Auto!")); - // RAW FORWARD command - autoCommands.add(1, new LastResortAuto(drivetrain)); - // smart forward command - autoCommands.add(2, - new SequentialCommandGroup(AutoBuilder.followPath(path))); - // no events so just use path instead of auto - } } From 7bb91cc2d06eb2c7f46e35249de0c15b4157d754 Mon Sep 17 00:00:00 2001 From: Rand0mAsianKid <144396869+Rand0mAsianKid@users.noreply.github.com> Date: Thu, 20 Feb 2025 01:57:42 -0800 Subject: [PATCH 41/51] added autos to selector --- simgui.json | 4 ++- .../org/carlmontrobotics/RobotContainer.java | 32 +++++++++++++++---- 2 files changed, 29 insertions(+), 7 deletions(-) diff --git a/simgui.json b/simgui.json index 4630a5c..fdc8b05 100644 --- a/simgui.json +++ b/simgui.json @@ -10,7 +10,9 @@ }, "NTProvider": { "types": { - "/FMSInfo": "FMSInfo" + "/FMSInfo": "FMSInfo", + "/Shuffleboard/Auto Chooser Tab/SendableChooser[0]": "String Chooser", + "/SmartDashboard/SendableChooser[0]": "String Chooser" } }, "NetworkTables Info": { diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index a7e2971..7a0a96f 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -30,6 +30,7 @@ import org.carlmontrobotics.commands.OuttakeCoral; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.DriverStation; //controllers import edu.wpi.first.wpilibj.GenericHID; @@ -67,14 +68,34 @@ public class RobotContainer { private List autoCommands = new ArrayList(); private SendableChooser autoSelector = new SendableChooser(); - private boolean hasSetupAutos = false; private final String[] autoNames = new String[] { /* These are assumed to be equal to the AUTO ames in pathplanner */ - "Left-Auto Ruiner", "Center-Auto Ruiner", "Right-Auto Ruiner", + "Left 1 Piece L1 Auto", "Center 1 Piece L1 Auto", "Right 1 Piece L1 Auto", + "Left 1 Piece L2 Auto", "Center 1 Piece L2 Auto", "Right 1 Piece L2 Auto", + "Left 1 Piece L3 Auto", "Center 1 Piece L3 Auto", "Right 1 Piece L3 Auto", + "Left 1 Piece L4 Auto", "Center 1 Piece L4 Auto", "Right 1 Piece L4 Auto", + + "Left 2 Piece L1 Auto", "Right 2 Piece L1 Auto", + "Left 2 Piece L2 Auto", "Right 2 Piece L2 Auto", + "Left 2 Piece L3 Auto", "Right 2 Piece L3 Auto", + "Left 2 Piece L4 Auto", "Right 2 Piece L4 Auto", + + "Left 3 Piece L1 Auto", "Right 3 Piece L1 Auto", + "Left 3 Piece L2 Auto", "Right 3 Piece L2 Auto", + "Left 3 Piece L3 Auto", "Right 3 Piece L3 Auto", + "Left 3 Piece L4 Auto", "Right 3 Piece L4 Auto", + + "Left 4 Piece L1 Auto", "Right 4 Piece L1 Auto", + "Left 4 Piece L2 Auto", "Right 4 Piece L2 Auto", + "Left 4 Piece L3 Auto", "Right 4 Piece L3 Auto", + "Left 4 Piece L4 Auto", "Right 4 Piece L4 Auto", + + "Left Forward", "Center Forward", "Right Forward", + + }; - - //DigitalInput[] autoSelectors = new DigitalInput[Math.min(autoNames.length, 10)]; + DigitalInput[] autoSelectors = new DigitalInput[Math.min(autoNames.length, 10)]; public RobotContainer() { @@ -183,11 +204,10 @@ private void setupAutos() { { for (int i = 0; i < autoNames.length; i++) { String name = autoNames[i]; - + //Run Auto autoCommands.add(new PathPlannerAuto(name)); } } - { //No Auto autoCommands.add(0, new PrintCommand("Running NULL Auto!")); From e033c90b8fc561677c18720dd4201cae867e6901 Mon Sep 17 00:00:00 2001 From: Rand0mAsianKid <144396869+Rand0mAsianKid@users.noreply.github.com> Date: Thu, 20 Feb 2025 18:42:49 -0800 Subject: [PATCH 42/51] finished auto settup without the autobuilder(need drivetrain stuff for it) --- .../org/carlmontrobotics/RobotContainer.java | 89 +++++-------------- .../subsystems/DrivetrainAutonTemp.java | 17 ++++ 2 files changed, 38 insertions(+), 68 deletions(-) create mode 100644 src/main/java/org/carlmontrobotics/subsystems/DrivetrainAutonTemp.java diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index 7a0a96f..111e887 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -61,16 +61,16 @@ public class RobotContainer { //1. using GenericHID allows us to use different kinds of controllers //2. Use absolute paths from constants to reduce confusion + private final AlgaeEffector algae = new AlgaeEffector(); + private final CoralEffector coral = new CoralEffector(); + public final GenericHID driverController = new GenericHID(OI.Driver.driverPort); public final GenericHID manipulatorController = new GenericHID(OI.Manipulator.manipulatorPort); private final AlgaeEffector algaeEffector = new AlgaeEffector(); private final CoralEffector coralEffector = new CoralEffector(); - private List autoCommands = new ArrayList(); - private SendableChooser autoSelector = new SendableChooser(); - private boolean hasSetupAutos = false; - private final String[] autoNames = new String[] { - /* These are assumed to be equal to the AUTO ames in pathplanner */ + private final SendableChooser autoChooser; + /*private final String[] autoNames = new String[] { "Left 1 Piece L1 Auto", "Center 1 Piece L1 Auto", "Right 1 Piece L1 Auto", "Left 1 Piece L2 Auto", "Center 1 Piece L2 Auto", "Right 1 Piece L2 Auto", "Left 1 Piece L3 Auto", "Center 1 Piece L3 Auto", "Right 1 Piece L3 Auto", @@ -94,44 +94,23 @@ public class RobotContainer { "Left Forward", "Center Forward", "Right Forward", - }; - DigitalInput[] autoSelectors = new DigitalInput[Math.min(autoNames.length, 10)]; + };*/ + + public RobotContainer() { setBindingsDriver(); setBindingsManipulator(); - - - registerAutoCommands(); - SmartDashboard.putData(autoSelector); - SmartDashboard.setPersistent("SendableChooser[0]"); - int i = 3; - for (String n : autoNames) { - autoSelector.addOption(n, i); - i++; - } - ShuffleboardTab autoSelectorTab = Shuffleboard.getTab("Auto Chooser Tab"); - autoSelectorTab.add(autoSelector).withSize(2, 1); - } - - private void setDefaultCommands() { - // drivetrain.setDefaultCommand(new TeleopDrive( - // drivetrain, - // () -> ProcessedAxisValue(driverController, Axis.kLeftY)), - // () -> ProcessedAxisValue(driverController, Axis.kLeftX)), - // () -> ProcessedAxisValue(driverController, Axis.kRightX)), - // () -> driverController.getRawButton(OI.Driver.slowDriveButton) - // )); + + RegisterAutoCommands(); + autoChooser = AutoBuilder.buildAutoChooser(); + SmartDashboard.putData("Auto Chooser", autoChooser); } - private void setBindingsDriver() { - // new Trigger(() -> driverController.getRawButton(OI.Driver.X)).onTrue(new RunAlgae(algaeEffector, 1, false)); //wrong - // new Trigger(() -> driverController.getRawButton(OI.Driver.Y)).onTrue(new RunAlgae(algaeEffector, 2, false)); - // new Trigger(() -> driverController.getRawButton(OI.Driver.B)).onTrue(new RunAlgae(algaeEffector, 3, false)); - // new Trigger(() -> driverController.getRawButton(OI.Driver.A)).onTrue(new RunAlgae(algaeEffector, 0, true)); - } + private void setDefaultCommands() {} + private void setBindingsDriver() {} private void setBindingsManipulator() { axisTrigger(manipulatorController, OI.Manipulator.IntakeTrigger) @@ -147,12 +126,17 @@ private void setBindingsManipulator() { .whileFalse(new OuttakeAlgae(algaeEffector)); } - private Trigger axisTrigger(GenericHID controller, Axis axis) { return new Trigger(() -> Math .abs(getStickValue(controller, axis)) > Constants.OI.MIN_AXIS_TRIGGER_VALUE); } + private void RegisterAutoCommands(){ + NamedCommands.registerCommand("IntakeAlgae", new IntakeAlgae(algae)); + NamedCommands.registerCommand("OuttakeAlgae", new OuttakeAlgae(algae)); + NamedCommands.registerCommand("IntakeCoral", new IntakeCoral(coral)); + NamedCommands.registerCommand("OuttakeCoral", new OuttakeCoral(coral)); + } /*public Command getAutonomousCommand() { return Commands.print("No autonomous command configured"); @@ -193,39 +177,8 @@ private double ProcessedAxisValue(GenericHID hid, Axis axis){ return inputProcessing(getStickValue(hid, axis)); } - - - private void registerAutoCommands() { - // NamedCommands.registerCommand("Intake", new Intake(intakeShooter)); - //NamedCommands.registerCommand("Eject", new Eject(intakeShooter)); - } - - private void setupAutos() { - { - for (int i = 0; i < autoNames.length; i++) { - String name = autoNames[i]; - //Run Auto - autoCommands.add(new PathPlannerAuto(name)); - } - } - { - //No Auto - autoCommands.add(0, new PrintCommand("Running NULL Auto!")); - } - } - public Command getAutonomousCommand() { - if (!hasSetupAutos) { - setupAutos(); - hasSetupAutos = true; - } - Integer autoIndex = autoSelector.getSelected(); - - if (autoIndex != null && autoIndex != 0) { - new PrintCommand("Running selected auto: " + autoSelector.toString()); - return autoCommands.get(autoIndex.intValue()); - } - return new PrintCommand("No auto :("); + return autoChooser.getSelected(); } } diff --git a/src/main/java/org/carlmontrobotics/subsystems/DrivetrainAutonTemp.java b/src/main/java/org/carlmontrobotics/subsystems/DrivetrainAutonTemp.java new file mode 100644 index 0000000..1702754 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/subsystems/DrivetrainAutonTemp.java @@ -0,0 +1,17 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package org.carlmontrobotics.subsystems; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class DrivetrainAutonTemp extends SubsystemBase { + /** Creates a new DrivetrainAutonTemp. */ + public DrivetrainAutonTemp() {} + + @Override + public void periodic() { + // This method will be called once per scheduler run + } +} From 776c6801c672bc8b7862cd43d5c24fc4ede73ea2 Mon Sep 17 00:00:00 2001 From: Rand0mAsianKid <144396869+Rand0mAsianKid@users.noreply.github.com> Date: Thu, 20 Feb 2025 19:05:03 -0800 Subject: [PATCH 43/51] whoops, made 2 instances of something --- .../org/carlmontrobotics/RobotContainer.java | 13 +++++-------- .../subsystems/DrivetrainAutonTemp.java | 17 ----------------- 2 files changed, 5 insertions(+), 25 deletions(-) delete mode 100644 src/main/java/org/carlmontrobotics/subsystems/DrivetrainAutonTemp.java diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index 111e887..f883edb 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -61,9 +61,6 @@ public class RobotContainer { //1. using GenericHID allows us to use different kinds of controllers //2. Use absolute paths from constants to reduce confusion - private final AlgaeEffector algae = new AlgaeEffector(); - private final CoralEffector coral = new CoralEffector(); - public final GenericHID driverController = new GenericHID(OI.Driver.driverPort); public final GenericHID manipulatorController = new GenericHID(OI.Manipulator.manipulatorPort); private final AlgaeEffector algaeEffector = new AlgaeEffector(); @@ -102,7 +99,7 @@ public class RobotContainer { public RobotContainer() { setBindingsDriver(); setBindingsManipulator(); - + RegisterAutoCommands(); autoChooser = AutoBuilder.buildAutoChooser(); SmartDashboard.putData("Auto Chooser", autoChooser); @@ -132,10 +129,10 @@ private Trigger axisTrigger(GenericHID controller, Axis axis) { } private void RegisterAutoCommands(){ - NamedCommands.registerCommand("IntakeAlgae", new IntakeAlgae(algae)); - NamedCommands.registerCommand("OuttakeAlgae", new OuttakeAlgae(algae)); - NamedCommands.registerCommand("IntakeCoral", new IntakeCoral(coral)); - NamedCommands.registerCommand("OuttakeCoral", new OuttakeCoral(coral)); + NamedCommands.registerCommand("IntakeAlgae", new IntakeAlgae(algaeEffector)); + NamedCommands.registerCommand("OuttakeAlgae", new OuttakeAlgae(algaeEffector)); + NamedCommands.registerCommand("IntakeCoral", new IntakeCoral(coralEffector)); + NamedCommands.registerCommand("OuttakeCoral", new OuttakeCoral(coralEffector)); } /*public Command getAutonomousCommand() { diff --git a/src/main/java/org/carlmontrobotics/subsystems/DrivetrainAutonTemp.java b/src/main/java/org/carlmontrobotics/subsystems/DrivetrainAutonTemp.java deleted file mode 100644 index 1702754..0000000 --- a/src/main/java/org/carlmontrobotics/subsystems/DrivetrainAutonTemp.java +++ /dev/null @@ -1,17 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package org.carlmontrobotics.subsystems; - -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -public class DrivetrainAutonTemp extends SubsystemBase { - /** Creates a new DrivetrainAutonTemp. */ - public DrivetrainAutonTemp() {} - - @Override - public void periodic() { - // This method will be called once per scheduler run - } -} From 79699c77826306e05b8e534a86cd8024eb825ec9 Mon Sep 17 00:00:00 2001 From: DriverStationComputer <35879629+DriverStationComputer@users.noreply.github.com> Date: Tue, 25 Feb 2025 16:59:56 -0800 Subject: [PATCH 44/51] added dt stuff --- .../java/org/carlmontrobotics/Config.java | 119 ++ .../java/org/carlmontrobotics/Constants.java | 196 ++- .../commands/RotateToFieldRelativeAngle.java | 51 + .../commands/TeleopDrive.java | 152 +++ .../subsystems/Drivetrain.java | 1153 +++++++++++++++++ 5 files changed, 1670 insertions(+), 1 deletion(-) create mode 100644 src/main/java/org/carlmontrobotics/Config.java create mode 100644 src/main/java/org/carlmontrobotics/commands/RotateToFieldRelativeAngle.java create mode 100644 src/main/java/org/carlmontrobotics/commands/TeleopDrive.java create mode 100644 src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java diff --git a/src/main/java/org/carlmontrobotics/Config.java b/src/main/java/org/carlmontrobotics/Config.java new file mode 100644 index 0000000..9a68135 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/Config.java @@ -0,0 +1,119 @@ +package org.carlmontrobotics; + +import java.lang.reflect.Method; +import java.lang.reflect.Modifier; +import java.util.ArrayList; +import java.util.List; + +import edu.wpi.first.util.sendable.Sendable; +import edu.wpi.first.util.sendable.SendableBuilder; + +public abstract class Config implements Sendable { + public static final Config CONFIG = new Config() { + { + // Override config settings here, like this: + // this.exampleFlagEnabled = true; + + // NOTE: PRs with overrides will NOT be merged because we don't want them + // polluting the master branch. + // Feel free to add them when testing, but remove them before pushing. + } + }; + + // Add additional config settings by declaring a protected field, and... + protected boolean exampleFlagEnabled = false; + protected boolean swimShady = false; + protected boolean setupSysId = false; + protected boolean useSmartDashboardControl = false; // whether to control arm position + rpm of + // outtake through SmartDashboard + // Note: disables joystick control of arm and + // outtake command if + // using SmartDashboard + + // ...a public getter starting with "is" for booleans or "get" for other types. + // Do NOT remove this example. It is used by unit tests. + + public boolean isExampleFlagEnabled() { + return exampleFlagEnabled; + } + + public boolean isSwimShady() { + return swimShady; + } + + public boolean isSysIdTesting() { + return setupSysId; + } + + public boolean useSmartDashboardControl() { + return useSmartDashboardControl; + } + + // --- For clarity, place additional config settings ^above^ this line --- + + private static class MethodResult { + String methodName = null; + Object retVal = null; + Object defaultRetVal = null; + + MethodResult(String name, Object retVal, Object defaultRetval) { + this.methodName = name; + this.retVal = retVal; + this.defaultRetVal = defaultRetval; + } + } + + private List getMethodResults() { + var methodResults = new ArrayList(); + var defaultConfig = new Config() { + }; + for (Method m : Config.class.getDeclaredMethods()) { + var name = m.getName(); + if (!Modifier.isPublic(m.getModifiers()) || m.isSynthetic() || m.getParameterCount() != 0 + || !name.matches("^(get|is)[A-Z].*")) { + continue; + } + Object retVal = null; + try { + retVal = m.invoke(this); + } catch (Exception ex) { + retVal = ex; + } + Object defaultRetVal = null; + try { + defaultRetVal = m.invoke(defaultConfig); + } catch (Exception ex) { + defaultRetVal = ex; + } + methodResults.add(new MethodResult(name, retVal, defaultRetVal)); + } + return methodResults; + } + + @Override + public void initSendable(SendableBuilder builder) { + getMethodResults().forEach(mr -> { + if (!mr.retVal.equals(mr.defaultRetVal)) { + builder.publishConstString("%s()".formatted(mr.methodName), + String.format("%s (default is %s)", mr.retVal, mr.defaultRetVal)); + } + }); + } + + @Override + public String toString() { + StringBuilder stringBuilder = new StringBuilder(); + getMethodResults().forEach(mr -> { + if (!mr.retVal.equals(mr.defaultRetVal)) { + stringBuilder.append( + String.format("%s() returns %s (default is %s)", mr.methodName, mr.retVal, mr.defaultRetVal)); + } + }); + if (stringBuilder.isEmpty()) { + stringBuilder.append("Using default config values"); + } else { + stringBuilder.insert(0, "WARNING: USING OVERRIDDEN CONFIG VALUES\n"); + } + return stringBuilder.toString(); + } +} \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index cd69add..466b516 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -1,8 +1,16 @@ package org.carlmontrobotics; +import org.carlmontrobotics.lib199.swerve.SwerveConfig; + +import com.pathplanner.lib.path.PathConstraints; + +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.XboxController.Axis; import edu.wpi.first.wpilibj.XboxController.Button; +import static org.carlmontrobotics.Config.CONFIG; + public final class Constants { // public static final class Drivetrain { // public static final double MAX_SPEED_MPS = 2; @@ -20,7 +28,7 @@ public final class Constants { public static final class OI { public static final double MIN_AXIS_TRIGGER_VALUE = 0.2; - + public static final double JOY_THRESH = 0.01; public static final class Driver { public static final int driverPort = 0; public static final int effectorMotorID = 4; @@ -59,6 +67,192 @@ public static final class AlgaeEffectorc { public static double outtakePincherRPM = 2100; } + + public static final class Drivetrainc { + // #region Subsystem Constants + public static final double wheelBase = 24.75; //CONFIG.isSwimShady() ? Units.inchesToMeters(19.75) : Units.inchesToMeters(16.75); + public static final double trackWidth = 24.75;//CONFIG.isSwimShady() ? Units.inchesToMeters(28.75) : Units.inchesToMeters(23.75); + // "swerveRadius" is the distance from the center of the robot to one of the + // modules + public static final double swerveRadius = Math.sqrt(Math.pow(wheelBase / 2, 2) + Math.pow(trackWidth / 2, 2)); + // The gearing reduction from the drive motor controller to the wheels + // Gearing for the Swerve Modules is 6.75 : 1e + public static final double driveGearing = 6.75; + // Turn motor shaft to "module shaft" + public static final double turnGearing = 150.0 / 7; + + public static final double driveModifier = 1; + public static final double wheelDiameterMeters = Units.inchesToMeters(4.0) * 7.36 / 7.65 /* + * empirical + * correction + */; + public static final double mu = 1; /* 70/83.2; */ + + public static final double NEOFreeSpeed = 5676 * (2 * Math.PI) / 60; // radians/s + // Angular speed to translational speed --> v = omega * r / gearing + public static final double maxSpeed = NEOFreeSpeed * (wheelDiameterMeters / 2.0) / driveGearing; + public static final double maxForward = maxSpeed; // todo: use smart dashboard to figure this out + public static final double maxStrafe = maxSpeed; // todo: use smart dashboard to figure this out + // seconds it takes to go from 0 to 12 volts(aka MAX) + public static final double secsPer12Volts = 0.1; + + // maxRCW is the angular velocity of the robot. + // Calculated by looking at one of the motors and treating it as a point mass + // moving around in a circle. + // Tangential speed of this point mass is maxSpeed and the radius of the circle + // is sqrt((wheelBase/2)^2 + (trackWidth/2)^2) + // Angular velocity = Tangential speed / radius + public static final double maxRCW = maxSpeed / swerveRadius; + + public static final boolean[] reversed = { false, false, false, false }; + // public static final boolean[] reversed = {true, true, true, true}; + // Determine correct turnZero constants (FL, FR, BL, BR) + public static final double[] turnZeroDeg = RobotBase.isSimulation() ? new double[] {-90.0, -90.0, -90.0, -90.0 } + : (CONFIG.isSwimShady() ? new double[] { 85.7812, 85.0782, -96.9433, -162.9492 } + : new double[] { 17.2266, -96.8555, -95.8008, 85.166 });/* real values here */ + + // kP, kI, and kD constants for turn motor controllers in the order of + // front-left, front-right, back-left, back-right. + // Determine correct turn PID constants + public static final double[] turnkP = {12, 12, 23, 23};//sysid for fr that didnt't work{0.099412, 0.13414, 3.6809, 3.6809} //{49, 23,33, 28};//{51.078, 25, 35.946, 30.986}; // {0.00374, 0.00374, 0.00374, + // 0.00374}; + public static final double[] turnkI = {0, 0, 0, 0};//{ 0, 0.1, 0, 0 }; + public static final double[] turnkD = {1, 1.55, 0, 2};//{ 0.2/* dont edit */, 0.3, 0.5, 0.4}; // todo: use d + // public static final double[] turnkS = {0.2, 0.2, 0.2, 0.2}; + public static final double[] turnkS = {0.050634, 0.033065, 0.018117, 0.021337};//sysid for fr that didnt't work{0.041796, 0.09111, 0.64804, 1.0873}//{ 0.13027, 0.17026, 0.2, 0.23262 }; + + // V = kS + kV * v + kA * a + // 12 = 0.2 + 0.00463 * v + // v = (12 - 0.2) / 0.00463 = 2548.596 degrees/s + public static final double[] turnkV = {2.6153, 2.5924, 2.6495, 2.6705};//sysid for fr that didnt't work{2.6403, 2.6603, 2.6168, 2.5002} //{2.6532, 2.7597, 2.7445, 2.7698}; + public static final double[] turnkA = {0.18525, 0.13879, 0.23625, 0.25589};//sysid for fr that didnt't work{0.33266, 0.25535, 0.17924, 0.17924} //{ 0.17924, 0.17924, 0.17924, 0.17924 }; + + // kP is an average of the forward and backward kP values + // Forward: 1.72, 1.71, 1.92, 1.94 + // Backward: 1.92, 1.92, 2.11, 1.89 + // Order of modules: (FL, FR, BL, BR) + public static final double[] drivekP = CONFIG.isSwimShady() ? new double[] { 2.8, 2.8, 2.8, 2.8 } + : new double[] {1.75, 1.75, 1.75, 1.75}; //{2.2319, 2.2462, 2.4136, 3.6862}; // {1.82/100, 1.815/100, 2.015/100, + // 1.915/100}; + public static final double[] drivekI = { 0.1, 0.1, 0.1, 0.1 }; + public static final double[] drivekD = { 0, 0, 0, 0 }; + public static final boolean[] driveInversion = (CONFIG.isSwimShady() + ? new boolean[] { false, false, false, false } + : new boolean[] { false, true, false, true }); + public static final boolean[] turnInversion = { true, true, true, true }; + // kS + // public static final double[] kForwardVolts = { 0.26744, 0.31897, 0.27967, 0.2461 }; + public static final double[] kForwardVolts = {0, 0, 0, 0}; //{0.59395, 0.52681, 0.11097, 0.17914}; //{ 0.2, 0.2, 0.2, 0.2 }; + public static final double[] kBackwardVolts = kForwardVolts; + + //kV + // public static final double[] kForwardVels = { 2.81, 2.9098, 2.8378, 2.7391 }; + public static final double[] kForwardVels = { 0, 0, 0, 0 };//{2.4114, 2.7465, 2.7546, 2.7412}; //{ 0, 0, 0, 0 };//volts per m/s + public static final double[] kBackwardVels = kForwardVels; + + //kA + // public static final double[] kForwardAccels = { 1.1047 / 2, 0.79422 / 2, 0.77114 / 2, 1.1003 / 2 }; + public static final double[] kForwardAccels = { 0, 0, 0, 0 };//{0.31958, 0.33557, 0.70264, 0.46644}; //{ 0, 0, 0, 0 };// volts per m/s^2 + public static final double[] kBackwardAccels = kForwardAccels; + + public static final double autoMaxSpeedMps = 0.35 * 4.4; // Meters / second + public static final double autoMaxAccelMps2 = mu * 9.81; // Meters / seconds^2 + public static final double autoMaxVolt = 10.0; // For Drivetrain voltage constraint in RobotPath.java + // The maximum acceleration the robot can achieve is equal to the coefficient of + // static friction times the gravitational acceleration + // a = mu * 9.8 m/s^2 + public static final double autoCentripetalAccel = mu * 9.81 * 2; + + public static final boolean isGyroReversed = true; + + // PID values are listed in the order kP, kI, and kD + public static final double[] xPIDController = CONFIG.isSwimShady() ? new double[] { 4, 0.0, 0.0 } + : new double[] { 2, 0.0, 0.0 }; + public static final double[] yPIDController = xPIDController; + public static final double[] thetaPIDController = CONFIG.isSwimShady() ? new double[] { 0.10, 0.0, 0.001 } + : new double[] {0.05, 0.0, 0.00}; + + public static final SwerveConfig swerveConfig = new SwerveConfig(wheelDiameterMeters, driveGearing, mu, + autoCentripetalAccel, kForwardVolts, kForwardVels, kForwardAccels, kBackwardVolts, kBackwardVels, + kBackwardAccels, drivekP, drivekI, drivekD, turnkP, turnkI, turnkD, turnkS, turnkV, turnkA, turnZeroDeg, + driveInversion, reversed, driveModifier, turnInversion); + + // public static final Limelight.Transform limelightTransformForPoseEstimation = + // Transform.BOTPOSE_WPIBLUE; + + // #endregion + + // #region Ports + //I think all of these are right + public static final int driveFrontLeftPort = 11; + public static final int driveFrontRightPort = 12; + public static final int driveBackLeftPort = 13; + public static final int driveBackRightPort = 14; + + public static final int turnFrontLeftPort = 1; + public static final int turnFrontRightPort = 2; + public static final int turnBackLeftPort = 3; + public static final int turnBackRightPort = 4; + //to be configured + public static final int canCoderPortFL = 1; //0 + public static final int canCoderPortFR = 2; + public static final int canCoderPortBL = 3; + public static final int canCoderPortBR = 0; //1 + + // #endregion + + // #region Command Constants + + public static double kNormalDriveSpeed = 1; // Percent Multiplier + public static double kNormalDriveRotation = 0.5; // Percent Multiplier + public static double kSlowDriveSpeed = 0.4; // Percent Multiplier + public static double kSlowDriveRotation = 0.250; // Percent Multiplier + + //baby speed values, i just guessed the percent multiplier. TODO: find actual ones we wana use + public static double kBabyDriveSpeed = 0.3; + public static double kBabyDriveRotation = 0.2; + public static double kAlignMultiplier = 1D / 3D; + public static final double kAlignForward = 0.6; + + public static final double wheelTurnDriveSpeed = 0.0001; // Meters / Second ; A non-zero speed just used to + // orient the wheels to the correct angle. This + // should be very small to avoid actually moving the + // robot. + + public static final double[] positionTolerance = { Units.inchesToMeters(.5), Units.inchesToMeters(.5), 5 }; // Meters, + // Meters, + // Degrees + public static final double[] velocityTolerance = { Units.inchesToMeters(1), Units.inchesToMeters(1), 5 }; // Meters, + // Meters, + // Degrees/Second + + // #endregion + // #region Subsystem Constants + + // "swerveRadius" is the distance from the center of the robot to one of the + // modules + public static final double turnkP_avg = (turnkP[0] + turnkP[1] + turnkP[2] + turnkP[3]) / 4; + public static final double turnIzone = .1; + + public static final double driveIzone = .1; + + public static final class Autoc { + // public static final RobotConfig robotConfig = new RobotConfig( /* + // * put in + // * Constants.Drivetrain.Auto + // */ + // false, // replan at start of path if robot not at start of path? + // false, // replan if total error surpasses total error/spike threshold? + // 1.5, // total error threshold in meters that will cause the path to be replanned + // 0.8 // error spike threshold, in meters, that will cause the path to be replanned + // ); + public static final PathConstraints pathConstraints = new PathConstraints(1.54, 6.86, 2 * Math.PI, + 2 * Math.PI); // The constraints for this path. If using a differential drivetrain, the + // angular constraints have no effect. + } + } + + public static final class CoralEffectorc { public static final double kP = 0.0; diff --git a/src/main/java/org/carlmontrobotics/commands/RotateToFieldRelativeAngle.java b/src/main/java/org/carlmontrobotics/commands/RotateToFieldRelativeAngle.java new file mode 100644 index 0000000..d1fb25b --- /dev/null +++ b/src/main/java/org/carlmontrobotics/commands/RotateToFieldRelativeAngle.java @@ -0,0 +1,51 @@ +package org.carlmontrobotics.commands; + +import static org.carlmontrobotics.Constants.Drivetrainc.*; + +import org.carlmontrobotics.subsystems.Drivetrain; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.util.sendable.SendableRegistry; +import edu.wpi.first.wpilibj2.command.Command; + +public class RotateToFieldRelativeAngle extends Command { + + public final TeleopDrive teleopDrive; + public final Drivetrain drivetrain; + + public final PIDController rotationPID = new PIDController(thetaPIDController[0], thetaPIDController[1], + thetaPIDController[2]); + + public RotateToFieldRelativeAngle(Rotation2d angle, Drivetrain drivetrain) { + this.drivetrain = drivetrain; + this.teleopDrive = (TeleopDrive) drivetrain.getDefaultCommand(); + + rotationPID.enableContinuousInput(-180, 180); + rotationPID.setSetpoint(MathUtil.inputModulus(angle.getDegrees(), -180, 180)); + rotationPID.setTolerance(positionTolerance[2], velocityTolerance[2]); + SendableRegistry.addChild(this, rotationPID); + // SmartDashboard.pu + + addRequirements(drivetrain); + } + + @Override + public void execute() { + if (teleopDrive == null) + drivetrain.drive(0, 0, rotationPID.calculate(drivetrain.getHeading())); + else { + double[] driverRequestedSpeeds = teleopDrive.getRequestedSpeeds(); + drivetrain.drive(driverRequestedSpeeds[0], driverRequestedSpeeds[1], + rotationPID.calculate(drivetrain.getHeading())); + } + } + + @Override + public boolean isFinished() { + //SmartDashboard.putBoolean("At Setpoint", rotationPID.atSetpoint()); + //SmartDashboard.putNumber("Error", rotationPID.getPositionError()); + return rotationPID.atSetpoint(); + } +} \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/commands/TeleopDrive.java b/src/main/java/org/carlmontrobotics/commands/TeleopDrive.java new file mode 100644 index 0000000..1689d27 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/commands/TeleopDrive.java @@ -0,0 +1,152 @@ +package org.carlmontrobotics.commands; + +import static org.carlmontrobotics.Constants.Drivetrainc.*; + +import java.util.function.BooleanSupplier; +import java.util.function.DoubleSupplier; + +import org.carlmontrobotics.Constants; +import org.carlmontrobotics.Robot; +import org.carlmontrobotics.subsystems.Drivetrain; +import static org.carlmontrobotics.RobotContainer.*; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; + +public class TeleopDrive extends Command { + + private static double robotPeriod = Robot.kDefaultPeriod; + private final Drivetrain drivetrain; + private DoubleSupplier fwd; + private DoubleSupplier str; + private DoubleSupplier rcw; + private BooleanSupplier slow; + private double currentForwardVel = 0; + private double currentStrafeVel = 0; + private double prevTimestamp; + private boolean babyMode; + /** + * Creates a new TeleopDrive. + */ + public TeleopDrive(Drivetrain drivetrain, DoubleSupplier fwd, DoubleSupplier str, DoubleSupplier rcw, + BooleanSupplier slow) { + addRequirements(this.drivetrain = drivetrain); + this.fwd = fwd; + this.str = str; + this.rcw = rcw; + this.slow = slow; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + // SmartDashboard.putNumber("slow turn const", kSlowDriveRotation); + // SmartDashboard.putNumber("slow speed const", kSlowDriveSpeed); + // SmartDashboard.putNumber("normal turn const", kNormalDriveRotation); + // SmartDashboard.putNumber("normal speed const", kNormalDriveSpeed); + prevTimestamp = Timer.getFPGATimestamp(); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + double currentTime = Timer.getFPGATimestamp(); + robotPeriod = currentTime - prevTimestamp; + double[] speeds = getRequestedSpeeds(); + // SmartDashboard.putNumber("Elapsed time", currentTime - prevTimestamp); + prevTimestamp = currentTime; + babyMode = SmartDashboard.getBoolean("babymode", false); + // kSlowDriveRotation = SmartDashboard.getNumber("slow turn const", kSlowDriveRotation); + // kSlowDriveSpeed = SmartDashboard.getNumber("slow speed const", kSlowDriveSpeed); + // kNormalDriveRotation = SmartDashboard.getNumber("normal turn const", kNormalDriveRotation); + // kNormalDriveSpeed = SmartDashboard.getNumber("normal speed const", kNormalDriveSpeed); + + // SmartDashboard.putNumber("fwd", speeds[0]); + // SmartDashboard.putNumber("strafe", speeds[1]); + // SmartDashboard.putNumber("turn", speeds[2]); + drivetrain.drive(speeds[0], speeds[1], speeds[2]); + } + + public double[] getRequestedSpeeds() { + // Sets all values less than or equal to a very small value (determined by the + // idle joystick state) to zero. + // Used to make sure that the robot does not try to change its angle unless it + // is moving, + double forward = fwd.getAsDouble(); + double strafe = str.getAsDouble(); + double rotateClockwise = rcw.getAsDouble(); + // SmartDashboard.putNumber("fwdIN", forward); + // SmartDashboard.putNumber("strafeIN", strafe); + // SmartDashboard.putNumber("turnIN", rotateClockwise); + if (Math.abs(forward) <= Constants.OI.JOY_THRESH) + forward = 0.0; + else + forward *= maxForward; + if (Math.abs(strafe) <= Constants.OI.JOY_THRESH) + strafe = 0.0; + else + strafe *= maxStrafe; + if (Math.abs(rotateClockwise) <= Constants.OI.JOY_THRESH) + rotateClockwise = 0.0; + else + rotateClockwise *= maxRCW; + + double driveMultiplier = slow.getAsBoolean() ? kSlowDriveSpeed : kNormalDriveSpeed; + double rotationMultiplier = slow.getAsBoolean() ? kSlowDriveRotation : kNormalDriveRotation; + if(babyMode == true){ + driveMultiplier = kBabyDriveSpeed; + rotationMultiplier = kBabyDriveRotation; + } + // double driveMultiplier = kNormalDriveSpeed; + // double rotationMultiplier = kNormalDriveRotation; + + forward *= driveMultiplier; + strafe *= driveMultiplier; + rotateClockwise *= rotationMultiplier; + + // Limit acceleration of the robot + double accelerationX = (forward - currentForwardVel) / robotPeriod; + double accelerationY = (strafe - currentStrafeVel) / robotPeriod; + double translationalAcceleration = Math.hypot(accelerationX, accelerationY); + // SmartDashboard.putNumber("Translational Acceleration", translationalAcceleration); + if (translationalAcceleration > autoMaxAccelMps2 && false) { + Translation2d limitedAccelerationVector = new Translation2d(autoMaxAccelMps2, + Rotation2d.fromRadians(Math.atan2(accelerationY, accelerationX))); + Translation2d limitedVelocityVector = limitedAccelerationVector.times(robotPeriod); + currentForwardVel += limitedVelocityVector.getX(); + currentStrafeVel += limitedVelocityVector.getY(); + } else { + currentForwardVel = forward; + currentStrafeVel = strafe; + } + // SmartDashboard.putNumber("current velocity", Math.hypot(currentForwardVel, currentStrafeVel)); + + // ATM, there is no rotational acceleration limit + // currentForwardVel = forward; + // currentStrafeVel = strafe; + // If the above math works, no velocity should be greater than the max velocity, + // so we don't need to limit it. + + return new double[] { currentForwardVel, currentStrafeVel, -rotateClockwise }; + } + + public boolean hasDriverInput() { + return Math.abs(fwd.getAsDouble()) > Constants.OI.JOY_THRESH + || Math.abs(str.getAsDouble()) > Constants.OI.JOY_THRESH + || Math.abs(rcw.getAsDouble()) > Constants.OI.JOY_THRESH; + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java new file mode 100644 index 0000000..a4f91e5 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java @@ -0,0 +1,1153 @@ +//Resolve 71 errors starting from Holonomics + +package org.carlmontrobotics.subsystems; + +import static org.carlmontrobotics.Constants.Drivetrainc.*; + +import java.util.Arrays; +import java.util.Map; +import java.util.function.Supplier; + +import org.carlmontrobotics.Constants; +import org.carlmontrobotics.Constants.Drivetrainc; +import org.carlmontrobotics.Constants.Drivetrainc.Autoc; +import org.carlmontrobotics.Robot; +// import org.carlmontrobotics.commands.RotateToFieldRelativeAngle; +import org.carlmontrobotics.commands.TeleopDrive; +import org.carlmontrobotics.lib199.MotorConfig; +import org.carlmontrobotics.lib199.MotorControllerFactory; +import org.carlmontrobotics.lib199.MotorErrors; +import org.carlmontrobotics.lib199.SensorFactory; +import org.carlmontrobotics.lib199.swerve.SwerveModule; +import static org.carlmontrobotics.Config.CONFIG; + +import com.ctre.phoenix6.hardware.CANcoder; +//import com.kauailabs.navx.frc.AHRS; +import com.studica.frc.AHRS; +import com.studica.frc.AHRS.NavXComType; +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.controllers.PPHolonomicDriveController; + +// import com.pathplanner.lib.util.HolonomicPathFollowerConfig; +import com.pathplanner.lib.config.PIDConstants; +import com.pathplanner.lib.config.RobotConfig; + +import org.carlmontrobotics.lib199.swerve.SwerveModuleSim; + +import com.revrobotics.spark.ClosedLoopSlot; +import com.revrobotics.spark.SparkClosedLoopController; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkBase.ControlType; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.config.SparkBaseConfig; +import com.revrobotics.spark.config.SparkMaxConfig; +import com.revrobotics.spark.config.ClosedLoopConfig; +import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.hal.SimDouble; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Twist2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveDriveOdometry; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +// import edu.wpi.first.units.Angle; +// import edu.wpi.first.units.Distance; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.units.measure.LinearVelocity; +import edu.wpi.first.units.measure.MutAngle; +import edu.wpi.first.units.measure.MutAngularVelocity; +import edu.wpi.first.units.measure.MutDistance; +import edu.wpi.first.units.measure.MutLinearVelocity; +import edu.wpi.first.units.measure.MutVoltage; +import edu.wpi.first.units.Measure; +import edu.wpi.first.units.MutableMeasure; +// import edu.wpi.first.units.Velocity; +// import edu.wpi.first.units.Voltage; +import edu.wpi.first.units.measure.Velocity; +import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.util.sendable.SendableBuilder; +import edu.wpi.first.util.sendable.SendableRegistry; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.SerialPort; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.PrintCommand; +import edu.wpi.first.wpilibj2.command.SelectCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.simulation.SimDeviceSim; + +import static edu.wpi.first.units.Units.Volts; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.Rotation; +import static edu.wpi.first.units.Units.RotationsPerSecond; +import static edu.wpi.first.units.Units.Seconds; +import static edu.wpi.first.units.Units.Volt; +import static edu.wpi.first.units.Units.Meter; +import org.carlmontrobotics.commands.RotateToFieldRelativeAngle; +// Make sure this code is extraneous +// import static edu.wpi.first.units.MutableMeasure.mutable; +import static edu.wpi.first.units.Units.Meters; +public class Drivetrain extends SubsystemBase { + private final AHRS gyro = new AHRS(NavXComType.kMXP_SPI); + private Pose2d autoGyroOffset = new Pose2d(0., 0., new Rotation2d(0.)); + // ^used by PathPlanner for chaining paths + + private SwerveDriveKinematics kinematics = null; + // private SwerveDriveOdometry odometry = null; + private SwerveDrivePoseEstimator poseEstimator = null; + + private SwerveModule modules[]; + private boolean fieldOriented = true; + private double fieldOffset = 0; + // FIXME not for permanent use!! + private SparkMax[] driveMotors = new SparkMax[] { null, null, null, null }; + private SparkMax[] turnMotors = new SparkMax[] { null, null, null, null }; + private CANcoder[] turnEncoders = new CANcoder[] { null, null, null, null }; + private final SparkClosedLoopController[] turnPidControllers = new SparkClosedLoopController[] {null, null, null, null}; + public final float initPitch; + public final float initRoll; + + // debug purposes + private SwerveModule moduleFL; + private SwerveModule moduleFR; + private SwerveModule moduleBL; + private SwerveModule moduleBR; + + private final Field2d field = new Field2d(); + private SwerveModuleSim[] moduleSims; + private SimDouble gyroYawSim; + private Timer simTimer = new Timer(); + + private double lastSetX = 0, lastSetY = 0, lastSetTheta = 0; + double kP = 0; + double kI = 0; + double kD = 0; + public Drivetrain() { + SmartDashboard.putNumber("Goal Velocity", 0); + SmartDashboard.putNumber("kP", 0); + SmartDashboard.putNumber("kI", 0); + SmartDashboard.putNumber("kD", 0); + + // SmartDashboard.putNumber("Pose Estimator t x (m)", lastSetX); + // SmartDashboard.putNumber("Pose Estimator set y (m)", lastSetY); + // SmartDashboard.putNumber("Pose Estimator set rotation (deg)", + // lastSetTheta); + + // SmartDashboard.putNumber("pose estimator std dev x", STD_DEV_X_METERS); + // SmartDashboard.putNumber("pose estimator std dev y", STD_DEV_Y_METERS); + SmartDashboard.putNumber("GoalPos", 0); + // Calibrate Gyro + { + + double initTimestamp = Timer.getFPGATimestamp(); + double currentTimestamp = initTimestamp; + while (gyro.isCalibrating() && currentTimestamp - initTimestamp < 10) { + currentTimestamp = Timer.getFPGATimestamp(); + try { + Thread.sleep(1000);// 1 second + } catch (InterruptedException e) { + e.printStackTrace(); + break; + } + System.out.println("Calibrating the gyro..."); + } + gyro.reset(); + // this.resetFieldOrientation(); + System.out.println("NavX-MXP firmware version: " + gyro.getFirmwareVersion()); + System.out.println("Magnetometer is calibrated: " + gyro.isMagnetometerCalibrated()); + } + + // Setup Kinematics + { + // Define the corners of the robot relative to the center of the robot using + // Translation2d objects. + // Positive x-values represent moving toward the front of the robot whereas + // positive y-values represent moving toward the left of the robot. + Translation2d locationFL = new Translation2d(wheelBase / 2, trackWidth / 2); + Translation2d locationFR = new Translation2d(wheelBase / 2, -trackWidth / 2); + Translation2d locationBL = new Translation2d(-wheelBase / 2, trackWidth / 2); + Translation2d locationBR = new Translation2d(-wheelBase / 2, -trackWidth / 2); + + kinematics = new SwerveDriveKinematics(locationFL, locationFR, locationBL, locationBR); + } + + // Initialize modules + { + // initPitch = 0; + // initRoll = 0; + Supplier pitchSupplier = () -> 0F; + Supplier rollSupplier = () -> 0F; + initPitch = gyro.getPitch(); + initRoll = gyro.getRoll(); + // Supplier pitchSupplier = () -> gyro.getPitch(); + // Supplier rollSupplier = () -> gyro.getRoll(); + + + moduleFL = new SwerveModule(Constants.Drivetrainc.swerveConfig, SwerveModule.ModuleType.FL, + driveMotors[0] = new SparkMax(driveFrontLeftPort, MotorType.kBrushless), + turnMotors[0] = new SparkMax(turnFrontLeftPort, MotorType.kBrushless), + turnEncoders[0] = SensorFactory.createCANCoder(Constants.Drivetrainc.canCoderPortFL), 0, pitchSupplier, rollSupplier); + SmartDashboard.putNumber("FL Motor Val", turnMotors[0].getEncoder().getPosition()); + moduleFR = new SwerveModule(Constants.Drivetrainc.swerveConfig, SwerveModule.ModuleType.FR, + driveMotors[1] = new SparkMax(driveFrontRightPort, MotorType.kBrushless), + turnMotors[1] = new SparkMax(turnFrontRightPort, MotorType.kBrushless), + turnEncoders[1] = SensorFactory.createCANCoder(Constants.Drivetrainc.canCoderPortFR), 1, pitchSupplier, rollSupplier); + + moduleBL = new SwerveModule(Constants.Drivetrainc.swerveConfig, SwerveModule.ModuleType.BL, + driveMotors[2] = new SparkMax(driveBackLeftPort, MotorType.kBrushless), + turnMotors[2] = new SparkMax(turnBackLeftPort, MotorType.kBrushless), + turnEncoders[2] = SensorFactory.createCANCoder(Constants.Drivetrainc.canCoderPortBL), 2, pitchSupplier, rollSupplier); + + moduleBR = new SwerveModule(Constants.Drivetrainc.swerveConfig, SwerveModule.ModuleType.BR, + driveMotors[3] = new SparkMax(driveBackRightPort, MotorType.kBrushless), + turnMotors[3] = new SparkMax(turnBackRightPort, MotorType.kBrushless), + turnEncoders[3] = SensorFactory.createCANCoder(Constants.Drivetrainc.canCoderPortBR), 3, pitchSupplier, rollSupplier); + modules = new SwerveModule[] { moduleFL, moduleFR, moduleBL, moduleBR }; + turnPidControllers[0] = turnMotors[0].getClosedLoopController(); + turnPidControllers[1] = turnMotors[1].getClosedLoopController(); + turnPidControllers[2] = turnMotors[2].getClosedLoopController(); + turnPidControllers[3] = turnMotors[3].getClosedLoopController(); + if (RobotBase.isSimulation()) { + moduleSims = new SwerveModuleSim[] { + moduleFL.createSim(), moduleFR.createSim(), moduleBL.createSim(), moduleBR.createSim() + }; + gyroYawSim = new SimDeviceSim("navX-Sensor[0]").getDouble("Yaw"); + } + SmartDashboard.putData("Module FL",moduleFL); + SmartDashboard.putData("Module FR",moduleFR); + SmartDashboard.putData("Module BL",moduleBL); + SmartDashboard.putData("Module BR",moduleBR); + SparkMaxConfig driveConfig = new SparkMaxConfig(); + driveConfig.openLoopRampRate(secsPer12Volts); + driveConfig.encoder.positionConversionFactor(wheelDiameterMeters * Math.PI / driveGearing); + driveConfig.encoder.velocityConversionFactor(wheelDiameterMeters * Math.PI / driveGearing / 60); + driveConfig.encoder.uvwAverageDepth(2); + driveConfig.encoder.uvwMeasurementPeriod(16); + driveConfig.smartCurrentLimit(MotorConfig.NEO.currentLimitAmps); + + for (SparkMax driveMotor : driveMotors) { + driveMotor.configure(driveConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + } + SparkMaxConfig turnConfig = new SparkMaxConfig(); + turnConfig.encoder.positionConversionFactor(360/turnGearing); + turnConfig.encoder.velocityConversionFactor(360/turnGearing/60); + turnConfig.encoder.uvwAverageDepth(2); + turnConfig.encoder.uvwMeasurementPeriod(16); + + //turnConfig.closedLoop.pid(kP, kI, kD).feedbackSensor(FeedbackSensor.kPrimaryEncoder); + for (SparkMax turnMotor : turnMotors) { + turnMotor.configure(turnConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + } + + for (CANcoder coder : turnEncoders) { + coder.getAbsolutePosition().setUpdateFrequency(500); + coder.getPosition().setUpdateFrequency(500); + coder.getVelocity().setUpdateFrequency(500); + + } + + SmartDashboard.putData("Field", field); + + // for(SparkMax driveMotor : driveMotors) + // driveMotor.setSmartCurrentLimit(80); + + // Must call this method for SysId to run + if (CONFIG.isSysIdTesting()) { + sysIdSetup(); + } + } + + // odometry = new SwerveDriveOdometry(kinematics, + // Rotation2d.fromDegrees(getHeading()), getModulePositions(), + // new Pose2d()); + + poseEstimator = new SwerveDrivePoseEstimator( + getKinematics(), + Rotation2d.fromDegrees(getHeading()), + getModulePositions(), + new Pose2d()); + + // Setup autopath builder + //configurePPLAutoBuilder(); + // SmartDashboard.putNumber("chassis speeds x", 0); + // SmartDashboard.putNumber("chassis speeds y", 0); + + // SmartDashboard.putNumber("chassis speeds theta", 0); + // SmartDashboard.putData(this); + + } + + @Override + public void simulationPeriodic() { + for (var moduleSim : moduleSims) { + moduleSim.update(); + } + SwerveModuleState[] measuredStates = + new SwerveModuleState[] { + moduleFL.getCurrentState(), moduleFR.getCurrentState(), moduleBL.getCurrentState(), moduleBR.getCurrentState() + }; + ChassisSpeeds speeds = kinematics.toChassisSpeeds(measuredStates); + + double dtSecs = simTimer.get(); + simTimer.restart(); + + Pose2d simPose = field.getRobotPose(); + simPose = simPose.exp( + new Twist2d( + speeds.vxMetersPerSecond * dtSecs, + speeds.vyMetersPerSecond * dtSecs, + speeds.omegaRadiansPerSecond * dtSecs)); + double newAngleDeg = simPose.getRotation().getDegrees(); + // Subtract the offset computed the last time setPose() was called because odometry.update() adds it back. + newAngleDeg -= simGyroOffset.getDegrees(); + newAngleDeg *= (isGyroReversed ? -1.0 : 1.0); + gyroYawSim.set(newAngleDeg); + } + + // public Command sysIdQuasistatic(SysIdRoutine.Direction direction, int + // frontorback) { + // switch(frontorback) { + // case 0: + // return frontOnlyRoutine.quasistatic(direction); + // case 1: + // return backOnlyRoutine.quasistatic(direction); + // case 2: + // return allWheelsRoutine.quasistatic(direction); + // } + // return new PrintCommand("Invalid Command"); + // } + + @Override + public void periodic() { + // SmartDashboard.getNumber("GoalPos", turnEncoders[0].getVelocity().getValueAsDouble()); + // SmartDashboard.putNumber("FL Motor Val", turnMotors[0].getEncoder().getPosition()); + // double goal = SmartDashboard.getNumber("GoalPos", 0); + // PIDController pid = new PIDController(kP, kI, kD); + // kP = SmartDashboard.getNumber("kP", 0); + // kI = SmartDashboard.getNumber("kI", 0); + // kD = SmartDashboard.getNumber("kD", 0); + //pid.setIZone(20); + //SmartDashboard.putBoolean("atgoal", pid.atSetpoint()); + // SparkMaxConfig config = new SparkMaxConfig(); + + //config.closedLoop.feedbackSensor(ClosedLoopConfig.FeedbackSensor.kPrimaryEncoder); + // System.out.println(kP); + // config.closedLoop.pid(kP + // ,kI,kD); + // config.encoder.positionConversionFactor(360/Constants.Drivetrainc.turnGearing); + // turnMotors[0].configure(config, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + // //moduleFL.move(0.0000001, 180); + //moduleFL.move(0.01, 180); + // moduleFR.move(0.000000001, 0); + // moduleBR.move(0.0000001, 0); + // moduleFL.move(0.000001, 0); + // moduleBL.move(0.000001, 0); + // turnPidControllers[0].setReference(goal + + // , ControlType.kPosition, ClosedLoopSlot.kSlot0); + + + // 167 -> -200 + // 138 -> 360 + // for (CANcoder coder : turnEncoders) { + // SignalLogger.writeDouble("Regular position " + coder.toString(), + // coder.getPosition().getValue()); + // SignalLogger.writeDouble("Velocity " + coder.toString(), + // coder.getVelocity().getValue()); + // SignalLogger.writeDouble("Absolute position " + coder.toString(), + // coder.getAbsolutePosition().getValue()); + // } + // lobotomized to prevent ucontrollabe swerve behavior + // turnMotors[2].setVoltage(SmartDashboard.getNumber("kS", 0)); + // moduleFL.periodic(); + // moduleFR.periodic(); + // moduleBL.periodic(); + // moduleBR.periodic(); + // double goal = SmartDashboard.getNumber("bigoal", 0); + for (SwerveModule module : modules) { + //module.turnPeriodic(); + // module.turnPeriodic(); + module.periodic(); + // module.move(0, goal); + } + + // field.setRobotPose(odometry.getPoseMeters()); + + field.setRobotPose(poseEstimator.getEstimatedPosition()); + + // odometry.update(gyro.getRotation2d(), getModulePositions()); + + poseEstimator.update(gyro.getRotation2d(), getModulePositions()); + //odometry.update(Rotation2d.fromDegrees(getHeading()), getModulePositions()); + + // updateMT2PoseEstimator(); + + // double currSetX = + // SmartDashboard.getNumber("Pose Estimator set x (m)", lastSetX); + // double currSetY = + // SmartDashboard.getNumber("Pose Estimator set y (m)", lastSetY); + // double currSetTheta = SmartDashboard + // .getNumber("Pose Estimator set rotation (deg)", lastSetTheta); + + // if (lastSetX != currSetX || lastSetY != currSetY + // || lastSetTheta != currSetTheta) { + // setPose(new Pose2d(currSetX, currSetY, + // Rotation2d.fromDegrees(currSetTheta))); + // } + + // setPose(new Pose2d(getPose().getTranslation().getX(), + // getPose().getTranslation().getY(), + // Rotation2d.fromDegrees(getHeading()))); + + // // // SmartDashboard.putNumber("Pitch", gyro.getPitch()); + // // // SmartDashboard.putNumber("Roll", gyro.getRoll()); + // SmartDashboard.putNumber("Raw gyro angle", gyro.getAngle()); + // SmartDashboard.putNumber("Robot Heading", getHeading()); + // // // SmartDashboard.putNumber("AdjRoll", gyro.getPitch() - initPitch); + // // // SmartDashboard.putNumber("AdjPitch", gyro.getRoll() - initRoll); + // SmartDashboard.putBoolean("Field Oriented", fieldOriented); + // SmartDashboard.putNumber("Gyro Compass Heading", gyro.getCompassHeading()); + // SmartDashboard.putNumber("Compass Offset", compassOffset); + // SmartDashboard.putBoolean("Current Magnetic Field Disturbance", gyro.isMagneticDisturbance()); + SmartDashboard.putNumber("front left encoder", moduleFL.getModuleAngle()); + SmartDashboard.putNumber("front right encoder", moduleFR.getModuleAngle()); + SmartDashboard.putNumber("back left encoder", moduleBL.getModuleAngle()); + SmartDashboard.putNumber("back right encoder", moduleBR.getModuleAngle()); + } + + @Override + public void initSendable(SendableBuilder builder) { + super.initSendable(builder); + + for (SwerveModule module : modules) + SendableRegistry.addChild(this, module); + + builder.addBooleanProperty("Magnetic Field Disturbance", + gyro::isMagneticDisturbance, null); + builder.addBooleanProperty("Gyro Calibrating", gyro::isCalibrating, null); + builder.addBooleanProperty("Field Oriented", () -> fieldOriented, + fieldOriented -> this.fieldOriented = fieldOriented); + builder.addDoubleProperty("Pose Estimator X", () -> getPose().getX(), + null); + builder.addDoubleProperty("Pose Estimator Y", () -> getPose().getY(), + null); + builder.addDoubleProperty("Pose Estimator Theta", + () -> + getPose().getRotation().getDegrees(), null); + builder.addDoubleProperty("Robot Heading", () -> getHeading(), null); + builder.addDoubleProperty("Raw Gyro Angle", gyro::getAngle, null); + builder.addDoubleProperty("Pitch", gyro::getPitch, null); + builder.addDoubleProperty("Roll", gyro::getRoll, null); + builder.addDoubleProperty("Field Offset", () -> fieldOffset, fieldOffset -> + this.fieldOffset = fieldOffset); + builder.addDoubleProperty("FL Turn Encoder (Deg)", + () -> moduleFL.getModuleAngle(), null); + builder.addDoubleProperty("FR Turn Encoder (Deg)", + () -> moduleFR.getModuleAngle(), null); + builder.addDoubleProperty("BL Turn Encoder (Deg)", + () -> moduleBL.getModuleAngle(), null); + builder.addDoubleProperty("BR Turn Encoder (Deg)", + () -> moduleBR.getModuleAngle(), null); + + } + + // #region Drive Methods + + /** + * Drives the robot using the given x, y, and rotation speed + * + * @param forward The desired forward speed, in m/s. Forward is positive. + * @param strafe The desired strafe speed, in m/s. Left is positive. + * @param rotation The desired rotation speed, in rad/s. Counter clockwise is + * positive + */ + public void drive(double forward, double strafe, double rotation) { + drive(getSwerveStates(forward, strafe, rotation)); + } + + public void drive(SwerveModuleState[] moduleStates) { + SwerveDriveKinematics.desaturateWheelSpeeds(moduleStates, maxSpeed); + for (int i = 0; i < 4; i++) { + // SmartDashboard.putNumber("moduleIn" + Integer.toString(i), moduleStates[i].angle.getDegrees()); + moduleStates[i].optimize(Rotation2d.fromDegrees(modules[i].getModuleAngle())); + // SmartDashboard.putNumber("moduleOT" + Integer.toString(i), moduleStates[i].angle.getDegrees()); + modules[i].move(moduleStates[i].speedMetersPerSecond, moduleStates[i].angle.getDegrees()); + } + } + +// public void configurePPLAutoBuilder() { +// /** +// * PATHPLANNER SETTINGS +// * Robot Width (m): .91 +// * Robot Length(m): .94 +// * Max Module Spd (m/s): 4.30 +// * Default Constraints +// * Max Vel: 1.54, Max Accel: 6.86 +// * Max Angvel: 360, Max AngAccel: 180 (guesses!) +// +// AutoBuilder.configure( +// this::getPose, // :D +// this::setPose, // :D +// this::getSpeeds, // :D +// (ChassisSpeeds cs) -> { +// //cs.vxMetersPerSecond = -cs.vxMetersPerSecond; +// // SmartDashboard.putNumber("chassis speeds x", cs.vxMetersPerSecond); +// // SmartDashboard.putNumber("chassis speeds y", cs.vyMetersPerSecond); +// // SmartDashboard.putNumber("chassis speeds theta", cs.omegaRadiansPerSecond); + +// drive(kinematics.toSwerveModuleStates(cs)); +// }, // :D +// new PPHolonomicDriveController( +// new PIDConstants(xPIDController[0], xPIDController[1], xPIDController[2], 0), //translation (drive) pid vals +// new PIDConstants(thetaPIDController[0], thetaPIDController[1], thetaPIDController[2], 0), //rotation pid vals +// Robot.kDefaultPeriod//robot period +// ), +// Autoc.robotConfig, +// () -> { +// // Boolean supplier that controls when the path will be mirrored for the red alliance +// // This will flip the path being followed to the red side of the field. +// // THE ORIGIN WILL REMAIN ON THE BLUE SIDE +// var alliance = DriverStation.getAlliance(); +// if (alliance.isPresent()) +// return alliance.get() == DriverStation.Alliance.Red; +// //else: +// return false; +// }, // :D +// this // :D +// ); + + + + + /* AutoBuilder.configureHolonomic( + () -> getPose().plus(new Transform2d(autoGyroOffset.getTranslation(),autoGyroOffset.getRotation())),//position supplier + (Pose2d pose) -> { autoGyroOffset=pose.times(-1); }, //position reset (by subtracting current pos) + this::getSpeeds, //chassisSpeed supplier + (ChassisSpeeds cs) -> drive( + cs.vxMetersPerSecond, + -cs.vyMetersPerSecond, + //flipped because drive assumes up is negative, but PPlanner assumes up is positive + cs.omegaRadiansPerSecond + ), + new HolonomicPathFollowerConfig( + new PIDConstants(drivekP[0], drivekI[0], drivekD[0], driveIzone), //translation (drive) pid vals + new PIDConstants(turnkP_avg, 0., 0., turnIzone), //rotation pid vals + maxSpeed, + swerveRadius, + Autoc.replanningConfig, + Robot.robot.getPeriod()//robot period + ), + () -> { + // Boolean supplier that controls when the path will be mirrored for the red alliance + // This will flip the path being followed to the red side of the field. + // THE ORIGIN WILL REMAIN ON THE BLUE SIDE + var alliance = DriverStation.getAlliance(); + if (alliance.isPresent()) + return alliance.get() == DriverStation.Alliance.Red; + //else: + return false; + }, + this + ); + */ + +// } + + public void autoCancelDtCommand() { + if(!(getDefaultCommand() instanceof TeleopDrive) || DriverStation.isAutonomous()) return; + + // Use hasDriverInput to get around acceleration limiting on slowdown + if (((TeleopDrive) getDefaultCommand()).hasDriverInput()) { + Command currentDtCommand = getCurrentCommand(); + if (currentDtCommand != getDefaultCommand() && !(currentDtCommand instanceof RotateToFieldRelativeAngle) + && currentDtCommand != null) { + currentDtCommand.cancel(); + } + } + } + + public void stop() { + for (SwerveModule module : modules) + module.move(0, 0); + } + + public boolean isStopped() { + return Math.abs(getSpeeds().vxMetersPerSecond) < 0.1 && + Math.abs(getSpeeds().vyMetersPerSecond) < 0.1 && + Math.abs(getSpeeds().omegaRadiansPerSecond) < 0.1; + } + + /** + * Constructs and returns a ChassisSpeeds objects using forward, strafe, and + * rotation values. + * + * @param forward The desired forward speed, in m/s. Forward is positive. + * @param strafe The desired strafe speed, in m/s. Left is positive. + * @param rotation The desired rotation speed, in rad/s. Counter clockwise is + * positive. + * @return A ChassisSpeeds object. + */ + private ChassisSpeeds getChassisSpeeds(double forward, double strafe, double rotation) { + ChassisSpeeds speeds; + if (fieldOriented) { + speeds = ChassisSpeeds.fromFieldRelativeSpeeds(forward, strafe, rotation, + Rotation2d.fromDegrees(getHeading())); + } else { + speeds = new ChassisSpeeds(forward, strafe, rotation); + } + return speeds; + } + + /** + * Constructs and returns four SwerveModuleState objects, one for each side, + * using forward, strafe, and rotation values. + * + * @param forward The desired forward speed, in m/s. Forward is positive. + * @param strafe The desired strafe speed, in m/s. Left is positive. + * @param rotation The desired rotation speed, in rad/s. Counter clockwise is + * positive. + * @return A SwerveModuleState array, one for each side of the drivetrain (FL, + * FR, etc.). + */ + private SwerveModuleState[] getSwerveStates(double forward, double strafe, double rotation) { + return kinematics.toSwerveModuleStates(getChassisSpeeds(forward, -strafe, rotation)); + } + + // #endregion + + // #region Getters and Setters + + // returns a value from -180 to 180 + public double getHeading() { + double x = gyro.getAngle(); + if (fieldOriented) + x -= fieldOffset; + return Math.IEEEremainder(x * (isGyroReversed ? -1.0 : 1.0), 360); + } + + public double getHeadingDeg() { + return getHeading();//...wait. + } + + public SwerveModulePosition[] getModulePositions() { + return Arrays.stream(modules).map(SwerveModule::getCurrentPosition).toArray(SwerveModulePosition[]::new); + } + + public Pose2d getPose() { + // return odometry.getPoseMeters(); + return poseEstimator.getEstimatedPosition(); + } + + private Rotation2d simGyroOffset = new Rotation2d(); + public void setPose(Pose2d initialPose) { + Rotation2d gyroRotation = gyro.getRotation2d(); + // odometry.resetPosition(gyroRotation, getModulePositions(), initialPose); + + poseEstimator.resetPosition(gyroRotation, getModulePositions(), initialPose); + // Remember the offset that the above call to resetPosition() will cause the odometry.update() will add to the gyro rotation in the future + // We need the offset so that we can compensate for it during simulationPeriodic(). + simGyroOffset = initialPose.getRotation().minus(gyroRotation); + //odometry.resetPosition(Rotation2d.fromDegrees(getHeading()), getModulePositions(), initialPose); + } + + // Resets the gyro, so that the direction the robotic currently faces is + // considered "forward" + public void resetHeading() { + gyro.reset(); + } + + public double getPitch() { + return gyro.getPitch(); + } + + public double getRoll() { + return gyro.getRoll(); + } + + public boolean getFieldOriented() { + return fieldOriented; + } + + public void setFieldOriented(boolean fieldOriented) { + this.fieldOriented = fieldOriented; + } + + public void resetFieldOrientation() { + fieldOffset = gyro.getAngle(); + } + + public void resetPoseEstimator() { + // odometry.resetPosition(new Rotation2d(), getModulePositions(), new Pose2d()); + + poseEstimator.resetPosition(new Rotation2d(), getModulePositions(), new Pose2d()); + gyro.reset(); + } + + public SwerveDriveKinematics getKinematics() { + return kinematics; + } + + public ChassisSpeeds getSpeeds() { + return kinematics.toChassisSpeeds(Arrays.stream(modules).map(SwerveModule::getCurrentState) + .toArray(SwerveModuleState[]::new)); + } + + public void toggleMode() { + for (SwerveModule module : modules) + module.toggleMode(); + } + + public void brake() { + for (SwerveModule module : modules) + module.brake(); + } + + public void coast() { + for (SwerveModule module : modules) + module.coast(); + } + + public double[][] getPIDConstants() { + return new double[][] { + xPIDController, + yPIDController, + thetaPIDController + }; + } + + // #region SysId Code + + // Mutable holder for unit-safe voltage values, persisted to avoid reallocation. + private final MutVoltage[] m_appliedVoltage = new MutVoltage[8]; + + // Mutable holder for unit-safe linear distance values, persisted to avoid + // reallocation. + private final MutDistance[] m_distance = new MutDistance[4]; + // Mutable holder for unit-safe linear velocity values, persisted to avoid + // reallocation. + private final MutLinearVelocity[] m_velocity = new MutLinearVelocity[4]; + // edu.wpi.first.math.util.Units.Rotations beans; + private final MutAngle[] m_revs = new MutAngle[4]; + private final MutAngularVelocity[] m_revs_vel = new MutAngularVelocity[4]; + + private enum SysIdTest { + FRONT_DRIVE, + BACK_DRIVE, + ALL_DRIVE, + // FLBR_TURN, + // FRBL_TURN, + // ALL_TURN + FL_ROT, + FR_ROT, + BL_ROT, + BR_ROT + } + + private SendableChooser sysIdChooser = new SendableChooser<>(); + + // ROUTINES FOR SYSID + // private SysIdRoutine.Config defaultSysIdConfig = new + // SysIdRoutine.Config(Volts.of(.1).per(Seconds.of(.1)), Volts.of(.6), + // Seconds.of(5)); + private SysIdRoutine.Config defaultSysIdConfig = new SysIdRoutine.Config(Volts.of(1).per(Seconds), + Volts.of(2.891), Seconds.of(10)); + + // DRIVE + private void motorLogShort_drive(SysIdRoutineLog log, int id) { + String name = new String[] { "fl", "fr", "bl", "br" }[id]; + log.motor(name) + .voltage(m_appliedVoltage[id].mut_replace( + driveMotors[id].getBusVoltage() * driveMotors[id].getAppliedOutput(), Volts)) + .linearPosition( + m_distance[id].mut_replace(driveMotors[id].getEncoder().getPosition(), Meters)) + .linearVelocity(m_velocity[id].mut_replace(driveMotors[id].getEncoder().getVelocity(), + MetersPerSecond)); + } + + // Create a new SysId routine for characterizing the drive. + private SysIdRoutine frontOnlyDriveRoutine = new SysIdRoutine( + defaultSysIdConfig, + new SysIdRoutine.Mechanism( + // Tell SysId how to give the driving voltage to the motors. + (Voltage volts) -> { + driveMotors[0].setVoltage(volts.in(Volts)); + driveMotors[1].setVoltage(volts.in(Volts)); + modules[2].coast(); + modules[3].coast(); + }, + log -> {// FRONT + motorLogShort_drive(log, 0);// fl named automatically + motorLogShort_drive(log, 1);// fr + }, + this)); + + private SysIdRoutine backOnlyDriveRoutine = new SysIdRoutine( + defaultSysIdConfig, + new SysIdRoutine.Mechanism( + (Voltage volts) -> { + modules[0].coast(); + modules[1].coast(); + modules[2].brake(); + modules[3].brake(); + driveMotors[2].setVoltage(volts.in(Volts)); + driveMotors[3].setVoltage(volts.in(Volts)); + }, + log -> {// BACK + motorLogShort_drive(log, 2);// bl + motorLogShort_drive(log, 3);// br + }, + this)); + + private SysIdRoutine allWheelsDriveRoutine = new SysIdRoutine( + defaultSysIdConfig, + new SysIdRoutine.Mechanism( + (Voltage volts) -> { + for (SparkMax dm : driveMotors) { + dm.setVoltage(volts.in(Volts)); + } + }, + log -> { + motorLogShort_drive(log, 0);// fl named automatically + motorLogShort_drive(log, 1);// fr + motorLogShort_drive(log, 2);// bl + motorLogShort_drive(log, 3);// br + }, + this)); + + private SysIdRoutine sysidroutshort_turn(int id, String logname) { + return new SysIdRoutine( + defaultSysIdConfig, + // new SysIdRoutine.Config(Volts.of(.1).per(Seconds.of(.1)), Volts.of(.6), + // Seconds.of(3)), + new SysIdRoutine.Mechanism( + (Voltage volts) -> turnMotors[id].setVoltage(volts.in(Volts)), + log -> log.motor(logname + "_turn") + .voltage(m_appliedVoltage[id + 4].mut_replace( + // ^because drivemotors take up the first 4 slots of the unit holders + turnMotors[id].getBusVoltage() * turnMotors[id].getAppliedOutput(), Volts)) + .angularPosition( + m_revs[id].mut_replace(turnEncoders[id].getPosition().getValue())) + .angularVelocity(m_revs_vel[id].mut_replace( + turnEncoders[id].getVelocity().getValueAsDouble(), RotationsPerSecond)), + this)); + } + + // as always, fl/fr/bl/br + private SysIdRoutine[] rotateRoutine = new SysIdRoutine[] { + sysidroutshort_turn(0, "fl"), // woaw, readable code??? + sysidroutshort_turn(1, "fr"), + sysidroutshort_turn(2, "bl"), + sysidroutshort_turn(3, "br") + }; + + private ShuffleboardTab sysIdTab = Shuffleboard.getTab("Drivetrain SysID"); + + // void sysidtabshorthand(String name, SysIdRoutine.Direction dir, int width, + // int height){ + // sysIdTab.add(name, dir).withSize(width, height); + // } + void sysidtabshorthand_qsi(String name, SysIdRoutine.Direction dir) { + sysIdTab.add(name, sysIdQuasistatic(dir)).withSize(2, 1); + } + + void sysidtabshorthand_dyn(String name, SysIdRoutine.Direction dir) { + sysIdTab.add(name, sysIdDynamic(dir)).withSize(2, 1); + } + + private void sysIdSetup() { + // SysId Setup + { + Supplier stopNwait = () -> new SequentialCommandGroup( + new InstantCommand(this::stop), new WaitCommand(2)); + + /* + * Alex's old sysId tests + * sysIdTab.add("All sysid tests", new SequentialCommandGroup( + * new + * SequentialCommandGroup(sysIdQuasistatic(SysIdRoutine.Direction.kForward,2), + * (Command)stopNwait.get()), + * new + * SequentialCommandGroup(sysIdQuasistatic(SysIdRoutine.Direction.kReverse,2), + * (Command)stopNwait.get()), + * new SequentialCommandGroup(sysIdDynamic(SysIdRoutine.Direction.kForward,2), + * (Command)stopNwait.get()), + * new SequentialCommandGroup(sysIdDynamic(SysIdRoutine.Direction.kReverse,2), + * (Command)stopNwait.get()) + * )); + * sysIdTab.add("All sysid tests - FRONT wheels", new SequentialCommandGroup( + * new + * SequentialCommandGroup(sysIdQuasistatic(SysIdRoutine.Direction.kForward,0), + * (Command)stopNwait.get()), + * new + * SequentialCommandGroup(sysIdQuasistatic(SysIdRoutine.Direction.kReverse,0), + * (Command)stopNwait.get()), + * new SequentialCommandGroup(sysIdDynamic(SysIdRoutine.Direction.kForward,0), + * (Command)stopNwait.get()), + * new SequentialCommandGroup(sysIdDynamic(SysIdRoutine.Direction.kReverse,0), + * (Command)stopNwait.get()) + * )); + * sysIdTab.add("All sysid tests - BACK wheels", new SequentialCommandGroup( + * new + * SequentialCommandGroup(sysIdQuasistatic(SysIdRoutine.Direction.kForward,1), + * (Command)stopNwait.get()), + * new + * SequentialCommandGroup(sysIdQuasistatic(SysIdRoutine.Direction.kReverse,1), + * (Command)stopNwait.get()), + * new SequentialCommandGroup(sysIdDynamic(SysIdRoutine.Direction.kForward,1), + * (Command)stopNwait.get()), + * new SequentialCommandGroup(sysIdDynamic(SysIdRoutine.Direction.kReverse,1), + * (Command)stopNwait.get()) + * )); + */ + + sysidtabshorthand_qsi("Quasistatic Forward", SysIdRoutine.Direction.kForward); + sysidtabshorthand_qsi("Quasistatic Backward", SysIdRoutine.Direction.kReverse); + sysidtabshorthand_dyn("Dynamic Forward", SysIdRoutine.Direction.kForward); + sysidtabshorthand_dyn("Dynamic Backward", SysIdRoutine.Direction.kReverse); + + + sysIdTab + .add(sysIdChooser) + .withSize(2, 1); + + sysIdChooser.addOption("Front Only Drive", SysIdTest.FRONT_DRIVE); + sysIdChooser.addOption("Back Only Drive", SysIdTest.BACK_DRIVE); + sysIdChooser.addOption("All Drive", SysIdTest.ALL_DRIVE); + // sysIdChooser.addOption("fl-br Turn", SysIdTest.FLBR_TURN); + // sysIdChooser.addOption("fr-bl Turn", SysIdTest.FRBL_TURN); + // sysIdChooser.addOption("All Turn", SysIdTest.ALL_TURN); + sysIdChooser.addOption("FL Rotate", SysIdTest.FL_ROT); + sysIdChooser.addOption("FR Rotate", SysIdTest.FR_ROT); + sysIdChooser.addOption("BL Rotate", SysIdTest.BL_ROT); + sysIdChooser.addOption("BR Rotate", SysIdTest.BR_ROT); + + + sysIdTab.add("ALL THE SYSID TESTS", allTheSYSID())// is this legal?? + .withSize(2, 1); + //sysIdTab.add("Dynamic Backward", sysIdDynamic(SysIdRoutine.Direction.kReverse)).withSize(2, 1); + //sysIdTab.add("Dynamic Forward", sysIdDynamic(SysIdRoutine.Direction.kForward)).withSize(2, 1); + sysIdTab.add("Quackson Backward", sysIdQuasistatic(SysIdRoutine.Direction.kReverse)).withSize(2, 1); + sysIdTab.add("Quackson Forward", sysIdQuasistatic(SysIdRoutine.Direction.kForward)).withSize(2, 1); + + sysIdTab.add("Dyanmic forward", sysIdDynamic(SysIdRoutine.Direction.kForward)).withSize(2, 1); + sysIdTab.add("Dyanmic backward", sysIdDynamic(SysIdRoutine.Direction.kReverse)).withSize(2, 1); + sysIdTab.add(this); + + for (int i = 0; i < 8; i++) {// first four are drive, next 4 are turn motors + m_appliedVoltage[i] = Volt.mutable(0); + } + for (int i = 0; i < 4; i++) { + m_distance[i] = Meter.mutable(0); + m_velocity[i] = MetersPerSecond.mutable(0); + + m_revs[i] = Rotation.mutable(0); + m_revs_vel[i] = RotationsPerSecond.mutable(0); + } + + // SmartDashboard.putNumber("Desired Angle", 0); + + // SmartDashboard.putNumber("kS", 0); + } + } + + // public Command sysIdQuasistatic(SysIdRoutine.Direction direction, int + // frontorback) { + // switch(frontorback) { + // case 0: + // return frontOnlyRoutine.quasistatic(direction); + // case 1: + // return backOnlyRoutine.quasistatic(direction); + // case 2: + // return allWheelsRoutine.quasistatic(direction); + // } + // return new PrintCommand("Invalid Command"); + // } + + private SysIdTest selector() { + //SysIdTest test = sysIdChooser.getSelected(); + SysIdTest test = SysIdTest.BACK_DRIVE; + System.out.println("Test Selected: " + test); + return test; + } + + public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { + return new SelectCommand<>( + Map.ofEntries( + // DRIVE + Map.entry(SysIdTest.FRONT_DRIVE, new ParallelCommandGroup( + direction == SysIdRoutine.Direction.kForward + ? new PrintCommand("Running front only quasistatic forward") + : new PrintCommand("Running front only quasistatic backward"), + frontOnlyDriveRoutine.quasistatic(direction))), + Map.entry(SysIdTest.BACK_DRIVE, new ParallelCommandGroup( + direction == SysIdRoutine.Direction.kForward + ? new PrintCommand("Running back only quasistatic forward") + : new PrintCommand("Running back only quasistatic backward"), + backOnlyDriveRoutine.quasistatic(direction))), + Map.entry(SysIdTest.ALL_DRIVE, new ParallelCommandGroup( + direction == SysIdRoutine.Direction.kForward + ? new PrintCommand("Running all drive quasistatic forward") + : new PrintCommand("Running all drive quasistatic backward"), + allWheelsDriveRoutine.quasistatic(direction))), + // ROTATE + Map.entry(SysIdTest.FL_ROT, new ParallelCommandGroup( + direction == SysIdRoutine.Direction.kForward + ? new PrintCommand("Running FL rotate quasistatic forward") + : new PrintCommand("Running FL rotate quasistatic backward"), + rotateRoutine[0].quasistatic(direction))), + Map.entry(SysIdTest.FR_ROT, new ParallelCommandGroup( + direction == SysIdRoutine.Direction.kForward + ? new PrintCommand("Running FR rotate quasistatic forward") + : new PrintCommand("Running FR rotate quasistatic backward"), + rotateRoutine[1].quasistatic(direction))), + Map.entry(SysIdTest.BL_ROT, new ParallelCommandGroup( + direction == SysIdRoutine.Direction.kForward + ? new PrintCommand("Running BL rotate quasistatic forward") + : new PrintCommand("Running BL rotate quasistatic backward"), + rotateRoutine[2].quasistatic(direction))), + Map.entry(SysIdTest.BR_ROT, new ParallelCommandGroup( + direction == SysIdRoutine.Direction.kForward + ? new PrintCommand("Running BR rotate quasistatic forward") + : new PrintCommand("Running BR rotate quasistatic backward"), + rotateRoutine[3].quasistatic(direction))) + + // //TURN + // Map.entry(SysIdTest.FLBR_TURN, new ParallelCommandGroup( + // direction == SysIdRoutine.Direction.kForward ? + // new PrintCommand("Running fL-bR turn quasistatic forward") : + // new PrintCommand("Running fL-bR turn quasistatic backward"), + // flbrTurn.quasistatic(direction) + // )), + // Map.entry(SysIdTest.FRBL_TURN, new ParallelCommandGroup( + // direction == SysIdRoutine.Direction.kForward ? + // new PrintCommand("Running fR-bL turn quasistatic forward") : + // new PrintCommand("Running fR-bL turn quasistatic backward"), + // frblTurn.quasistatic(direction) + // )), + // Map.entry(SysIdTest.ALL_TURN, new ParallelCommandGroup( + // direction == SysIdRoutine.Direction.kForward ? + // new PrintCommand("Running all turn quasistatic forward") : + // new PrintCommand("Running all turn quasistatic backward"), + // allWheelsTurn.quasistatic(direction) + // )) + ), + this::selector); + } + + // public Command sysIdDynamic(SysIdRoutine.Direction direction, int + // frontorback) { + // switch(frontorback) { + // case 0: + // return frontOnlyDrive.dynamic(direction); + // case 1: + // return backOnlyDrive.dynamic(direction); + // case 2: + // return allWheelsDrive.dynamic(direction); + // } + // return new PrintCommand("Invalid Command"); + // } + private Command allTheSYSID(SysIdRoutine.Direction direction) { + return new SequentialCommandGroup( + frontOnlyDriveRoutine.dynamic(direction), + backOnlyDriveRoutine.dynamic(direction), + allWheelsDriveRoutine.dynamic(direction), + rotateRoutine[0].dynamic(direction), + rotateRoutine[1].dynamic(direction), + rotateRoutine[2].dynamic(direction), + rotateRoutine[3].dynamic(direction), + + frontOnlyDriveRoutine.quasistatic(direction), + backOnlyDriveRoutine.quasistatic(direction), + allWheelsDriveRoutine.quasistatic(direction), + rotateRoutine[0].quasistatic(direction), + rotateRoutine[1].quasistatic(direction), + rotateRoutine[2].quasistatic(direction), + rotateRoutine[3].quasistatic(direction)); + } + + public Command allTheSYSID() { + return new SequentialCommandGroup( + allTheSYSID(SysIdRoutine.Direction.kForward), + allTheSYSID(SysIdRoutine.Direction.kReverse)); + } + + public Command sysIdDynamic(SysIdRoutine.Direction direction) { + return new SelectCommand<>( + Map.ofEntries( + // DRIVE + Map.entry(SysIdTest.FRONT_DRIVE, new ParallelCommandGroup( + direction == SysIdRoutine.Direction.kForward + ? new PrintCommand("Running front only dynamic forward") + : new PrintCommand("Running front only dynamic backward"), + frontOnlyDriveRoutine.dynamic(direction))), + Map.entry(SysIdTest.BACK_DRIVE, new ParallelCommandGroup( + direction == SysIdRoutine.Direction.kForward + ? new PrintCommand("Running back only dynamic forward") + : new PrintCommand("Running back only dynamic backward"), + backOnlyDriveRoutine.dynamic(direction))), + Map.entry(SysIdTest.ALL_DRIVE, new ParallelCommandGroup( + direction == SysIdRoutine.Direction.kForward + ? new PrintCommand("Running all wheels dynamic forward") + : new PrintCommand("Running all wheels dynamic backward"), + allWheelsDriveRoutine.dynamic(direction))), + // ROTATE + Map.entry(SysIdTest.FL_ROT, new ParallelCommandGroup( + direction == SysIdRoutine.Direction.kForward + ? new PrintCommand("Running FL rotate dynamic forward") + : new PrintCommand("Running FL rotate dynamic backward"), + rotateRoutine[0].dynamic(direction))), + Map.entry(SysIdTest.FR_ROT, new ParallelCommandGroup( + direction == SysIdRoutine.Direction.kForward + ? new PrintCommand("Running FR rotate dynamic forward") + : new PrintCommand("Running FR rotate dynamic backward"), + rotateRoutine[1].dynamic(direction))), + Map.entry(SysIdTest.BL_ROT, new ParallelCommandGroup( + direction == SysIdRoutine.Direction.kForward + ? new PrintCommand("Running BL rotate dynamic forward") + : new PrintCommand("Running BL rotate dynamic backward"), + rotateRoutine[2].dynamic(direction))), + Map.entry(SysIdTest.BR_ROT, new ParallelCommandGroup( + direction == SysIdRoutine.Direction.kForward + ? new PrintCommand("Running BR rotate dynamic forward") + : new PrintCommand("Running BR rotate dynamic backward"), + rotateRoutine[3].dynamic(direction)))), + this::selector); + } + + public void keepRotateMotorsAtDegrees(int angle) { + for (SwerveModule module : modules) { + module.turnPeriodic(); + module.move(0.0000000000001, angle); + } + } + + public double getGyroRate() { + return gyro.getRate(); + } + // #endregion +} \ No newline at end of file From 1e87b9f22069fdec544aff3bd1d868a2e0549239 Mon Sep 17 00:00:00 2001 From: Matthew Date: Mon, 3 Mar 2025 22:58:21 -0800 Subject: [PATCH 45/51] made an autobuilder with errors(commented out) --- .../subsystems/Drivetrain.java | 121 +++++++----------- 1 file changed, 45 insertions(+), 76 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java index a4f91e5..11f8317 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java @@ -8,6 +8,8 @@ import java.util.Map; import java.util.function.Supplier; +import javax.swing.text.html.CSS; + import org.carlmontrobotics.Constants; import org.carlmontrobotics.Constants.Drivetrainc; import org.carlmontrobotics.Constants.Drivetrainc.Autoc; @@ -109,6 +111,7 @@ import static edu.wpi.first.units.Units.Volt; import static edu.wpi.first.units.Units.Meter; import org.carlmontrobotics.commands.RotateToFieldRelativeAngle; + // Make sure this code is extraneous // import static edu.wpi.first.units.MutableMeasure.mutable; import static edu.wpi.first.units.Units.Meters; @@ -499,85 +502,51 @@ public void drive(SwerveModuleState[] moduleStates) { modules[i].move(moduleStates[i].speedMetersPerSecond, moduleStates[i].angle.getDegrees()); } } - -// public void configurePPLAutoBuilder() { -// /** -// * PATHPLANNER SETTINGS -// * Robot Width (m): .91 -// * Robot Length(m): .94 -// * Max Module Spd (m/s): 4.30 -// * Default Constraints -// * Max Vel: 1.54, Max Accel: 6.86 -// * Max Angvel: 360, Max AngAccel: 180 (guesses!) -// -// AutoBuilder.configure( -// this::getPose, // :D -// this::setPose, // :D -// this::getSpeeds, // :D -// (ChassisSpeeds cs) -> { -// //cs.vxMetersPerSecond = -cs.vxMetersPerSecond; -// // SmartDashboard.putNumber("chassis speeds x", cs.vxMetersPerSecond); -// // SmartDashboard.putNumber("chassis speeds y", cs.vyMetersPerSecond); -// // SmartDashboard.putNumber("chassis speeds theta", cs.omegaRadiansPerSecond); - -// drive(kinematics.toSwerveModuleStates(cs)); -// }, // :D -// new PPHolonomicDriveController( -// new PIDConstants(xPIDController[0], xPIDController[1], xPIDController[2], 0), //translation (drive) pid vals -// new PIDConstants(thetaPIDController[0], thetaPIDController[1], thetaPIDController[2], 0), //rotation pid vals -// Robot.kDefaultPeriod//robot period -// ), -// Autoc.robotConfig, -// () -> { -// // Boolean supplier that controls when the path will be mirrored for the red alliance -// // This will flip the path being followed to the red side of the field. -// // THE ORIGIN WILL REMAIN ON THE BLUE SIDE -// var alliance = DriverStation.getAlliance(); -// if (alliance.isPresent()) -// return alliance.get() == DriverStation.Alliance.Red; -// //else: -// return false; -// }, // :D -// this // :D -// ); - +///--------------------------------------------------------- + +//TODO: AUTOBUILDER + /*public void AutoBuilder() { + // All other subsystem initialization + // ... + + // Load the RobotConfig from the GUI settings. You should probably + // store this in your Constants file + RobotConfig config; + try{ + config = RobotConfig.fromGUISettings(); + } catch (Exception e) { + // Handle exception as needed + e.printStackTrace(); + } - - /* AutoBuilder.configureHolonomic( - () -> getPose().plus(new Transform2d(autoGyroOffset.getTranslation(),autoGyroOffset.getRotation())),//position supplier - (Pose2d pose) -> { autoGyroOffset=pose.times(-1); }, //position reset (by subtracting current pos) - this::getSpeeds, //chassisSpeed supplier - (ChassisSpeeds cs) -> drive( - cs.vxMetersPerSecond, - -cs.vyMetersPerSecond, - //flipped because drive assumes up is negative, but PPlanner assumes up is positive - cs.omegaRadiansPerSecond - ), - new HolonomicPathFollowerConfig( - new PIDConstants(drivekP[0], drivekI[0], drivekD[0], driveIzone), //translation (drive) pid vals - new PIDConstants(turnkP_avg, 0., 0., turnIzone), //rotation pid vals - maxSpeed, - swerveRadius, - Autoc.replanningConfig, - Robot.robot.getPeriod()//robot period - ), - () -> { - // Boolean supplier that controls when the path will be mirrored for the red alliance - // This will flip the path being followed to the red side of the field. - // THE ORIGIN WILL REMAIN ON THE BLUE SIDE - var alliance = DriverStation.getAlliance(); - if (alliance.isPresent()) - return alliance.get() == DriverStation.Alliance.Red; - //else: - return false; - }, - this - ); - */ - -// } + // Configure AutoBuilder last + AutoBuilder.configure( + this::getPose, // Robot pose supplier + this::setPose, // Method to reset odometry (will be called if your auto has a starting pose) + this::getSpeeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE + (speeds, feedforwards) -> drive(kinematics.toSwerveModuleStates(speeds), // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds. Also optionally outputs individual module feedforwards + new PPHolonomicDriveController( // PPHolonomicController is the built in path following controller for holonomic drive trains + new PIDConstants(5.0, 0.0, 0.0), // Translation PID constants + new PIDConstants(5.0, 0.0, 0.0) // Rotation PID constants + ), + config, // The robot configuration + () -> { + // Boolean supplier that controls when the path will be mirrored for the red alliance + // This will flip the path being followed to the red side of the field. + // THE ORIGIN WILL REMAIN ON THE BLUE SIDE + + var alliance = DriverStation.getAlliance(); + if (alliance.isPresent()) { + return alliance.get() == DriverStation.Alliance.Red; + } + return false; + }, + this // Reference to this subsystem to set requirements + )); + }*/ +//---------------------------------------------------------- public void autoCancelDtCommand() { if(!(getDefaultCommand() instanceof TeleopDrive) || DriverStation.isAutonomous()) return; From 42f243275d1e0fb9fa972efef4c0139c04f45faa Mon Sep 17 00:00:00 2001 From: FriedLongJohns <81837862+FriedLongJohns@users.noreply.github.com> Date: Tue, 4 Mar 2025 16:13:07 -0800 Subject: [PATCH 46/51] write autobuilder --- .../java/org/carlmontrobotics/Constants.java | 402 ++++++++++-------- .../subsystems/Drivetrain.java | 49 ++- 2 files changed, 251 insertions(+), 200 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 466b516..116be78 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -2,9 +2,18 @@ import org.carlmontrobotics.lib199.swerve.SwerveConfig; +import com.pathplanner.lib.config.ModuleConfig; +import com.pathplanner.lib.config.RobotConfig; import com.pathplanner.lib.path.PathConstraints; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.AngularMomentumUnit; +import edu.wpi.first.units.MassUnit; +import edu.wpi.first.units.MomentOfInertiaUnit; +import edu.wpi.first.units.measure.Mass; +import edu.wpi.first.units.measure.MomentOfInertia; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.XboxController.Axis; import edu.wpi.first.wpilibj.XboxController.Button; @@ -69,186 +78,219 @@ public static final class AlgaeEffectorc { } public static final class Drivetrainc { - // #region Subsystem Constants - public static final double wheelBase = 24.75; //CONFIG.isSwimShady() ? Units.inchesToMeters(19.75) : Units.inchesToMeters(16.75); - public static final double trackWidth = 24.75;//CONFIG.isSwimShady() ? Units.inchesToMeters(28.75) : Units.inchesToMeters(23.75); - // "swerveRadius" is the distance from the center of the robot to one of the - // modules - public static final double swerveRadius = Math.sqrt(Math.pow(wheelBase / 2, 2) + Math.pow(trackWidth / 2, 2)); - // The gearing reduction from the drive motor controller to the wheels - // Gearing for the Swerve Modules is 6.75 : 1e - public static final double driveGearing = 6.75; - // Turn motor shaft to "module shaft" - public static final double turnGearing = 150.0 / 7; - - public static final double driveModifier = 1; - public static final double wheelDiameterMeters = Units.inchesToMeters(4.0) * 7.36 / 7.65 /* - * empirical - * correction - */; - public static final double mu = 1; /* 70/83.2; */ - - public static final double NEOFreeSpeed = 5676 * (2 * Math.PI) / 60; // radians/s - // Angular speed to translational speed --> v = omega * r / gearing - public static final double maxSpeed = NEOFreeSpeed * (wheelDiameterMeters / 2.0) / driveGearing; - public static final double maxForward = maxSpeed; // todo: use smart dashboard to figure this out - public static final double maxStrafe = maxSpeed; // todo: use smart dashboard to figure this out - // seconds it takes to go from 0 to 12 volts(aka MAX) - public static final double secsPer12Volts = 0.1; - - // maxRCW is the angular velocity of the robot. - // Calculated by looking at one of the motors and treating it as a point mass - // moving around in a circle. - // Tangential speed of this point mass is maxSpeed and the radius of the circle - // is sqrt((wheelBase/2)^2 + (trackWidth/2)^2) - // Angular velocity = Tangential speed / radius - public static final double maxRCW = maxSpeed / swerveRadius; - - public static final boolean[] reversed = { false, false, false, false }; - // public static final boolean[] reversed = {true, true, true, true}; - // Determine correct turnZero constants (FL, FR, BL, BR) - public static final double[] turnZeroDeg = RobotBase.isSimulation() ? new double[] {-90.0, -90.0, -90.0, -90.0 } - : (CONFIG.isSwimShady() ? new double[] { 85.7812, 85.0782, -96.9433, -162.9492 } - : new double[] { 17.2266, -96.8555, -95.8008, 85.166 });/* real values here */ - - // kP, kI, and kD constants for turn motor controllers in the order of - // front-left, front-right, back-left, back-right. - // Determine correct turn PID constants - public static final double[] turnkP = {12, 12, 23, 23};//sysid for fr that didnt't work{0.099412, 0.13414, 3.6809, 3.6809} //{49, 23,33, 28};//{51.078, 25, 35.946, 30.986}; // {0.00374, 0.00374, 0.00374, - // 0.00374}; - public static final double[] turnkI = {0, 0, 0, 0};//{ 0, 0.1, 0, 0 }; - public static final double[] turnkD = {1, 1.55, 0, 2};//{ 0.2/* dont edit */, 0.3, 0.5, 0.4}; // todo: use d - // public static final double[] turnkS = {0.2, 0.2, 0.2, 0.2}; - public static final double[] turnkS = {0.050634, 0.033065, 0.018117, 0.021337};//sysid for fr that didnt't work{0.041796, 0.09111, 0.64804, 1.0873}//{ 0.13027, 0.17026, 0.2, 0.23262 }; - - // V = kS + kV * v + kA * a - // 12 = 0.2 + 0.00463 * v - // v = (12 - 0.2) / 0.00463 = 2548.596 degrees/s - public static final double[] turnkV = {2.6153, 2.5924, 2.6495, 2.6705};//sysid for fr that didnt't work{2.6403, 2.6603, 2.6168, 2.5002} //{2.6532, 2.7597, 2.7445, 2.7698}; - public static final double[] turnkA = {0.18525, 0.13879, 0.23625, 0.25589};//sysid for fr that didnt't work{0.33266, 0.25535, 0.17924, 0.17924} //{ 0.17924, 0.17924, 0.17924, 0.17924 }; - - // kP is an average of the forward and backward kP values - // Forward: 1.72, 1.71, 1.92, 1.94 - // Backward: 1.92, 1.92, 2.11, 1.89 - // Order of modules: (FL, FR, BL, BR) - public static final double[] drivekP = CONFIG.isSwimShady() ? new double[] { 2.8, 2.8, 2.8, 2.8 } - : new double[] {1.75, 1.75, 1.75, 1.75}; //{2.2319, 2.2462, 2.4136, 3.6862}; // {1.82/100, 1.815/100, 2.015/100, - // 1.915/100}; - public static final double[] drivekI = { 0.1, 0.1, 0.1, 0.1 }; - public static final double[] drivekD = { 0, 0, 0, 0 }; - public static final boolean[] driveInversion = (CONFIG.isSwimShady() - ? new boolean[] { false, false, false, false } - : new boolean[] { false, true, false, true }); - public static final boolean[] turnInversion = { true, true, true, true }; - // kS - // public static final double[] kForwardVolts = { 0.26744, 0.31897, 0.27967, 0.2461 }; - public static final double[] kForwardVolts = {0, 0, 0, 0}; //{0.59395, 0.52681, 0.11097, 0.17914}; //{ 0.2, 0.2, 0.2, 0.2 }; - public static final double[] kBackwardVolts = kForwardVolts; - - //kV - // public static final double[] kForwardVels = { 2.81, 2.9098, 2.8378, 2.7391 }; - public static final double[] kForwardVels = { 0, 0, 0, 0 };//{2.4114, 2.7465, 2.7546, 2.7412}; //{ 0, 0, 0, 0 };//volts per m/s - public static final double[] kBackwardVels = kForwardVels; - - //kA - // public static final double[] kForwardAccels = { 1.1047 / 2, 0.79422 / 2, 0.77114 / 2, 1.1003 / 2 }; - public static final double[] kForwardAccels = { 0, 0, 0, 0 };//{0.31958, 0.33557, 0.70264, 0.46644}; //{ 0, 0, 0, 0 };// volts per m/s^2 - public static final double[] kBackwardAccels = kForwardAccels; - - public static final double autoMaxSpeedMps = 0.35 * 4.4; // Meters / second - public static final double autoMaxAccelMps2 = mu * 9.81; // Meters / seconds^2 - public static final double autoMaxVolt = 10.0; // For Drivetrain voltage constraint in RobotPath.java - // The maximum acceleration the robot can achieve is equal to the coefficient of - // static friction times the gravitational acceleration - // a = mu * 9.8 m/s^2 - public static final double autoCentripetalAccel = mu * 9.81 * 2; - - public static final boolean isGyroReversed = true; - - // PID values are listed in the order kP, kI, and kD - public static final double[] xPIDController = CONFIG.isSwimShady() ? new double[] { 4, 0.0, 0.0 } - : new double[] { 2, 0.0, 0.0 }; - public static final double[] yPIDController = xPIDController; - public static final double[] thetaPIDController = CONFIG.isSwimShady() ? new double[] { 0.10, 0.0, 0.001 } - : new double[] {0.05, 0.0, 0.00}; - - public static final SwerveConfig swerveConfig = new SwerveConfig(wheelDiameterMeters, driveGearing, mu, - autoCentripetalAccel, kForwardVolts, kForwardVels, kForwardAccels, kBackwardVolts, kBackwardVels, - kBackwardAccels, drivekP, drivekI, drivekD, turnkP, turnkI, turnkD, turnkS, turnkV, turnkA, turnZeroDeg, - driveInversion, reversed, driveModifier, turnInversion); - - // public static final Limelight.Transform limelightTransformForPoseEstimation = - // Transform.BOTPOSE_WPIBLUE; - - // #endregion - - // #region Ports - //I think all of these are right - public static final int driveFrontLeftPort = 11; - public static final int driveFrontRightPort = 12; - public static final int driveBackLeftPort = 13; - public static final int driveBackRightPort = 14; - - public static final int turnFrontLeftPort = 1; - public static final int turnFrontRightPort = 2; - public static final int turnBackLeftPort = 3; - public static final int turnBackRightPort = 4; - //to be configured - public static final int canCoderPortFL = 1; //0 - public static final int canCoderPortFR = 2; - public static final int canCoderPortBL = 3; - public static final int canCoderPortBR = 0; //1 - - // #endregion - - // #region Command Constants - - public static double kNormalDriveSpeed = 1; // Percent Multiplier - public static double kNormalDriveRotation = 0.5; // Percent Multiplier - public static double kSlowDriveSpeed = 0.4; // Percent Multiplier - public static double kSlowDriveRotation = 0.250; // Percent Multiplier - - //baby speed values, i just guessed the percent multiplier. TODO: find actual ones we wana use - public static double kBabyDriveSpeed = 0.3; - public static double kBabyDriveRotation = 0.2; - public static double kAlignMultiplier = 1D / 3D; - public static final double kAlignForward = 0.6; - - public static final double wheelTurnDriveSpeed = 0.0001; // Meters / Second ; A non-zero speed just used to - // orient the wheels to the correct angle. This - // should be very small to avoid actually moving the - // robot. - - public static final double[] positionTolerance = { Units.inchesToMeters(.5), Units.inchesToMeters(.5), 5 }; // Meters, - // Meters, - // Degrees - public static final double[] velocityTolerance = { Units.inchesToMeters(1), Units.inchesToMeters(1), 5 }; // Meters, - // Meters, - // Degrees/Second - - // #endregion - // #region Subsystem Constants - - // "swerveRadius" is the distance from the center of the robot to one of the - // modules - public static final double turnkP_avg = (turnkP[0] + turnkP[1] + turnkP[2] + turnkP[3]) / 4; - public static final double turnIzone = .1; - - public static final double driveIzone = .1; - - public static final class Autoc { - // public static final RobotConfig robotConfig = new RobotConfig( /* - // * put in - // * Constants.Drivetrain.Auto - // */ - // false, // replan at start of path if robot not at start of path? - // false, // replan if total error surpasses total error/spike threshold? - // 1.5, // total error threshold in meters that will cause the path to be replanned - // 0.8 // error spike threshold, in meters, that will cause the path to be replanned - // ); - public static final PathConstraints pathConstraints = new PathConstraints(1.54, 6.86, 2 * Math.PI, - 2 * Math.PI); // The constraints for this path. If using a differential drivetrain, the - // angular constraints have no effect. + // #region Subsystem Constants + public static final double wheelBase = 24.75; //CONFIG.isSwimShady() ? Units.inchesToMeters(19.75) : Units.inchesToMeters(16.75); + public static final double trackWidth = 24.75;//CONFIG.isSwimShady() ? Units.inchesToMeters(28.75) : Units.inchesToMeters(23.75); + // "swerveRadius" is the distance from the center of the robot to one of the + // modules + public static final double swerveRadius = Math.sqrt(Math.pow(wheelBase / 2, 2) + Math.pow(trackWidth / 2, 2)); + // The gearing reduction from the drive motor controller to the wheels + // Gearing for the Swerve Modules is 6.75 : 1e + public static final double driveGearing = 6.75; + // Turn motor shaft to "module shaft" + public static final double turnGearing = 150.0 / 7; + + public static final double driveModifier = 1; + public static final double wheelDiameterMeters = Units.inchesToMeters(4.0) * 7.36 / 7.65 /* + * empirical + * correction + */; + public static final double mu = 1; /* 70/83.2; */ //coefficient of friction. less means less max acceleration. + public static final double ROBOTMASS_KG = 135;//max is 135kg + //moment of inertia, kg/mm + //calculated by integral of mass * radius^2 for every point of the robot + //easy way? just do total mass * radius^2 + public static final double MOI = ROBOTMASS_KG * swerveRadius*swerveRadius; + + public static final double NEOFreeSpeed = 5676 * (2 * Math.PI) / 60; // radians/s + // Angular speed to translational speed --> v = omega * r / gearing + public static final double maxSpeed = NEOFreeSpeed * (wheelDiameterMeters / 2.0) / driveGearing; + public static final double maxForward = maxSpeed; // todo: use smart dashboard to figure this out + public static final double maxStrafe = maxSpeed; // todo: use smart dashboard to figure this out + // seconds it takes to go from 0 to 12 volts(aka MAX) + public static final double secsPer12Volts = 0.1; + + // maxRCW is the angular velocity of the robot. + // Calculated by looking at one of the motors and treating it as a point mass + // moving around in a circle. + // Tangential speed of this point mass is maxSpeed and the radius of the circle + // is sqrt((wheelBase/2)^2 + (trackWidth/2)^2) + // Angular velocity = Tangential speed / radius + public static final double maxRCW = maxSpeed / swerveRadius; + + public static final boolean[] reversed = { false, false, false, false }; + // public static final boolean[] reversed = {true, true, true, true}; + // Determine correct turnZero constants (FL, FR, BL, BR) + public static final double[] turnZeroDeg = RobotBase.isSimulation() ? new double[] {-90.0, -90.0, -90.0, -90.0 } + : (CONFIG.isSwimShady() ? new double[] { 85.7812, 85.0782, -96.9433, -162.9492 } + : new double[] { 17.2266, -96.8555, -95.8008, 85.166 });/* real values here */ + + // kP, kI, and kD constants for turn motor controllers in the order of + // front-left, front-right, back-left, back-right. + // Determine correct turn PID constants + public static final double[] turnkP = {12, 12, 23, 23};//sysid for fr that didnt't work{0.099412, 0.13414, 3.6809, 3.6809} //{49, 23,33, 28};//{51.078, 25, 35.946, 30.986}; // {0.00374, 0.00374, 0.00374, + // 0.00374}; + public static final double[] turnkI = {0, 0, 0, 0};//{ 0, 0.1, 0, 0 }; + public static final double[] turnkD = {1, 1.55, 0, 2};//{ 0.2/* dont edit */, 0.3, 0.5, 0.4}; // todo: use d + // public static final double[] turnkS = {0.2, 0.2, 0.2, 0.2}; + public static final double[] turnkS = {0.050634, 0.033065, 0.018117, 0.021337};//sysid for fr that didnt't work{0.041796, 0.09111, 0.64804, 1.0873}//{ 0.13027, 0.17026, 0.2, 0.23262 }; + + // V = kS + kV * v + kA * a + // 12 = 0.2 + 0.00463 * v + // v = (12 - 0.2) / 0.00463 = 2548.596 degrees/s + public static final double[] turnkV = {2.6153, 2.5924, 2.6495, 2.6705};//sysid for fr that didnt't work{2.6403, 2.6603, 2.6168, 2.5002} //{2.6532, 2.7597, 2.7445, 2.7698}; + public static final double[] turnkA = {0.18525, 0.13879, 0.23625, 0.25589};//sysid for fr that didnt't work{0.33266, 0.25535, 0.17924, 0.17924} //{ 0.17924, 0.17924, 0.17924, 0.17924 }; + + // kP is an average of the forward and backward kP values + // Forward: 1.72, 1.71, 1.92, 1.94 + // Backward: 1.92, 1.92, 2.11, 1.89 + // Order of modules: (FL, FR, BL, BR) + public static final double[] drivekP = CONFIG.isSwimShady() ? new double[] { 2.8, 2.8, 2.8, 2.8 } + : new double[] {1.75, 1.75, 1.75, 1.75}; //{2.2319, 2.2462, 2.4136, 3.6862}; // {1.82/100, 1.815/100, 2.015/100, + // 1.915/100}; + public static final double[] drivekI = { 0.1, 0.1, 0.1, 0.1 }; + public static final double[] drivekD = { 0, 0, 0, 0 }; + public static final boolean[] driveInversion = (CONFIG.isSwimShady() + ? new boolean[] { false, false, false, false } + : new boolean[] { false, true, false, true }); + public static final boolean[] turnInversion = { true, true, true, true }; + // kS + // public static final double[] kForwardVolts = { 0.26744, 0.31897, 0.27967, 0.2461 }; + public static final double[] kForwardVolts = {0, 0, 0, 0}; //{0.59395, 0.52681, 0.11097, 0.17914}; //{ 0.2, 0.2, 0.2, 0.2 }; + public static final double[] kBackwardVolts = kForwardVolts; + + //kV + // public static final double[] kForwardVels = { 2.81, 2.9098, 2.8378, 2.7391 }; + public static final double[] kForwardVels = { 0, 0, 0, 0 };//{2.4114, 2.7465, 2.7546, 2.7412}; //{ 0, 0, 0, 0 };//volts per m/s + public static final double[] kBackwardVels = kForwardVels; + + //kA + // public static final double[] kForwardAccels = { 1.1047 / 2, 0.79422 / 2, 0.77114 / 2, 1.1003 / 2 }; + public static final double[] kForwardAccels = { 0, 0, 0, 0 };//{0.31958, 0.33557, 0.70264, 0.46644}; //{ 0, 0, 0, 0 };// volts per m/s^2 + public static final double[] kBackwardAccels = kForwardAccels; + + public static final double autoMaxSpeedMps = 0.35 * 4.4; // Meters / second + public static final double autoMaxAccelMps2 = mu * 9.81; // Meters / seconds^2 + public static final double autoMaxVolt = 10.0; // For Drivetrain voltage constraint in RobotPath.java + // The maximum acceleration the robot can achieve is equal to the coefficient of + // static friction times the gravitational acceleration + // a = mu * 9.8 m/s^2 + public static final double autoCentripetalAccel = mu * 9.81 * 2; + + public static final boolean isGyroReversed = true; + + // PID values are listed in the order kP, kI, and kD + public static final double[] xPIDController = CONFIG.isSwimShady() ? new double[] { 4, 0.0, 0.0 } + : new double[] { 2, 0.0, 0.0 }; + public static final double[] yPIDController = xPIDController; + public static final double[] thetaPIDController = CONFIG.isSwimShady() ? new double[] { 0.10, 0.0, 0.001 } + : new double[] {0.05, 0.0, 0.00}; + + public static final SwerveConfig swerveConfig = new SwerveConfig(wheelDiameterMeters, driveGearing, mu, + autoCentripetalAccel, kForwardVolts, kForwardVels, kForwardAccels, kBackwardVolts, kBackwardVels, + kBackwardAccels, drivekP, drivekI, drivekD, turnkP, turnkI, turnkD, turnkS, turnkV, turnkA, turnZeroDeg, + driveInversion, reversed, driveModifier, turnInversion); + + // public static final Limelight.Transform limelightTransformForPoseEstimation = + // Transform.BOTPOSE_WPIBLUE; + + // #endregion + + // #region Ports + //I think all of these are right + public static final int driveFrontLeftPort = 11; + public static final int driveFrontRightPort = 12; + public static final int driveBackLeftPort = 13; + public static final int driveBackRightPort = 14; + + public static final int turnFrontLeftPort = 1; + public static final int turnFrontRightPort = 2; + public static final int turnBackLeftPort = 3; + public static final int turnBackRightPort = 4; + //to be configured + public static final int canCoderPortFL = 1; //0 + public static final int canCoderPortFR = 2; + public static final int canCoderPortBL = 3; + public static final int canCoderPortBR = 0; //1 + + // #endregion + + // #region Command Constants + + public static double kNormalDriveSpeed = 1; // Percent Multiplier + public static double kNormalDriveRotation = 0.5; // Percent Multiplier + public static double kSlowDriveSpeed = 0.4; // Percent Multiplier + public static double kSlowDriveRotation = 0.250; // Percent Multiplier + + //baby speed values, i just guessed the percent multiplier. TODO: find actual ones we wana use + public static double kBabyDriveSpeed = 0.3; + public static double kBabyDriveRotation = 0.2; + public static double kAlignMultiplier = 1D / 3D; + public static final double kAlignForward = 0.6; + + public static final double wheelTurnDriveSpeed = 0.0001; // Meters / Second ; A non-zero speed just used to + // orient the wheels to the correct angle. This + // should be very small to avoid actually moving the + // robot. + + public static final double[] positionTolerance = { Units.inchesToMeters(.5), Units.inchesToMeters(.5), 5 }; // Meters, + // Meters, + // Degrees + public static final double[] velocityTolerance = { Units.inchesToMeters(1), Units.inchesToMeters(1), 5 }; // Meters, + // Meters, + // Degrees/Second + + // #endregion + // #region Subsystem Constants + + // "swerveRadius" is the distance from the center of the robot to one of the + // modules + public static final double turnkP_avg = (turnkP[0] + turnkP[1] + turnkP[2] + turnkP[3]) / 4; + public static final double turnIzone = .1; + + public static final double driveIzone = .1; + + public static final class Autoc { + public static final RobotConfig robotConfig = new RobotConfig( + // Mass mass, kg + ROBOTMASS_KG, + // double Moment Oof Inertia, kg/mm + MOI,//==1 + // ModuleConfig moduleConfig, + new ModuleConfig( + // double wheelRadiusMeters, + swerveRadius, + // double maxDriveVelocityMPS, + autoMaxSpeedMps, + // double wheelCOF, + mu, + // DCMotor driveMotor, + DCMotor.getNEO(4),//FIXME is it 1 for 1 each or 4 for 1 robot + // double driveGearing, + driveGearing, + // double driveCurrentLimit, + autoMaxVolt, + // int numMotors + 4 + ), + // Translation2d... moduleOffsets + new Translation2d(wheelBase / 2, trackWidth / 2), + new Translation2d(wheelBase / 2, -trackWidth / 2), + new Translation2d(-wheelBase / 2, trackWidth / 2), + new Translation2d(-wheelBase / 2, -trackWidth / 2) + ); + // public static final ReplanningConfig repConfig = new ReplanningConfig( /* + // * put in + // * Constants.Drivetrain.Auto + // */ + // false, // replan at start of path if robot not at start of path? + // false, // replan if total error surpasses total error/spike threshold? + // 1.5, // total error threshold in meters that will cause the path to be replanned + // 0.8 // error spike threshold, in meters, that will cause the path to be replanned + // ); + public static final PathConstraints pathConstraints = new PathConstraints(1.54, 6.86, 2 * Math.PI, + 2 * Math.PI); // The constraints for this path. If using a differential drivetrain, the + // angular constraints have no effect. } } diff --git a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java index 11f8317..a81f1d7 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java @@ -506,45 +506,54 @@ public void drive(SwerveModuleState[] moduleStates) { ///--------------------------------------------------------- //TODO: AUTOBUILDER - /*public void AutoBuilder() { + public void AutoBuilder() { // All other subsystem initialization // ... // Load the RobotConfig from the GUI settings. You should probably // store this in your Constants file RobotConfig config; - try{ - config = RobotConfig.fromGUISettings(); - } catch (Exception e) { - // Handle exception as needed - e.printStackTrace(); - } + // try{ + // config = RobotConfig.fromGUISettings(); + // } catch (Exception e) { + // config = Constants. + // // Handle exception as needed + // e.printStackTrace(); + // } + config = Constants.Drivetrainc.Autoc.robotConfig; // Configure AutoBuilder last AutoBuilder.configure( + //Supplier poseSupplier, this::getPose, // Robot pose supplier + //Consumer resetPose, this::setPose, // Method to reset odometry (will be called if your auto has a starting pose) + //Supplier robotRelativeSpeedsSupplier, this::getSpeeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE - (speeds, feedforwards) -> drive(kinematics.toSwerveModuleStates(speeds), // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds. Also optionally outputs individual module feedforwards + //BiConsumer output, + (speeds, feedforwards) -> drive(kinematics.toSwerveModuleStates(speeds)), // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds. Also optionally outputs individual module feedforwards + //PathFollowingController controller, new PPHolonomicDriveController( // PPHolonomicController is the built in path following controller for holonomic drive trains - new PIDConstants(5.0, 0.0, 0.0), // Translation PID constants + new PIDConstants(5.0, 0.0, 0.0), // Translation PID constants FIXME do these need to be accurate? new PIDConstants(5.0, 0.0, 0.0) // Rotation PID constants ), + //RobotConfig robotConfig, config, // The robot configuration + //BooleanSupplier shouldFlipPath, () -> { - // Boolean supplier that controls when the path will be mirrored for the red alliance - // This will flip the path being followed to the red side of the field. - // THE ORIGIN WILL REMAIN ON THE BLUE SIDE - - var alliance = DriverStation.getAlliance(); - if (alliance.isPresent()) { - return alliance.get() == DriverStation.Alliance.Red; - } - return false; + // Boolean supplier that controls when the path will be mirrored for the red alliance + // This will flip the path being followed to the red side of the field. + // THE ORIGIN WILL REMAIN ON THE BLUE SIDE + var alliance = DriverStation.getAlliance(); + if (alliance.isPresent()) { + return alliance.get() == DriverStation.Alliance.Red;//stolen from official pplib + } + return false; }, + //Subsystem... driveRequirements this // Reference to this subsystem to set requirements - )); - }*/ + ); + } //---------------------------------------------------------- public void autoCancelDtCommand() { From baf6d71d3e3c17c8c292526a0b8cae4ccd8b0f3e Mon Sep 17 00:00:00 2001 From: FriedLongJohns - WindowsLaptop <81837862+FriedLongJohns@users.noreply.github.com> Date: Tue, 4 Mar 2025 19:11:19 -0600 Subject: [PATCH 47/51] guh --- src/main/java/org/carlmontrobotics/Constants.java | 6 +++--- src/main/java/org/carlmontrobotics/RobotContainer.java | 2 ++ .../java/org/carlmontrobotics/subsystems/Drivetrain.java | 8 ++++++++ 3 files changed, 13 insertions(+), 3 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 116be78..02b5b57 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -60,9 +60,9 @@ public static final class Manipulator { } } public static final class AlgaeEffectorc { - public static final int upperMotorID = 1; - public static final int lowerMotorID = 2; - public static final int pinchMotorID = 3; + public static final int upperMotorID = 17; + public static final int lowerMotorID = 24; + public static final int pinchMotorID = 56; public static final int TopkS = 1; public static final int BottomkS = 1; public static final int PincherkS = 1; diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index f883edb..b027fba 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -18,6 +18,7 @@ import org.carlmontrobotics.commands.OuttakeAlgae; import org.carlmontrobotics.subsystems.AlgaeEffector; import org.carlmontrobotics.subsystems.CoralEffector; +import org.carlmontrobotics.subsystems.Drivetrain; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; @@ -65,6 +66,7 @@ public class RobotContainer { public final GenericHID manipulatorController = new GenericHID(OI.Manipulator.manipulatorPort); private final AlgaeEffector algaeEffector = new AlgaeEffector(); private final CoralEffector coralEffector = new CoralEffector(); + private final Drivetrain drivetrain = new Drivetrain(); private final SendableChooser autoChooser; /*private final String[] autoNames = new String[] { diff --git a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java index a81f1d7..865cf7b 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java @@ -151,6 +151,7 @@ public class Drivetrain extends SubsystemBase { double kI = 0; double kD = 0; public Drivetrain() { + //AutoBuilder(); SmartDashboard.putNumber("Goal Velocity", 0); SmartDashboard.putNumber("kP", 0); SmartDashboard.putNumber("kI", 0); @@ -297,6 +298,9 @@ public Drivetrain() { // Setup autopath builder //configurePPLAutoBuilder(); + AutoBuilder(); + System.out.println("BBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBB"); + // SmartDashboard.putNumber("chassis speeds x", 0); // SmartDashboard.putNumber("chassis speeds y", 0); @@ -347,6 +351,8 @@ public void simulationPeriodic() { @Override public void periodic() { + // AutoBuilder(); + // SmartDashboard.getNumber("GoalPos", turnEncoders[0].getVelocity().getValueAsDouble()); // SmartDashboard.putNumber("FL Motor Val", turnMotors[0].getEncoder().getPosition()); // double goal = SmartDashboard.getNumber("GoalPos", 0); @@ -507,6 +513,7 @@ public void drive(SwerveModuleState[] moduleStates) { //TODO: AUTOBUILDER public void AutoBuilder() { + System.out.println("AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA"); // All other subsystem initialization // ... @@ -523,6 +530,7 @@ public void AutoBuilder() { config = Constants.Drivetrainc.Autoc.robotConfig; // Configure AutoBuilder last + AutoBuilder.configure( //Supplier poseSupplier, this::getPose, // Robot pose supplier From 09abb22c969665a22cc63515458f4eb39801dceb Mon Sep 17 00:00:00 2001 From: Brandon Date: Thu, 6 Mar 2025 13:51:43 -0800 Subject: [PATCH 48/51] updated constants to new Hammerhead constants --- .../java/org/carlmontrobotics/Constants.java | 26 +++++++++---------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 02b5b57..93568ea 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -128,40 +128,40 @@ public static final class Drivetrainc { // kP, kI, and kD constants for turn motor controllers in the order of // front-left, front-right, back-left, back-right. // Determine correct turn PID constants - public static final double[] turnkP = {12, 12, 23, 23};//sysid for fr that didnt't work{0.099412, 0.13414, 3.6809, 3.6809} //{49, 23,33, 28};//{51.078, 25, 35.946, 30.986}; // {0.00374, 0.00374, 0.00374, + public static final double[] turnkP = { 51.078, 60.885, 60.946, 60.986/2};//sysid for fr that didnt't work{0.099412, 0.13414, 3.6809, 3.6809} //{49, 23,33, 28};//{51.078, 25, 35.946, 30.986}; // {0.00374, 0.00374, 0.00374, // 0.00374}; - public static final double[] turnkI = {0, 0, 0, 0};//{ 0, 0.1, 0, 0 }; - public static final double[] turnkD = {1, 1.55, 0, 2};//{ 0.2/* dont edit */, 0.3, 0.5, 0.4}; // todo: use d +` public static final double[] turnkI = { 0, 0, 0, 0};//{ 0, 0.1, 0, 0 }; +/ public static final double[] turnkD = { 0, 0.5, 0.42, 1};//{ 0.2/* dont edit */, 0.3, 0.5, 0.4}; // todo: use d // public static final double[] turnkS = {0.2, 0.2, 0.2, 0.2}; - public static final double[] turnkS = {0.050634, 0.033065, 0.018117, 0.021337};//sysid for fr that didnt't work{0.041796, 0.09111, 0.64804, 1.0873}//{ 0.13027, 0.17026, 0.2, 0.23262 }; + public static final double[] turnkS = { 0.13027, 0.17026, 0.2, 0.23262};//sysid for fr that didnt't work{0.041796, 0.09111, 0.64804, 1.0873}//{ 0.13027, 0.17026, 0.2, 0.23262 }; // V = kS + kV * v + kA * a // 12 = 0.2 + 0.00463 * v // v = (12 - 0.2) / 0.00463 = 2548.596 degrees/s - public static final double[] turnkV = {2.6153, 2.5924, 2.6495, 2.6705};//sysid for fr that didnt't work{2.6403, 2.6603, 2.6168, 2.5002} //{2.6532, 2.7597, 2.7445, 2.7698}; - public static final double[] turnkA = {0.18525, 0.13879, 0.23625, 0.25589};//sysid for fr that didnt't work{0.33266, 0.25535, 0.17924, 0.17924} //{ 0.17924, 0.17924, 0.17924, 0.17924 }; + public static final double[] turnkV = { 2.6532, 2.7597, 2.7445, 2.7698 };//sysid for fr that didnt't work{2.6403, 2.6603, 2.6168, 2.5002} //{2.6532, 2.7597, 2.7445, 2.7698}; + public static final double[] turnkA = { 0.17924, 0.17924, 0.17924, 0.17924 };//sysid for fr that didnt't work{0.33266, 0.25535, 0.17924, 0.17924} //{ 0.17924, 0.17924, 0.17924, 0.17924 }; // kP is an average of the forward and backward kP values // Forward: 1.72, 1.71, 1.92, 1.94 // Backward: 1.92, 1.92, 2.11, 1.89 // Order of modules: (FL, FR, BL, BR) public static final double[] drivekP = CONFIG.isSwimShady() ? new double[] { 2.8, 2.8, 2.8, 2.8 } - : new double[] {1.75, 1.75, 1.75, 1.75}; //{2.2319, 2.2462, 2.4136, 3.6862}; // {1.82/100, 1.815/100, 2.015/100, + : new double[] {1.75*1.275, 1.75*1.275, 1.75*1.275, 1.75*1.275}; //{2.2319, 2.2462, 2.4136, 3.6862}; // {1.82/100, 1.815/100, 2.015/100, // 1.915/100}; - public static final double[] drivekI = { 0.1, 0.1, 0.1, 0.1 }; - public static final double[] drivekD = { 0, 0, 0, 0 }; + public static final double[] drivekI = { 0, 0, 0, 0 }; + public static final double[] drivekD = { 0.005, 0.005, 0.005, 0.005 }; public static final boolean[] driveInversion = (CONFIG.isSwimShady() ? new boolean[] { false, false, false, false } : new boolean[] { false, true, false, true }); public static final boolean[] turnInversion = { true, true, true, true }; // kS - // public static final double[] kForwardVolts = { 0.26744, 0.31897, 0.27967, 0.2461 }; - public static final double[] kForwardVolts = {0, 0, 0, 0}; //{0.59395, 0.52681, 0.11097, 0.17914}; //{ 0.2, 0.2, 0.2, 0.2 }; + public static final double[] kForwardVolts = { 0.26744, 0.31897, 0.27967, 0.2461 }; + //public static final double[] kForwardVolts = { 0, 0, 0, 0}; //{0.59395, 0.52681, 0.11097, 0.17914}; //{ 0.2, 0.2, 0.2, 0.2 }; public static final double[] kBackwardVolts = kForwardVolts; //kV - // public static final double[] kForwardVels = { 2.81, 2.9098, 2.8378, 2.7391 }; - public static final double[] kForwardVels = { 0, 0, 0, 0 };//{2.4114, 2.7465, 2.7546, 2.7412}; //{ 0, 0, 0, 0 };//volts per m/s + public static final double[] kForwardVels = { 2.81, 2.9098, 2.8378, 2.7391 }; + //public static final double[] kForwardVels = { 0, 0, 0, 0 };//{2.4114, 2.7465, 2.7546, 2.7412}; //{ 0, 0, 0, 0 };//volts per m/s public static final double[] kBackwardVels = kForwardVels; //kA From f291ac627dd8988e765e62b81d11132806fb5623 Mon Sep 17 00:00:00 2001 From: Brandon Date: Thu, 6 Mar 2025 16:43:12 -0800 Subject: [PATCH 49/51] Revert "updated constants to new Hammerhead constants" This reverts commit 09abb22c969665a22cc63515458f4eb39801dceb. --- .../java/org/carlmontrobotics/Constants.java | 26 +++++++++---------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 93568ea..02b5b57 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -128,40 +128,40 @@ public static final class Drivetrainc { // kP, kI, and kD constants for turn motor controllers in the order of // front-left, front-right, back-left, back-right. // Determine correct turn PID constants - public static final double[] turnkP = { 51.078, 60.885, 60.946, 60.986/2};//sysid for fr that didnt't work{0.099412, 0.13414, 3.6809, 3.6809} //{49, 23,33, 28};//{51.078, 25, 35.946, 30.986}; // {0.00374, 0.00374, 0.00374, + public static final double[] turnkP = {12, 12, 23, 23};//sysid for fr that didnt't work{0.099412, 0.13414, 3.6809, 3.6809} //{49, 23,33, 28};//{51.078, 25, 35.946, 30.986}; // {0.00374, 0.00374, 0.00374, // 0.00374}; -` public static final double[] turnkI = { 0, 0, 0, 0};//{ 0, 0.1, 0, 0 }; -/ public static final double[] turnkD = { 0, 0.5, 0.42, 1};//{ 0.2/* dont edit */, 0.3, 0.5, 0.4}; // todo: use d + public static final double[] turnkI = {0, 0, 0, 0};//{ 0, 0.1, 0, 0 }; + public static final double[] turnkD = {1, 1.55, 0, 2};//{ 0.2/* dont edit */, 0.3, 0.5, 0.4}; // todo: use d // public static final double[] turnkS = {0.2, 0.2, 0.2, 0.2}; - public static final double[] turnkS = { 0.13027, 0.17026, 0.2, 0.23262};//sysid for fr that didnt't work{0.041796, 0.09111, 0.64804, 1.0873}//{ 0.13027, 0.17026, 0.2, 0.23262 }; + public static final double[] turnkS = {0.050634, 0.033065, 0.018117, 0.021337};//sysid for fr that didnt't work{0.041796, 0.09111, 0.64804, 1.0873}//{ 0.13027, 0.17026, 0.2, 0.23262 }; // V = kS + kV * v + kA * a // 12 = 0.2 + 0.00463 * v // v = (12 - 0.2) / 0.00463 = 2548.596 degrees/s - public static final double[] turnkV = { 2.6532, 2.7597, 2.7445, 2.7698 };//sysid for fr that didnt't work{2.6403, 2.6603, 2.6168, 2.5002} //{2.6532, 2.7597, 2.7445, 2.7698}; - public static final double[] turnkA = { 0.17924, 0.17924, 0.17924, 0.17924 };//sysid for fr that didnt't work{0.33266, 0.25535, 0.17924, 0.17924} //{ 0.17924, 0.17924, 0.17924, 0.17924 }; + public static final double[] turnkV = {2.6153, 2.5924, 2.6495, 2.6705};//sysid for fr that didnt't work{2.6403, 2.6603, 2.6168, 2.5002} //{2.6532, 2.7597, 2.7445, 2.7698}; + public static final double[] turnkA = {0.18525, 0.13879, 0.23625, 0.25589};//sysid for fr that didnt't work{0.33266, 0.25535, 0.17924, 0.17924} //{ 0.17924, 0.17924, 0.17924, 0.17924 }; // kP is an average of the forward and backward kP values // Forward: 1.72, 1.71, 1.92, 1.94 // Backward: 1.92, 1.92, 2.11, 1.89 // Order of modules: (FL, FR, BL, BR) public static final double[] drivekP = CONFIG.isSwimShady() ? new double[] { 2.8, 2.8, 2.8, 2.8 } - : new double[] {1.75*1.275, 1.75*1.275, 1.75*1.275, 1.75*1.275}; //{2.2319, 2.2462, 2.4136, 3.6862}; // {1.82/100, 1.815/100, 2.015/100, + : new double[] {1.75, 1.75, 1.75, 1.75}; //{2.2319, 2.2462, 2.4136, 3.6862}; // {1.82/100, 1.815/100, 2.015/100, // 1.915/100}; - public static final double[] drivekI = { 0, 0, 0, 0 }; - public static final double[] drivekD = { 0.005, 0.005, 0.005, 0.005 }; + public static final double[] drivekI = { 0.1, 0.1, 0.1, 0.1 }; + public static final double[] drivekD = { 0, 0, 0, 0 }; public static final boolean[] driveInversion = (CONFIG.isSwimShady() ? new boolean[] { false, false, false, false } : new boolean[] { false, true, false, true }); public static final boolean[] turnInversion = { true, true, true, true }; // kS - public static final double[] kForwardVolts = { 0.26744, 0.31897, 0.27967, 0.2461 }; - //public static final double[] kForwardVolts = { 0, 0, 0, 0}; //{0.59395, 0.52681, 0.11097, 0.17914}; //{ 0.2, 0.2, 0.2, 0.2 }; + // public static final double[] kForwardVolts = { 0.26744, 0.31897, 0.27967, 0.2461 }; + public static final double[] kForwardVolts = {0, 0, 0, 0}; //{0.59395, 0.52681, 0.11097, 0.17914}; //{ 0.2, 0.2, 0.2, 0.2 }; public static final double[] kBackwardVolts = kForwardVolts; //kV - public static final double[] kForwardVels = { 2.81, 2.9098, 2.8378, 2.7391 }; - //public static final double[] kForwardVels = { 0, 0, 0, 0 };//{2.4114, 2.7465, 2.7546, 2.7412}; //{ 0, 0, 0, 0 };//volts per m/s + // public static final double[] kForwardVels = { 2.81, 2.9098, 2.8378, 2.7391 }; + public static final double[] kForwardVels = { 0, 0, 0, 0 };//{2.4114, 2.7465, 2.7546, 2.7412}; //{ 0, 0, 0, 0 };//volts per m/s public static final double[] kBackwardVels = kForwardVels; //kA From 38864b7f1aab54e695a7a2da2406c67e432d4946 Mon Sep 17 00:00:00 2001 From: Matthew Date: Sun, 9 Mar 2025 14:30:31 -0700 Subject: [PATCH 50/51] commented out named commands for auton --- .../org/carlmontrobotics/RobotContainer.java | 72 +++++++++++++++++-- 1 file changed, 68 insertions(+), 4 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index b027fba..ea0a6f8 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -131,10 +131,74 @@ private Trigger axisTrigger(GenericHID controller, Axis axis) { } private void RegisterAutoCommands(){ - NamedCommands.registerCommand("IntakeAlgae", new IntakeAlgae(algaeEffector)); - NamedCommands.registerCommand("OuttakeAlgae", new OuttakeAlgae(algaeEffector)); - NamedCommands.registerCommand("IntakeCoral", new IntakeCoral(coralEffector)); - NamedCommands.registerCommand("OuttakeCoral", new OuttakeCoral(coralEffector)); + //I made some of the constants up, so change once merged + //AlgaeEffector + // NamedCommands.registerCommand("GroundIntakeAlgae", new GroundIntakeAlgae(algaeEffector)); + // NamedCommands.registerCommand("DealgaficationIntake", new DealgaficationIntake(algaeEffector)); + // NamedCommands.registerCommand("ShootAlgae", new ShootAlgae(algaeEffector)); + //CoralEffector + // NamedCommands.registerCommand("CoralIntake", new CoralIntake(coralEffector)); + // NamedCommands.registerCommand("CoralOutake", new CoralOutake(coralEffector)); + // //AlgaeArm + // NamedCommands.registerCommand("ArmToDeAlgafy", new ArmToPosition(AlgaeEffector, Armc.DeAlgafy_Angle)); + // NamedCommands.registerCommand("ArmToIntake", new ArmToPosition(AlgaeEffector, Armc.Intake_Angle)); + // NamedCommands.registerCommand("ArmToShoot", new ArmToPosition(AlgaeEffector, Armc.Shoot_Angle)); + // //Elevator + // NamedCommands.registerCommand("ElevatorIntake", new ElevatorLPos(Elevator, Elevatorc.IntakePos)); + // NamedCommands.registerCommand("ElevatorL1", new ElevatorLPos(Elevator, Elevatorc.L1Pos)); + // NamedCommands.registerCommand("ElevatorL2", new ElevatorLPos(Elevator, Elevatorc.L2Pos)); + // NamedCommands.registerCommand("ElevatorL3", new ElevatorLPos(Elevator, Elevatorc.L3Pos)); + // NamedCommands.registerCommand("ElevatorL4", new ElevatorLPos(Elevator, Elevatorc.L4Pos)); + + // //Limelight + // NamedCommands.registerCommand("AlignToCoralStation", new AlignToCoralStation(Limelight, drivetrain)); + // NamedCommands.registerCommand("AlignToReef", new AlignToReef(Limelight, drivetrain)); + // NamedCommands.registerCommand("MoveToLeftBranch", new MoveToLeftBranch(Limelight, LimelightHelpers, drivetrain)); + // NamedCommands.registerCommand("MoveToRightBranch", new MoveToRightBranch(Limelight, LimelightHelpers, drivetrain)); + + //Sequential and Parralel Commands + /*NamedCommands.registerCommand("L2", new SequentialCommandGroup( + new ParallelDeadlineGroup( + new WaitCommand(3.0), + new SequentialCommandGroup( + new ParallelCommandGroup( + new AlignToReef(drivetrain, limelight), + new ElevatorL2(Elevator, Elevatorc.L2Pos), + new ArmToPos(arm, Armc.DeAlgafy_Angle)), + new DealgaficationIntake(algaeEffector), + new CoralOutake(coralEffector), + new ElevatorIntake(Elevator, Elevatorc.IntakePos)))));*/ + + /*NamedCommands.registerCommand("L3", new SequentialCommandGroup( + new ParallelDeadlineGroup( + new WaitCommand(3.0), + new SequentialCommandGroup( + new ParallelCommandGroup( + new AlignToReef(drivetrain, limelight), + new ElevatorL3(Elevator, Elevatorc.L3Pos), + new ArmToPos(arm, Armc.DeAlgafy_Angle)), + new DealgaficationIntake(algaeEffector), + new CoralOutake(coralEffector), + new ElevatorIntake(Elevator, Elevatorc.IntakePos)))));*/ + + /*NamedCommands.registerCommand("L4", new SequentialCommandGroup( + new ParallelDeadlineGroup( + new WaitCommand(3.0), + new SequentialCommandGroup( + new ParallelCommandGroup( + new AlignToReef(drivetrain, limelight), + new ElevatorL3(Elevator, Elevatorc.L4Pos), + new ArmToPos(arm, Armc.DeAlgafy_Angle)), + new DealgaficationIntake(algaeEffector), + new CoralOutake(coralEffector), + new ElevatorIntake(Elevator, Elevatorc.IntakePos)))));*/ + + /*NamedCommands.registerCommand("IntakeCoral", + new SequentialCommandGroup( + new ParallelCommandGroup( + new AlignToCoralStation(drivetrain, limelight), + new IntakeCoral(coralEffector))));*/ + } /*public Command getAutonomousCommand() { From 77b61482105e4a64fb31a7daa84a4d9e44a760e7 Mon Sep 17 00:00:00 2001 From: Rand0mAsianKid <144396869+Rand0mAsianKid@users.noreply.github.com> Date: Mon, 10 Mar 2025 18:39:29 -0700 Subject: [PATCH 51/51] deleted algae and coral subsystems --- .../org/carlmontrobotics/RobotContainer.java | 28 +--- .../commands/IntakeAlgae.java | 48 ------ .../commands/IntakeCoral.java | 44 ------ .../commands/LastResortAuto.java | 50 ++++++ .../commands/OuttakeAlgae.java | 39 ----- .../commands/OuttakeCoral.java | 45 ------ .../subsystems/AlgaeEffector.java | 146 ------------------ .../subsystems/CoralEffector.java | 97 ------------ 8 files changed, 51 insertions(+), 446 deletions(-) delete mode 100644 src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java delete mode 100644 src/main/java/org/carlmontrobotics/commands/IntakeCoral.java create mode 100644 src/main/java/org/carlmontrobotics/commands/LastResortAuto.java delete mode 100644 src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java delete mode 100644 src/main/java/org/carlmontrobotics/commands/OuttakeCoral.java delete mode 100644 src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java delete mode 100644 src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index ea0a6f8..3ddb80f 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -15,9 +15,6 @@ import org.carlmontrobotics.Constants.OI; import org.carlmontrobotics.Constants.OI.Manipulator.*; import org.carlmontrobotics.Constants.OI.Manipulator; -import org.carlmontrobotics.commands.OuttakeAlgae; -import org.carlmontrobotics.subsystems.AlgaeEffector; -import org.carlmontrobotics.subsystems.CoralEffector; import org.carlmontrobotics.subsystems.Drivetrain; import com.pathplanner.lib.auto.AutoBuilder; @@ -26,10 +23,6 @@ import com.pathplanner.lib.path.GoalEndState; import com.pathplanner.lib.path.PathPlannerPath; -import org.carlmontrobotics.commands.IntakeAlgae; -import org.carlmontrobotics.commands.IntakeCoral; -import org.carlmontrobotics.commands.OuttakeCoral; - import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.DriverStation; @@ -64,8 +57,6 @@ public class RobotContainer { //2. Use absolute paths from constants to reduce confusion public final GenericHID driverController = new GenericHID(OI.Driver.driverPort); public final GenericHID manipulatorController = new GenericHID(OI.Manipulator.manipulatorPort); - private final AlgaeEffector algaeEffector = new AlgaeEffector(); - private final CoralEffector coralEffector = new CoralEffector(); private final Drivetrain drivetrain = new Drivetrain(); private final SendableChooser autoChooser; @@ -111,24 +102,7 @@ public RobotContainer() { private void setDefaultCommands() {} private void setBindingsDriver() {} - private void setBindingsManipulator() { - axisTrigger(manipulatorController, OI.Manipulator.IntakeTrigger) - .whileTrue(new IntakeCoral(coralEffector)); - - axisTrigger(manipulatorController, OI.Manipulator.OuttakeTrigger) - .whileTrue(new OuttakeCoral(coralEffector)); - - new JoystickButton(manipulatorController, OI.Manipulator.IntakeBumper) - .whileTrue(new IntakeAlgae(algaeEffector)); - - new JoystickButton(manipulatorController, OI.Manipulator.OuttakeBumper) - .whileFalse(new OuttakeAlgae(algaeEffector)); - } - - private Trigger axisTrigger(GenericHID controller, Axis axis) { - return new Trigger(() -> Math - .abs(getStickValue(controller, axis)) > Constants.OI.MIN_AXIS_TRIGGER_VALUE); - } + private void setBindingsManipulator() {} private void RegisterAutoCommands(){ //I made some of the constants up, so change once merged diff --git a/src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java b/src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java deleted file mode 100644 index ee38226..0000000 --- a/src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java +++ /dev/null @@ -1,48 +0,0 @@ -package org.carlmontrobotics.commands; - -import static org.carlmontrobotics.Constants.CoralEffectorc.*; - -import org.carlmontrobotics.Constants; -import org.carlmontrobotics.subsystems.AlgaeEffector; -import org.carlmontrobotics.subsystems.CoralEffector; - -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; - -public class IntakeAlgae extends Command { - private final AlgaeEffector Algae; - private final Timer timer = new Timer(); - public IntakeAlgae(AlgaeEffector Algae) { - addRequirements(this.Algae = Algae); - } - - @Override - public void initialize() { - timer.reset(); - timer.start(); - Algae.setTopRPM(Constants.AlgaeEffectorc.intakeTopRPM); - Algae.setBottomRPM(Constants.AlgaeEffectorc.intakeBottomRPM); - Algae.setPincherRPM(Constants.AlgaeEffectorc.intakePincherRPM); - } - - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() {} - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - Algae.stopMotors(); - //TODO: Test different times - } - - @Override - public boolean isFinished() { - //distance sensor doesn't detect coral - //TODO: make distance sensor stuff - //TODO: add smartdashboard - return Algae.limitDetects() || timer.get() > 1; //Simulator doesnt work propperly because limiswtich is non existant (only for simulator) - } -} \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/commands/IntakeCoral.java b/src/main/java/org/carlmontrobotics/commands/IntakeCoral.java deleted file mode 100644 index 980387f..0000000 --- a/src/main/java/org/carlmontrobotics/commands/IntakeCoral.java +++ /dev/null @@ -1,44 +0,0 @@ -package org.carlmontrobotics.commands; - -import static org.carlmontrobotics.Constants.CoralEffectorc.*; - -import org.carlmontrobotics.Constants; -import org.carlmontrobotics.subsystems.CoralEffector; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; - -public class IntakeCoral extends Command { - private final CoralEffector Coral; - private final Timer timer = new Timer(); - public IntakeCoral(CoralEffector Coral) { - addRequirements(this.Coral = Coral); - } - - @Override - public void initialize() { - timer.reset(); - timer.start(); - Coral.setRPM(Constants.CoralEffectorc.intakeRPM); - } - - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() {} - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - Coral.stopEffector(); - //TODO: Test different times - } - - @Override - public boolean isFinished() { - //distance sensor doesn't detect coral - //TODO: make distance sensor stuff - //TODO: add smartdashboard - return (Coral.coralDetects() && Coral.limitDetects()) || timer.get() > 4; - } -} \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/commands/LastResortAuto.java b/src/main/java/org/carlmontrobotics/commands/LastResortAuto.java new file mode 100644 index 0000000..8ca8d26 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/commands/LastResortAuto.java @@ -0,0 +1,50 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package org.carlmontrobotics.commands; + +import org.carlmontrobotics.subsystems.Drivetrain; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; + +public class LastResortAuto extends Command { + private final Timer timer = new Timer(); + /** Creates a new LastResortAuto. */ + private final Drivetrain drivetrain; + private boolean prev; + + int MAX_SECONDS_DRIVE = 4; + + public LastResortAuto(Drivetrain drivetrain) { + addRequirements(this.drivetrain = drivetrain); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + timer.reset(); + timer.start(); + prev = drivetrain.getFieldOriented(); + drivetrain.setFieldOriented(false); + drivetrain.drive(1, 0, 0); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + drivetrain.setFieldOriented(prev); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return timer.hasElapsed(4); + } +} \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java b/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java deleted file mode 100644 index 10d885e..0000000 --- a/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java +++ /dev/null @@ -1,39 +0,0 @@ -package org.carlmontrobotics.commands; -import org.carlmontrobotics.Constants; -import org.carlmontrobotics.subsystems.AlgaeEffector; - -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj2.command.Command; - -public class OuttakeAlgae extends Command { - AlgaeEffector Algae; - private final Timer timer = new Timer(); - public OuttakeAlgae(AlgaeEffector algaeEffector) { - addRequirements(this.Algae = algaeEffector); - } - - @Override - public void initialize() { - Algae.setTopRPM(Constants.AlgaeEffectorc.outtakeTopRPM); - Algae.setBottomRPM(Constants.AlgaeEffectorc.outtakeBottomRPM); - Algae.setPincherRPM(Constants.AlgaeEffectorc.outtakePincherRPM); - timer.start(); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - Algae.stopMotors(); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return Algae.limitDetects() || timer.get() > 1; //Simulator doesnt work propperly because limiswtich is non existant (only for simulator) - } -} \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/commands/OuttakeCoral.java b/src/main/java/org/carlmontrobotics/commands/OuttakeCoral.java deleted file mode 100644 index 40436ed..0000000 --- a/src/main/java/org/carlmontrobotics/commands/OuttakeCoral.java +++ /dev/null @@ -1,45 +0,0 @@ -package org.carlmontrobotics.commands; - -import static org.carlmontrobotics.Constants.CoralEffectorc.*; - -import org.carlmontrobotics.Constants; -import org.carlmontrobotics.subsystems.CoralEffector; - -import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; - -public class OuttakeCoral extends Command { - private final CoralEffector Coral; - private final Timer timer = new Timer(); - public OuttakeCoral(CoralEffector Coral) { - addRequirements(this.Coral = Coral); - } - - @Override - public void initialize() { - timer.reset(); - timer.start(); - Coral.setRPM(Constants.CoralEffectorc.outtakeRPM); - } - - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() {} - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - Coral.stopEffector(); - } - - @Override - public boolean isFinished() { - //distance sensor doesn't detect coral - //TODO: make distance sensor stuff - //TODO: add smartdashboard - return (!Coral.coralDetects() && !Coral.limitDetects()) || timer.get() > 4; - } -} \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java deleted file mode 100644 index 89ae98a..0000000 --- a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java +++ /dev/null @@ -1,146 +0,0 @@ -package org.carlmontrobotics.subsystems; - - -import org.carlmontrobotics.lib199.MotorConfig; -import org.carlmontrobotics.lib199.MotorControllerFactory; - - -import static org.carlmontrobotics.RobotContainer.*; - -import org.carlmontrobotics.Constants; -import org.carlmontrobotics.RobotContainer; - -import com.revrobotics.RelativeEncoder; -import com.revrobotics.spark.ClosedLoopSlot; -import com.revrobotics.spark.SparkBase.ControlType; -import com.revrobotics.spark.SparkBase.PersistMode; -import com.revrobotics.spark.SparkBase.ResetMode; -import com.revrobotics.spark.SparkClosedLoopController; -import com.revrobotics.spark.SparkFlex; -import com.revrobotics.spark.SparkLowLevel.MotorType; -import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; -import com.revrobotics.spark.config.SparkFlexConfig; -import com.revrobotics.spark.config.SparkMaxConfig; -import com.revrobotics.spark.SparkMax; - -import edu.wpi.first.hal.SimDouble; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.controller.ArmFeedforward; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; -import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.math.util.Units; - -import edu.wpi.first.units.Measure; -import edu.wpi.first.units.MutableMeasure; - -import edu.wpi.first.util.sendable.SendableBuilder; -import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.RobotBase; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; -import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; -import edu.wpi.first.wpilibj.simulation.SimDeviceSim; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - - -public class AlgaeEffector extends SubsystemBase { - private SparkFlex topMotor = new SparkFlex(Constants.AlgaeEffectorc.upperMotorID, MotorType.kBrushless); //why is there a vortex motor on coral effectors? - private SparkMax bottomMotor = new SparkMax(Constants.AlgaeEffectorc.lowerMotorID, MotorType.kBrushless); //why is there a vortex motor on coral effectors? - private SparkFlex pincherMotor = new SparkFlex(Constants.AlgaeEffectorc.pinchMotorID, MotorType.kBrushless); //why is there a vortex motor on coral effectors? - - private final RelativeEncoder topEncoder = topMotor.getEncoder(); - private final RelativeEncoder bottomEncoder = bottomMotor.getEncoder(); - private final RelativeEncoder pincherEncoder = pincherMotor.getEncoder(); - - private final SparkClosedLoopController pidControllerTop = topMotor.getClosedLoopController(); - private final SparkClosedLoopController pidControllerBottom = bottomMotor.getClosedLoopController(); - private final SparkClosedLoopController pidControllerPincher = pincherMotor.getClosedLoopController(); - - private final SimpleMotorFeedforward topFeedforward = new SimpleMotorFeedforward(Constants.kS[Constants.AlgaeEffectorc.TopkS], Constants.kV[Constants.AlgaeEffectorc.TopkS], Constants.kA[Constants.AlgaeEffectorc.TopkS]); - private final SimpleMotorFeedforward bottomFeedforward = new SimpleMotorFeedforward(Constants.kS[Constants.AlgaeEffectorc.BottomkS], Constants.kV[Constants.AlgaeEffectorc.BottomkS], Constants.kA[Constants.AlgaeEffectorc.BottomkS]); - private final SimpleMotorFeedforward pincherFeedforward = new SimpleMotorFeedforward(Constants.kS[Constants.AlgaeEffectorc.PincherkS], Constants.kV[Constants.AlgaeEffectorc.PincherkS], Constants.kA[Constants.AlgaeEffectorc.PincherkS]); - //TODO: add feedforward - - DigitalInput limitSwitch = new DigitalInput(1); - - public boolean limitDetects() { - return limitSwitch.get(); - } - //-------------------------------------------------------------------------------------------- - public AlgaeEffector() { - SparkFlexConfig pincherMotorConfig = new SparkFlexConfig(); - SparkMaxConfig bottomMotorConfig = new SparkMaxConfig(); - SparkFlexConfig topMotorConfig = new SparkFlexConfig(); - - - topMotorConfig.closedLoop.pid( - Constants.kP[Constants.AlgaeEffectorc.TopkS], - Constants.kI[Constants.AlgaeEffectorc.TopkS], - Constants.kD[Constants.AlgaeEffectorc.TopkS] - ).feedbackSensor(FeedbackSensor.kPrimaryEncoder); - topMotor.configure(topMotorConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); - - bottomMotorConfig.closedLoop.pid( - Constants.kP[Constants.AlgaeEffectorc.BottomkS], - Constants.kI[Constants.AlgaeEffectorc.BottomkS], - Constants.kD[Constants.AlgaeEffectorc.BottomkS] - ).feedbackSensor(FeedbackSensor.kPrimaryEncoder); - bottomMotor.configure(bottomMotorConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); - - pincherMotorConfig.closedLoop.pid( - Constants.kP[Constants.AlgaeEffectorc.PincherkS], - Constants.kI[Constants.AlgaeEffectorc.PincherkS], - Constants.kD[Constants.AlgaeEffectorc.PincherkS] - ).feedbackSensor(FeedbackSensor.kPrimaryEncoder); - pincherMotor.configure(pincherMotorConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); - } - //---------------------------------------------------------------------------------------- - - public void setTopRPM(double toprpm) { - pidControllerTop.setReference(toprpm, ControlType.kVelocity, ClosedLoopSlot.kSlot0); - topFeedforward.calculate(toprpm); - } - - public void setBottomRPM(double bottomrpm) { - pidControllerBottom.setReference(bottomrpm, ControlType.kVelocity, ClosedLoopSlot.kSlot0); - bottomFeedforward.calculate(bottomrpm); - } - - public void setPincherRPM(double pincherrpm) { - pidControllerPincher.setReference(pincherrpm, ControlType.kVelocity, ClosedLoopSlot.kSlot0); - pincherFeedforward.calculate(pincherrpm); - } - - public void runRPM() { - //TODO: Change RPM according to design - setTopRPM(1000); - setBottomRPM(2100); - setPincherRPM(2100); - } - - public void stopMotors() { - setTopRPM(0); - setBottomRPM(0); - setPincherRPM(0); - } - - public boolean checkIfAtTopRPM(double rpm) { - return topEncoder.getVelocity() == rpm; - } - - public boolean checkIfAtBottomRPM(double rpm) { - return bottomEncoder.getVelocity() == rpm; - } - - public void setSpeed(double speed) { - topMotor.set(speed); - bottomMotor.set(speed); - pincherMotor.set(speed); - } -} diff --git a/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java b/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java deleted file mode 100644 index ba500c9..0000000 --- a/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java +++ /dev/null @@ -1,97 +0,0 @@ -package org.carlmontrobotics.subsystems; - -import org.carlmontrobotics.Constants; -import org.carlmontrobotics.lib199.MotorConfig; -import org.carlmontrobotics.lib199.MotorControllerFactory; - -import com.playingwithfusion.TimeOfFlight; -import com.playingwithfusion.TimeOfFlight.RangingMode; -import com.revrobotics.RelativeEncoder; -import com.revrobotics.spark.SparkFlex; -import com.revrobotics.spark.SparkMax; -import com.revrobotics.spark.SparkBase.ControlType; -import com.revrobotics.spark.SparkBase.PersistMode; -import com.revrobotics.spark.SparkBase.ResetMode; -import com.revrobotics.spark.ClosedLoopSlot; -import com.revrobotics.spark.SparkClosedLoopController; - -import edu.wpi.first.math.controller.SimpleMotorFeedforward; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.util.datalog.DataLogEntry; -import edu.wpi.first.util.datalog.StringLogEntry; -import edu.wpi.first.util.sendable.SendableBuilder; -import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import com.revrobotics.spark.SparkLowLevel.MotorType; -import com.revrobotics.spark.config.SparkFlexConfig; -import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; -import com.revrobotics.spark.SparkFlex; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; -import static org.carlmontrobotics.Constants.CoralEffectorc.*; - -public class CoralEffector extends SubsystemBase { - private SparkMax effectorMotor = new SparkMax(Constants.CoralEffectorc.effectorMotorID, MotorType.kBrushless); //why is there a vortex motor on coral effectors? - private final RelativeEncoder effectorEncoder = effectorMotor.getEncoder(); - private final SparkClosedLoopController pidControllerEffector = effectorMotor.getClosedLoopController(); - // private final SimpleMotorFeedforward effectorFeedforward = new SimpleMotorFeedforward(kS[OUTTAKE], kV[OUTTAKE], kA[OUTTAKE]); - private double goalOutakeRPM = effectorEncoder.getVelocity(); - DigitalInput limitSwitch = new DigitalInput(0); //TODO: change channel after wired - private Timer effectorTOFTimer = new Timer(); - private TimeOfFlight effectorDistanceSensor = new TimeOfFlight(Constants.CoralEffectorc.effectorDistanceSensorID); - - private double lastValidDistance = Double.POSITIVE_INFINITY; - - public boolean coralDetects() { - return lastValidDistance < DETECT_DISTANCE_INCHES; - } - - public boolean limitDetects() { - return limitSwitch.get(); - } - - public CoralEffector() { - SparkFlexConfig effectorConfig = new SparkFlexConfig(); - effectorConfig.closedLoop.pid( - Constants.CoralEffectorc.kP, - Constants.CoralEffectorc.kI, - Constants.CoralEffectorc.kD - ).feedbackSensor(FeedbackSensor.kPrimaryEncoder); - effectorMotor.configure(effectorConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); - //Will add distance sensor later - - } - - public void setSpeed(double speed) { - effectorMotor.set(speed); - } - - public void setRPM(double rpm) { - pidControllerEffector.setReference(rpm, ControlType.kVelocity, ClosedLoopSlot.kSlot0); - } - - public void stopEffector() { - setRPM(0); - } - - public void setRPM() { - setRPM(2100); - } - - public void updateValues() { - if (effectorDistanceSensor.isRangeValid()) { - if (lastValidDistance != 5.75) { - effectorTOFTimer.reset(); - } else - effectorTOFTimer.start(); - lastValidDistance = Units.metersToInches(effectorDistanceSensor.getRange()); - } - } - - @Override - public void periodic() { - updateValues(); - limitDetects(); - } -}