diff --git a/.readthedocs.yaml b/.readthedocs.yaml new file mode 100644 index 0000000..9138a7f --- /dev/null +++ b/.readthedocs.yaml @@ -0,0 +1,13 @@ +version: "2" + +build: + os: "ubuntu-22.04" + tools: + python: "3.10" + +python: + install: + - requirements: docs/requirements.txt + +sphinx: + configuration: docs/source/conf.py diff --git a/docs/Makefile b/docs/Makefile new file mode 100644 index 0000000..d0c3cbf --- /dev/null +++ b/docs/Makefile @@ -0,0 +1,20 @@ +# Minimal makefile for Sphinx documentation +# + +# You can set these variables from the command line, and also +# from the environment for the first two. +SPHINXOPTS ?= +SPHINXBUILD ?= sphinx-build +SOURCEDIR = source +BUILDDIR = build + +# Put it first so that "make" without argument is like "make help". +help: + @$(SPHINXBUILD) -M help "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) + +.PHONY: help Makefile + +# Catch-all target: route all unknown targets to Sphinx using the new +# "make mode" option. $(O) is meant as a shortcut for $(SPHINXOPTS). +%: Makefile + @$(SPHINXBUILD) -M $@ "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) diff --git a/docs/make.bat b/docs/make.bat new file mode 100644 index 0000000..9534b01 --- /dev/null +++ b/docs/make.bat @@ -0,0 +1,35 @@ +@ECHO OFF + +pushd %~dp0 + +REM Command file for Sphinx documentation + +if "%SPHINXBUILD%" == "" ( + set SPHINXBUILD=sphinx-build +) +set SOURCEDIR=source +set BUILDDIR=build + +if "%1" == "" goto help + +%SPHINXBUILD% >NUL 2>NUL +if errorlevel 9009 ( + echo. + echo.The 'sphinx-build' command was not found. Make sure you have Sphinx + echo.installed, then set the SPHINXBUILD environment variable to point + echo.to the full path of the 'sphinx-build' executable. Alternatively you + echo.may add the Sphinx directory to PATH. + echo. + echo.If you don't have Sphinx installed, grab it from + echo.http://sphinx-doc.org/ + exit /b 1 +) + +%SPHINXBUILD% -M %1 %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% %O% +goto end + +:help +%SPHINXBUILD% -M help %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% %O% + +:end +popd diff --git a/docs/requirements.txt b/docs/requirements.txt new file mode 100644 index 0000000..53fc1f3 --- /dev/null +++ b/docs/requirements.txt @@ -0,0 +1,2 @@ +sphinx==7.1.2 +sphinx-rtd-theme==1.3.0rc1 diff --git a/docs/source/api.rst b/docs/source/api.rst new file mode 100644 index 0000000..9f86dc9 --- /dev/null +++ b/docs/source/api.rst @@ -0,0 +1,7 @@ +POPLib API Documentation +======================== + +.. toctree:: + + base + application \ No newline at end of file diff --git a/docs/source/application.rst b/docs/source/application.rst new file mode 100644 index 0000000..4839f22 --- /dev/null +++ b/docs/source/application.rst @@ -0,0 +1,10 @@ +Application Layer POPLib Documentation +====================================== + +.. toctree:: + + application/swerve + application/elevator + application/pivot + application/pnuematics + application/flywheel diff --git a/docs/source/application/elevator.rst b/docs/source/application/elevator.rst new file mode 100644 index 0000000..2e5af1f --- /dev/null +++ b/docs/source/application/elevator.rst @@ -0,0 +1,2 @@ +Elevator +======== \ No newline at end of file diff --git a/docs/source/application/flywheel.rst b/docs/source/application/flywheel.rst new file mode 100644 index 0000000..a5fda6b --- /dev/null +++ b/docs/source/application/flywheel.rst @@ -0,0 +1,2 @@ +Flywheel +======== \ No newline at end of file diff --git a/docs/source/application/pivot.rst b/docs/source/application/pivot.rst new file mode 100644 index 0000000..6a92c4e --- /dev/null +++ b/docs/source/application/pivot.rst @@ -0,0 +1,2 @@ +Pivot +===== \ No newline at end of file diff --git a/docs/source/application/pnuematics.rst b/docs/source/application/pnuematics.rst new file mode 100644 index 0000000..e65026a --- /dev/null +++ b/docs/source/application/pnuematics.rst @@ -0,0 +1,2 @@ +Pnuematics +========== \ No newline at end of file diff --git a/docs/source/application/swerve.rst b/docs/source/application/swerve.rst new file mode 100644 index 0000000..7eac7df --- /dev/null +++ b/docs/source/application/swerve.rst @@ -0,0 +1,2 @@ +Swerve +====== \ No newline at end of file diff --git a/docs/source/base.rst b/docs/source/base.rst new file mode 100644 index 0000000..b558109 --- /dev/null +++ b/docs/source/base.rst @@ -0,0 +1,14 @@ +Base POPLib Documentation +========================= + +.. toctree:: + + base/motors + base/pid + base/gyro + base/absolute encoders + base/camera + base/limelight + base/io + base/beambreak + base/tunable numbers \ No newline at end of file diff --git a/docs/source/base/absolute encoders.rst b/docs/source/base/absolute encoders.rst new file mode 100644 index 0000000..18755b0 --- /dev/null +++ b/docs/source/base/absolute encoders.rst @@ -0,0 +1,40 @@ +Absolute Encoders +================= + +There are many use cases for absolute encoders, typically as a QOL feature for Swerve. +Because of this, we provide an easy-to-use API for absolute encoders. Our team only uses +one type of absolute encoder: a CANCoder. To create a new CANCoder, you first need to create a +CANCoderConfig: + +.. code-block:: java + + CANCoderConfig canCoderConfig = new CANCoderConfig(CAN_ID, CAN_BUS_NAME, OFFSET, INVERTED); + +We first require you to give the CAN ID of the CANCoder. Then, we ask you the CAN Bus name +that the CANCoder is on. We then ask you the offset that should be applied to this CANCoders. +CANCoders are absolute encoders, which means that they will remember this offset and can be used +to move a motor back to the same place every time, even when the robot has just started up. This +differs from normal encoders as those need to be zeroed out at the start of every match to be usable, +and can't "remember" anything. Finally, we ask if the CANCoder is inverted. We can now use this config +to create a CANCoder: + +.. code-block:: java + + CANCoder encoder = new CANCoder(Constants.canCoderConfig); + +.. note:: + + Whenever a CAN device (like a CANCoder) is created in POPLib, it is registered in the internal + CAN ID Registry. If a duplicate CAN ID is found, an error message will be printed out to + DriverStation (instead of actually crashing the robot program). Please take POPLib DriverStation + error messages seriously. Duplicate CAN IDs make it difficult to debug a robot during competition + and can lead to unexcepted errors. + +There is only one use of the CANCoder: moving your motors to thier "zero" position at the start of a match. +Lets say that we already have created a Motor variable (named motor) that is configured with PID. If we +want to move it to the zero position, we would do something like this: + +.. code-block:: java + + motor.setTargetPosition(encoder.getPosition().getRotations()); + diff --git a/docs/source/base/beambreak.rst b/docs/source/base/beambreak.rst new file mode 100644 index 0000000..2f1844b --- /dev/null +++ b/docs/source/base/beambreak.rst @@ -0,0 +1,38 @@ +BeamBreak +========= + +In FRC, it is very useful to know if an object is in a mechanism on your robot. To do +that, a beambreak sensor is typically used, so POPLib provides a helpful BeamBreak API +to help you query your beambreak. We start by creating a object like so: + +.. code-block:: java + + BeamBreak beambreakSensor = new BeamBreak(Constants.BEAMBREAK_PORT_ID); + +To create a BeamBreak object, all you need is the port id that the beambreak is on. + +.. note:: + + The "Port Id" is NOT a CAN ID as the beambreak is not on the CAN Loop. It instead + directly connects to the roborio's digitial input ports. We are asking what digital + input port your beambreak is on. If you dont know, consult your local electrical subteam. + +Lets talk about how to use the BeamBreak API. If you want to see if the beambreak sensor is +blocked, do something like this: + +.. code-block:: java + + boolean isObjectBlockingSensor = beambreakSensor.isBlocked(); + +Another cool thing you can do with the BeamBreak API is command chaining. Say you have a Command +(we are representing our example command as a variable named "command") and you want to run this +command until the beambreak is blocked. This would be helpful, say, if you have a beambreak on your +intake and you want to stop your intake motors once the beambreak detects that a gamepiece is in the +intake: + +.. code-block:: java + + command.until(beambreakSensor.getBlockedSupplier()).andThen(...); + +And thats all you really need to know about the BeamBreak API. If you want to know more, read the java +docs. \ No newline at end of file diff --git a/docs/source/base/camera.rst b/docs/source/base/camera.rst new file mode 100644 index 0000000..d7b2405 --- /dev/null +++ b/docs/source/base/camera.rst @@ -0,0 +1,66 @@ +Photonvision Cameras +==================== + +Here at Pack of Parts, we use our PhotonVision Camera to detect April Tags and +do vision-based pose estimation, and we use our limelight cameras to detect objects +on the ground (such as gamepieces). This page will be dedicated to covering the +Camera API, which represents PHOTONVISION Cameras. + +To create a new Camera, we of course have to create a new CameraConfig. Lets take a +look at the constructor: + +.. code-block:: java + + public CameraConfig(String cameraName, + Transform3d cameraToRobot, + double poseAmbiguityThreshold, + double poseDistanceThreshold, + StdDevStategy stdDevStategy, + AprilTagFields thisYearsField) + +The first parameter is the name of the Camera. Note that this is the name of the +CAMERA as in the PhotonVision settings and NOT the name of the computer that is +running PhotonVision. The second parameter is the position of the camera's focal +lens relative to the center of the robot, represented as a Transform3d and in +WPILIB's Robot Coordinate System. Next we ask you for the maximum allowed ambiguity +and distance that an April Tag detection can have for it to still be considered valid. +The higher these numbers, more "accurate" your April Tag detections will be but you +will detect less AprilTags. Finally, we ask for the April Tag map that tell us where +each April Tag is on the field. This is given to you by WPILIB. + +We can now use the Config to create a new PhotonVision Camera: + +.. code-block:: java + + Camera camera = new Camera(Constants.CAMERA_CONFIG); + +We can now use this camera in various ways. One of the most popular ways of using a +PhotonVision Camera is like so. Let us assume you have created a variable of the type +SwerveDrivePoseEstimator (which is a WPILIB class, so go read those docs for more info). +Let us call this variable "poseEstimator". You can then do something like this in your +drivebase's periodic loop (note that this is done for you if you are using POPLib Swerve): + +.. code-block:: java + + Optional estPose = camera.getEstimatedPose(this.getOdomPose()); + if (estPose.isPresent()) { + this.odom.addVisionMeasurement(estPose.get().estimatedPose.toPose2d(), + estPose.get().timestampSeconds, + camera.getVisionStdDevs()); + } + +This is how you do vision-based pose estimation with variable standard deviations. That's +it, if you use the magic of POPLib. + +Now, the second way to use the Camera API is to retieve the distance from the that the +camera is from the April Tag, which is helpful for vision-based alignment. Do something +like this: + +.. code-block:: java + + Pose2d tagToCameraDifference; + Optional possiblePose = camera.relativeDistanceFromCameraToAprilTag(); + if (possiblePose.isPresent()) { + tagToCameraDifference = possiblePose.get(); + } + diff --git a/docs/source/base/gyro.rst b/docs/source/base/gyro.rst new file mode 100644 index 0000000..d8c2468 --- /dev/null +++ b/docs/source/base/gyro.rst @@ -0,0 +1,72 @@ +Gyro +==== + +It is incredibly easy to create a new Gyro object using POPLib. It is first important to +note that the Gyro class is an abstract class, meaning that you will actally want to create +a new Pigeon object instead of a new Gryo object (where the Pigeon is a specific type of gyro +our team uses). + +To create a new Pigeon object simply do this: + +.. code-block:: java + + PigeonConfig config = new PigeonConfig(Constants.GYRO_CAN_ID, Constants.GYRO_INVERTED, Constants.CAN_BUS_NAME) + Pigeon gyro = new Pigeon(config); + +The constructor is rather simple: it requires the CAN ID of the Pigeon, whether or not the Pigeon +is inverted (the front of your Pigeon should always be pointing to the front of your robot, and your +Pigeon should always be at the EXACT center of your robot. If the front of the Pigeon is facing the +back of your robot, then set this Pigeon as being inverted). Finally, we ask for the name of the CAN +Bus Loop where this device is located. + +.. note:: + + Whenever a CAN device (like a Pigeon) is created in POPLib, it is registered in the internal + CAN ID Registry. If a duplicate CAN ID is found, an error message will be printed out to + DriverStation (instead of actually crashing the robot program). Please take POPLib DriverStation + error messages seriously. Duplicate CAN IDs make it difficult to debug a robot during competition + and can lead to unexcepted errors. + +Now lets move on to how to use the gyro. During the start of your match you will likely want to zero out +your gyro, which can be done like so: + +.. code-block:: java + + gyro.zeroGyro(); // both of these do the same thing + gyro.setAngle(new Rotation2d(0)); // both of these do the same thing + +One of the main uses for a gyro is to get the yaw angle of your robot, normalized (fit from 0 to 360). This +may be done like so: + +.. code-block:: java + + Angle rotationOfRobotAsAngle = gyro.getNormalizedAngle(); + Rotation2d rotationOfRobotAsRot2d = gyro.getNormalizedRotation2dAngle(); + +Again, both of these do the exact same thing, just in different formats. Now, sometimes, you may want to be +really fancy, and you want to know the angle of the robot with the latency already compensated (because +technically when you call gyro.getNormalizedAngle(), the value it returns is a few milliseconds old due to +fact that the electrical signal has to travel from the gyro to the robot brain). If you want to know the +exact angle of the robot at that exact moment in time use: + +.. code-block:: java + + Angle rotationOfRobotWithLatComp = gyro.getLatencyCompensatedAngle(); + +However, note that this is truly unnessesary and very over the top. Something that may be more helpful is +knowing the AngluarVelocity of your robot. To get this from your gyro, simply do: + +.. code-block:: java + + AngluarVelocity gyroRotVelo = gyro.getAngularVelo(); + +Finally, there may be cases where you need to access the raw values of the pitch, yaw, and roll of the robot. +These values will be NOT BE INVERTED and will NOT BE NORMALIZED. They can be accessed like so: + +.. code-block:: java + + Angle gyroYaw = gyro.getYaw(); + Angle gyroPitch = gyro.getPitch(); + Angle gyroRoll = gyro.getRoll(); + + diff --git a/docs/source/base/io.rst b/docs/source/base/io.rst new file mode 100644 index 0000000..28b4a57 --- /dev/null +++ b/docs/source/base/io.rst @@ -0,0 +1,67 @@ +Input/Controllers +================= + +POPLib also makes it extremely easy to access inputs from controllers and map controller +buttons to Commands. Lets take a look at how to use POPLib's IO APIs. + +There are two controller setups that are programmed into POPLib: Having a setup with two +Microsoft Flight Simulator Joysticks as drive controllers and one Xbox Controller as an +operator controller. The second setup, which is what we recommend, is having two Xbox +controllers, one for the driver and one the operator. Lets take a look at how to create +objects to represent both of these setups: + +.. code-block:: java + + Joysticks input = Joysticks.getInstance(); + // OR + XboxIO input = XboxIO.getInstance(); + +Note that, for once, we don't have to pass in a Config object or some parameters. However, +you must ensure that your physical joysticks/controllers are in the right port in DriverStation. +For a Joysticks setup, your Microsoft Flight Simulator Joysticks must be on ports 0 and 1, +while your Xbox controller must be on port 2. For a XboxIO setup, your driver Xbox controller +must be on port 0 while your operator Xbox controller must be on port 1. + +We will now go over how to use the Controller API. Note that no matter your selection of IO +(Joysticks or XboxIO), the way that these methods/functions work will not change. It was +designed such that you will easily be able to change what hardware you are using to control +everything. + +Lets first explore how to get access to instructions from the driver on how to move the robot: + +.. code-block:: java + + double horizontalMovement = input.getDriveTrainTranslationX(); + double verticalMovement = input.getDriveTrainTranslationY(); + double rotationMovement = input.getDriveTrainRotation(); + +It is also recommended to apply a deadzone to these values to avoid controller drift: + +.. code-block:: java + + horizontalMovement = ControllerMath.applyDeadband(horizontalMovement, Constants.CONTROLLER_DEADZONE); + verticalMovement = ControllerMath.applyDeadband(verticalMovement, Constants.CONTROLLER_DEADZONE); + rotationMovement = ControllerMath.applyDeadband(rotationMovement, Constants.CONTROLLER_DEADZONE); + +A good value to set controller deadzone to is 0.1 or lower. We can now move on to how to map +buttons to commands. To do this, we use WPILIB's Triggers. Do something like this: + +.. code-block:: java + + input.getDriverButton(XboxController.Button.kA.value).onTrue(exampleCommand); + +What this will do is run the "exampleCommand" when the "A" button is pressed on the driver +controller. We can also do something like this: + +.. code-block:: java + + input.getDriverTrigger(XboxController.Axis.kRightTrigger.value).onTrue(exampleCommand); + +When the right trigger on the Xbox controller is at least half way pressed, then the program +will run the exampleCommand. Of course, similar methods are provided for the operator controller +as well. + +.. note:: + + You do NOT need to call .schedule on your commands using this method. WPILIB will autoschedule + them when nessesary. \ No newline at end of file diff --git a/docs/source/base/limelight.rst b/docs/source/base/limelight.rst new file mode 100644 index 0000000..06f6ad7 --- /dev/null +++ b/docs/source/base/limelight.rst @@ -0,0 +1,54 @@ +Limelight Cameras +================= + +As mentioned previously, we use PhotonVision Cameras (which are controlled using the Camera +API) for detecting AprilTags and doing additional math on them (such as vision-based pose +estimation). To detect objects on the ground, such as game pieces, we use limelights. This +document covers how to use POPLib's Limelight API to control your limelight camera and get +the object detection results from it. + +Lets start by creating a new Limelight object. To do this, we first need to create a new +LimelightConfig. Lets look at the constructor: + +.. code-block:: java + + public LimelightConfig(String limelightName, + int pipeline, + int ledState, + double minValidArea) + +The first parameter asks for the name of the limelight. The next parameter asks what the +ID of the object detection pipeline is. Both of these things can be viewed in the limelight +dashboard. Then, we ask for the ledState, which controls the brightness and intensity of +the green LED lights on the limelight. 0 will set the lights to be at the default state +according to the selected pipeline, 1 will force the lights to be off, 2 will force the lights +to blink, and 3 will force the lights to be on all the time. Finally, we ask for the minimum +area an object needs to take up on the screen in order for the object detection to be valid. + +We then create a new Limelight object like so: + +.. code-block:: java + + Limelight limelightCamera = new Limelight(Constants.LIMELIGHT_CONFIG); + +We can get a detection from the limelight like so: + +.. code-block:: java + + DetectedObject detection = null; + Optional possibleDetection = limelightCamera.getLatestDetection(); + if (possibleDetection.isPresent()) { + detection = possibleDetection.get(); + } + +The getLatestDetection method returns a Optional because the limelight might not have +detected anything yet. Thus, we must use the isPresent() to check if there was a detection, +and then use the .get() to get the DetectedObject object. A DetectedObject is a custom +container defined in POPLib. Lets look at how to use it: + +.. code-block:: java + + String detectedClass = detection.objectType; + double objectArea = detection.area; // this is a percentage of how much of the cameras screen this object takes up + double horizontalAngleOffset = detection.xAngleOffset; + double verticalAngleOffset = detection.yAngleOffset; \ No newline at end of file diff --git a/docs/source/base/motors.rst b/docs/source/base/motors.rst new file mode 100644 index 0000000..a36b73f --- /dev/null +++ b/docs/source/base/motors.rst @@ -0,0 +1,127 @@ +Motors +====== + +In POPLib, Motors are represented using the Motor.java API. +The main reason to use the Motor object to represent your motors is that the Motor object abstracts away +many of the differences between vendor specific APIs (e.g. the TalonFX and SparkMax APIs). +This allows you to quickly change what type of motor you are using in code +(e.g. Using the Motor.java API, I can easily change between a SparkMax motor and a TalonFX motor). +This also allows the application layer of POPLib to work with any type of motor that may be on your robot, +without you changing your code. + +So, how do you create a Motor? In POPLib, we use Config objects as containers +and helper objects that assist in the creation of hardware objects. To that end, +we must first create a MotorConfig object before creating a Motor object. +Lets take a look at how to do that by looking at the constructor: + +.. code-block:: java + + public MotorConfig(int canID, + String canBUS, + MotorVendor motorVendor, + PIDConfig pidConfig, + int currentLimit, + boolean inverted, + IdleBehavior idleMode, + ConversionConfig conversionConfig) + +Lets go through each parameter. The canID is pretty simple: its the CAN ID of the motor controller +(the Spark/Kraken on the robot). This can be set using vendor specific tools +(Phoenix Tuner X for CTRE Devices and REV Hardware Client for Sparks). The canBUS parameter +allows you to specific what CAN Bus this motor is on (typically only used if you are using a CANnivore). +The default canBUS should be "rio". + +.. note:: + + Currently, if you specify a CAN Bus for Spark devices, it won't apply. + This is because the Vendor provided SparkMax API has no way to specify what CAN Bus a motor is on. + Thus, all Sparks should be put on the RoboRio CAN Bus loop. + However, the CAN Bus parameter still works for TalonFX devices. + +Then, you need to specify what type of motor this is using the MotorVendor enum. +This is how the Motor API knows which native vendor API to use. +After this, you need to create a PIDConfig. For more information on PID and PIDConfigs, +see :doc:`pid`. You then need to specify the current limit for this motor. +The recommended current limits for your motor can be found using the vendor documentation for your motor. +After this, specify if the motor signals need to be inverted. Then, specify the IdleBehavior for this motor. +This is the default behavior of the motor when it is not receiving active commands to run. +The IdleBehavior can be specified using the IdleBehavior enum, and the options are Brake and Coast. +Brake mode will have the motor actively resist external forces, and coast will allow the motor to spin freely. +Finally, you must pass in a ConversionConfig object. This object details the gear ratio between the motor shaft +rotation and the final mechanism rotation. If you don't want to deal with gear ratios and conversions, +simply create a new ConversionConfig object without any parameters. However, if you do want the +Motor API to automatically consider gear ratios, create a new ConversionConfig object using this constructor: + +.. code-block:: java + + public ConversionConfig(double gearRatio, AngleUnit units) + +Now that you have created a MotorConfig object, we can create a new Motor like this: + +.. code-block:: java + + Motor motor = new Motor(Constants.SUBSYSTEM_NAME.motorConfig); + +.. note:: + + Because of how long the MotorConfig constructor is rather long, we recommend creating MotorConfig + objects in a file named Constants.java. These MotorConfigs can also be divided by class into what + subsystem you are using them for, just as in the above code example. + +.. note:: + + Whenever a CAN device (like a Motor) is created in POPLib, it is registered in the internal + CAN ID Registry. If a duplicate CAN ID is found, an error message will be printed out to + DriverStation (instead of actually crashing the robot program). Please take POPLib DriverStation + error messages seriously. Duplicate CAN IDs make it difficult to debug a robot during competition + and can lead to unexcepted errors. + +Now that we have created our Motor, let us discuss how to use it. It is important to note +that any methods/commands that use PID will be discussed in the :doc:`pid` Documentation. + +Let us first see how we can set the speed of a motor without using PID: + +.. code-block:: java + + motor.set(speed); + +Where speed is some number from -1.0 to 1.0. + +.. note:: + + If you have this motor configured with PID (as in you passed non-zero PID Constants in PIDConfig), + then POPLib will print an error message in DriverStation. It is not recommended to mix PID methods + with non-PID methods as they can send conflicting commands to the motor. + +Now let us look at how to access encoder readings from a motor. Note that this is NOT from an absolutee +encoder like a CANCoder. See the :doc:`absolute encoders` Documentation for that. To access position and velocity readings +from your encoder, do something like this: + +.. code-block:: java + + double motorPos = motor.getPosition(); + double motorVel = motor.getVelocity(); + +We can also change the IdleBehavior of a motor after it has been created. Do something like this: + +.. code-block:: java + + motor.changeIdleBehavior(IdleBehavior.BRAKE); + motor.changeIdleBehavior(IdleBehavior.COAST); + +The final important thing you need to know how to do is add a follower motor. +This is extremely useful if you need two motors to run in the exact same direction. +Lets say you have already created a lead motor (we will use the same motor as we have been previously +using in our examples). Now lets say that I want a different motor (which I have not created a MotorConfig +object or Motor object for) to follow my lead motor. I would do something like this: + +.. code-block:: java + + FollowerConfig followerConfig = new FollowerConfig(Constants.FOLLOWER_CANID, Constants.FOLLOWER_INVERTED); + motor.addFollower(); + +This will create a new motor behind the scenes and make it follow the commands of the lead motor +(the variable named "motor" in our examples. We need to pass in the CAN ID of the follower motor +and whether or not the follower motor should be inverted). + +Now that you have read this doc page, please read the docs for :doc:`pid`. \ No newline at end of file diff --git a/docs/source/base/pid.rst b/docs/source/base/pid.rst new file mode 100644 index 0000000..516a895 --- /dev/null +++ b/docs/source/base/pid.rst @@ -0,0 +1,136 @@ +PID +=== + +In POPLib, PID Constants are stored in PIDConfigs that are then applied to a Motor. +These constants can later be changed by creating a new PIDConfig and sending it to a Motor, +which we will discuss later. + +Let us first look at the constructor for creating a PIDConfig object. It is rather simple: + +.. code-block:: java + + public PIDConfig(double P, double I, double D, double F) + +This is a rather straight-forward constructor, the P, I, and D terms are your standard proportional, +integral, and derivative gains, and the F term is a velocity feedforward gain. If you plan to use a +seperate feedforward scheme in your subsystem (such as elevator feedforward), then set F to zero. +If you apply non-zero gains to a PIDConfig object and pass it MotorConfig object, and then create a +Motor object using that (see :doc:`/base/motors` Documentation), then that Motor will enable all of the +PID specific methods and commands, which we will discuss a bit later. + +However, if you do not want to use PID for a motor, you can either create a new PIDConfig with all zero +gains, or use the empty PIDConfig constructor + +.. code-block:: java + + PIDConfig configForMotorThatDoesntUsePID = new PIDConfig(0, 0, 0, 0); + PIDConfig configForMotorThatDoesntUsePID = new PIDConfig(); + +Both of these will do the same thing. Now, when you pass this into your MotorConfig constructor, +and create your Motor, it will mark this motor as a "PIDless" motor and will print errors to the +DriverStation if you attempt to use any of the PID specific methods, which we will discuss below. + +So how do you use PID on your motors? Assuming you have created a PIDConfig with non-zero gains, +passed it into a MotorConfig, and used the MotorConfig to create a Motor (again see :doc:`/base/motors`), +then we can access methods that allow us to easily use PID control on our motors. + +For these examples, we assume you have created a Motor object like so: + +.. code-block:: java + + Motor motor = new Motor(Constants.SUBSYSTEM_NAME.MotorConfig); + +The first method that we will use allows us to set the desired target position of a motor. It is +important to note that you should be calling this is in the periodic loop of your subsystem. The +method looks like this: + +.. code-block:: java + + motor.setTargetPosition(position, Constants.ENABLE_FOC, feedforwardOutput); + +Let us go through the parameters of this together. It is important to note that only the first parameter +(the desired position) is required. Also note that, behind the scenes, the gear ratio that you provided +in ConversionConfig (if you gave one) has already been applied to the motor. Thus, if you want the motor +to spin three rotations (and you applied the correct gear ratio) it will spin three rotations. If you haven't +applied a gear ratio, that also fine. We recommend using :doc:`/base/tunable numbers` as your "setpoint" +if you want to tune your setpoints. Of course, don't worry about this for now, as you will likely be using the +application layer of POPLib and POPLib's pre-made subsystems, which already have that fuctionality. + +Now lets move on to the other two parameters. Its important to note that these are OPTIONAL parameters. The +first one is a boolean that describes whether or not to use FOC on this motor. This will only really affect +Talon devices. FOC allows you to run a motor 15% faster for some trade-offs. However, it requires a Phoenix +Pro license. This license also needs to be applied to all of the motors using Phoenix Tuner X. + +The second is what feedforwardOutput to add to the motor. This is again, OPTIONAL. This would typically be +calculated by using a WPILIB feedforward object (like ElevatorFeedForward or ArmFeedForward) and calling +feedforward.calculate. To learn more about feedforward, read the WPILib docs. Note that the feedforwardOutput +should be in volts (which is the default for WPILIB). + +Everything we have talked about so far is for Position PID, but obviously, POPLib (aka the best library on the +planet) also comes with method for Velocity PID: + +.. code-block:: java + + motor.setTargetVelocity(velocity, Constants.ENABLE_FOC, feedforwardOutput); + +Everything that was previously mentioned about the parameters also applies here. + +Of course, you will also want to check if you reached a setpoint if you want to end a command, and POPLib provides +a way to do that too: + +.. code-block:: java + + boolean motorIsAtPositionSetpoint = motor.atPositionSetpoint(position, Constants.ACCEPTABLE_PID_TOLERANCE); + boolean motorIsAtVelocitySetpoint = motor.atVelocitySetpoint(velocity, Constants.ACCEPTABLE_PID_TOLERANCE); + +Where the "position" and "velocity" parameters are the setpoints to check (should be the same setpoints that you +used for setTargetPosition/setTargetVelocity) and the tolerance/error is how close the motor needs to be to the +setpoint. Note that this uses relative encoders. + +PID Tuning +========== + +POPLib provides our own PIDTuning class that can be used to tune PID efficiently and easily. The class allows you +to change your PID variables on the fly, and all you have to do is set it up. All pre-made subsystem from POPLib that +use PID Control come with the PIDTuning class already set up, all you have to do is put it into tuning mode (this is +discussed later in the application layer docs). + +Lets go over a overview of how to use the PIDTuning class for a custom subsystem. + +.. code-block:: java + + public Subsystem() { + Motor motor = new Motor(Constants.motorConfig); + TunableNumber setpoint = new TunableNumber("Motor Setpoing", 0.0, Constants.tuningModeEnabled); + PIDTuning pidTuning = new PIDTuning("Example Motor", Constants.PIDConfig, Constants.tuningModeEnabled); + } + +This will create a new motor and pidTuning class. The parameters of the PIDTuning class are as follows: the motor +name that will be used in SmartDashboard (to learn more about SmartDashboard read the WPILIB docs), the PIDConfig +object that you used to create the motor, and whether or not to turn on tuning mode. We are also using a new concept +called TunableNumbers, which is a number that can be update from SmartDashboard. For more information, see +:doc:`/base/tunable numbers`. + +.. note:: + + When you are creating a PIDConfig for tuning mode, make sure to initalize it with non-zero values before passing + it in to MotorConfig. If you don't, POPLib will print some error messages in SmartDashboard. + +Now lets look at how to use it: + +.. code-block:: java + + @Override + public void periodic() { + motor.changePID(tuning.generatePIDConfig()); + SmartDashboard.putNumber("motor position", motor.getPosition()); + SmartDashboard.putNumber("motor velocity", motor.getVelocity()); + motor.setTargetPosition(setpoint.get()); + } + +This will first change the PID Constants that are applied to the motor, then will log encoder values to SmartDashboard +for later use when we want to create graphs. Finnally, it runs the PID on the motor using setTargetPosition and +setpoint.get(). + +Now, you can go into the Glass tool, click on "SmartDashboard" and then "Tunable Numbers", and then you can change your +PID Constants and your setpoint value. You can also make graphs in Glass for easy tuning. Have Fun! diff --git a/docs/source/base/tunable numbers.rst b/docs/source/base/tunable numbers.rst new file mode 100644 index 0000000..29b0c2a --- /dev/null +++ b/docs/source/base/tunable numbers.rst @@ -0,0 +1,41 @@ +Tunable Numbers +=============== + +Have you ever wanted to change a constant/number in your code WITHOUT having to +redeploy your code? Well, look no further. Here at POPLib Inc. we have come up +with a solution: Tunable Numbers. + +Using the TunableNumber API, you can edit a Constant in your code without having +to redeploy your robot. This is useful for tuning setpoints, and it is also utilized +by POPLib's PIDTuning API. Here's how you create a new TunableNumber: + +.. code-block:: java + + TunableNumber setpoint = new TunableNumber("Subsystem Setpoint", 0.0, Constants.TUNING_MODE); + +The first variable is the name of this TunableNumber - which you will need when you +want to edit the value of the variable later. Then, you need to set a default value +(I used 0.0 as this is a setpoint). The default value is used when the user has not +given us a new value to use. Tuning mode dictates whether or not the TunableNumber +should accept new inputs from a user. Typically, you should turn tuning mode off +during a competition. + +Now lets look at how to get the latest reading from a TunableNumber: + +.. code-block:: java + + setpoint.get() // wow that was really hard and complex + +We should also see how to change the value of a TunableNumber in code: + +.. code-block:: java + + setpoint.set(someNumber); // wow, this was also really hard + complex + +Now lets get to the part you have been waiting for: changing the value of this +number WITHOUT having to restart your robot code. To do this, make sure tuning +mode is on, and open your favorite SmartDashboard viewing tool (Glass or Elastic). +Then go into SmartDashboard -> TunableNumbers. You should see the name of your +tunable number there. If you change that number (and dont press enter or space, +that would disable the robot), then the change will be reflected in your robot code +(the next time you call setpoint.get(), it will return a different number!). \ No newline at end of file diff --git a/docs/source/conf.py b/docs/source/conf.py new file mode 100644 index 0000000..892a7d1 --- /dev/null +++ b/docs/source/conf.py @@ -0,0 +1,35 @@ +# Configuration file for the Sphinx documentation builder. + +# -- Project information + +project = 'POPLib' +copyright = '2025, Pack of Parts' +author = 'Vishruth Rao and Team 1294 Programming subteam' + +release = '0.1' +version = '0.1.0' + +# -- General configuration + +extensions = [ + 'sphinx.ext.duration', + 'sphinx.ext.doctest', + 'sphinx.ext.autodoc', + 'sphinx.ext.autosummary', + 'sphinx.ext.intersphinx', +] + +intersphinx_mapping = { + 'python': ('https://docs.python.org/3/', None), + 'sphinx': ('https://www.sphinx-doc.org/en/master/', None), +} +intersphinx_disabled_domains = ['std'] + +templates_path = ['_templates'] + +# -- Options for HTML output + +html_theme = 'sphinx_rtd_theme' + +# -- Options for EPUB output +epub_show_urls = 'footnote' diff --git a/docs/source/index.rst b/docs/source/index.rst new file mode 100644 index 0000000..18a595a --- /dev/null +++ b/docs/source/index.rst @@ -0,0 +1,24 @@ +Welcome to POPLib documentation! +=================================== + +**POPLib** is a FRC Java library that uses Vendor API's and WPILIB to make FRC robot programming easy. +It allows for the easy and safe creation of basic hardware components, such as Motors, Gyros, BeamBreak Sensors, Photonvision Cameras, Limelight Cameras, etc. +POPLib also includes subsystem bases so that the programming of more complex subsystems such as Swerve, Elevators, Pivots, Intakes, etc. are simple. + +Check out the :doc:`installation` section to get started with installing POPLib. + +.. note:: + + This documentation is for POPLibv2, which is in active development and the first production release will be before 2026. + +Contents +-------- + +.. toctree:: + + installation + overview + api + +.. note:: + Any comments made against the electrical subteam were made in jest and do not represent the actual views of the programming subteam or the documentation author. Actual disrespect of any kind is not tolerated at Pack of Parts. \ No newline at end of file diff --git a/docs/source/installation.rst b/docs/source/installation.rst new file mode 100644 index 0000000..933972d --- /dev/null +++ b/docs/source/installation.rst @@ -0,0 +1,36 @@ +Installation +============ + +Before using POPLib, you must first install the latest version of WPILib, which can be found `here. `_ + +Then, you will want to create a new project with the project templete "Command Robot." The language of the project should be Java. + +After that, please clone our `repository `_ and copy the **poplib** folder. You will now want to go back into your project and paste your the poplib folder so that your file structure looks like so. + +:: + + project_name + ├── .Glass + ├── .gradle + ├── .vscode + ├── .wpilib + ├── build + ├── gradle + ├── misc + ├── src/main + | ├── deploy + │ ├── java + | | ├── frc/robot + | │ └── poplib + ├── vendordeps + ├── .gitignore + ├── build.gradle + ├── gradlew + ├── gradlew.bat + ├── settings.gradle + └── WPILib-License.md + +.. note:: + This is the current installation method. We are also working on publishing POPLib as a github package, but there are several challenges with that. + +Now that you are finished installing POPLib, you can check out the POPLib :doc:`overview` to see how POPLib works and how you can use it to speed up robot programming during the competition season. diff --git a/docs/source/overview.rst b/docs/source/overview.rst new file mode 100644 index 0000000..e85cb4f --- /dev/null +++ b/docs/source/overview.rst @@ -0,0 +1,11 @@ +Overview +======== + +POPLib can essientally be split up into two parts: the "base" layer and the "application" layer. +The "base" layer includes hardware abstractions to make robot programming for these things easier. This includes a motor class that handles the brunt of the PID work, a PhotonVision Camera class that helps in retrieving useful infomation from the Photonvision Camera without having a lot of boilerplate code and complex math, a Gyro class, an Absolute Encoder class ... you get the point. If you just want to use the base layer of POPLib ... that's great! However, there is also the "application" layer of POPLib. + +The "application" layer of POPLib directly uses the "base" layer to create pre-made, ready to go subsystems. These subsystems include a swerve drivebase, an elevator implementation, a flywheel subystem, a pnuematics subsystem, etc. Since these subsystems are "ready to go" there is very little code that you have to write. All you need to do is fill out some constants (Ok, a lot of constants) so that the subsystems know what type of hardware thier dealing with, what PID Constants to use (if needed), etc. + +Regardless, it's important to first familiarize yourself with the :doc:`base` layer of POPLib, as using the "application" layer requires some knowledge of how to use the base layer. + +After reading the base layer documentation, you can read the :doc:`application` layer documentation that details how to use POPLib's premade subsystems. \ No newline at end of file diff --git a/hi.txt b/hi.txt new file mode 100644 index 0000000..45b983b --- /dev/null +++ b/hi.txt @@ -0,0 +1 @@ +hi diff --git a/pyproject.toml b/pyproject.toml new file mode 100644 index 0000000..f9f92c7 --- /dev/null +++ b/pyproject.toml @@ -0,0 +1,8 @@ +[build-system] +requires = ["flit_core >=3.2,<4"] +build-backend = "flit_core.buildapi" + +[project] +name = "POPLib" +authors = [{name = "Vishruth Rao and Team 1294's Programming subteam", email = "info@packofparts.org"}] +dynamic = ["version", "description"] diff --git a/src/main/java/poplib/motor/MotorConfig.java b/src/main/java/poplib/motor/MotorConfig.java index 6f1d159..faf0f17 100644 --- a/src/main/java/poplib/motor/MotorConfig.java +++ b/src/main/java/poplib/motor/MotorConfig.java @@ -57,7 +57,6 @@ public TalonFXConfiguration getTalonConfig() { talonConfig.MotorOutput.NeutralMode = mode.getTalonMode(); talonConfig.MotorOutput.Inverted = inversion ? InvertedValue.CounterClockwise_Positive : InvertedValue.Clockwise_Positive; - pid.updatePidConfig(talonConfig); conversion.updateConfig(talonConfig); diff --git a/src/main/java/poplib/swerve/CTREModuleState.java b/src/main/java/poplib/swerve/CTREModuleState.java index ad6779d..0718556 100644 --- a/src/main/java/poplib/swerve/CTREModuleState.java +++ b/src/main/java/poplib/swerve/CTREModuleState.java @@ -32,20 +32,7 @@ public static SwerveModuleState optimize( return new SwerveModuleState(targetSpeed, Rotation2d.fromDegrees(targetAngle)); } - public static SwerveModuleState optimizeWithPidWrapping( - SwerveModuleState desiredState, Rotation2d currentAngle - ) { - double targetAngle = currentAngle.getDegrees(); - double targetSpeed = desiredState.speedMetersPerSecond; - double delta = targetAngle - currentAngle.getDegrees(); - if (Math.abs(delta) > 90) { - targetSpeed = -targetSpeed; - targetAngle = delta > 90 ? (targetAngle -= 180) : (targetAngle += 180); - } - - return new SwerveModuleState(targetSpeed, Rotation2d.fromDegrees(targetAngle)); - } /** * Normalize Angle. diff --git a/src/main/java/poplib/swerve/swerve_modules/SwerveModule.java b/src/main/java/poplib/swerve/swerve_modules/SwerveModule.java index 0ed763d..6cffe34 100644 --- a/src/main/java/poplib/swerve/swerve_modules/SwerveModule.java +++ b/src/main/java/poplib/swerve/swerve_modules/SwerveModule.java @@ -116,7 +116,6 @@ public void setDesiredState(SwerveModuleState state) { // Prevents angle motor from turning further than it needs to. // E.G. rotating from 10 to 270 degrees CW vs CCW. state = CTREModuleState.optimize(state, lastAngle); - Rotation2d angle = Math.abs(state.speedMetersPerSecond) <= swerveModuleConstants.moduleInfo.maxSpeed.in(Units.MetersPerSecond) * (0.01) ? lastAngle : state.angle; @@ -132,25 +131,4 @@ public void setDesiredState(SwerveModuleState state) { } public abstract void runSysIdRoutine(double voltage); - - public LinearVelocity accelLimit(LinearVelocity newVelocity) { - LinearVelocity velocityChange = newVelocity.minus(lastVelo); - Time ellapsedTime = Units.Seconds.of(Timer.getFPGATimestamp()).minus(lastVeloTime); - - // TODO: Update - // newVelocity = lastVelo.plus( - // ( - // Math.max( - // velocityChange.div(ellapsedTime).abs(Units.MetersPerSecondPerSecond), - // swerveModuleConstants.moduleInfo.maxAcceleration.times( - // 1 - lastVelo.div(swerveModuleConstants.moduleInfo.maxSpeed).magnitude() - // ).in(Units.MetersPerSecondPerSecond) - // ) - // * ellapsedTime.in(Units.Seconds) * (velocityChange.lt(Units.MetersPerSecond.of(0)) ? -1 : 1))); - - lastVelo = newVelocity; - lastVeloTime.plus(ellapsedTime); - - return newVelocity; - } } diff --git a/src/main/java/poplib/swerve/swerve_templates/BaseSwerve.java b/src/main/java/poplib/swerve/swerve_templates/BaseSwerve.java index 7ac55f9..33ac9c4 100644 --- a/src/main/java/poplib/swerve/swerve_templates/BaseSwerve.java +++ b/src/main/java/poplib/swerve/swerve_templates/BaseSwerve.java @@ -39,9 +39,6 @@ abstract public class BaseSwerve extends SubsystemBase { protected PIDTuning angleTuning; protected PIDTuning driveTuning; - private Translation2d lastTranslationVector; - private double lastTranslationVectorTime; - public static final double MAX_SKID_ACCEL = 100.0; public static final double MAX_X_TILT_ACCEL = 100.0; public static final double MAX_Y_TILT_ACCEL = 0.5; @@ -59,9 +56,6 @@ public BaseSwerve(SwerveModule[] swerveMods, Gyro gyro) { this.maxSpeed = swerveMods[0].swerveModuleConstants.moduleInfo.maxSpeed.in(Units.MetersPerSecond); this.maxAngularVelocity = swerveMods[0].swerveModuleConstants.moduleInfo.maxAngularVelocity.in(Units.RadiansPerSecond); - lastTranslationVector = new Translation2d(); - lastTranslationVectorTime = Timer.getFPGATimestamp(); - angleTuning = new PIDTuning("Swerve Angle", swerveMods[0].swerveModuleConstants.angleConfig.pid, swerveMods[0].swerveModuleConstants.swerveTuningMode); driveTuning = new PIDTuning("Swerve Drive", swerveMods[0].swerveModuleConstants.driveConfig.pid, swerveMods[0].swerveModuleConstants.swerveTuningMode); @@ -79,28 +73,6 @@ public void driveChassis(ChassisSpeeds chassisSpeeds) { public abstract void driveRobotOriented(Translation2d vector, double rot); - public Translation2d accelrationLimit(Translation2d wantedVelcotiy) { - Translation2d delta = wantedVelcotiy.minus(lastTranslationVector); - double ellapsedTime = Timer.getFPGATimestamp() - lastTranslationVectorTime; - lastTranslationVectorTime += ellapsedTime; - - if (delta.getNorm() > MAX_SKID_ACCEL * ellapsedTime) { - delta = delta.div(delta.getNorm() / (MAX_SKID_ACCEL * ellapsedTime)); - } - - if (delta.getX() > MAX_X_TILT_ACCEL * ellapsedTime) { - delta = new Translation2d(MAX_X_TILT_ACCEL * ellapsedTime, delta.getY()); - } - - if (delta.getY() > MAX_Y_TILT_ACCEL * ellapsedTime) { - delta = new Translation2d(delta.getX(), MAX_Y_TILT_ACCEL * ellapsedTime); - } - - lastTranslationVector = delta; - - return null; - } - public void driveRobotOriented(SwerveModuleState[] states) { desaturateWheelSpeeds(states, maxSpeed); diff --git a/src/main/java/poplib/swerve/swerve_templates/VisionBaseSwerve.java b/src/main/java/poplib/swerve/swerve_templates/VisionBaseSwerve.java index a6a848f..cdf33da 100644 --- a/src/main/java/poplib/swerve/swerve_templates/VisionBaseSwerve.java +++ b/src/main/java/poplib/swerve/swerve_templates/VisionBaseSwerve.java @@ -1,5 +1,12 @@ package poplib.swerve.swerve_templates; +import java.util.ArrayList; +import java.util.Iterator; +import java.util.List; +import java.util.Optional; + +import org.photonvision.EstimatedRobotPose; + import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; @@ -12,12 +19,6 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; -import java.util.ArrayList; -import java.util.Iterator; -import java.util.List; -import java.util.Optional; -import org.photonvision.EstimatedRobotPose; - import poplib.sensors.camera.Camera; import poplib.sensors.camera.CameraConfig; import poplib.sensors.camera.DetectedObject; @@ -25,7 +26,6 @@ import poplib.sensors.camera.LimelightConfig; import poplib.sensors.gyro.Gyro; import poplib.swerve.swerve_modules.SwerveModule; -import poplib.swerve.swerve_templates.BaseSwerve; public abstract class VisionBaseSwerve extends BaseSwerve { protected final SwerveDrivePoseEstimator odom; @@ -67,7 +67,7 @@ public void updateVisionPoses() { Camera camera = (Camera)var1.next(); Optional estPose = camera.getEstimatedPose(this.getOdomPose()); if (estPose.isPresent()) { - this.odom.addVisionMeasurement(((EstimatedRobotPose)estPose.get()).estimatedPose.toPose2d(), ((EstimatedRobotPose)estPose.get()).timestampSeconds, camera.getVisionStdDevs()); + this.odom.addVisionMeasurement((estPose.get()).estimatedPose.toPose2d(), (estPose.get()).timestampSeconds, camera.getVisionStdDevs()); } } diff --git a/src/main/java/poplibv2/control_systems/PIDConfig.java b/src/main/java/poplibv2/control_systems/PIDConfig.java new file mode 100644 index 0000000..aa32418 --- /dev/null +++ b/src/main/java/poplibv2/control_systems/PIDConfig.java @@ -0,0 +1,93 @@ +package poplibv2.control_systems; + +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.revrobotics.spark.config.SparkMaxConfig; + +import edu.wpi.first.math.controller.PIDController; + +/** + * Respondible for holding PID constants and applying them to a motor. + */ +public class PIDConfig { + public double P; + public double I; + public double D; + public double F; + private boolean usePID; + + /** + * Full Constuctor for creating PIDConfig object. If you do not need to use a + * value, simple set it to zero. + * + * @param P the proportional value of the your PID Controller + * @param I the interval value multiplier of the your PID Controller + * @param D the dervirative value multiplier of the your PID Controller + * @param F the velocity Feedforward value + */ + public PIDConfig(double P, double I, double D, double F) { + this.P = P; + this.I = I; + this.D = D; + this.F = F; + this.usePID = true; + if (P == 0 && I == 0 && D == 0 && F == 0) { + this.usePID = false; + } + } + + /** + * Use this constructor if you do not want to use PID for the motor. + */ + public PIDConfig() { + this.usePID = false; + } + + /** + * You probably wont have to use this. It just creates a normal PIDController. + * When you create a MotorConfig, the MotorConfig automagically applies this + * PIDConfig to the Motor. + * + * @return PIDController + */ + public PIDController createPIDController() { + return new PIDController(P, I, D); + } + + /** + * INTERNAL POPLIB FUNCTION. + *

+ *

+ * Used to apply PID Constants to a Spark Motor config + * + * @param config + */ + public boolean applyToMotor(SparkMaxConfig config) { + if (usePID) { + config.closedLoop.pidf(P, I, D, F); + } + return usePID; + } + + /** + * INTERNAL POPLIB FUNCTION. + *

+ *

+ * Used to apply PID Constants to a Talon Motor config + * + * @param config + */ + public boolean applyToMotor(TalonFXConfiguration config) { + if (usePID) { + Slot0Configs slot0Configs = new Slot0Configs(); + + slot0Configs.kV = F; + slot0Configs.kP = P; + slot0Configs.kI = I; + slot0Configs.kD = D; + config.withSlot0(slot0Configs); + } + return usePID; + + } +} diff --git a/src/main/java/poplibv2/control_systems/PIDTuning.java b/src/main/java/poplibv2/control_systems/PIDTuning.java new file mode 100644 index 0000000..0e193d6 --- /dev/null +++ b/src/main/java/poplibv2/control_systems/PIDTuning.java @@ -0,0 +1,53 @@ +package poplibv2.control_systems; + +import java.util.Optional; + +import poplibv2.misc.TunableNumber; +import poplibv2.motors.Motor; + +public class PIDTuning { + private final TunableNumber kP; + private final TunableNumber kI; + private final TunableNumber kD; + private final TunableNumber kF; + private final boolean tuningMode; + + /** + * Creates a new PIDTuning helper + * @param motorName The motor name that you are doing the PID Tuning for + * @param pid The current PID Config + * @param kTuningMode whether or not to do pid tuning + */ + public PIDTuning(String motorName, PIDConfig pid, boolean tuningMode) { + this(motorName, pid.P, pid.I, pid.D, pid.F, tuningMode); + } + + private PIDTuning(String motorName, double motor_kP, double motor_kI, double motor_kD, double motor_kF, boolean kTuningMode){ + kP = new TunableNumber(motorName + "kP", motor_kP, kTuningMode); + kI = new TunableNumber(motorName + "kI", motor_kI, kTuningMode); + kD = new TunableNumber(motorName + "kD", motor_kD, kTuningMode); + kF = new TunableNumber(motorName + "kF", motor_kF, kTuningMode); + tuningMode = kTuningMode; + } + + /** + * POPLIB INTERNAL FUNCTION + *

+ * @return if the pid constants have been changed or not. + */ + private boolean isPIDChanged() { + return (kP.hasChanged() || kI.hasChanged() || kD.hasChanged() || kF.hasChanged()) && (tuningMode); + } + + /** + * Updates the PID Constants on your motor using the constants on Smart Dashboard if tuning mode is on. + * @param motor + */ + public Optional generatePIDConfig(){ + if (isPIDChanged()) { + PIDConfig config = new PIDConfig(kP.get(), kI.get(), kD.get(), kF.get()); + return Optional.of(config); + } + return Optional.empty(); + } +} \ No newline at end of file diff --git a/src/main/java/poplibv2/controllers/ControllerMath.java b/src/main/java/poplibv2/controllers/ControllerMath.java new file mode 100644 index 0000000..604f44f --- /dev/null +++ b/src/main/java/poplibv2/controllers/ControllerMath.java @@ -0,0 +1,29 @@ +package poplibv2.controllers; + +import java.lang.Math; + +public class ControllerMath { + /** + * Returns the cube of a number (number^3) + * @param triggerVal the number to cube + * @return the cube + */ + public static double cube(double triggerVal) { + return Math.pow(triggerVal, 3); + } + + /** + * Applies a deadband/deadzone that minizimes controller drift. Also applies scaling based on the deadzone + * @param initalVal the raw controller value + * @param deadband The deadzone to be applied + * @return the scaled output + */ + public static double applyDeadband(double initalVal, double deadband) { + if (initalVal < deadband) { + return 0; + } else { + double scaling = (initalVal < 0 ? -1 : 1) * deadband; + return (initalVal - scaling) / (1 - deadband); + } + } +} \ No newline at end of file diff --git a/src/main/java/poplibv2/controllers/io/IO.java b/src/main/java/poplibv2/controllers/io/IO.java new file mode 100644 index 0000000..d1688b7 --- /dev/null +++ b/src/main/java/poplibv2/controllers/io/IO.java @@ -0,0 +1,89 @@ +package poplibv2.controllers.io; + +import edu.wpi.first.wpilibj2.command.button.CommandGenericHID; +import edu.wpi.first.wpilibj2.command.button.Trigger; + +/** +* Abstract class for OI control +*/ +abstract public class IO { + public static final double DEADBAND = 0.1; + + public IO() { } + + /** + * Gets the speed that the driver wants the drive train to rotate + * @return the speed from -1.0 to 1.0 + */ + public abstract double getDriveTrainRotation(); + + /** + * Gets the speed that the driver wants the drive train to move forward + * @return the forward speed from -1.0 to 1.0 + */ + public abstract double getDriveTrainTranslationY(); + + /** + * Gets the speed that the driver wants the drive train to move right + * @return the right speed from -1.0 to 1.0 + */ + public abstract double getDriveTrainTranslationX(); + + /** + * Gets the DriverController + * @return a CommandGenericHID that can be used for button mapping + */ + public abstract CommandGenericHID getDriverController(); + + /** + * Gets the OperatorController + * @return a CommandGenericHID that can be used for button mapping + */ + public abstract CommandGenericHID getOperatorController(); + + /** + * Gets the value of an axis on a controller. + * @param id the axis id + * @param controller the controller + * @return the value from -1.0 to 1.0 + */ + protected double getRawAxis(int id, CommandGenericHID controller) { + return controller.getHID().getRawAxis(id) * -1; + } + + /** + * Gets a Trigger that can be used to map a button to an command + * @param id the button id + * @return the Trigger + */ + public Trigger getDriverButton(int id) { + return getDriverController().button(id); + } + + /** + * Gets a Trigger that can be used to map a trigger(like the d-pad (the povs) or the left and right triggers) to an command + * @param id the trigger id + * @return the Trigger + */ + public Trigger getDriverTrigger(int id) { + return getDriverController().axisGreaterThan(id, 0.5); + } + + /** + * Gets a Trigger that can be used to map a button to an command + * @param id the button id + * @return the Trigger + */ + public Trigger getOperatorButton(int id) { + return getOperatorController().button(id); + } + + /** + * Gets a Trigger that can be used to map a trigger(like the d-pad (the povs) or the left and right triggers) to an command + * @param id the trigger id + * @return the Trigger + */ + public Trigger getOperatorrigger(int id) { + return getOperatorController().axisGreaterThan(id, 0.5); + } +} diff --git a/src/main/java/poplibv2/controllers/io/Joysticks.java b/src/main/java/poplibv2/controllers/io/Joysticks.java new file mode 100644 index 0000000..65614ca --- /dev/null +++ b/src/main/java/poplibv2/controllers/io/Joysticks.java @@ -0,0 +1,62 @@ +package poplibv2.controllers.io; +import edu.wpi.first.wpilibj2.command.button.CommandGenericHID; +import edu.wpi.first.wpilibj2.command.button.CommandJoystick; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; + +/** + * A joystick set up that uses two Microsoft flight simulator joysticks as the driver controller + * and one XboxController as the operator controller. (we dont use this anymore.) + */ +public class Joysticks extends IO { + public static Joysticks instance; + + private CommandJoystick driveJoystick; + private CommandJoystick rotJoystick; + private CommandXboxController operatorController; + + public static Joysticks getInstance() { + if (instance == null) { + instance = new Joysticks(); + } + + return instance; + } + + public static final class OIConstants { + public static final int DRIVE_PORT = 0; + public static final int ROT_PORT = 1; + public static final int OPERATOR_PORT = 2; + } + + public Joysticks() { + driveJoystick = new CommandJoystick(OIConstants.DRIVE_PORT); + rotJoystick = new CommandJoystick(OIConstants.ROT_PORT); + operatorController = new CommandXboxController(OIConstants.OPERATOR_PORT); + } + + @Override + public double getDriveTrainRotation() { + return rotJoystick.getX(); + } + + @Override + public double getDriveTrainTranslationY() { + return driveJoystick.getY(); + } + + @Override + public double getDriveTrainTranslationX() { + return driveJoystick.getX(); + + } + + @Override + public CommandGenericHID getDriverController() { + return driveJoystick; + } + + @Override + public CommandGenericHID getOperatorController() { + return operatorController; + } +} diff --git a/src/main/java/poplibv2/controllers/io/XboxIO.java b/src/main/java/poplibv2/controllers/io/XboxIO.java new file mode 100644 index 0000000..5a52e1f --- /dev/null +++ b/src/main/java/poplibv2/controllers/io/XboxIO.java @@ -0,0 +1,64 @@ +package poplibv2.controllers.io; + +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj2.command.button.CommandGenericHID; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; + +/** + * A setup where we use two Xbox controller: one for the driver and one for the operator. + * Its our current, go to setup. + */ +public class XboxIO extends IO { + private CommandXboxController driverController; + private CommandXboxController operatorController; + + public static XboxIO instance = null; + + public static XboxIO getInstance() { + if (instance == null) { + instance = new XboxIO(); + } + + return instance; + } + + public static final class OIConstants { + public static final int DRIVE_TRANSLATION_Y = XboxController.Axis.kLeftY.value; + public static final int DRIVE_TRANSLATION_X = XboxController.Axis.kLeftX.value; + public static final int DRIVE_ROTATE = XboxController.Axis.kRightX.value; + + + public static final int DRIVE_PORT = 0; + public static final int OPERATOR_PORT = 1; + } + + public XboxIO() { + driverController = new CommandXboxController(OIConstants.DRIVE_PORT); + operatorController = new CommandXboxController(OIConstants.OPERATOR_PORT); + } + + @Override + public double getDriveTrainRotation() { + return getRawAxis(OIConstants.DRIVE_ROTATE, driverController); + } + + @Override + public double getDriveTrainTranslationY() { + return getRawAxis(OIConstants.DRIVE_TRANSLATION_Y, driverController); + } + + @Override + public double getDriveTrainTranslationX() { + return getRawAxis(OIConstants.DRIVE_TRANSLATION_X, driverController); + } + + @Override + public CommandGenericHID getDriverController() { + return driverController; + } + + @Override + public CommandGenericHID getOperatorController() { + return operatorController; + } +} diff --git a/src/main/java/poplibv2/misc/AllianceColor.java b/src/main/java/poplibv2/misc/AllianceColor.java new file mode 100644 index 0000000..90cb832 --- /dev/null +++ b/src/main/java/poplibv2/misc/AllianceColor.java @@ -0,0 +1,51 @@ +package poplibv2.misc; + +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; + +public class AllianceColor { + private boolean isRedAlliance; + private NetworkTable table; + + public static AllianceColor instance; + + /** + * Gets the AllianceColor object. + * @return + */ + public static AllianceColor getInstance() { + if (instance == null) { + instance = new AllianceColor(); + } + return instance; + } + + /** + * Creates a new AllianceColor object. + */ + private AllianceColor() { + table = NetworkTableInstance.getDefault().getTable("FMSInfo"); + isRedAlliance = table.getEntry("IsRedAlliance").getBoolean(true); + } + + /** + * Updates the current alliance color. Should be called in Teleop Init, or after the match has started. + */ + public void updateAllianceColor() { + isRedAlliance = table.getEntry("IsRedAlliance").getBoolean(true); + } + + /** + * Returns if we are on Red Alliance. + */ + public boolean isRed() { + return isRedAlliance; + } + + /** + * Returns if we are on Blue Alliance. + */ + public boolean isBlue() { + return !isRedAlliance; + } +} diff --git a/src/main/java/poplibv2/misc/CanIdRegistry.java b/src/main/java/poplibv2/misc/CanIdRegistry.java new file mode 100644 index 0000000..bb8a5ed --- /dev/null +++ b/src/main/java/poplibv2/misc/CanIdRegistry.java @@ -0,0 +1,41 @@ +package poplibv2.misc; + +import java.util.HashSet; + +public class CanIdRegistry { + static CanIdRegistry registry = null; + HashSet set; + + /** + * POPLIB INTERNAL FUNCTION + *

+ * Gets the CAN ID Registry + * @return + */ + public static CanIdRegistry getRegistry() { + if (registry == null) { + registry = new CanIdRegistry(); + } + return registry; + } + + /** + * Creates a new registry. + */ + private CanIdRegistry() { + set = new HashSet<>(); + } + + /** + * Registers a new CAN id to the registry. + * If the id is already in the registry, to outputs an error in driverstation that complains about dupicate can ids. + * @param id + */ + public void registerCanId(int id) { + int firstLength = set.size(); + set.add(id); + if (set.size() == firstLength) { + ErrorHandling.complainAboutDuplicatedCANIDException("Duplicate Can Id number " + id + " was used. Duplicate Can Ids lead to confusion during debugging and possibly errors. Dont use them."); + } + } +} diff --git a/src/main/java/poplibv2/misc/ErrorHandling.java b/src/main/java/poplibv2/misc/ErrorHandling.java new file mode 100644 index 0000000..af6cfd7 --- /dev/null +++ b/src/main/java/poplibv2/misc/ErrorHandling.java @@ -0,0 +1,33 @@ +package poplibv2.misc; + +import com.revrobotics.REVLibError; + +import edu.wpi.first.wpilibj.DriverStation; + +public class ErrorHandling { + /** + * Handles a error that happens when a config fails to apply to a motor. + * @param error The potienial error + * @param useCase Why the error happened + */ + public static void handleRevLibError(REVLibError error, String useCase) { + if (error != REVLibError.kOk) { + System.out.println("REVLibError: " + error.toString() + " when " + useCase); + } + } + + /** + * Complains about pid. + */ + public static void complainAboutIncorrectUseOfPID(String m) { + DriverStation.reportError("IncorrectUseOfPIDException: " + m, false); + } + + + /** + * Complains about CAN. + */ + public static void complainAboutDuplicatedCANIDException(String m) { + DriverStation.reportError("DuplicatedCANIDException: " + m, false); + } +} diff --git a/src/main/java/poplibv2/misc/TunableNumber.java b/src/main/java/poplibv2/misc/TunableNumber.java new file mode 100644 index 0000000..d3ffdb1 --- /dev/null +++ b/src/main/java/poplibv2/misc/TunableNumber.java @@ -0,0 +1,68 @@ +package poplibv2.misc; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +/** + * Class for a tunable number. This class gets a value from smart dashboard in tuning mode, + * it returns the default value if tuning mode is not on or the value is not in dashboard. + */ +public final class TunableNumber { + private static final String tableKey = "TunableNumbers"; + private final String key; + private double defaultValue; + private double lastHasChangedValue = defaultValue; + private final boolean tuningMode; + + /** + * Creates a new TunableNumber + * @param dashboardKey the key to use when displaying this on smart dashboard + */ + public TunableNumber(String dashboardKey, double defaultValue, boolean tuningMode) { + this.key = tableKey + "/" + dashboardKey; + this.tuningMode = tuningMode; + set(defaultValue); + } + + /** + * Get the default value for the number + * @return The default value + */ + public double getDefault() { + return defaultValue; + } + + /** + * Sets a new value for the TunableNumber + * @param value The default value + */ + public void set(double value) { + this.defaultValue = value; + if (tuningMode) { + // This makes sure the data is on NetworkTables but will not change it + SmartDashboard.putNumber(key, SmartDashboard.getNumber(key, defaultValue)); + } else if (SmartDashboard.containsKey(key) && SmartDashboard.isPersistent(key)) { + SmartDashboard.clearPersistent(key); + } + } + + /** + * Get the current value of the TunableNumber + * @return The current value + */ + public double get() { + return tuningMode ? SmartDashboard.getNumber(key, defaultValue) : defaultValue; + } + + /** + * Checks whether the number has changed since the last check + * @return true if the number has changed since the last time this method was called, false otherwise + */ + public boolean hasChanged() { + double currentValue = get(); + if (currentValue != lastHasChangedValue) { + lastHasChangedValue = currentValue; + return true; + } + return false; + } +} diff --git a/src/main/java/poplibv2/motors/ConversionConfig.java b/src/main/java/poplibv2/motors/ConversionConfig.java new file mode 100644 index 0000000..c2d16e5 --- /dev/null +++ b/src/main/java/poplibv2/motors/ConversionConfig.java @@ -0,0 +1,56 @@ +package poplibv2.motors; + +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.revrobotics.spark.config.SparkMaxConfig; +import edu.wpi.first.units.AngleUnit; +import edu.wpi.first.units.Units; + +/** + * Responsible for taking into account a gear ratio when trying to run a motor at a certain speed + * or turn a motor a certain amount of degrees + */ +public class ConversionConfig { + public double gearRatio; + public AngleUnit unit; + + /** + * The constructor for creating a ConversionConfig + * @param gearRatio The ratio between the amount of rotation done on the mechinaism vs the amount the motor has turned + * If you don't know, ask the mechanical or design team. + * @param unit The unit the gear ratio is in + */ + public ConversionConfig(double gearRatio, AngleUnit unit) { + this.gearRatio = gearRatio; + this.unit = unit; + } + + /** + * The default, basic gear ratio. Only use if there is no gear conversion happening. + */ + public ConversionConfig() { + this(1.0, Units.Rotations); + } + + /** + * POPLIB INTERNAL FUNCTION. + *

+ * Updates the motor config for a Spark Max motor to include the gear ratio for both position and velocity. + * @param config the SparkMax Motor config that creates is used to create the motor later on. + * NOTE: this is NOT the poplib MotorConfig class. + */ + public void applyToMotor(SparkMaxConfig config) { + config.encoder.positionConversionFactor(unit.convertFrom(1.0, Units.Rotation) / gearRatio); + config.encoder.velocityConversionFactor((unit.convertFrom(1.0, Units.Rotation) / gearRatio) / 60); + } + + /** + * POPLIB INTERNAL FUNCTION. + *

+ * Updates the motor config for a Spark Max motor to include the gear ratio for both position and velocity. + * @param config the Talon Motor config that creates is used to create the motor later on. + * NOTE: this is NOT the poplib MotorConfig class. + */ + public void applyToMotor(TalonFXConfiguration config) { + config.Feedback.SensorToMechanismRatio = 1.0 / (unit.convertFrom(1.0, Units.Rotation) / gearRatio); + } +} \ No newline at end of file diff --git a/src/main/java/poplibv2/motors/FollowerConfig.java b/src/main/java/poplibv2/motors/FollowerConfig.java new file mode 100644 index 0000000..9f93e98 --- /dev/null +++ b/src/main/java/poplibv2/motors/FollowerConfig.java @@ -0,0 +1,16 @@ +package poplibv2.motors; + +public class FollowerConfig { + int canID; + boolean inverted; + + /** + * Creates a new Follower Config + * @param CANID The CAN Id of the FOLLOWER Motor + * @param inverted Whether or not the motor should do the exact same (false) or exact opposite (true) of what this motor is doing + */ + public FollowerConfig(int canID, boolean inverted) { + this.canID = canID; + this.inverted = inverted; + } +} \ No newline at end of file diff --git a/src/main/java/poplibv2/motors/IdleBehavior.java b/src/main/java/poplibv2/motors/IdleBehavior.java new file mode 100644 index 0000000..feddd1b --- /dev/null +++ b/src/main/java/poplibv2/motors/IdleBehavior.java @@ -0,0 +1,11 @@ +package poplibv2.motors; + +/** + * The default behavior of the motor when no control signals are being sent to it. + * Brake causes the motor to resist extrenal force and coast allows the motor to freely spin. + */ +public enum IdleBehavior { + BRAKE, + COAST +} + diff --git a/src/main/java/poplibv2/motors/Motor.java b/src/main/java/poplibv2/motors/Motor.java new file mode 100644 index 0000000..bde36e9 --- /dev/null +++ b/src/main/java/poplibv2/motors/Motor.java @@ -0,0 +1,392 @@ +package poplibv2.motors; + +import java.util.ArrayList; +import java.util.Optional; + +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.revrobotics.spark.ClosedLoopSlot; +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.SparkMaxConfig; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; + +import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.Voltage; +import poplibv2.control_systems.PIDConfig; +import poplibv2.misc.CanIdRegistry; +import poplibv2.misc.ErrorHandling; + + +public class Motor { + + private SparkMax spark; + private TalonFX talon; + private int canID; + private MotorVendor motorType; + private MotorConfig config; + private boolean isConfiguredWithPID; + private PositionVoltage positionVoltage; + private VelocityVoltage velocityVoltage; + private VoltageOut voltageOut; + private ArrayList sparkFollowers; + private ArrayList talonFollowers; + + /** + * Creates a new motor using the motor config. + * Since a motor is a CAN Device, it registers it's id in the + * CAN ID Registry to make sure duplicate motors are not created. + * @param config The config to use when creating the motor. + */ + public Motor(MotorConfig config) { + this.canID = config.getCANID(); + CanIdRegistry.getRegistry().registerCanId(canID); + this.motorType = config.getMotorVendor(); + this.sparkFollowers = new ArrayList<>(); + this.talonFollowers = new ArrayList<>(); + this.config = config; + + if (this.motorType == MotorVendor.REV_ROBOTICS_SPARK_MAX) { + this.spark = new SparkMax(canID, MotorType.kBrushless); + ErrorHandling.handleRevLibError( + spark.configure(config.createSparkMaxConfig(), ResetMode.kResetSafeParameters, PersistMode.kPersistParameters), + "configuring motor " + canID + ); + this.talon = null; + this.positionVoltage = null; + this.velocityVoltage = null; + this.voltageOut = null; + } + + else if (this.motorType == MotorVendor.CTRE_TALON_FX) { + talon = new TalonFX(canID, config.getCANBUS()); + talon.getConfigurator().apply(config.createTalonFXConfiguration()); + this.spark = null; + this.positionVoltage = new PositionVoltage(0.0).withSlot(talon.getClosedLoopSlot().getValue()); + this.velocityVoltage = new VelocityVoltage(0.0).withSlot(talon.getClosedLoopSlot().getValue()); + this.voltageOut = new VoltageOut(0.0); + } + + this.isConfiguredWithPID = config.getIsConfiguredWithPID(); // yes this needs to be here. + } + + /** + * Uses a PID Loop to turn the motor to the desired position. (this should be called in periodic) + * @param position the desired position of the motor. This will already include any calulations done in ConversionConfig + * @throws IncorrectUseOfPIDException + */ + public void setTargetPosition(double position) { + checkForPID(); + if (motorType == MotorVendor.CTRE_TALON_FX) { + talon.setControl(positionVoltage.withPosition(position)); + } else if (motorType == MotorVendor.REV_ROBOTICS_SPARK_MAX) { + spark.getClosedLoopController().setReference(position, ControlType.kPosition, ClosedLoopSlot.kSlot0); + } + } + + /** + * Uses a PID Loop to turn the motor to the desired position. (this should be called in periodic) + * Also includes whether to use FOC. This will allow your motor to run 15% faster for some trade offs. + * FOC only works on Kraken Motors (Talon FX), and it requires a paid Pheonix Pro license that should be applied in Pheonix Tuner X. + * @param position the desired position of the motor. This will already include any calulations done in ConversionConfig + * @param enableFOC whether or not to use FOC. + * @throws IncorrectUseOfPIDException + */ + public void setTargetPosition(double position, boolean enableFOC) { + checkForPID(); + if (motorType == MotorVendor.CTRE_TALON_FX) { + talon.setControl(positionVoltage.withPosition(position).withEnableFOC(enableFOC)); + } else if (motorType == MotorVendor.REV_ROBOTICS_SPARK_MAX) { + spark.getClosedLoopController().setReference(position, ControlType.kPosition, ClosedLoopSlot.kSlot0); + } + } + + /** + * Uses a PID Loop to turn the motor to the desired position. (this should be called in periodic) + * Also includes the feedForwardOutput that you want to apply to this motor. + * @param position the desired position of the motor. This will already include any calulations done in ConversionConfig + * @param feedforwardOutput the output you get from doing something like feedforwardController.calculate(velocity) + * @throws IncorrectUseOfPIDException + */ + public void setTargetPosition(double position, double feedforwardOutput) { + checkForPID(); + if (motorType == MotorVendor.CTRE_TALON_FX) { + talon.setControl(positionVoltage.withPosition(position).withFeedForward(feedforwardOutput)); + } else if (motorType == MotorVendor.REV_ROBOTICS_SPARK_MAX) { + spark.getClosedLoopController().setReference(position, ControlType.kPosition, ClosedLoopSlot.kSlot0, feedforwardOutput); + } + } + + /** + * Uses a PID Loop to turn the motor to the desired position. (this should be called in periodic) + * Also includes the feedForwardOutput that you want to apply to this motor. + * Also includes whether to use FOC. This will allow your motor to run 15% faster for some trade offs. + * FOC only works on Kraken Motors (Talon FX), and it requires a paid Pheonix Pro license that should be applied in Pheonix Tuner X. + * @param position the desired position of the motor. This will already include any calulations done in ConversionConfig + * @param feedforwardOutput the output you get from doing something like feedforwardController.calculate(velocity) + * @param enableFOC whether or not to use FOC. + * @throws IncorrectUseOfPIDException except not really its just printed in DriverStation + */ + public void setTargetPosition(double position, double feedforwardOutput, boolean enableFOC) { + checkForPID(); + if (motorType == MotorVendor.CTRE_TALON_FX) { + talon.setControl(positionVoltage.withPosition(position).withFeedForward(feedforwardOutput).withEnableFOC(enableFOC)); + } else if (motorType == MotorVendor.REV_ROBOTICS_SPARK_MAX) { + spark.getClosedLoopController().setReference(position, ControlType.kPosition, ClosedLoopSlot.kSlot0, feedforwardOutput); + } + } + + + + + + + /** + * Uses a PID Loop to run the motor at the desired velocity. (this should be called in periodic) + * @param velocity the desired velocity of the motor. This will already include any calulations done in ConversionConfig + * @throws IncorrectUseOfPIDException + */ + public void setTargetVelocity(double velocity) { + checkForPID(); + if (motorType == MotorVendor.CTRE_TALON_FX) { + talon.setControl(velocityVoltage.withVelocity(velocity)); + } else if (motorType == MotorVendor.REV_ROBOTICS_SPARK_MAX) { + spark.getClosedLoopController().setReference(velocity, ControlType.kVelocity, ClosedLoopSlot.kSlot0); + } + } + + /** + * Uses a PID Loop to run the motor at the desired velocity. (this should be called in periodic) + * Also includes whether to use FOC. This will allow your motor to run 15% faster for some trade offs. + * FOC only works on Kraken Motors (Talon FX), and it requires a paid Pheonix Pro license that should be applied in Pheonix Tuner X. + * @param velocity the desired velocity of the motor. This will already include any calulations done in ConversionConfig + * @param enableFOC whether or not to use FOC. + * @throws IncorrectUseOfPIDException + */ + public void setTargetVelocity(double velocity, boolean enableFOC) { + checkForPID(); + if (motorType == MotorVendor.CTRE_TALON_FX) { + talon.setControl(velocityVoltage.withVelocity(velocity).withEnableFOC(enableFOC)); + } else if (motorType == MotorVendor.REV_ROBOTICS_SPARK_MAX) { + spark.getClosedLoopController().setReference(velocity, ControlType.kVelocity, ClosedLoopSlot.kSlot0); + } + } + + /** + * Uses a PID Loop to run the motor at the desired velocity. (this should be called in periodic) + * Also includes the feedForwardOutput that you want to apply to this motor. + * @param velocity the desired velocity of the motor. This will already include any calulations done in ConversionConfig + * @param feedforwardOutput the output you get from doing something like feedforwardController.calculate(velocity) + * @throws IncorrectUseOfPIDException + */ + public void setTargetVelocity(double velocity, double feedForwardOutput) { + checkForPID(); + if (motorType == MotorVendor.CTRE_TALON_FX) { + talon.setControl(velocityVoltage.withVelocity(velocity).withFeedForward(feedForwardOutput)); + } else if (motorType == MotorVendor.REV_ROBOTICS_SPARK_MAX) { + spark.getClosedLoopController().setReference(velocity, ControlType.kVelocity, ClosedLoopSlot.kSlot0, feedForwardOutput); + } + } + + /** + * Uses a PID Loop to run the motor at the desired velocity. (this should be called in periodic) + * Also includes the feedForwardOutput that you want to apply to this motor. + * Also includes whether to use FOC. This will allow your motor to run 15% faster for some trade offs. + * FOC only works on Kraken Motors (Talon FX), and it requires a paid Pheonix Pro license that should be applied in Pheonix Tuner X. + * @param velocity the desired velocity of the motor. This will already include any calulations done in ConversionConfig + * @param feedforwardOutput the output you get from doing something like feedforwardController.calculate(velocity) + * @param enableFOC whether or not to use FOC. + * @throws IncorrectUseOfPIDException + */ + public void setTargetVelocity(double velocity, double feedForwardOutput, boolean enableFOC) { + checkForPID(); + if (motorType == MotorVendor.CTRE_TALON_FX) { + talon.setControl(velocityVoltage.withVelocity(velocity).withFeedForward(feedForwardOutput).withEnableFOC(enableFOC)); + } else if (motorType == MotorVendor.REV_ROBOTICS_SPARK_MAX) { + spark.getClosedLoopController().setReference(velocity, ControlType.kVelocity, ClosedLoopSlot.kSlot0, feedForwardOutput); + } + } + + + + + + /** + * Common interface for setting the speed of a speed controller. IF you have configured this motor to run with PID, DO NOT USE THIS. + * @param speed The speed to set. Value should be between -1.0 and 1.0. + */ + public void set(double speed) { + if (this.isConfiguredWithPID) { + ErrorHandling.complainAboutIncorrectUseOfPID( + "Attempted to use motor.set(speed) on a motor that is configured to use PID. This action will make the motor fight with itself. " + + "If you want to make the motor go at a certain speed, tune the PID for velocity and do motor.setTargetVelocity(velocity), which will use the PID loop to run at a certain speed. " + + "The motor this happened to has the CAN ID: " + this.canID + ); + } + if (motorType == MotorVendor.CTRE_TALON_FX) { + talon.set(speed); + } else if (motorType == MotorVendor.REV_ROBOTICS_SPARK_MAX) { + spark.set(speed); + } + } + + public void setVoltage(double voltage) { + if (motorType == MotorVendor.CTRE_TALON_FX) { + talon.setControl(voltageOut.withOutput(voltage)); + } else if (motorType == MotorVendor.REV_ROBOTICS_SPARK_MAX) { + spark.setVoltage(voltage); + } + } + + /** + * Returns whether or not the motor has reached its setpoint. + * @param setpoint the setpoint to use when checking motor position + * @param error the allowed error + * @return Whether or not the motor has reached its setpoint within the allowed error. + */ + public boolean atPositionSetpoint(double setpoint, double error) { + return Math.abs(setpoint - getPosition()) < error; + } + + /** + * Returns whether or not the motor has reached its setpoint. + * @param setpoint the setpoint to use when checking motor position + * @param error the allowed error + * @return Whether or not the motor has reached its setpoint within the allowed error. + */ + public boolean atVelocitySetpoint(double setpoint, double error) { + return Math.abs(setpoint - getVelocity()) < error; + } + + /** + * Gets the position of the encoder. Note that this position has already been scaled + * according to the value in the ConversionConfig object that was used to create the MotorConfig object. + * @return The amount of rotations + */ + public double getPosition() { + if (motorType == MotorVendor.CTRE_TALON_FX) { + return talon.getPosition().getValue().in(Units.Rotations); + } else if (motorType == MotorVendor.REV_ROBOTICS_SPARK_MAX) { + return spark.getEncoder().getPosition(); + } + return 0.0; + } + + /** + * Gets the angluar velocity of the motor. Note that this velocity has already been scaled + * according to the value in the ConversionConfig object that was used to create the MotorConfig object. + * @return the velocity in RPM + */ + public double getVelocity() { + if (motorType == MotorVendor.CTRE_TALON_FX) { + return talon.getVelocity().getValue().in(Units.RPM); + } else if (motorType == MotorVendor.REV_ROBOTICS_SPARK_MAX) { + return spark.getEncoder().getVelocity(); + } + return 0.0; + } + + public Voltage getVoltage() { + if (motorType == MotorVendor.CTRE_TALON_FX) { + return talon.getMotorVoltage().getValue(); + } else if (motorType == MotorVendor.REV_ROBOTICS_SPARK_MAX) { + return Units.Volts.of(spark.getBusVoltage()); + } + return Units.Volts.of(0.0); + } + + /** + * Changes the Idle Behavior of the motor (the behavior when the motor is not receiving commands or when the robot is disabled) + * @param newBehavior the new idle behavior + */ + public void changeIdleBehavior(IdleBehavior newBehavior) { + if (motorType == MotorVendor.CTRE_TALON_FX) { + talon.setNeutralMode(newBehavior == IdleBehavior.BRAKE ? NeutralModeValue.Brake : NeutralModeValue.Coast); + } else if (motorType == MotorVendor.REV_ROBOTICS_SPARK_MAX) { + SparkMaxConfig sparkConfig = config.createSparkMaxConfig(); + sparkConfig.idleMode(newBehavior == IdleBehavior.BRAKE ? IdleMode.kBrake : IdleMode.kCoast); + ErrorHandling.handleRevLibError( + spark.configure(sparkConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters), + "configuring motor " + canID + ); + } + } + + + /** + * Changes the PID Settings applied to this motor. + * @param pidConfig the new PID Config to use. + */ + public void changePID(PIDConfig pidConfig) { + checkForPID(); + if (motorType == MotorVendor.CTRE_TALON_FX) { + Slot0Configs slot0Configs = new Slot0Configs(); + slot0Configs.kV = pidConfig.F; + slot0Configs.kP = pidConfig.P; + slot0Configs.kI = pidConfig.I; + slot0Configs.kD = pidConfig.D; + talon.getConfigurator().apply(slot0Configs); + } else if (motorType == MotorVendor.REV_ROBOTICS_SPARK_MAX) { + SparkMaxConfig sparkConfig = config.createSparkMaxConfig(); + pidConfig.applyToMotor(sparkConfig); + ErrorHandling.handleRevLibError( + spark.configure(sparkConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters), + "configuring motor " + canID + ); + } + } + + /** + * Changes the PID Settings applied to this motor. + * @param pidConfig the optional PID Config to use. For use with the PIDTuning class. + */ + public void changePID(Optional pidConfig) { + if (pidConfig.isPresent()) { + changePID(pidConfig.get()); + } + } + + /** + * Creates a new motor and sets it up to automatically follow the actions of this Motor. + * Note: you do not get access to the newly created motor, you actually don't need it. + * Just tell this motor what to do and the follower motor will do the same thing using + * most of the same configs that were used to make this motor (including PID and Conversion Configs). + * This also regisiters the follower motor CAN ID in the CAN ID Registry. + * @param followerConfig the follower config + */ + public void addFollowerMotor(FollowerConfig followerConfig) { + CanIdRegistry.getRegistry().registerCanId(followerConfig.canID); + if (motorType == MotorVendor.CTRE_TALON_FX) { + TalonFX talonFollower = new TalonFX(followerConfig.canID); + talonFollower.getConfigurator().apply(config.createTalonFXConfiguration()); + talonFollower.setControl(new Follower(canID, followerConfig.inverted)); + talonFollowers.add(talonFollower); + } else if (motorType == MotorVendor.REV_ROBOTICS_SPARK_MAX) { + SparkMax sparkFollower = new SparkMax(followerConfig.canID, SparkMax.MotorType.kBrushless); + SparkMaxConfig sparkConfig = config.createSparkMaxConfig(); + sparkConfig.follow(canID, followerConfig.inverted); + ErrorHandling.handleRevLibError( + sparkFollower.configure(sparkConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters), + "configuring follower motor " + followerConfig.canID + ); + sparkFollowers.add(sparkFollower); + } + } + + private void checkForPID() { + if (!this.isConfiguredWithPID) { + ErrorHandling.complainAboutIncorrectUseOfPID( + "Attempted to use PID on a motor that was not configured with PID Constants." + + "The motor this happened to has the CAN ID: " + this.canID + ); + } + } +} diff --git a/src/main/java/poplibv2/motors/MotorConfig.java b/src/main/java/poplibv2/motors/MotorConfig.java new file mode 100644 index 0000000..07bc41e --- /dev/null +++ b/src/main/java/poplibv2/motors/MotorConfig.java @@ -0,0 +1,111 @@ +package poplibv2.motors; + +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.revrobotics.spark.config.SparkMaxConfig; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; + +import poplibv2.control_systems.PIDConfig; + +/** + * A container for all the Contansts a Motor needs. + */ +public class MotorConfig { + private int canID; + private String canBUS; + private MotorVendor motorVendor; + private PIDConfig pidConfig; + private int currentLimit; + private boolean inverted; + private IdleBehavior idleMode; + private ConversionConfig conversionConfig; + private boolean isConfiguredWithPID; + + /** + * The constuctor for creating the motorConfig + * @param canID The can ID that is used to access the motor + * @param canBUS What can bus the motor is on. Default is "rio" + * @param motorVendor What type of motor this is + * @param pidConfig The PID constants you want to apply to this motor + * @param currentLimit The current limit that you want to set in software + * @param inverted Whether or not the motor should be inverted + * @param idleMode What the default behavior of the motor should be + * @param conversionConfig What the gear box conversion constants are for this motor + */ + public MotorConfig(int canID, String canBUS, MotorVendor motorVendor, PIDConfig pidConfig, + int currentLimit, boolean inverted, IdleBehavior idleMode, ConversionConfig conversionConfig) { + this.canID = canID; + this.canBUS = canBUS; + this.motorVendor = motorVendor; + this.pidConfig = pidConfig; + this.currentLimit = currentLimit; + this.inverted = inverted; + this.idleMode = idleMode; + this.conversionConfig = conversionConfig; + } + + /** + * INTERNAL POPLIB FUNCTION. + *

+ * Used to apply the current MotorConfig and fit it into a SparkMaxConfig + * @return The SparkMaxConfig object + */ + public SparkMaxConfig createSparkMaxConfig() { + SparkMaxConfig config = new SparkMaxConfig(); + + config.inverted(inverted); + config.idleMode(idleMode == IdleBehavior.BRAKE ? IdleMode.kBrake : IdleMode.kCoast); + config.smartCurrentLimit(currentLimit); + isConfiguredWithPID = pidConfig.applyToMotor(config); + conversionConfig.applyToMotor(config); + + return config; + } + + /** + * INTERNAL POPLIB FUNCTION. + *

+ * Used to apply the current MotorConfig and fit it into a TalonFXConfiguration + * @return The TalonFXConfiguration object + */ + public TalonFXConfiguration createTalonFXConfiguration() { + TalonFXConfiguration config = new TalonFXConfiguration(); + config.CurrentLimits.StatorCurrentLimit = currentLimit; + config.CurrentLimits.SupplyCurrentLimitEnable = false; + config.CurrentLimits.StatorCurrentLimitEnable = true; + config.MotorOutput.NeutralMode = idleMode == IdleBehavior.BRAKE ? NeutralModeValue.Brake : NeutralModeValue.Coast; + config.MotorOutput.Inverted = inverted ? InvertedValue.CounterClockwise_Positive : InvertedValue.Clockwise_Positive; + isConfiguredWithPID = pidConfig.applyToMotor(config); + conversionConfig.applyToMotor(config); + return config; + } + + /** + * @return the motor vendor enum + */ + public MotorVendor getMotorVendor() { + return this.motorVendor; + } + + /** + * @return the can id of the motor + */ + public int getCANID() { + return this.canID; + } + + /** + * @return the can bus to create the motor + */ + public String getCANBUS() { + return this.canBUS; + } + + /** + * @return whether or not to use pid + */ + public boolean getIsConfiguredWithPID() { + return this.isConfiguredWithPID; + } +} diff --git a/src/main/java/poplibv2/motors/MotorVendor.java b/src/main/java/poplibv2/motors/MotorVendor.java new file mode 100644 index 0000000..50c033b --- /dev/null +++ b/src/main/java/poplibv2/motors/MotorVendor.java @@ -0,0 +1,10 @@ +package poplibv2.motors; + +/** + * Defines the different Motor Vendors that we work with. Needed for MotorConfig to let it know which motor type to create. + * Format is COMPANY_NAME_API_NAME + */ +public enum MotorVendor { + REV_ROBOTICS_SPARK_MAX, + CTRE_TALON_FX +} diff --git a/src/main/java/poplibv2/sensors/absolute_encoder/CANCoder.java b/src/main/java/poplibv2/sensors/absolute_encoder/CANCoder.java new file mode 100644 index 0000000..574863c --- /dev/null +++ b/src/main/java/poplibv2/sensors/absolute_encoder/CANCoder.java @@ -0,0 +1,37 @@ +package poplibv2.sensors.absolute_encoder; + +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.signals.SensorDirectionValue; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.units.Units; +import poplibv2.misc.CanIdRegistry; + +public class CANCoder { + private CANcoder encoder; + + /** + * Creates a new CANCoder (absolute encoder) using the CANCoderConfig + * Since this is a CAN Device, the ID is registered in the CAN ID Registry to make sure duplicate IDs are not created. + * @param config the config to be used + */ + public CANCoder(CANCoderConfig config) { + CanIdRegistry.getRegistry().registerCanId(config.id); + encoder = new CANcoder(config.id, config.CANBus); + + CANcoderConfiguration sensorConfig = new CANcoderConfiguration(); + sensorConfig.MagnetSensor.SensorDirection = SensorDirectionValue.valueOf(config.inversion ? 1 : 0); + sensorConfig.MagnetSensor.MagnetOffset = -config.offset.getRotations(); + + encoder.getConfigurator().apply(sensorConfig); + } + + /** + * Gets the position the CANCoder is displaying + * @return The position as a Rotation2d in rotations. + */ + public Rotation2d getPosition() { + return Rotation2d.fromRotations(encoder.getAbsolutePosition().getValue().in(Units.Rotations)); + } +} diff --git a/src/main/java/poplibv2/sensors/absolute_encoder/CANCoderConfig.java b/src/main/java/poplibv2/sensors/absolute_encoder/CANCoderConfig.java new file mode 100644 index 0000000..8ea05c8 --- /dev/null +++ b/src/main/java/poplibv2/sensors/absolute_encoder/CANCoderConfig.java @@ -0,0 +1,25 @@ +package poplibv2.sensors.absolute_encoder; + +import edu.wpi.first.math.geometry.Rotation2d; + +public class CANCoderConfig { + public final int id; + public final String CANBus; + public final Rotation2d offset; + public final boolean inversion; + + /** + * Creates a new Config for a CANCoder. + * @param id The CAN Id of the CANCoder. + * @param CANBus The CANBus of the CANCoder + * @param offset The offset of the CANCoder, used to align your swerve wheels. + * @param inversion Whether or not the encoder should be inverted. + */ + public CANCoderConfig(int id, String CANBus, Rotation2d offset, boolean inversion) { + this.id = id; + this.CANBus = CANBus; + this.offset = offset; + this.inversion = inversion; + } + +} diff --git a/src/main/java/poplibv2/sensors/beam_break/BeamBreak.java b/src/main/java/poplibv2/sensors/beam_break/BeamBreak.java new file mode 100644 index 0000000..8544a17 --- /dev/null +++ b/src/main/java/poplibv2/sensors/beam_break/BeamBreak.java @@ -0,0 +1,49 @@ +package poplibv2.sensors.beam_break; + +import java.util.function.BooleanSupplier; +import edu.wpi.first.wpilibj.DigitalInput; + +public class BeamBreak { + private final DigitalInput beamBreak; + + public BeamBreak(int portId) { + this.beamBreak = new DigitalInput(portId); + } + + /** + * @return whether or not there is an object infront of the beam break. + */ + public boolean isBlocked() { + return !beamBreak.get(); + } + + /** + * Creates a BooleanSupplier that returns if the beambreak blocked status matches the parameter boolean value + * @param value The value to check for matching + * @return A BooleanSupplier that can be used for command chaining + */ + public BooleanSupplier isEqualTo(boolean value) { + return () -> { + if (value) { + return isBlocked(); + } + return !isBlocked(); + }; + } + + /** + * Creates a BooleanSupplier that returns if the is beambreak blocked + * @return A BooleanSupplier that can be used for command chaining + */ + public BooleanSupplier getBlockedSupplier() { + return () -> isBlocked(); + } + + /** + * Creates a BooleanSupplier that returns if the is beambreak unblocked + * @return A BooleanSupplier that can be used for command chaining + */ + public BooleanSupplier getUnBlockedSupplier() { + return () -> !isBlocked(); + } +} diff --git a/src/main/java/poplibv2/sensors/camera/Camera.java b/src/main/java/poplibv2/sensors/camera/Camera.java new file mode 100644 index 0000000..407ae87 --- /dev/null +++ b/src/main/java/poplibv2/sensors/camera/Camera.java @@ -0,0 +1,204 @@ +package poplibv2.sensors.camera; + +import java.util.ArrayList; +import java.util.List; +import java.util.Optional; + +import org.photonvision.EstimatedRobotPose; +import org.photonvision.PhotonCamera; +import org.photonvision.PhotonPoseEstimator; +import org.photonvision.PhotonPoseEstimator.PoseStrategy; +import org.photonvision.targeting.PhotonPipelineResult; +import org.photonvision.targeting.PhotonTrackedTarget; + +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.wpilibj.DriverStation; + + + +public class Camera { + private final PhotonCamera camera; + private final CameraConfig config; + private PhotonPoseEstimator poseEstimator; + private final Matrix singleTagStdDevs = VecBuilder.fill(4, 4, 8); + private Matrix currStdDevs = null; + private AprilTagFieldLayout layout; + private List store; + + /** + * Creates a new Photon Vision Camera + * @param config + */ + public Camera(CameraConfig config) { + this.config = config; + camera = new PhotonCamera(config.cameraName); + layout = AprilTagFieldLayout.loadField(config.aprilTagField); + poseEstimator = new PhotonPoseEstimator(layout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, config.cameraToRobot); + poseEstimator.setMultiTagFallbackStrategy(PoseStrategy.CLOSEST_TO_REFERENCE_POSE); + } + + /** + * returns the distance between the best April tag (tag with the lowest reprojection error) and the focal lens of the camera. + * Can be combined with the cameraToRobot mapping that you gave in CameraConfig to make an AprilTag to robot vector. + * @return The Pose2d as an Optional as the camera might not see a valid april tag. Always use the .isPresent() method before the .get() method. + */ + public Optional relativeDistanceFromCameraToAprilTag() { + if (!camera.isConnected()) { + DriverStation.reportError("Camera named: " + config.cameraName + " is not connected!!!!!!!!", false); + // the above code should save to the log file that you can view in the DS Log Viewer + return Optional.empty(); + } + ArrayList poses = new ArrayList<>(); + + Optional ret1 = Optional.empty(); + Optional ret = Optional.empty(); + for (PhotonPipelineResult result : store) { + if (result.hasTargets()) { + List target = result.getTargets(); + for (var i : target) { + poses.add(i); + } + } + } + + for (var pose : poses) { + if (ret1.isEmpty() || dist(ret1.get().getBestCameraToTarget()) > dist(pose.getBestCameraToTarget())) { + ret1 = Optional.of(pose); + } + } + + if (ret1.isPresent()) { + Optional pose = layout.getTagPose(ret1.get().getFiducialId()); + if (pose.isPresent()) { + ret = Optional.of(new Pose2d( + new Translation2d(ret1.get().getBestCameraToTarget().getX(), ret1.get().getBestCameraToTarget().getY()), + Rotation2d.fromRadians(pose.get().getRotation().getZ()))); + }} + return ret; + } + + /** + * The mathematical distance formula that you learn in algebra. + */ + private double dist(Transform3d pose) { + return Math.sqrt(Math.pow(pose.getX(), 2) + Math.pow(pose.getY(), 2)); + } + + /** + * Returns the estimated position of the robot based on april tag readings from the camera and the current pose of the robot. + * Meant to be used in SwerveDrivePoseEstimator.addVisionMeasurement() + * @param currPose the current position of the robot + * @return The new position of the robot as a EstimatedRobotPose wrapped in an Optional as the camera might not have seen anything. Always use .isPresent() before .get() + */ + public Optional getEstimatedPose(Pose2d currPose) { + if (!camera.isConnected()) { + DriverStation.reportError("Camera named: " + config.cameraName + " is not connected!!!!!!!!", false); + // the above code should save to the log file that you can view in the DS Log Viewer + return Optional.empty(); + } + poseEstimator.setReferencePose(currPose); + Optional visionEst = Optional.empty(); + store.clear(); + store = camera.getAllUnreadResults(); + for (PhotonPipelineResult change : store) { + visionEst = poseEstimator.update(change); + updateStdDevs(visionEst, change.getTargets()); + } + if (visionEst.isPresent() && isValidPose(visionEst.get())) { + return visionEst; + } + return Optional.empty(); + } + + /** + * returns the current calculuated standard deviations for the getEstimatedPose calculation. + * Meant to be used in SwerveDrivePoseEstimator.addVisionMeasurement() + * @return the stdDevs in X, Y, Z format. + */ + public Matrix getVisionStdDevs() { + return currStdDevs; + } + + /** + * Updates the standard deviations of the April Tag Pose. + * @param visionEst The current estimated pose of the robot. + * @param targets The targets (april tags to go through) + */ + private void updateStdDevs(Optional visionEst, List targets) { + if (visionEst.isEmpty()) { + currStdDevs = singleTagStdDevs; + } else { + Matrix stdDevs = singleTagStdDevs; + int numTags = 0; + double avg = 0; + for (PhotonTrackedTarget target : targets) { + if (config.stdDevStategy == StdDevStategy.DISTANCE) { + Optional tagPose = poseEstimator.getFieldTags().getTagPose(target.getFiducialId()); + if (tagPose.isPresent()) { + numTags++; + avg += tagPose.get().toPose2d().getTranslation().getDistance( + visionEst.get().estimatedPose.toPose2d().getTranslation()); + } + } else if (config.stdDevStategy == StdDevStategy.AMBIGUITY) { + avg += target.getPoseAmbiguity(); + numTags++; + } + } + if (numTags == 0) { + currStdDevs = singleTagStdDevs; + } else { + avg /= numTags; + if (numTags > 1) { + stdDevs = VecBuilder.fill(0.5, 0.5, 1); + } + if (config.stdDevStategy == StdDevStategy.DISTANCE) { + if (numTags == 1 && avg > 4) { + stdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE); // tag too unreliable + } else { + stdDevs = stdDevs.times(1 + (avg * avg / 30)); + } + } else if (config.stdDevStategy == StdDevStategy.AMBIGUITY) { + if (avg > 0.4) { // ur cooked lil bro + stdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE); + } else { + stdDevs = stdDevs.times(1 + (avg * avg * 30)); + } + } + currStdDevs = stdDevs; + } + } + } + + /** + * Checks if a pose is valid or not based on the thresholds provided in CameraConfig + * @param pose The pose to check + * @return Whether or not it is valid. + */ + private boolean isValidPose(EstimatedRobotPose pose) { + List targets = pose.targetsUsed; + double max_dist = 0; + for (PhotonTrackedTarget target : targets) { + Optional tagPose = poseEstimator.getFieldTags().getTagPose(target.getFiducialId()); + if (tagPose.isPresent()) { + max_dist = Math.max(max_dist, tagPose.get().toPose2d().getTranslation(). + getDistance(pose.estimatedPose.toPose2d().getTranslation())); + } + } + if (max_dist >= config.poseDistanceThreshold) { + return false; + } + if (targets.size() == 1) { + return targets.get(0).getPoseAmbiguity() <= config.poseAmbiguityThreshold; + } + return true; + } +} \ No newline at end of file diff --git a/src/main/java/poplibv2/sensors/camera/CameraConfig.java b/src/main/java/poplibv2/sensors/camera/CameraConfig.java new file mode 100644 index 0000000..32e7400 --- /dev/null +++ b/src/main/java/poplibv2/sensors/camera/CameraConfig.java @@ -0,0 +1,33 @@ +package poplibv2.sensors.camera; + +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.geometry.Transform3d; + +public class CameraConfig { + public final String cameraName; + public final Transform3d cameraToRobot; + public final double poseAmbiguityThreshold; + public final double poseDistanceThreshold; + public final StdDevStategy stdDevStategy; + public final AprilTagFields aprilTagField; + + /** + * Creates a new CameraConfig that can be used to create a new Camera Object. + * This will be used when creating a new PhotonVision Camera + * @param cameraName The camera name, as set in the PhotonVision Dashboard (Note: this is the CAMERA name, not the name of the computer running photonvision) + * @param cameraToRobot A 3d vector detailing the difference in positions between the center of the robot (where the gyro is) and the focal lens of the camera. See the WPILIB robot coordinate system on how to do this + * @param poseAmbiguityThreshold The maximum allowed ambiguity for a detected april tag pose to still be valid + * @param poseDistanceThreshold The maximum allowed distance for a detected april tag pose to still be valid + * @param stdDevStategy The Standard Deviation calculuated stategy to be used when calculating the variable standard deviations of an april tag pose. + * @param thisYearsField The AprilTagFields object that details the april tag layout for this years field. + */ + public CameraConfig(String cameraName, Transform3d cameraToRobot, double poseAmbiguityThreshold, + double poseDistanceThreshold, StdDevStategy stdDevStategy, AprilTagFields thisYearsField) { + this.cameraName = cameraName; + this.cameraToRobot = cameraToRobot; + this.poseAmbiguityThreshold = poseAmbiguityThreshold; + this.poseDistanceThreshold = poseDistanceThreshold; + this.stdDevStategy = stdDevStategy; + this.aprilTagField = thisYearsField; + } +} diff --git a/src/main/java/poplibv2/sensors/camera/DetectedObject.java b/src/main/java/poplibv2/sensors/camera/DetectedObject.java new file mode 100644 index 0000000..18dedf3 --- /dev/null +++ b/src/main/java/poplibv2/sensors/camera/DetectedObject.java @@ -0,0 +1,22 @@ +package poplibv2.sensors.camera; + +public class DetectedObject { + public final double xAngleOffset; + public final double yAngleOffset; + public final double area; + public final String objectType; + + /** + * A container for storing objects detected by a Camera. This is a public container, please access the values directly + * @param xAngleOffset The pixel offset between the object and the center of the camera in the x direction + * @param yAngleOffset The pixel offset between the object and the center of the camera in the x direction + * @param area What percent of the camera screen the object takes up + * @param objectType The detected type of the object + */ + public DetectedObject(double xAngleOffset, double yAngleOffset, double area, String objectType) { + this.xAngleOffset = xAngleOffset; + this.yAngleOffset = yAngleOffset; + this.area = area; + this.objectType = objectType; + } +} \ No newline at end of file diff --git a/src/main/java/poplibv2/sensors/camera/Limelight.java b/src/main/java/poplibv2/sensors/camera/Limelight.java new file mode 100644 index 0000000..1e564fb --- /dev/null +++ b/src/main/java/poplibv2/sensors/camera/Limelight.java @@ -0,0 +1,39 @@ +package poplibv2.sensors.camera; + +import java.util.Optional; + +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; + +public class Limelight { + private NetworkTable networkTable; + private LimelightConfig config; + + /** + * Creates a new Limelight from the config + * @param config the config to use + */ + public Limelight(LimelightConfig config) { + this.config = config; + networkTable = NetworkTableInstance.getDefault().getTable(config.limelightName); + networkTable.getEntry("ledMode").setNumber(config.ledState); + networkTable.getEntry("pipeline").setNumber(config.pipeline); + + } + + /** + * Returns the latest object that the limelight has detected. + * @return The object in the DetectedObject format, wrapped in an optional in case the limelight didn't detect anything new. + */ + public Optional getLastestDetection() { + double area = networkTable.getEntry("ta").getDouble(0.0); + if (networkTable.getEntry("tv").getInteger(0) == 1 && area > config.minValidArea) { + return Optional.of(new DetectedObject( + networkTable.getEntry("tx").getDouble(0.0), + networkTable.getEntry("ty").getDouble(0.0), area, + networkTable.getEntry("tclass").getString("") + )); + } + return Optional.empty(); + } +} \ No newline at end of file diff --git a/src/main/java/poplibv2/sensors/camera/LimelightConfig.java b/src/main/java/poplibv2/sensors/camera/LimelightConfig.java new file mode 100644 index 0000000..cfb62ae --- /dev/null +++ b/src/main/java/poplibv2/sensors/camera/LimelightConfig.java @@ -0,0 +1,22 @@ +package poplibv2.sensors.camera; + +public class LimelightConfig { + public String limelightName; + public int pipeline; + public int ledState; + public double minValidArea; + + /** + * Creates a config that can be used to make a new Limelight. + * @param limelightName The name of the limelight + * @param pipeline What detection pipeline to use + * @param ledState What state the green leds should be in + * @param minValidArea The minimum area a detected object needs to ocupy before being considered as a valid detection. + */ + public LimelightConfig(String limelightName, int pipeline, int ledState, double minValidArea) { + this.limelightName = limelightName; + this.pipeline = pipeline; + this.ledState = ledState; + this.minValidArea = minValidArea; + } +} \ No newline at end of file diff --git a/src/main/java/poplibv2/sensors/camera/StdDevStategy.java b/src/main/java/poplibv2/sensors/camera/StdDevStategy.java new file mode 100644 index 0000000..48ffe76 --- /dev/null +++ b/src/main/java/poplibv2/sensors/camera/StdDevStategy.java @@ -0,0 +1,9 @@ +package poplibv2.sensors.camera; + +/** + * Details what strategy to use when calculating the variable standard deviations for april tag detection + */ +public enum StdDevStategy { + DISTANCE, // recommended strategy + AMBIGUITY +} diff --git a/src/main/java/poplibv2/sensors/gyro/Gyro.java b/src/main/java/poplibv2/sensors/gyro/Gyro.java new file mode 100644 index 0000000..0009cec --- /dev/null +++ b/src/main/java/poplibv2/sensors/gyro/Gyro.java @@ -0,0 +1,63 @@ +package poplibv2.sensors.gyro; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; + +/** + * Defines an structure for Gyroscope classes to follow. + */ +public abstract class Gyro { + /** + * This method should return the normalized yaw angle of the robot (meaning that it should fit the angle from 0 to 360). + * The angle should be pre-inverted if nessesary. + * @return the Angle in Degrees + */ + public abstract Angle getNormalizedAngle(); + + /** + * This method should return the normalized yaw angle of the robot (meaning that it should fit the angle from 0 to 360). + * The angle should be pre-inverted if nessesary. + * @return the Angle in Degrees as a Rotation2d object + */ + public abstract Rotation2d getNormalizedRotation2dAngle(); + + /** + * This method will zero out the yaw, roll, and pitch of the gyro. + */ + public abstract void zeroGyro(); + + /** + * Allows the user to set the gyro yaw to a new angle + * @param newAngle the new angle of the gyro in degrees as a Rotation2D object. + */ + public abstract void setAngle(Rotation2d newAngle); + + /** + * @return the reported roll of the gyro + */ + public abstract Angle getRoll(); + + /** + * @return the reported pitch of the gyro + */ + public abstract Angle getPitch(); + + /** + * @return the reported yaw of the gyro + */ + public abstract Angle getYaw(); + + /** + * Gets the angluar velocity around the z-axis (the yaw) + * @return The AngularVelocity + */ + public abstract AngularVelocity getAngularVelo(); + + /** + * Get an angle that already has the latency of the CAN Bus taken into account for. + * The angle should be inverted (if nessesary) and normalized (fit from 0 to 360). + * @return The Angle in Degrees + */ + public abstract Angle getLatencyCompensatedAngle(); +} diff --git a/src/main/java/poplibv2/sensors/gyro/Pigeon.java b/src/main/java/poplibv2/sensors/gyro/Pigeon.java new file mode 100644 index 0000000..7add588 --- /dev/null +++ b/src/main/java/poplibv2/sensors/gyro/Pigeon.java @@ -0,0 +1,83 @@ +package poplibv2.sensors.gyro; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.configs.Pigeon2Configuration; +import com.ctre.phoenix6.hardware.Pigeon2; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.units.AngleUnit; +import edu.wpi.first.units.Measure; +import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import poplibv2.misc.CanIdRegistry; + + +public class Pigeon extends Gyro { + private final Pigeon2 gyro; + private final boolean inversion; + + /** + * Creates a new CTRE Pigeon Device (uses the Pigeon2 api) + * Since this is a CAN Device, the ID is registered in the CAN ID Registry to make sure duplicate IDs are not created. + * @param pigeonConfig the config of the pigeon + */ + public Pigeon(PigeonConfig config) { + CanIdRegistry.getRegistry().registerCanId(config.id); + gyro = new Pigeon2(config.id, config.canBusName); + gyro.getConfigurator().apply(new Pigeon2Configuration()); // apperently this actually needs to be done + this.inversion = config.inversion; + } + + @Override + public Angle getNormalizedAngle() { + return invertAndNormalizeAngle(getYaw()); + } + + @Override + public Angle getYaw() { + return gyro.getYaw().getValue(); + } + + @Override + public Angle getPitch() { + return gyro.getPitch().getValue(); + } + + @Override + public Angle getRoll() { + return gyro.getRoll().getValue(); + } + + @Override + public void zeroGyro() { + gyro.setYaw(0); + } + + @Override + public void setAngle(Rotation2d newAngle) { + gyro.setYaw(newAngle.getDegrees()); + } + + @Override + public AngularVelocity getAngularVelo() { + return gyro.getAngularVelocityZDevice().getValue(); + } + + @Override + public Angle getLatencyCompensatedAngle() { + Measure yaw = BaseStatusSignal.getLatencyCompensatedValue(gyro.getYaw().refresh(), gyro.getAngularVelocityZDevice().refresh()); + return invertAndNormalizeAngle(yaw); + } + + private Angle invertAndNormalizeAngle(Measure angle) { + return inversion ? Units.Degrees.of(MathUtil.inputModulus(360 - angle.in(Units.Degrees), 0, 360)) + : Units.Degrees.of(MathUtil.inputModulus(angle.in(Units.Degrees), 0, 360)); + } + + @Override + public Rotation2d getNormalizedRotation2dAngle() { + return new Rotation2d(getNormalizedAngle()); + } + +} diff --git a/src/main/java/poplibv2/sensors/gyro/PigeonConfig.java b/src/main/java/poplibv2/sensors/gyro/PigeonConfig.java new file mode 100644 index 0000000..809b2b9 --- /dev/null +++ b/src/main/java/poplibv2/sensors/gyro/PigeonConfig.java @@ -0,0 +1,19 @@ +package poplibv2.sensors.gyro; + +public class PigeonConfig { + public int id; + public boolean inversion; + public String canBusName; + + /** + * A Config for the Pigeon/gyro + * @param id the CAN ID + * @param inversion if it should be inverted bc electrical place it backward again + * @param canBusName the name of the can bus loop it is on + */ + public PigeonConfig(int id, boolean inversion, String canBusName) { + this.id = id; + this.inversion = inversion; + this.canBusName = canBusName; + } +} diff --git a/src/main/java/poplibv2/subsystems/arm/Arm.java b/src/main/java/poplibv2/subsystems/arm/Arm.java new file mode 100644 index 0000000..08ac5cd --- /dev/null +++ b/src/main/java/poplibv2/subsystems/arm/Arm.java @@ -0,0 +1,52 @@ +package poplibv2.subsystems.arm; + +import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.controller.ElevatorFeedforward; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import poplib.subsytems.pivot.Pivot; +import poplibv2.motors.MotorConfig; +import poplibv2.control_systems.PIDConfig; +import poplibv2.control_systems.PIDTuning; +import poplibv2.misc.TunableNumber; +import poplibv2.motors.FollowerConfig; +import poplibv2.motors.Motor; + +public class Arm extends Pivot { + private ArmFeedforward feedforward; + private TrapezoidProfile setPointCalc; + private TrapezoidProfile.State goal; + private TrapezoidProfile.State calculated; + private Angle rotationToDegrees; + private final double loopTime = 0.02; + + public Arm(MotorConfig leadMotorConfig, FollowerConfig[] followerConfigs, ArmFeedforward feedforward, Angle rotationToDegrees, + double maxAngularVelocity, double maxAngularAcceleration, double maxRotation, boolean tuningMode, String subsystemName) { + super(subsystemName); + + this.motor = new Motor(leadMotorConfig); + for (FollowerConfig followerConfig : followerConfigs) { + this.motor.addFollowerMotor(followerConfig); + } + this.setpoint = new TunableNumber("Elevator Setpoint", 0, tuningMode); + this.tuning = new PIDTuning("Arm", new PIDConfig(), tuningMode); + this.feedforward = feedforward; + this.setPointCalc = new TrapezoidProfile(new TrapezoidProfile.Constraints(maxAngularVelocity, maxAngularAcceleration)); + this.goal = new TrapezoidProfile.State(); + this.calculated = new TrapezoidProfile.State(); + this.rotationToDegrees = rotationToDegrees; + this.maxRotation = maxRotation; + } + + @Override + public void periodic() { + motor.changePID(tuning.generatePIDConfig()); + + goal = new TrapezoidProfile.State(setpoint.get(), 0); + calculated = setPointCalc.calculate(loopTime, calculated, goal); + motor.setTargetPosition(calculated.position, feedforward.calculate(calculated.position, calculated.velocity)); + } + +} diff --git a/src/main/java/poplibv2/subsystems/elevator/Elevator.java b/src/main/java/poplibv2/subsystems/elevator/Elevator.java new file mode 100644 index 0000000..2dc8cd7 --- /dev/null +++ b/src/main/java/poplibv2/subsystems/elevator/Elevator.java @@ -0,0 +1,146 @@ +package poplibv2.subsystems.elevator; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import poplibv2.control_systems.PIDConfig; +import poplibv2.control_systems.PIDTuning; +import poplibv2.misc.TunableNumber; +import poplibv2.motors.FollowerConfig; +import poplibv2.motors.Motor; +import poplibv2.motors.MotorConfig; +import edu.wpi.first.math.controller.ElevatorFeedforward; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.math.trajectory.TrapezoidProfile.State; +import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; +import edu.wpi.first.wpilibj2.command.Command; + +public abstract class Elevator extends SubsystemBase{ + private TunableNumber setpoint; + private PIDTuning tuning; + private ElevatorFeedforward feedforward; + private TrapezoidProfile setPointCalc; + private TrapezoidProfile.State goal; + private TrapezoidProfile.State calculated; + private Distance rotationToMeters; + private Motor motor; + private double maxExtension; + private final double loopTime = 0.02; + + /** + * Creates a new Elevator + * @param leadMotor the lead motor config + * @param followerMotors an array of FollowerConfigs for any follower motors + * @param feedforward the feedforward calculator + * @param rotationToMeters how many meters the elevator goes up when the motors are driven 1 rotation + * @param maxVelocity in Meters/s + * @param maxAcceleration in Meters/s/s + * @param maxExtension in Meters + * @param tuningMode whether or not to allow PID and setpoint tuning + * @param subsytemName the name of the subsystem, for logging + */ + public Elevator(MotorConfig leadMotor, FollowerConfig[] followerMotors, ElevatorFeedforward feedforward, + Distance rotationToMeters, double maxVelocity, double maxAcceleration, double maxExtension, boolean tuningMode, String subsytemName) { + super(subsytemName); + + setpoint = new TunableNumber("Elevator Setpoint", 0, tuningMode); + tuning = new PIDTuning("Elevator", new PIDConfig(), tuningMode); + this.feedforward = feedforward; + motor = new Motor(leadMotor); + for (FollowerConfig config : followerMotors) { + motor.addFollowerMotor(config); + } + this.rotationToMeters = rotationToMeters; + this.setPointCalc = new TrapezoidProfile(new TrapezoidProfile.Constraints(maxVelocity, maxAcceleration)); + this.maxExtension = maxExtension; + } + + @Override + public void periodic() { + motor.changePID(tuning.generatePIDConfig()); // update pid constants (for pid tuning) + + goal = new State(setpoint.get(), 0); // creates a new goal for the elevator using the setpoint + calculated = setPointCalc.calculate(loopTime, calculated, goal); // motion profiling uses the current state and the goal state to calculuate new curr state + motor.setTargetPosition(calculated.position / rotationToMeters.in(Units.Meters), feedforward.calculate(calculated.velocity)); // actually sets the PID and Feedfoward + } + + /** + * @return Returns the position of the motor in rotations + */ + public double getMotorPos() { + return motor.getPosition(); + }; + + /** + * @return Returns the elevator position in meters + */ + public double getElevatorPos() { + return getMotorPos() * rotationToMeters.in(Units.Meters); + } + + /** + * Returns the elevators velocity in meters/second + * @return + */ + public double getElevatorVelo() { + return (motor.getVelocity() * rotationToMeters.in(Units.Meters)) / 60.0; + } + + /** + * Moves the elevator to your desired position + * @param setPoint the desired position in meters + * @param error the amount of acceptable error in meters + * @return + */ + public Command moveElevator(double setPoint, double error) { + return run(() -> setpoint.set(setPoint)). + until(() -> motor.atPositionSetpoint(setPoint / rotationToMeters.in(Units.Meters), + error / rotationToMeters.in(Units.Meters))); + } + + /** + * This is really not recommended. + * Please try to talk your drive team out of doing this. + * However, if they really want "manual" elevator control, here it is. + *

+ * Moves the elevator up + * @param adjustment the number of meters to move it up + * @return + */ + public Command moveUp(double adjustment) { + return moveElevator(Math.min(getElevatorPos() + adjustment, maxExtension), 0.1 * adjustment); + } + + /** + * This is really not recommended. + * Please try to talk your drive team out of doing this. + * However, if they really want "manual" elevator control, here it is. + *

+ * Moves the elevator down + * @param adjustment the number of meters to move it down + * @return + */ + public Command moveDown(double adjustment) { + return moveElevator(Math.max(getElevatorPos() - adjustment, 0), 0.1 * adjustment); + } + + /** + * Sets the motors voltage + * @param voltage the voltage to be set + */ + public void runSysIdRoutine(Voltage voltage) { + motor.setVoltage(voltage.in(Units.Volts)); + } + + /** + * Logs the motors behavior. + * @param log + */ + public void sysIdLogMotors(SysIdRoutineLog log) { + log.motor("Elevator Motor"). + linearPosition(Units.Meters.of(getElevatorPos())). + linearVelocity(Units.MetersPerSecond.of(getElevatorVelo())). + voltage(motor.getVoltage()); + } +} diff --git a/src/main/java/poplibv2/subsystems/elevator/SysIdElevator.java b/src/main/java/poplibv2/subsystems/elevator/SysIdElevator.java new file mode 100644 index 0000000..1a84f1b --- /dev/null +++ b/src/main/java/poplibv2/subsystems/elevator/SysIdElevator.java @@ -0,0 +1,28 @@ +package poplibv2.subsystems.elevator; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; + +public class SysIdElevator { + SysIdRoutine routine; + + /** + * Runs a System Identification on the elevator which can be used to find kF (kV) constants for drive motors + * @param Elevator the Elevator Subsystem + */ + public SysIdElevator(Elevator elevator) { + routine = new SysIdRoutine( + new SysIdRoutine.Config(), + new SysIdRoutine.Mechanism(elevator::runSysIdRoutine, elevator::sysIdLogMotors, elevator, "Elevator") + ); + } + + public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { + return routine.quasistatic(direction); + } + + public Command sysIdDynamic(SysIdRoutine.Direction direction) { + return routine.dynamic(direction); + } + +} diff --git a/src/main/java/poplibv2/subsystems/pivot/Pivot.java b/src/main/java/poplibv2/subsystems/pivot/Pivot.java new file mode 100644 index 0000000..e5196fb --- /dev/null +++ b/src/main/java/poplibv2/subsystems/pivot/Pivot.java @@ -0,0 +1,15 @@ +package poplibv2.subsystems.pivot; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import poplibv2.control_systems.PIDTuning; +import poplibv2.misc.TunableNumber; +import poplibv2.motors.Motor; + +public class Pivot extends SubsystemBase { + private TunableNumber setpoint; + private PIDTuning tuning; + private Motor motor; + private double maxRotation; + + +} diff --git a/src/main/java/poplibv2/subsystems/swerve/Swerve.java b/src/main/java/poplibv2/subsystems/swerve/Swerve.java new file mode 100644 index 0000000..4a6579d --- /dev/null +++ b/src/main/java/poplibv2/subsystems/swerve/Swerve.java @@ -0,0 +1,318 @@ +package poplibv2.subsystems.swerve; + +import java.util.Arrays; +import java.util.Collections; +import java.util.Optional; + +import org.photonvision.EstimatedRobotPose; + +import edu.wpi.first.math.MathUtil; +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.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.LinearVelocity; +import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +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.SubsystemBase; +import poplibv2.sensors.camera.DetectedObject; +import poplibv2.control_systems.PIDConfig; +import poplibv2.control_systems.PIDTuning; +import poplibv2.sensors.camera.Camera; +import poplibv2.sensors.camera.Limelight; +import poplibv2.sensors.gyro.Pigeon; +import poplibv2.subsystems.swerve.setup.SwerveConfig; + +public class Swerve extends SubsystemBase { + + private SwerveModule[] wheels; + private Pigeon gyro; + public Camera[] cameras; + public Limelight[] limelights; + public Field2d field; + public Pose2d prevPose; + public double prevPoseTimeStamp; + public LinearVelocity maxSpeed; + public AngularVelocity maxAngularVelocity; + private PIDTuning driveMotorTuning; + private PIDTuning rotMotorTuning; + private SwerveDriveKinematics kinematics; + private SwerveDrivePoseEstimator odometry; + + /** + * Creates a new Swerve Drivetrain, and zeros all of the wheels. + * Sets up all of the swerve things. + * @param config + */ + public Swerve(SwerveConfig config) { + wheels = new SwerveModule[4]; + for (int i = 0; i < 4; i++) { + wheels[i] = new SwerveModule(config.swerveModuleConfigs[i]); + } + + gyro = new Pigeon(config.gyro); + gyro.zeroGyro(); + + cameras = new Camera[config.cameraConfigs.length]; + for (int i = 0; i < config.cameraConfigs.length; i++) { + cameras[i] = new Camera(config.cameraConfigs[i]); + } + + limelights = new Limelight[config.limelightConfigs.length]; + for (int i = 0; i < config.limelightConfigs.length; i++) { + limelights[i] = new Limelight(config.limelightConfigs[i]); + } + + field = new Field2d(); + driveMotorTuning = new PIDTuning("Swerve Motor Drive", new PIDConfig(), SwerveConfig.tuningEnable); + rotMotorTuning = new PIDTuning("Swerve Motor Rotation", new PIDConfig(), SwerveConfig.tuningEnable); + maxSpeed = config.swerveModuleConfigs[0].maxSpeed; + maxAngularVelocity = config.swerveModuleConfigs[0].maxAngularVelocity; + + kinematics = new SwerveDriveKinematics(config.wheelPos); + odometry = new SwerveDrivePoseEstimator(kinematics, gyro.getNormalizedRotation2dAngle(), getModulePositions(), new Pose2d(0.0, 0.0, this.gyro.getNormalizedRotation2dAngle())); + + updatePrevPose(); + } + + @Override + public void periodic() { + updatePID(); + updatePrevPose(); + updateOdom(); + field.setRobotPose(getRobotPose()); + log(); + } + + /** + * Updates the PID Constants of each of the swerve module motors + */ + private void updatePID() { + for (int i = 0; i < 4; i++) { + wheels[i].updatePID(driveMotorTuning, rotMotorTuning); + } + } + + /** + * LOG EVERYTHING. (this will come in handy later so we can blame electrical). + */ + private void log() { + SmartDashboard.putNumber("Angle", MathUtil.inputModulus(gyro.getNormalizedAngle().in(Units.Degrees), 0, 360)); + SmartDashboard.putNumber("Robot Angle Velo", getAngleVelo()); + SmartDashboard.putNumber("Robot Velo", getDriveVelo()); + SmartDashboard.putData("Field2d", field); + for (int i = 0; i < 4; i++) { + wheels[i].log(); + } + } + + /** + * Updates the odometry using encoder and vision based methods. + */ + private void updateOdom() { + odometry.update(gyro.getNormalizedRotation2dAngle(), getModulePositions()); + for (int i = 0; i < cameras.length; i++) { + Optional estimatedPose = cameras[i].getEstimatedPose(getRobotPose()); + if (estimatedPose.isPresent()) { + odometry.addVisionMeasurement(estimatedPose.get().estimatedPose.toPose2d(), + estimatedPose.get().timestampSeconds, + cameras[i].getVisionStdDevs()); + } + } + } + + /** + * Drives the robot oriented and applies desaturation + * @param states + */ + private void driveRobotOriented(SwerveModuleState[] states) { + desaturateWheelSpeeds(states, maxSpeed.in(Units.MetersPerSecond)); + for (int i = 0; i < 4; i++) { + wheels[i].setDesiredState(states[i]); + } + } + + /** + * Drives the robot (as robot oriented) + * @param vector (The forward/back and left/right speeds to drive in meters per second) + * @param rot The speed to turn in radians per second + */ + public void driveRobotOriented(Translation2d vector, double rot) { + SwerveModuleState[] states = this.kinematics.toSwerveModuleStates(new ChassisSpeeds(vector.getX(), vector.getY(), rot)); + this.driveRobotOriented(states); + } + + /** + * Drives the robot (as robot oriented) + * @param chassisSpeeds The speeds to follow + */ + public void driveRobotOriented(ChassisSpeeds chassisSpeeds) { + SwerveModuleState[] states = this.kinematics.toSwerveModuleStates(chassisSpeeds); + this.driveRobotOriented(states); + } + + // Vector is in mps, and rot is in radians per sec + // Also this is field oriented + /** + * Drives the robot as field oriented + * @param vector (The forward/back and left/right speeds to drive as a percent of the drivebases max speed (a number from -1.0 to 1.0)) + * @param rot The rotational speed to drive as a percent of the drivebases max rotational speed (a number from -1.0 to 1.0) + * @param color I swear I'm not racist it matters + */ + public void drive(Translation2d vector, double rot, Alliance color) { + vector = vector.times(maxSpeed.in(Units.MetersPerSecond)); + rot *= maxAngularVelocity.in(Units.RadiansPerSecond); + + vector = vector.rotateBy(new Rotation2d( + Units.Degrees.of(color == Alliance.Red ? 180 : 0).minus(gyro.getLatencyCompensatedAngle()) + )); + + driveRobotOriented(vector, rot); + } + + /** + * Caps the wheel speeds once they are over the maximum wheel speed that the motors can handle + * @param states + * @param maxSpeed + */ + public void desaturateWheelSpeeds(SwerveModuleState[] states, double maxSpeed) { + double realMaxSpeed = Collections.max(Arrays.asList(states)).speedMetersPerSecond; + + if (realMaxSpeed > maxSpeed) { + for (SwerveModuleState moduleState : states) { + moduleState.speedMetersPerSecond = + (moduleState.speedMetersPerSecond / realMaxSpeed) * maxSpeed; + } + } + } + + /** + * Gets an array of the wheel positions + * @return An array of SwerveModulePositions + */ + private SwerveModulePosition[] getModulePositions() { + SwerveModulePosition[] positions = new SwerveModulePosition[4]; + for (int i = 0; i < 4; i++) { + positions[i] = wheels[i].getPose(); + } + return positions; + } + + /** + * Gets the current robot estimated pose + * @return the pose + */ + public Pose2d getRobotPose() { + return odometry.getEstimatedPosition(); + } + + /** + * Updates the previous pose, used for drive and angle velocity calculations + */ + private void updatePrevPose() { + prevPose = odometry.getEstimatedPosition(); + prevPoseTimeStamp = System.currentTimeMillis(); + } + + /** + * Rezeros all wheels to thier absolute encoders + */ + public void rezeroAllWheels() { + for (int i = 0; i < 4; i++) { + wheels[i].resetToAbsolute(); + } + } + + /** + * Runs the drive motors at a specific voltage + * @param voltage + */ + public void runSysIdRoutine(Voltage voltage) { + for (int i = 0; i < 4; i++) { + wheels[i].runSysIdRoutine(voltage.in(Units.Volts)); + } + } + + /** + * Gets logs from the motors + * @param log + */ + public void sysIdLogMotors(SysIdRoutineLog log) { + for (int i = 0; i < 4; i++) { + wheels[i].logSysId(log); + } + } + + /** + * Zeros the gyro. + */ + public void resetGyro() { + gyro.zeroGyro(); + } + + /** + * A command to zero the gyro + * @return + */ + public Command resetGyroCommand() { + return runOnce(() -> { + resetGyro(); + }); + } + + /** + * Gets the angluar velocity of the robot in radians per millisecond + * @return + */ + public double getAngleVelo() { + return 1000 * (getRobotPose().getRotation().getRadians() - prevPose.getRotation().getRadians()) + / (System.currentTimeMillis() - prevPoseTimeStamp); // in radians per milisecond + } + + /** + * Gets the velocity of the robot in meters per millisecond + * @return + */ + public double getDriveVelo() { + return 1000 * (getRobotPose().getTranslation().getNorm() - prevPose.getTranslation().getNorm()) + / (System.currentTimeMillis() - prevPoseTimeStamp); // in meters per milisecond + } + + /** + * Based on whether an object is detected using a limelight, adjusts the driver input to move closer to the object + * @param driverInput + * @return the new driver input + */ + public Transform2d addVisionMovementAdjustment(Transform2d driverInput) { + DetectedObject bestDetection = null; + double bestArea = -1.0; + for (Limelight limelight : limelights) { + Optional detection = limelight.getLastestDetection(); + if (detection.isPresent() && ((DetectedObject)detection.get()).area > bestArea) { + bestDetection = (DetectedObject)detection.get(); + bestArea = bestDetection.area; + } + } + + if (bestArea != -1.0 && bestDetection != null) { + Rotation2d newAngle = driverInput.getRotation().plus(Rotation2d.fromDegrees(bestDetection.xAngleOffset / 10.0)); + double newY = driverInput.getY() + bestDetection.xAngleOffset / 52.0; + double newX = driverInput.getX() + 1.0 / bestDetection.area; + return new Transform2d(newX, newY, newAngle); + } else { + return driverInput; + } + } + +} diff --git a/src/main/java/poplibv2/subsystems/swerve/SwerveModule.java b/src/main/java/poplibv2/subsystems/swerve/SwerveModule.java new file mode 100644 index 0000000..699cd43 --- /dev/null +++ b/src/main/java/poplibv2/subsystems/swerve/SwerveModule.java @@ -0,0 +1,210 @@ +package poplibv2.subsystems.swerve; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.units.Units; +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.Time; +import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; +import poplibv2.control_systems.PIDTuning; +import poplibv2.motors.Motor; +import poplibv2.sensors.absolute_encoder.CANCoder; +import poplibv2.subsystems.swerve.setup.SwerveConfig; +import poplibv2.subsystems.swerve.setup.SwerveModuleConfig; + +public class SwerveModule { + Motor driveMotor; + Motor rotMotor; + CANCoder encoder; + int id; + LinearVelocity maxSpeed; + + Rotation2d lastAngle; + LinearVelocity lastVelo; + Time lastVeloTime; + + /** + * Creates a new swerve module (wheel) using a config + * Also sets the wheel of the swerve module to be straight + * during intialization, assuming the absolute encoder offset + * is correct. + * @param config + */ + public SwerveModule(SwerveModuleConfig config) { + driveMotor = new Motor(config.driveMotorConfig); + rotMotor = new Motor(config.rotMotorConfig); + encoder = new CANCoder(config.canCoderConfig); + maxSpeed = config.maxSpeed; + resetToAbsolute(); + // lastAngle = getPose().rotation(); + } + + /** + * "Zeros out" the motors using an absolute encoder. + * Call in the intializer + */ + public void resetToAbsolute() { + rotMotor.setTargetPosition(encoder.getPosition().getRotations()); + } + + /** + * Drives the motors toward a certain state + * @param velocity The drive motor velocity to apply (in RPM) + * @param rotation The wheel position to apply (in rotations) + */ + public void applySwerveModuleState(double velocity, Rotation2d rotation) { + driveMotor.setTargetVelocity(velocity); + rotMotor.setTargetPosition(rotation.getRotations()); + } + + /** + * Gets the angle of the wheel + * @return The angle as an Angle object + */ + public Angle getWheelAngle() { + return Units.Rotations.of(rotMotor.getPosition()); + } + + /** + * Gets the amount of rotations the drive motor has driven the wheel + * @return The rotations as an Angle object + */ + public Angle getDriveRotations() { + return Units.Rotations.of(driveMotor.getPosition()); + } + + /** + * Gets the angluar velocity of the wheel + * @return The AngularVelocity in rotations per second + */ + public AngularVelocity getWheelVelocity() { + return Units.RPM.of(driveMotor.getVelocity()); + } + + /** + * Gets the amount of meters this wheel has driven. + * @return the meters as a Distance object + */ + public Distance getDrivePosition() { + return Units.Meters.of( + getDriveRotations().in(Units.Rotations) * SwerveConfig.wheelCircumference.in(Units.Meters) + ); + } + + /** + * Gets the velocity of the wheel in MPS + * @return the velocity as a LinearVelocity object + */ + public LinearVelocity getVelocity() { + return Units.MetersPerSecond.of( + getWheelVelocity().in(Units.RotationsPerSecond) * + SwerveConfig.wheelCircumference.in(Units.Meters) + ); + } + + /** + * Gets the voltage of the drive motor + * @return + */ + public Voltage getDriveVoltage() { + return driveMotor.getVoltage(); + } + + /** + * For use in deteremining kF + * @param log + */ + public void logSysId(SysIdRoutineLog log) { + log.motor("Drive " + id).linearPosition(getDrivePosition()) + .linearVelocity(getVelocity()).voltage(getDriveVoltage()); + } + + /** + * Gets the angle of the wheel as a Rotation2d object + * @return + */ + Rotation2d getRotation2dAngle() { + return new Rotation2d(getWheelAngle()); + } + + /** + * Logs important values + */ + public void log() { + double angle = getWheelAngle().in(Units.Degrees); + putNumber("Module Angle", angle); + putNumber("Normalized Module Angle", MathUtil.inputModulus(angle, 0.0, 360.0)); + putNumber("Drive Velo", getVelocity().in(Units.MetersPerSecond)); + putNumber("CanCoder Angle", getAbsoluteAngleDegrees()); + } + + private void putNumber(String key, double value) { + SmartDashboard.putNumber(key + " " + id, value); + } + + /** + * Updates the motor pid + * @param drive + * @param rot + */ + public void updatePID(PIDTuning drive, PIDTuning rot) { + driveMotor.changePID(drive.generatePIDConfig()); + rotMotor.changePID(rot.generatePIDConfig()); + } + + /** + * Normalizes the abs encoder reading and returns it in degrees + * @return + */ + public double getAbsoluteAngleDegrees() { + return MathUtil.inputModulus(encoder.getPosition().getDegrees(), 0, 360); + } + + /** + * Gets the state of the wheel + * @return + */ + public SwerveModuleState getState() { + return new SwerveModuleState(getVelocity(), getRotation2dAngle()); + } + + /** + * Gets the pose of the swerve module + * @return + */ + public SwerveModulePosition getPose() { + return new SwerveModulePosition(getDrivePosition(), getRotation2dAngle()); + } + + /** + * Essientially runs the swerve module, given a swerve module state + * @param state the SwerveModuleState + */ + public void setDesiredState(SwerveModuleState state) { + state.optimize(getRotation2dAngle()); + state.speedMetersPerSecond *= state.angle.minus(getRotation2dAngle()).getCos(); + + putNumber("Target Drive Velo", state.speedMetersPerSecond); + putNumber("Target Angle", state.angle.getDegrees()); + + double metersPerMinute = state.speedMetersPerSecond * 60.0; + double rotationsPerMinute = metersPerMinute / SwerveConfig.wheelCircumference.in(Units.Meters); + applySwerveModuleState(rotationsPerMinute, state.angle); + } + + /** + * Applies a voltage to the drive motor. + * @param voltage + */ + public void runSysIdRoutine(double voltage) { + driveMotor.setVoltage(voltage); + rotMotor.setTargetPosition(0.0); + } +} diff --git a/src/main/java/poplibv2/subsystems/swerve/commands/SysIdSwerve.java b/src/main/java/poplibv2/subsystems/swerve/commands/SysIdSwerve.java new file mode 100644 index 0000000..2cd6da0 --- /dev/null +++ b/src/main/java/poplibv2/subsystems/swerve/commands/SysIdSwerve.java @@ -0,0 +1,28 @@ +package poplibv2.subsystems.swerve.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import poplibv2.subsystems.swerve.Swerve; + +public class SysIdSwerve { + SysIdRoutine routine; + + /** + * Runs a System Identification on swerve which can be used to find kF (kV) constants for drive motors + * @param swerve the Swerve Subsystem + */ + public SysIdSwerve(Swerve swerve) { + routine = new SysIdRoutine( + new SysIdRoutine.Config(), + new SysIdRoutine.Mechanism(swerve::runSysIdRoutine, swerve::sysIdLogMotors, swerve, "Swerve") + ); + } + + public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { + return routine.quasistatic(direction); + } + + public Command sysIdDynamic(SysIdRoutine.Direction direction) { + return routine.dynamic(direction); + } +} diff --git a/src/main/java/poplibv2/subsystems/swerve/commands/TeleopSwerveDrive.java b/src/main/java/poplibv2/subsystems/swerve/commands/TeleopSwerveDrive.java new file mode 100644 index 0000000..5bea4af --- /dev/null +++ b/src/main/java/poplibv2/subsystems/swerve/commands/TeleopSwerveDrive.java @@ -0,0 +1,57 @@ +package poplibv2.subsystems.swerve.commands; + + +import poplibv2.controllers.ControllerMath; +import poplibv2.controllers.io.IO; +import poplibv2.subsystems.swerve.Swerve; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.Command; + +import java.util.function.Supplier; + +/** + * Command that controls teleop swerve. + */ +public class TeleopSwerveDrive extends Command { + private final Swerve swerve; + private final Supplier xAxisSupplier; + private final Supplier yAxisSupplier; + private final Supplier rotSupplier; + private final double speedMultiplier; + private final double stickDeadBand; + + /** + * Creates a command to drive swerve in Teleop. + * @param swerve The swerve subsystem. + * @param io The Controller + * @param speedMultiplier The amount to multiply the inputs by. Use a lower value for "baby mode" (aka when PR wants to drive the robot) + */ + public TeleopSwerveDrive(Swerve swerve, IO io, double speedMultiplier) { + this.swerve = swerve; + this.xAxisSupplier = io::getDriveTrainTranslationX; + this.yAxisSupplier = io::getDriveTrainTranslationX;; + this.rotSupplier = io::getDriveTrainTranslationX;; + this.speedMultiplier = speedMultiplier; + this.stickDeadBand = IO.DEADBAND; + addRequirements(swerve); + } + + @Override + public void execute() { + double forwardBack = yAxisSupplier.get() * speedMultiplier; + double leftRight = xAxisSupplier.get() * speedMultiplier; + double rot = rotSupplier.get() * speedMultiplier; + + forwardBack = ControllerMath.applyDeadband(forwardBack, stickDeadBand); + leftRight = ControllerMath.applyDeadband(leftRight, stickDeadBand); + + Translation2d translation = new Translation2d(forwardBack, leftRight); + + swerve.drive( + translation, + ControllerMath.cube(rot), + DriverStation.getAlliance().get() + ); + } +} \ No newline at end of file diff --git a/src/main/java/poplibv2/subsystems/swerve/setup/SwerveConfig.java b/src/main/java/poplibv2/subsystems/swerve/setup/SwerveConfig.java new file mode 100644 index 0000000..7e66f86 --- /dev/null +++ b/src/main/java/poplibv2/subsystems/swerve/setup/SwerveConfig.java @@ -0,0 +1,106 @@ +package poplibv2.subsystems.swerve.setup; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.wpilibj.DriverStation; +import poplibv2.motors.ConversionConfig; +import poplibv2.motors.IdleBehavior; +import poplibv2.motors.MotorConfig; +import poplibv2.control_systems.PIDConfig; +import poplibv2.motors.MotorVendor; +import poplibv2.sensors.absolute_encoder.CANCoderConfig; +import poplibv2.sensors.camera.CameraConfig; +import poplibv2.sensors.camera.LimelightConfig; +import poplibv2.sensors.gyro.PigeonConfig; + +public class SwerveConfig { + + /** + * Creates a new SwerveConfig that can be used to create a new Swerve Drivetrain + * @param moduleConstants SwerveModuleConstants objects that describe the swerve module. Should be ordered as: [Top Left, Top Right, Bottom Left, Bottom Right] + * @param CANBus What CANBus everything is on + * @param driveMotorType The MotorVendor of all of your drive motors + * @param rotMotorType The MotorVendor of all of your rotation motors + * @param drivePidConfig The PID Config for your drive motors + * @param rotPidConfig The PID Config for your rotation motors + * @param driveCurrentLimit The current limit of your drive motors + * @param rotCurrentLimit The current limit of your rotation motors + * @param type The type of swerve module you are using + * @param wheelDiameter The wheel diameter in inches, default is 4 + * @param gyroConfig The config used to create a Gyro + * @param cameraConfigs The configs for creating Cameras + * @param limelightConfigs The configs for creating Limelights + * @param tuningEnabled Whether or not to enable tuning + * @return The final SwerveConfig to be passed in when creating a new Swerve + */ + public static SwerveConfig generateConfig( + SwerveModuleConstants[] moduleConstants, + String CANBus, + MotorVendor driveMotorType, + MotorVendor rotMotorType, + PIDConfig drivePidConfig, + PIDConfig rotPidConfig, + int driveCurrentLimit, + int rotCurrentLimit, + SwerveModuleType type, + double wheelDiameter, + Translation2d[] wheelPosForSDK, + PigeonConfig gyroConfig, + CameraConfig[] cameraConfigs, + LimelightConfig[] limelightConfigs, + boolean tuningEnabled) { + + if (moduleConstants.length != 4) { + DriverStation.reportError("THERE ARE NOT 4 SWERVE MODULES!!!!!!!!!!!!!!! IM SCREAMING!!!! THIS WILL CRASH STUFF!!!!!", false); + } + + ConversionConfig driveConversion = new ConversionConfig(type.driveGearRatio, Units.Rotations); + ConversionConfig rotConversion = new ConversionConfig(type.angleGearRatio, Units.Rotations); + + + SwerveModuleConfig[] moduleConfigs = new SwerveModuleConfig[4]; + for (int i = 4; i < 4; i++) { + MotorConfig driveMotorConfig = new MotorConfig( + moduleConstants[i].driveMotorCANID, CANBus, driveMotorType, drivePidConfig, driveCurrentLimit, true, IdleBehavior.BRAKE, driveConversion + ); + MotorConfig rotMotorConfig = new MotorConfig( + moduleConstants[i].rotMotorCANID, CANBus, rotMotorType, rotPidConfig, rotCurrentLimit, false, IdleBehavior.BRAKE, rotConversion + ); + CANCoderConfig canCoderConfig = new CANCoderConfig(moduleConstants[i].absEncCANID, CANBus, moduleConstants[i].absEncOffset, false); + moduleConfigs[i] = new SwerveModuleConfig(driveMotorConfig, rotMotorConfig, canCoderConfig, i, type.maxSpeed, type.maxAngularVelocity); + } + return new SwerveConfig(moduleConfigs, gyroConfig, wheelDiameter, cameraConfigs, limelightConfigs, wheelPosForSDK, tuningEnabled); + } + + public SwerveModuleConfig[] swerveModuleConfigs; + public PigeonConfig gyro; + public static Distance wheelCircumference; + public CameraConfig[] cameraConfigs; + public LimelightConfig[] limelightConfigs; + public static boolean tuningEnable; + public Translation2d[] wheelPos; + + /** + * INTERNAL POPLIB FUNCTION. + * + * USE THE generateConfig to create a new config + * @param swerveModuleConfigs + * @param gyro + * @param wheelDiameter + * @param cameraConfigs + * @param limelightConfigs + * @param wheelPos + */ + private SwerveConfig(SwerveModuleConfig[] swerveModuleConfigs, + PigeonConfig gyro, double wheelDiameter, CameraConfig[] cameraConfigs, + LimelightConfig[] limelightConfigs, Translation2d[] wheelPos, boolean tuningEnabled) { + wheelCircumference = Units.Inches.of(wheelDiameter).times(Math.PI); + this.swerveModuleConfigs = swerveModuleConfigs; + this.gyro = gyro; + this.cameraConfigs = cameraConfigs; + this.limelightConfigs = limelightConfigs; + this.wheelPos = wheelPos; + tuningEnable = tuningEnabled; + } +} \ No newline at end of file diff --git a/src/main/java/poplibv2/subsystems/swerve/setup/SwerveModuleConfig.java b/src/main/java/poplibv2/subsystems/swerve/setup/SwerveModuleConfig.java new file mode 100644 index 0000000..77e9667 --- /dev/null +++ b/src/main/java/poplibv2/subsystems/swerve/setup/SwerveModuleConfig.java @@ -0,0 +1,33 @@ +package poplibv2.subsystems.swerve.setup; + +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.LinearVelocity; +import poplibv2.motors.MotorConfig; +import poplibv2.sensors.absolute_encoder.CANCoderConfig; + +public class SwerveModuleConfig { + public MotorConfig driveMotorConfig; + public MotorConfig rotMotorConfig; + public CANCoderConfig canCoderConfig; + public int id; + public LinearVelocity maxSpeed; + public AngularVelocity maxAngularVelocity; + + /** + * POPLIB INTERNAL FUNCTION. + * This is a container for storing configs for modules. + * @param driveMotorConfig + * @param rotMotorConfig + * @param canCoderConfig + * @param id + */ + public SwerveModuleConfig(MotorConfig driveMotorConfig, MotorConfig rotMotorConfig, CANCoderConfig canCoderConfig, int id, LinearVelocity maxSpeed, AngularVelocity maxAngularVelocity) { + this.driveMotorConfig = driveMotorConfig; + this.rotMotorConfig = rotMotorConfig; + this.canCoderConfig = canCoderConfig; + this.id = id; + this.maxSpeed = maxSpeed; + this.maxAngularVelocity = maxAngularVelocity; + } + +} diff --git a/src/main/java/poplibv2/subsystems/swerve/setup/SwerveModuleConstants.java b/src/main/java/poplibv2/subsystems/swerve/setup/SwerveModuleConstants.java new file mode 100644 index 0000000..5d63909 --- /dev/null +++ b/src/main/java/poplibv2/subsystems/swerve/setup/SwerveModuleConstants.java @@ -0,0 +1,24 @@ +package poplibv2.subsystems.swerve.setup; + +import edu.wpi.first.math.geometry.Rotation2d; + +public class SwerveModuleConstants { + public int driveMotorCANID; + public int rotMotorCANID; + public int absEncCANID; + public Rotation2d absEncOffset; + + /** + * Sets some constants for a swerve module + * @param driveMotorCANID the can id of the drive motor + * @param rotMotorCANID the can id of the rotation motor + * @param absEncCANID the can id of the absolute encoder + * @param absEncOffset the offset to apply to the absolute encoder in degrees + */ + public SwerveModuleConstants(int driveMotorCANID, int rotMotorCANID, int absEncCANID, double absEncOffset) { + this.driveMotorCANID = driveMotorCANID; + this.rotMotorCANID = rotMotorCANID; + this.absEncCANID = absEncCANID; + this.absEncOffset = Rotation2d.fromDegrees(absEncOffset); + } +} diff --git a/src/main/java/poplibv2/subsystems/swerve/setup/SwerveModuleType.java b/src/main/java/poplibv2/subsystems/swerve/setup/SwerveModuleType.java new file mode 100644 index 0000000..ef20512 --- /dev/null +++ b/src/main/java/poplibv2/subsystems/swerve/setup/SwerveModuleType.java @@ -0,0 +1,41 @@ +package poplibv2.subsystems.swerve.setup; + +import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.LinearVelocity; + +/** + * A enum that describes constants for the different types of swerve modules + */ +public enum SwerveModuleType { + MK4iL3( + (150.0 / 7.0), + 6.12, + Units.MetersPerSecond.of(5.6), + Units.RadiansPerSecond.of(4 * Math.PI) + ), + MK4iL2FOC( + (150.0 / 7.0), + 6.75, + Units.MetersPerSecond.of(4.572), + Units.RadiansPerSecond.of(4 * Math.PI) + ), + MK4( + 12.8, + 6.12, + Units.MetersPerSecond.of(5.21208), + Units.RadiansPerSecond.of(4 * Math.PI) + ); + + public double angleGearRatio; + public double driveGearRatio; + public LinearVelocity maxSpeed; + public AngularVelocity maxAngularVelocity; + + private SwerveModuleType(double angleGearRatio, double driveGearRatio, LinearVelocity maxSpeed, AngularVelocity maxAngularVelocity) { + this.angleGearRatio = angleGearRatio; + this.driveGearRatio = driveGearRatio; + this.maxSpeed = maxSpeed; + this.maxAngularVelocity = maxAngularVelocity; + } +} \ No newline at end of file