astatin3/photonvision-2025.0.0-beta-6
1
0
Fork 0
mirror of https://github.com/Astatin3/photonvision-2025.0.0-beta-6.git synced 2026-06-09 08:38:00 -06:00
Code Issues Packages Projects Releases Wiki Activity

Initial commit

Browse Source
This commit is contained in:
astatin3
2024-12-09 08:01:09 -07:00
commit 9e4ab26005
1408 changed files with 143829 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
photonlib-java-examples/aimattarget/src/main/deploy/example.txt
+3
View File
@@ -0,0 +1,3 @@
Files placed in this directory will be deployed to the RoboRIO into the
'deploy' directory in the home folder. Use the 'Filesystem.getDeployDirectory' wpilib function
to get a proper path relative to the deploy directory.
photonlib-java-examples/aimattarget/src/main/java/frc/robot/Constants.java
+144
View File
@@ -0,0 +1,144 @@
/*
* MIT License
*
* Copyright (c) PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
package frc.robot;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.apriltag.AprilTagFields;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.math.util.Units;
public class Constants {
public static class Vision {
public static final String kCameraName = "YOUR CAMERA NAME";
// Cam mounted facing forward, half a meter forward of center, half a meter up from center,
// pitched upward.
private static final double camPitch = Units.degreesToRadians(30.0);
public static final Transform3d kRobotToCam =
new Transform3d(new Translation3d(0.5, 0.0, 0.5), new Rotation3d(0, -camPitch, 0));
// The layout of the AprilTags on the field
public static final AprilTagFieldLayout kTagLayout =
AprilTagFields.kDefaultField.loadAprilTagLayoutField();
// The standard deviations of our vision estimated poses, which affect correction rate
// (Fake values. Experiment and determine estimation noise on an actual robot.)
public static final Matrix<N3, N1> kSingleTagStdDevs = VecBuilder.fill(4, 4, 8);
public static final Matrix<N3, N1> kMultiTagStdDevs = VecBuilder.fill(0.5, 0.5, 1);
}
public static class Swerve {
// Physical properties
public static final double kTrackWidth = Units.inchesToMeters(18.5);
public static final double kTrackLength = Units.inchesToMeters(18.5);
public static final double kRobotWidth = Units.inchesToMeters(25 + 3.25 * 2);
public static final double kRobotLength = Units.inchesToMeters(25 + 3.25 * 2);
public static final double kMaxLinearSpeed = Units.feetToMeters(15.5);
public static final double kMaxAngularSpeed = Units.rotationsToRadians(2);
public static final double kWheelDiameter = Units.inchesToMeters(4);
public static final double kWheelCircumference = kWheelDiameter * Math.PI;
public static final double kDriveGearRatio = 6.75; // 6.75:1 SDS MK4 L2 ratio
public static final double kSteerGearRatio = 12.8; // 12.8:1
public static final double kDriveDistPerPulse = kWheelCircumference / 1024 / kDriveGearRatio;
public static final double kSteerRadPerPulse = 2 * Math.PI / 1024;
public enum ModuleConstants {
FL( // Front left
1, 0, 0, 1, 1, 2, 3, 0, kTrackLength / 2, kTrackWidth / 2),
FR( // Front Right
2, 2, 4, 5, 3, 6, 7, 0, kTrackLength / 2, -kTrackWidth / 2),
BL( // Back Left
3, 4, 8, 9, 5, 10, 11, 0, -kTrackLength / 2, kTrackWidth / 2),
BR( // Back Right
4, 6, 12, 13, 7, 14, 15, 0, -kTrackLength / 2, -kTrackWidth / 2);
public final int moduleNum;
public final int driveMotorID;
public final int driveEncoderA;
public final int driveEncoderB;
public final int steerMotorID;
public final int steerEncoderA;
public final int steerEncoderB;
public final double angleOffset;
public final Translation2d centerOffset;
private ModuleConstants(
int moduleNum,
int driveMotorID,
int driveEncoderA,
int driveEncoderB,
int steerMotorID,
int steerEncoderA,
int steerEncoderB,
double angleOffset,
double xOffset,
double yOffset) {
this.moduleNum = moduleNum;
this.driveMotorID = driveMotorID;
this.driveEncoderA = driveEncoderA;
this.driveEncoderB = driveEncoderB;
this.steerMotorID = steerMotorID;
this.steerEncoderA = steerEncoderA;
this.steerEncoderB = steerEncoderB;
this.angleOffset = angleOffset;
centerOffset = new Translation2d(xOffset, yOffset);
}
}
// Feedforward
// Linear drive feed forward
public static final SimpleMotorFeedforward kDriveFF =
new SimpleMotorFeedforward( // real
0.25, // Voltage to break static friction
2.5, // Volts per meter per second
0.3 // Volts per meter per second squared
);
// Steer feed forward
public static final SimpleMotorFeedforward kSteerFF =
new SimpleMotorFeedforward( // real
0.5, // Voltage to break static friction
0.25, // Volts per radian per second
0.01 // Volts per radian per second squared
);
// PID
public static final double kDriveKP = 1;
public static final double kDriveKI = 0;
public static final double kDriveKD = 0;
public static final double kSteerKP = 20;
public static final double kSteerKI = 0;
public static final double kSteerKD = 0.25;
}
}
photonlib-java-examples/aimattarget/src/main/java/frc/robot/Main.java
+35
View File
@@ -0,0 +1,35 @@
/*
* MIT License
*
* Copyright (c) PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
package frc.robot;
import edu.wpi.first.wpilibj.RobotBase;
public final class Main {
private Main() {}
public static void main(String... args) {
RobotBase.startRobot(Robot::new);
}
}
photonlib-java-examples/aimattarget/src/main/java/frc/robot/Robot.java
+148
View File
@@ -0,0 +1,148 @@
/*
* MIT License
*
* Copyright (c) PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
package frc.robot;
import static frc.robot.Constants.Vision.*;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.simulation.BatterySim;
import edu.wpi.first.wpilibj.simulation.RoboRioSim;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import frc.robot.subsystems.drivetrain.SwerveDrive;
import org.photonvision.PhotonCamera;
public class Robot extends TimedRobot {
private SwerveDrive drivetrain;
private VisionSim visionSim;
private PhotonCamera camera;
private final double VISION_TURN_kP = 0.01;
private XboxController controller;
@Override
public void robotInit() {
drivetrain = new SwerveDrive();
camera = new PhotonCamera(kCameraName);
visionSim = new VisionSim(camera);
controller = new XboxController(0);
}
@Override
public void robotPeriodic() {
// Update drivetrain subsystem
drivetrain.periodic();
// Log values to the dashboard
drivetrain.log();
}
@Override
public void disabledPeriodic() {
drivetrain.stop();
}
@Override
public void teleopInit() {
resetPose();
}
@Override
public void teleopPeriodic() {
// Calculate drivetrain commands from Joystick values
double forward = -controller.getLeftY() * Constants.Swerve.kMaxLinearSpeed;
double strafe = -controller.getLeftX() * Constants.Swerve.kMaxLinearSpeed;
double turn = -controller.getRightX() * Constants.Swerve.kMaxAngularSpeed;
// Read in relevant data from the Camera
boolean targetVisible = false;
double targetYaw = 0.0;
var results = camera.getAllUnreadResults();
if (!results.isEmpty()) {
// Camera processed a new frame since last
// Get the last one in the list.
var result = results.get(results.size() - 1);
if (result.hasTargets()) {
// At least one AprilTag was seen by the camera
for (var target : result.getTargets()) {
if (target.getFiducialId() == 7) {
// Found Tag 7, record its information
targetYaw = target.getYaw();
targetVisible = true;
}
}
}
}
// Auto-align when requested
if (controller.getAButton() && targetVisible) {
// Driver wants auto-alignment to tag 7
// And, tag 7 is in sight, so we can turn toward it.
// Override the driver's turn command with an automatic one that turns toward the tag.
turn = -1.0 * targetYaw * VISION_TURN_kP * Constants.Swerve.kMaxAngularSpeed;
}
// Command drivetrain motors based on target speeds
drivetrain.drive(forward, strafe, turn);
// Put debug information to the dashboard
SmartDashboard.putBoolean("Vision Target Visible", targetVisible);
}
@Override
public void simulationPeriodic() {
// Update drivetrain simulation
drivetrain.simulationPeriodic();
// Update camera simulation
visionSim.simulationPeriodic(drivetrain.getSimPose());
var debugField = visionSim.getSimDebugField();
debugField.getObject("EstimatedRobot").setPose(drivetrain.getPose());
debugField.getObject("EstimatedRobotModules").setPoses(drivetrain.getModulePoses());
// Calculate battery voltage sag due to current draw
var batteryVoltage =
BatterySim.calculateDefaultBatteryLoadedVoltage(drivetrain.getCurrentDraw());
// Using max(0.1, voltage) here isn't a *physically correct* solution,
// but it avoids problems with battery voltage measuring 0.
RoboRioSim.setVInVoltage(Math.max(0.1, batteryVoltage));
}
public void resetPose() {
// Example Only - startPose should be derived from some assumption
// of where your robot was placed on the field.
// The first pose in an autonomous path is often a good choice.
var startPose = new Pose2d(1, 1, new Rotation2d());
drivetrain.resetPose(startPose, true);
visionSim.resetSimPose(startPose);
}
}
photonlib-java-examples/aimattarget/src/main/java/frc/robot/VisionSim.java
+82
View File
@@ -0,0 +1,82 @@
/*
* MIT License
*
* Copyright (c) PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
package frc.robot;
import static frc.robot.Constants.Vision.*;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
import org.photonvision.PhotonCamera;
import org.photonvision.simulation.PhotonCameraSim;
import org.photonvision.simulation.SimCameraProperties;
import org.photonvision.simulation.VisionSystemSim;
public class VisionSim {
// Simulation
private PhotonCameraSim cameraSim;
private VisionSystemSim visionSim;
public VisionSim(PhotonCamera cam_in) {
// ----- Simulation
if (Robot.isSimulation()) {
// Create the vision system simulation which handles cameras and targets on the field.
visionSim = new VisionSystemSim("main");
// Add all the AprilTags inside the tag layout as visible targets to this simulated field.
visionSim.addAprilTags(kTagLayout);
// Create simulated camera properties. These can be set to mimic your actual camera.
var cameraProp = new SimCameraProperties();
cameraProp.setCalibration(320, 240, Rotation2d.fromDegrees(90));
cameraProp.setCalibError(0.35, 0.10);
cameraProp.setFPS(70);
cameraProp.setAvgLatencyMs(30);
cameraProp.setLatencyStdDevMs(10);
// Create a PhotonCameraSim which will update the linked PhotonCamera's values with visible
// targets.
cameraSim = new PhotonCameraSim(cam_in, cameraProp);
// Add the simulated camera to view the targets on this simulated field.
visionSim.addCamera(cameraSim, kRobotToCam);
cameraSim.enableDrawWireframe(true);
}
}
// ----- Simulation
public void simulationPeriodic(Pose2d robotSimPose) {
visionSim.update(robotSimPose);
}
/** Reset pose history of the robot in the vision system simulation. */
public void resetSimPose(Pose2d pose) {
if (Robot.isSimulation()) visionSim.resetRobotPose(pose);
}
/** A Field2d for visualizing our robot and objects on the field. */
public Field2d getSimDebugField() {
if (!Robot.isSimulation()) return null;
return visionSim.getDebugField();
}
}
photonlib-java-examples/aimattarget/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java
+332
View File
@@ -0,0 +1,332 @@
/*
* MIT License
*
* Copyright (c) PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
package frc.robot.subsystems.drivetrain;
import static frc.robot.Constants.Swerve.*;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Transform2d;
import edu.wpi.first.math.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.math.numbers.*;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.wpilibj.ADXRS450_Gyro;
import edu.wpi.first.wpilibj.SPI.Port;
import edu.wpi.first.wpilibj.simulation.ADXRS450_GyroSim;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import frc.robot.Robot;
public class SwerveDrive {
// Construct the swerve modules with their respective constants.
// The SwerveModule class will handle all the details of controlling the modules.
private final SwerveModule[] swerveMods = {
new SwerveModule(ModuleConstants.FL),
new SwerveModule(ModuleConstants.FR),
new SwerveModule(ModuleConstants.BL),
new SwerveModule(ModuleConstants.BR)
};
// The kinematics for translating between robot chassis speeds and module states.
private final SwerveDriveKinematics kinematics =
new SwerveDriveKinematics(
swerveMods[0].getModuleConstants().centerOffset,
swerveMods[1].getModuleConstants().centerOffset,
swerveMods[2].getModuleConstants().centerOffset,
swerveMods[3].getModuleConstants().centerOffset);
private final ADXRS450_Gyro gyro = new ADXRS450_Gyro(Port.kOnboardCS0);
// The robot pose estimator for tracking swerve odometry and applying vision corrections.
private final SwerveDrivePoseEstimator poseEstimator;
private ChassisSpeeds targetChassisSpeeds = new ChassisSpeeds();
// ----- Simulation
private final ADXRS450_GyroSim gyroSim;
private final SwerveDriveSim swerveDriveSim;
private double totalCurrentDraw = 0;
public SwerveDrive() {
// Define the standard deviations for the pose estimator, which determine how fast the pose
// estimate converges to the vision measurement. This should depend on the vision measurement
// noise
// and how many or how frequently vision measurements are applied to the pose estimator.
var stateStdDevs = VecBuilder.fill(0.1, 0.1, 0.1);
var visionStdDevs = VecBuilder.fill(1, 1, 1);
poseEstimator =
new SwerveDrivePoseEstimator(
kinematics,
getGyroYaw(),
getModulePositions(),
new Pose2d(),
stateStdDevs,
visionStdDevs);
// ----- Simulation
gyroSim = new ADXRS450_GyroSim(gyro);
swerveDriveSim =
new SwerveDriveSim(
kDriveFF,
DCMotor.getFalcon500(1),
kDriveGearRatio,
kWheelDiameter / 2.0,
kSteerFF,
DCMotor.getFalcon500(1),
kSteerGearRatio,
kinematics);
}
public void periodic() {
for (SwerveModule module : swerveMods) {
module.periodic();
}
// Update the odometry of the swerve drive using the wheel encoders and gyro.
poseEstimator.update(getGyroYaw(), getModulePositions());
}
/**
* Basic drive control. A target field-relative ChassisSpeeds (vx, vy, omega) is converted to
* specific swerve module states.
*
* @param vxMeters X velocity (forwards/backwards)
* @param vyMeters Y velocity (strafe left/right)
* @param omegaRadians Angular velocity (rotation CCW+)
*/
public void drive(double vxMeters, double vyMeters, double omegaRadians) {
var targetChassisSpeeds =
ChassisSpeeds.fromFieldRelativeSpeeds(vxMeters, vyMeters, omegaRadians, getHeading());
setChassisSpeeds(targetChassisSpeeds, true, false);
}
/**
* Command the swerve drive to the desired chassis speeds by converting them to swerve module
* states and using {@link #setModuleStates(SwerveModuleState[], boolean)}.
*
* @param targetChassisSpeeds Target robot-relative chassis speeds (vx, vy, omega).
* @param openLoop If swerve modules should use feedforward only and ignore velocity feedback
* control.
* @param steerInPlace If modules should steer to the target angle when target velocity is 0.
*/
public void setChassisSpeeds(
ChassisSpeeds targetChassisSpeeds, boolean openLoop, boolean steerInPlace) {
setModuleStates(kinematics.toSwerveModuleStates(targetChassisSpeeds), openLoop, steerInPlace);
this.targetChassisSpeeds = targetChassisSpeeds;
}
/**
* Command the swerve modules to the desired states. Velocites exceeding the maximum speed will be
* desaturated (while preserving the ratios between modules).
*
* @param openLoop If swerve modules should use feedforward only and ignore velocity feedback
* control.
* @param steerInPlace If modules should steer to the target angle when target velocity is 0.
*/
public void setModuleStates(
SwerveModuleState[] desiredStates, boolean openLoop, boolean steerInPlace) {
SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, kMaxLinearSpeed);
for (int i = 0; i < swerveMods.length; i++) {
swerveMods[i].setDesiredState(desiredStates[i], openLoop, steerInPlace);
}
}
/** Stop the swerve drive. */
public void stop() {
drive(0, 0, 0);
}
/** See {@link SwerveDrivePoseEstimator#addVisionMeasurement(Pose2d, double)}. */
public void addVisionMeasurement(Pose2d visionMeasurement, double timestampSeconds) {
poseEstimator.addVisionMeasurement(visionMeasurement, timestampSeconds);
}
/** See {@link SwerveDrivePoseEstimator#addVisionMeasurement(Pose2d, double, Matrix)}. */
public void addVisionMeasurement(
Pose2d visionMeasurement, double timestampSeconds, Matrix<N3, N1> stdDevs) {
poseEstimator.addVisionMeasurement(visionMeasurement, timestampSeconds, stdDevs);
}
/**
* Reset the estimated pose of the swerve drive on the field.
*
* @param pose New robot pose.
* @param resetSimPose If the simulated robot pose should also be reset. This effectively
* teleports the robot and should only be used during the setup of the simulation world.
*/
public void resetPose(Pose2d pose, boolean resetSimPose) {
if (resetSimPose) {
swerveDriveSim.reset(pose, false);
// we shouldnt realistically be resetting pose after startup, but we will handle it anyway for
// testing
for (int i = 0; i < swerveMods.length; i++) {
swerveMods[i].simulationUpdate(0, 0, 0, 0, 0, 0);
}
gyroSim.setAngle(-pose.getRotation().getDegrees());
gyroSim.setRate(0);
}
poseEstimator.resetPosition(getGyroYaw(), getModulePositions(), pose);
}
/** Get the estimated pose of the swerve drive on the field. */
public Pose2d getPose() {
return poseEstimator.getEstimatedPosition();
}
/** The heading of the swerve drive's estimated pose on the field. */
public Rotation2d getHeading() {
return getPose().getRotation();
}
/** Raw gyro yaw (this may not match the field heading!). */
public Rotation2d getGyroYaw() {
return gyro.getRotation2d();
}
/** Get the chassis speeds of the robot (vx, vy, omega) from the swerve module states. */
public ChassisSpeeds getChassisSpeeds() {
return kinematics.toChassisSpeeds(getModuleStates());
}
/**
* Get the SwerveModuleState of each swerve module (speed, angle). The returned array order
* matches the kinematics module order.
*/
public SwerveModuleState[] getModuleStates() {
return new SwerveModuleState[] {
swerveMods[0].getState(),
swerveMods[1].getState(),
swerveMods[2].getState(),
swerveMods[3].getState()
};
}
/**
* Get the SwerveModulePosition of each swerve module (position, angle). The returned array order
* matches the kinematics module order.
*/
public SwerveModulePosition[] getModulePositions() {
return new SwerveModulePosition[] {
swerveMods[0].getPosition(),
swerveMods[1].getPosition(),
swerveMods[2].getPosition(),
swerveMods[3].getPosition()
};
}
/**
* Get the Pose2d of each swerve module based on kinematics and current robot pose. The returned
* array order matches the kinematics module order.
*/
public Pose2d[] getModulePoses() {
Pose2d[] modulePoses = new Pose2d[swerveMods.length];
for (int i = 0; i < swerveMods.length; i++) {
var module = swerveMods[i];
modulePoses[i] =
getPose()
.transformBy(
new Transform2d(
module.getModuleConstants().centerOffset, module.getAbsoluteHeading()));
}
return modulePoses;
}
/** Log various drivetrain values to the dashboard. */
public void log() {
String table = "Drive/";
Pose2d pose = getPose();
SmartDashboard.putNumber(table + "X", pose.getX());
SmartDashboard.putNumber(table + "Y", pose.getY());
SmartDashboard.putNumber(table + "Heading", pose.getRotation().getDegrees());
ChassisSpeeds chassisSpeeds = getChassisSpeeds();
SmartDashboard.putNumber(table + "VX", chassisSpeeds.vxMetersPerSecond);
SmartDashboard.putNumber(table + "VY", chassisSpeeds.vyMetersPerSecond);
SmartDashboard.putNumber(
table + "Omega Degrees", Math.toDegrees(chassisSpeeds.omegaRadiansPerSecond));
SmartDashboard.putNumber(table + "Target VX", targetChassisSpeeds.vxMetersPerSecond);
SmartDashboard.putNumber(table + "Target VY", targetChassisSpeeds.vyMetersPerSecond);
SmartDashboard.putNumber(
table + "Target Omega Degrees", Math.toDegrees(targetChassisSpeeds.omegaRadiansPerSecond));
for (SwerveModule module : swerveMods) {
module.log();
}
}
// ----- Simulation
public void simulationPeriodic() {
// Pass commanded motor voltages into swerve drive simulation
double[] driveInputs = new double[swerveMods.length];
double[] steerInputs = new double[swerveMods.length];
for (int i = 0; i < swerveMods.length; i++) {
driveInputs[i] = swerveMods[i].getDriveVoltage();
steerInputs[i] = swerveMods[i].getSteerVoltage();
}
swerveDriveSim.setDriveInputs(driveInputs);
swerveDriveSim.setSteerInputs(steerInputs);
// Simulate one timestep
swerveDriveSim.update(Robot.kDefaultPeriod);
// Update module and gyro values with simulated values
var driveStates = swerveDriveSim.getDriveStates();
var steerStates = swerveDriveSim.getSteerStates();
totalCurrentDraw = 0;
double[] driveCurrents = swerveDriveSim.getDriveCurrentDraw();
for (double current : driveCurrents) totalCurrentDraw += current;
double[] steerCurrents = swerveDriveSim.getSteerCurrentDraw();
for (double current : steerCurrents) totalCurrentDraw += current;
for (int i = 0; i < swerveMods.length; i++) {
double drivePos = driveStates.get(i).get(0, 0);
double driveRate = driveStates.get(i).get(1, 0);
double steerPos = steerStates.get(i).get(0, 0);
double steerRate = steerStates.get(i).get(1, 0);
swerveMods[i].simulationUpdate(
drivePos, driveRate, driveCurrents[i], steerPos, steerRate, steerCurrents[i]);
}
gyroSim.setRate(-swerveDriveSim.getOmegaRadsPerSec());
gyroSim.setAngle(-swerveDriveSim.getPose().getRotation().getDegrees());
}
/**
* The "actual" pose of the robot on the field used in simulation. This is different from the
* swerve drive's estimated pose.
*/
public Pose2d getSimPose() {
return swerveDriveSim.getPose();
}
public double getCurrentDraw() {
return totalCurrentDraw;
}
}
photonlib-java-examples/aimattarget/src/main/java/frc/robot/subsystems/drivetrain/SwerveDriveSim.java
+495
View File
@@ -0,0 +1,495 @@
/*
* MIT License
*
* Copyright (c) PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
package frc.robot.subsystems.drivetrain;
import edu.wpi.first.math.MatBuilder;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
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.math.numbers.N1;
import edu.wpi.first.math.numbers.N2;
import edu.wpi.first.math.system.Discretization;
import edu.wpi.first.math.system.LinearSystem;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.wpilibj.RobotController;
import java.util.ArrayList;
import java.util.List;
import java.util.Random;
/**
* This class attempts to simulate the dynamics of a swerve drive. In simulationPeriodic, users
* should first set inputs from motors with {@link #setDriveInputs(double...)} and {@link
* #setSteerInputs(double...)}, call {@link #update(double)} to update the simulation, and then set
* swerve module's encoder values and the drivetrain's gyro values with the results from this class.
*
* <p>In this class, distances are expressed with meters, angles with radians, and inputs with
* voltages.
*
* <p>Teams can use {@link edu.wpi.first.wpilibj.smartdashboard.Field2d} to visualize their robot on
* the Sim GUI's field.
*/
public class SwerveDriveSim {
private final LinearSystem<N2, N1, N2> drivePlant;
private final double driveKs;
private final DCMotor driveMotor;
private final double driveGearing;
private final double driveWheelRadius;
private final LinearSystem<N2, N1, N2> steerPlant;
private final double steerKs;
private final DCMotor steerMotor;
private final double steerGearing;
private final SwerveDriveKinematics kinematics;
private final int numModules;
private final double[] driveInputs;
private final List<Matrix<N2, N1>> driveStates;
private final double[] steerInputs;
private final List<Matrix<N2, N1>> steerStates;
private final Random rand = new Random();
// noiseless "actual" pose of the robot on the field
private Pose2d pose = new Pose2d();
private double omegaRadsPerSec = 0;
/**
* Creates a swerve drive simulation.
*
* @param driveFF The feedforward for the drive motors of this swerve drive. This should be in
* units of meters.
* @param driveMotor The DCMotor model for the drive motor(s) of this swerve drive's modules. This
* should not have any gearing applied.
* @param driveGearing The gear ratio of the drive system. Positive values indicate a reduction
* where one rotation of the drive wheel equals driveGearing rotations of the drive motor.
* @param driveWheelRadius The radius of the module's driving wheel.
* @param steerFF The feedforward for the steer motors of this swerve drive. This should be in
* units of radians.
* @param steerMotor The DCMotor model for the steer motor(s) of this swerve drive's modules. This
* should not have any gearing applied.
* @param steerGearing The gear ratio of the steer system. Positive values indicate a reduction
* where one rotation of the module heading/azimuth equals steerGearing rotations of the steer
* motor.
* @param kinematics The kinematics for this swerve drive. All swerve module information used in
* this class should match the order of the modules this kinematics object was constructed
* with.
*/
public SwerveDriveSim(
SimpleMotorFeedforward driveFF,
DCMotor driveMotor,
double driveGearing,
double driveWheelRadius,
SimpleMotorFeedforward steerFF,
DCMotor steerMotor,
double steerGearing,
SwerveDriveKinematics kinematics) {
this(
new LinearSystem<N2, N1, N2>(
MatBuilder.fill(Nat.N2(), Nat.N2(), 0.0, 1.0, 0.0, -driveFF.getKv() / driveFF.getKa()),
VecBuilder.fill(0.0, 1.0 / driveFF.getKa()),
MatBuilder.fill(Nat.N2(), Nat.N2(), 1.0, 0.0, 0.0, 1.0),
VecBuilder.fill(0.0, 0.0)),
driveFF.getKs(),
driveMotor,
driveGearing,
driveWheelRadius,
new LinearSystem<N2, N1, N2>(
MatBuilder.fill(Nat.N2(), Nat.N2(), 0.0, 1.0, 0.0, -steerFF.getKv() / steerFF.getKa()),
VecBuilder.fill(0.0, 1.0 / steerFF.getKa()),
MatBuilder.fill(Nat.N2(), Nat.N2(), 1.0, 0.0, 0.0, 1.0),
VecBuilder.fill(0.0, 0.0)),
steerFF.getKs(),
steerMotor,
steerGearing,
kinematics);
}
/**
* Creates a swerve drive simulation.
*
* @param drivePlant The {@link LinearSystem} representing a swerve module's drive system. The
* state should be in units of meters and input in volts.
* @param driveKs The static gain in volts of the drive system's feedforward, or the minimum
* voltage to cause motion. Set this to 0 to ignore static friction.
* @param driveMotor The DCMotor model for the drive motor(s) of this swerve drive's modules. This
* should not have any gearing applied.
* @param driveGearing The gear ratio of the drive system. Positive values indicate a reduction
* where one rotation of the drive wheel equals driveGearing rotations of the drive motor.
* @param driveWheelRadius The radius of the module's driving wheel.
* @param steerPlant The {@link LinearSystem} representing a swerve module's steer system. The
* state should be in units of radians and input in volts.
* @param steerKs The static gain in volts of the steer system's feedforward, or the minimum
* voltage to cause motion. Set this to 0 to ignore static friction.
* @param steerMotor The DCMotor model for the steer motor(s) of this swerve drive's modules. This
* should not have any gearing applied.
* @param steerGearing The gear ratio of the steer system. Positive values indicate a reduction
* where one rotation of the module heading/azimuth equals steerGearing rotations of the steer
* motor.
* @param kinematics The kinematics for this swerve drive. All swerve module information used in
* this class should match the order of the modules this kinematics object was constructed
* with.
*/
public SwerveDriveSim(
LinearSystem<N2, N1, N2> drivePlant,
double driveKs,
DCMotor driveMotor,
double driveGearing,
double driveWheelRadius,
LinearSystem<N2, N1, N2> steerPlant,
double steerKs,
DCMotor steerMotor,
double steerGearing,
SwerveDriveKinematics kinematics) {
this.drivePlant = drivePlant;
this.driveKs = driveKs;
this.driveMotor = driveMotor;
this.driveGearing = driveGearing;
this.driveWheelRadius = driveWheelRadius;
this.steerPlant = steerPlant;
this.steerKs = steerKs;
this.steerMotor = steerMotor;
this.steerGearing = steerGearing;
this.kinematics = kinematics;
numModules = kinematics.toSwerveModuleStates(new ChassisSpeeds()).length;
driveInputs = new double[numModules];
driveStates = new ArrayList<>(numModules);
steerInputs = new double[numModules];
steerStates = new ArrayList<>(numModules);
for (int i = 0; i < numModules; i++) {
driveStates.add(VecBuilder.fill(0, 0));
steerStates.add(VecBuilder.fill(0, 0));
}
}
/**
* Sets the input voltages of the drive motors.
*
* @param inputs Input voltages. These should match the module order used in the kinematics.
*/
public void setDriveInputs(double... inputs) {
final double battVoltage = RobotController.getBatteryVoltage();
for (int i = 0; i < driveInputs.length; i++) {
double input = inputs[i];
driveInputs[i] = MathUtil.clamp(input, -battVoltage, battVoltage);
}
}
/**
* Sets the input voltages of the steer motors.
*
* @param inputs Input voltages. These should match the module order used in the kinematics.
*/
public void setSteerInputs(double... inputs) {
final double battVoltage = RobotController.getBatteryVoltage();
for (int i = 0; i < steerInputs.length; i++) {
double input = inputs[i];
steerInputs[i] = MathUtil.clamp(input, -battVoltage, battVoltage);
}
}
/**
* Computes the new x given the old x and the control input. Includes the effect of static
* friction.
*
* @param discA The discretized system matrix.
* @param discB The discretized input matrix.
* @param x The position/velocity state of the drive/steer system.
* @param input The input voltage.
* @param ks The kS value of the feedforward model.
* @return The updated x, including the effect of static friction.
*/
protected static Matrix<N2, N1> calculateX(
Matrix<N2, N2> discA, Matrix<N2, N1> discB, Matrix<N2, N1> x, double input, double ks) {
var Ax = discA.times(x);
double nextStateVel = Ax.get(1, 0);
// input required to make next state vel == 0
double inputToStop = nextStateVel / -discB.get(1, 0);
// ks effect on system velocity
double ksSystemEffect = MathUtil.clamp(inputToStop, -ks, ks);
// after ks system effect:
nextStateVel += discB.get(1, 0) * ksSystemEffect;
inputToStop = nextStateVel / -discB.get(1, 0);
double signToStop = Math.signum(inputToStop);
double inputSign = Math.signum(input);
double ksInputEffect = 0;
// system velocity was reduced to 0, resist any input
if (Math.abs(ksSystemEffect) < ks) {
double absInput = Math.abs(input);
ksInputEffect = -MathUtil.clamp(ks * inputSign, -absInput, absInput);
}
// non-zero system velocity, but input causes velocity sign change. Resist input after sign
// change
else if ((input * signToStop) > (inputToStop * signToStop)) {
double absInput = Math.abs(input - inputToStop);
ksInputEffect = -MathUtil.clamp(ks * inputSign, -absInput, absInput);
}
// calculate next x including static friction
var Bu = discB.times(VecBuilder.fill(input + ksSystemEffect + ksInputEffect));
return Ax.plus(Bu);
}
/**
* Update the drivetrain states with the given timestep.
*
* @param dtSeconds The timestep in seconds. This should be the robot loop period.
*/
public void update(double dtSeconds) {
var driveDiscAB = Discretization.discretizeAB(drivePlant.getA(), drivePlant.getB(), dtSeconds);
var steerDiscAB = Discretization.discretizeAB(steerPlant.getA(), steerPlant.getB(), dtSeconds);
var moduleDeltas = new SwerveModulePosition[numModules];
for (int i = 0; i < numModules; i++) {
double prevDriveStatePos = driveStates.get(i).get(0, 0);
driveStates.set(
i,
calculateX(
driveDiscAB.getFirst(),
driveDiscAB.getSecond(),
driveStates.get(i),
driveInputs[i],
driveKs));
double currDriveStatePos = driveStates.get(i).get(0, 0);
steerStates.set(
i,
calculateX(
steerDiscAB.getFirst(),
steerDiscAB.getSecond(),
steerStates.get(i),
steerInputs[i],
steerKs));
double currSteerStatePos = steerStates.get(i).get(0, 0);
moduleDeltas[i] =
new SwerveModulePosition(
currDriveStatePos - prevDriveStatePos, new Rotation2d(currSteerStatePos));
}
var twist = kinematics.toTwist2d(moduleDeltas);
pose = pose.exp(twist);
omegaRadsPerSec = twist.dtheta / dtSeconds;
}
/**
* Reset the simulated swerve drive state. This effectively teleports the robot and should only be
* used during the setup of the simulation world.
*
* @param pose The new pose of the simulated swerve drive.
* @param preserveMotion If true, the current module states will be preserved. Otherwise, they
* will be reset to zeros.
*/
public void reset(Pose2d pose, boolean preserveMotion) {
this.pose = pose;
if (!preserveMotion) {
for (int i = 0; i < numModules; i++) {
driveStates.set(i, VecBuilder.fill(0, 0));
steerStates.set(i, VecBuilder.fill(0, 0));
}
omegaRadsPerSec = 0;
}
}
/**
* Reset the simulated swerve drive state. This effectively teleports the robot and should only be
* used during the setup of the simulation world.
*
* @param pose The new pose of the simulated swerve drive.
* @param moduleDriveStates The new states of the modules' drive systems in [meters, meters/sec].
* These should match the module order used in the kinematics.
* @param moduleSteerStates The new states of the modules' steer systems in [radians,
* radians/sec]. These should match the module order used in the kinematics.
*/
public void reset(
Pose2d pose, List<Matrix<N2, N1>> moduleDriveStates, List<Matrix<N2, N1>> moduleSteerStates) {
if (moduleDriveStates.size() != driveStates.size()
|| moduleSteerStates.size() != steerStates.size())
throw new IllegalArgumentException("Provided module states do not match number of modules!");
this.pose = pose;
for (int i = 0; i < numModules; i++) {
driveStates.set(i, moduleDriveStates.get(i).copy());
steerStates.set(i, moduleSteerStates.get(i).copy());
}
omegaRadsPerSec = kinematics.toChassisSpeeds(getModuleStates()).omegaRadiansPerSecond;
}
/**
* Get the pose of the simulated swerve drive. Note that this is the "actual" pose of the robot in
* the simulation world, without any noise. If you are simulating a pose estimator, this pose
* should only be used for visualization or camera simulation. This should be called after {@link
* #update(double)}.
*/
public Pose2d getPose() {
return pose;
}
/**
* Get the {@link SwerveModulePosition} of each module. The returned array order matches the
* kinematics module order. This should be called after {@link #update(double)}.
*/
public SwerveModulePosition[] getModulePositions() {
var positions = new SwerveModulePosition[numModules];
for (int i = 0; i < numModules; i++) {
positions[i] =
new SwerveModulePosition(
driveStates.get(i).get(0, 0), new Rotation2d(steerStates.get(i).get(0, 0)));
}
return positions;
}
/**
* Get the {@link SwerveModulePosition} of each module with rudimentary noise simulation. The
* returned array order matches the kinematics module order. This should be called after {@link
* #update(double)}.
*
* @param driveStdDev The standard deviation in meters of the drive wheel position.
* @param steerStdDev The standard deviation in radians of the steer angle.
*/
public SwerveModulePosition[] getNoisyModulePositions(double driveStdDev, double steerStdDev) {
var positions = new SwerveModulePosition[numModules];
for (int i = 0; i < numModules; i++) {
positions[i] =
new SwerveModulePosition(
driveStates.get(i).get(0, 0) + rand.nextGaussian() * driveStdDev,
new Rotation2d(steerStates.get(i).get(0, 0) + rand.nextGaussian() * steerStdDev));
}
return positions;
}
/**
* Get the {@link SwerveModuleState} of each module. The returned array order matches the
* kinematics module order. This should be called after {@link #update(double)}.
*/
public SwerveModuleState[] getModuleStates() {
var positions = new SwerveModuleState[numModules];
for (int i = 0; i < numModules; i++) {
positions[i] =
new SwerveModuleState(
driveStates.get(i).get(1, 0), new Rotation2d(steerStates.get(i).get(0, 0)));
}
return positions;
}
/**
* Get the state of each module's drive system in [meters, meters/sec]. The returned list order
* matches the kinematics module order. This should be called after {@link #update(double)}.
*/
public List<Matrix<N2, N1>> getDriveStates() {
List<Matrix<N2, N1>> states = new ArrayList<>();
for (int i = 0; i < driveStates.size(); i++) {
states.add(driveStates.get(i).copy());
}
return states;
}
/**
* Get the state of each module's steer system in [radians, radians/sec]. The returned list order
* matches the kinematics module order. This should be called after {@link #update(double)}.
*/
public List<Matrix<N2, N1>> getSteerStates() {
List<Matrix<N2, N1>> states = new ArrayList<>();
for (int i = 0; i < steerStates.size(); i++) {
states.add(steerStates.get(i).copy());
}
return states;
}
/**
* Get the angular velocity of the robot, which can be useful for gyro simulation. CCW positive.
* This should be called after {@link #update(double)}.
*/
public double getOmegaRadsPerSec() {
return omegaRadsPerSec;
}
/**
* Calculates the current drawn from the battery by the motor(s). Ignores regenerative current
* from back-emf.
*
* @param motor The motor(s) used.
* @param radiansPerSec The velocity of the motor in radians per second.
* @param inputVolts The voltage commanded by the motor controller (battery voltage * duty cycle).
* @param battVolts The voltage of the battery.
*/
protected static double getCurrentDraw(
DCMotor motor, double radiansPerSec, double inputVolts, double battVolts) {
double effVolts = inputVolts - radiansPerSec / motor.KvRadPerSecPerVolt;
// ignore regeneration
if (inputVolts >= 0) effVolts = MathUtil.clamp(effVolts, 0, inputVolts);
else effVolts = MathUtil.clamp(effVolts, inputVolts, 0);
// calculate battery current
return (inputVolts / battVolts) * (effVolts / motor.rOhms);
}
/**
* Get the current draw in amps for each module's drive motor(s). This should be called after
* {@link #update(double)}. The returned array order matches the kinematics module order.
*/
public double[] getDriveCurrentDraw() {
double[] currents = new double[numModules];
for (int i = 0; i < numModules; i++) {
double radiansPerSec = driveStates.get(i).get(1, 0) * driveGearing / driveWheelRadius;
currents[i] =
getCurrentDraw(
driveMotor, radiansPerSec, driveInputs[i], RobotController.getBatteryVoltage());
}
return currents;
}
/**
* Get the current draw in amps for each module's steer motor(s). This should be called after
* {@link #update(double)}. The returned array order matches the kinematics module order.
*/
public double[] getSteerCurrentDraw() {
double[] currents = new double[numModules];
for (int i = 0; i < numModules; i++) {
double radiansPerSec = steerStates.get(i).get(1, 0) * steerGearing;
currents[i] =
getCurrentDraw(
steerMotor, radiansPerSec, steerInputs[i], RobotController.getBatteryVoltage());
}
return currents;
}
/**
* Get the total current draw in amps of all swerve motors. This should be called after {@link
* #update(double)}.
*/
public double getTotalCurrentDraw() {
double sum = 0;
for (double val : getDriveCurrentDraw()) sum += val;
for (double val : getSteerCurrentDraw()) sum += val;
return sum;
}
}
photonlib-java-examples/aimattarget/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java
+192
View File
@@ -0,0 +1,192 @@
/*
* MIT License
*
* Copyright (c) PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
package frc.robot.subsystems.drivetrain;
import static frc.robot.Constants.Swerve.*;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.controller.PIDController;
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.math.util.Units;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
import edu.wpi.first.wpilibj.simulation.EncoderSim;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
public class SwerveModule {
// --- Module Constants
private final ModuleConstants moduleConstants;
// --- Hardware
private final PWMSparkMax driveMotor;
private final Encoder driveEncoder;
private final PWMSparkMax steerMotor;
private final Encoder steerEncoder;
// --- Control
private SwerveModuleState desiredState = new SwerveModuleState();
private boolean openLoop = false;
// Simple PID feedback controllers run on the roborio
private PIDController drivePidController = new PIDController(kDriveKP, kDriveKI, kDriveKD);
// (A profiled steering PID controller may give better results by utilizing feedforward.)
private PIDController steerPidController = new PIDController(kSteerKP, kSteerKI, kSteerKD);
// --- Simulation
private final EncoderSim driveEncoderSim;
private double driveCurrentSim = 0;
private final EncoderSim steerEncoderSim;
private double steerCurrentSim = 0;
public SwerveModule(ModuleConstants moduleConstants) {
this.moduleConstants = moduleConstants;
driveMotor = new PWMSparkMax(moduleConstants.driveMotorID);
driveEncoder = new Encoder(moduleConstants.driveEncoderA, moduleConstants.driveEncoderB);
driveEncoder.setDistancePerPulse(kDriveDistPerPulse);
steerMotor = new PWMSparkMax(moduleConstants.steerMotorID);
steerEncoder = new Encoder(moduleConstants.steerEncoderA, moduleConstants.steerEncoderB);
steerEncoder.setDistancePerPulse(kSteerRadPerPulse);
steerPidController.enableContinuousInput(-Math.PI, Math.PI);
// --- Simulation
driveEncoderSim = new EncoderSim(driveEncoder);
steerEncoderSim = new EncoderSim(steerEncoder);
}
public void periodic() {
// Perform PID feedback control to steer the module to the target angle
double steerPid =
steerPidController.calculate(
getAbsoluteHeading().getRadians(), desiredState.angle.getRadians());
steerMotor.setVoltage(steerPid);
// Use feedforward model to translate target drive velocities into voltages
double driveFF = kDriveFF.calculate(desiredState.speedMetersPerSecond);
double drivePid = 0;
if (!openLoop) {
// Perform PID feedback control to compensate for disturbances
drivePid =
drivePidController.calculate(driveEncoder.getRate(), desiredState.speedMetersPerSecond);
}
driveMotor.setVoltage(driveFF + drivePid);
}
/**
* Command this swerve module to the desired angle and velocity.
*
* @param steerInPlace If modules should steer to target angle when target velocity is 0
*/
public void setDesiredState(
SwerveModuleState desiredState, boolean openLoop, boolean steerInPlace) {
Rotation2d currentRotation = getAbsoluteHeading();
// Avoid turning more than 90 degrees by inverting speed on large angle changes
desiredState = SwerveModuleState.optimize(desiredState, currentRotation);
this.desiredState = desiredState;
}
/** Module heading reported by steering encoder. */
public Rotation2d getAbsoluteHeading() {
return Rotation2d.fromRadians(steerEncoder.getDistance());
}
/**
* {@link SwerveModuleState} describing absolute module rotation and velocity in meters per
* second.
*/
public SwerveModuleState getState() {
return new SwerveModuleState(driveEncoder.getRate(), getAbsoluteHeading());
}
/** {@link SwerveModulePosition} describing absolute module rotation and position in meters. */
public SwerveModulePosition getPosition() {
return new SwerveModulePosition(driveEncoder.getDistance(), getAbsoluteHeading());
}
/** Voltage of the drive motor */
public double getDriveVoltage() {
return driveMotor.get() * RobotController.getBatteryVoltage();
}
/** Voltage of the steer motor */
public double getSteerVoltage() {
return steerMotor.get() * RobotController.getBatteryVoltage();
}
/**
* Constants about this module, like motor IDs, encoder angle offset, and translation from center.
*/
public ModuleConstants getModuleConstants() {
return moduleConstants;
}
public void log() {
var state = getState();
String table = "Module " + moduleConstants.moduleNum + "/";
SmartDashboard.putNumber(
table + "Steer Degrees", Math.toDegrees(MathUtil.angleModulus(state.angle.getRadians())));
SmartDashboard.putNumber(
table + "Steer Target Degrees", Math.toDegrees(steerPidController.getSetpoint()));
SmartDashboard.putNumber(
table + "Drive Velocity Feet", Units.metersToFeet(state.speedMetersPerSecond));
SmartDashboard.putNumber(
table + "Drive Velocity Target Feet",
Units.metersToFeet(desiredState.speedMetersPerSecond));
SmartDashboard.putNumber(table + "Drive Current", driveCurrentSim);
SmartDashboard.putNumber(table + "Steer Current", steerCurrentSim);
}
// ----- Simulation
public void simulationUpdate(
double driveEncoderDist,
double driveEncoderRate,
double driveCurrent,
double steerEncoderDist,
double steerEncoderRate,
double steerCurrent) {
driveEncoderSim.setDistance(driveEncoderDist);
driveEncoderSim.setRate(driveEncoderRate);
this.driveCurrentSim = driveCurrent;
steerEncoderSim.setDistance(steerEncoderDist);
steerEncoderSim.setRate(steerEncoderRate);
this.steerCurrentSim = steerCurrent;
}
public double getDriveCurrentSim() {
return driveCurrentSim;
}
public double getSteerCurrentSim() {
return steerCurrentSim;
}
}
photonlib-java-examples/aimattarget/src/test/java/frc/robot/JniLoadTest.java
+39
View File
@@ -0,0 +1,39 @@
/*
* MIT License
*
* Copyright (c) PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
package frc.robot;
import static org.junit.Assert.fail;
import org.junit.Test;
import org.photonvision.timesync.TimeSyncSingleton;
public class JniLoadTest {
@Test
public void smoketest() {
if (!TimeSyncSingleton.load()) {
fail("Could not load TimeSync JNI????????");
}
}
}