Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-19 00:41:41 +00:00
Code Issues Packages Projects Releases Wiki Activity

Start on updating examples for 2027

Browse Source 
Signed-off-by: Jade Turner <spacey-sooty@proton.me>
This commit is contained in:
Jade Turner
2025-06-24 10:17:44 +08:00
committed by samfreund
parent ed1b31cb7f
commit a5f9a0b673
16 changed files with 100 additions and 99 deletions
Download Patch File Download Diff File Expand all files Collapse all files

42
photonlib-java-examples/aimandrange/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java
View File

@@ -38,9 +38,8 @@ 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.OnboardIMU;
import edu.wpi.first.wpilibj.OnboardIMU.MountOrientation;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import frc.robot.Robot;
@@ -62,7 +61,7 @@ public class SwerveDrive {
swerveMods[2].getModuleConstants().centerOffset,
swerveMods[3].getModuleConstants().centerOffset);
private final ADXRS450_Gyro gyro = new ADXRS450_Gyro(Port.kOnboardCS0);
private final OnboardIMU gyro = new OnboardIMU(MountOrientation.kFlat);
// The robot pose estimator for tracking swerve odometry and applying vision corrections.
private final SwerveDrivePoseEstimator poseEstimator;
@@ -70,7 +69,8 @@ public class SwerveDrive {
private ChassisSpeeds targetChassisSpeeds = new ChassisSpeeds();
// ----- Simulation
private final ADXRS450_GyroSim gyroSim;
// TODO(Jade) Onboard IMU doesnt have sim yet
// private final ADXRS450_GyroSim gyroSim;
private final SwerveDriveSim swerveDriveSim;
private double totalCurrentDraw = 0;
@@ -91,7 +91,7 @@ public class SwerveDrive {
visionStdDevs);
// ----- Simulation
gyroSim = new ADXRS450_GyroSim(gyro);
// gyroSim = new ADXRS450_GyroSim(gyro);
swerveDriveSim =
new SwerveDriveSim(
kDriveFF,
@@ -117,13 +117,13 @@ public class SwerveDrive {
* 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+)
* @param vx X velocity (forwards/backwards)
* @param vy Y velocity (strafe left/right)
* @param omega Angular velocity (rotation CCW+)
*/
public void drive(double vxMeters, double vyMeters, double omegaRadians) {
public void drive(double vx, double vy, double omega) {
var targetChassisSpeeds =
ChassisSpeeds.fromFieldRelativeSpeeds(vxMeters, vyMeters, omegaRadians, getHeading());
new ChassisSpeeds(vx, vy, omega).toRobotRelative(getHeading());
setChassisSpeeds(targetChassisSpeeds, true, false);
}
@@ -189,8 +189,8 @@ public class SwerveDrive {
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);
// gyroSim.setAngle(-pose.getRotation().getDegrees());
// gyroSim.setRate(0);
}
poseEstimator.resetPosition(getGyroYaw(), getModulePositions(), pose);
@@ -267,14 +267,14 @@ public class SwerveDrive {
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 + "VX", chassisSpeeds.vx);
SmartDashboard.putNumber(table + "VY", chassisSpeeds.vy);
SmartDashboard.putNumber(
table + "Omega Degrees", Math.toDegrees(chassisSpeeds.omegaRadiansPerSecond));
SmartDashboard.putNumber(table + "Target VX", targetChassisSpeeds.vxMetersPerSecond);
SmartDashboard.putNumber(table + "Target VY", targetChassisSpeeds.vyMetersPerSecond);
table + "Omega Degrees", Math.toDegrees(chassisSpeeds.omega));
SmartDashboard.putNumber(table + "Target VX", targetChassisSpeeds.vx);
SmartDashboard.putNumber(table + "Target VY", targetChassisSpeeds.vy);
SmartDashboard.putNumber(
table + "Target Omega Degrees", Math.toDegrees(targetChassisSpeeds.omegaRadiansPerSecond));
table + "Target Omega Degrees", Math.toDegrees(targetChassisSpeeds.omega));
for (SwerveModule module : swerveMods) {
module.log();
@@ -314,8 +314,8 @@ public class SwerveDrive {
drivePos, driveRate, driveCurrents[i], steerPos, steerRate, steerCurrents[i]);
}
gyroSim.setRate(-swerveDriveSim.getOmegaRadsPerSec());
gyroSim.setAngle(-swerveDriveSim.getPose().getRotation().getDegrees());
// gyroSim.setRate(-swerveDriveSim.getOmegaRadsPerSec());
// gyroSim.setAngle(-swerveDriveSim.getPose().getRotation().getDegrees());
}
/**

6
photonlib-java-examples/aimandrange/src/main/java/frc/robot/subsystems/drivetrain/SwerveDriveSim.java
View File

@@ -341,7 +341,7 @@ public class SwerveDriveSim {
driveStates.set(i, moduleDriveStates.get(i).copy());
steerStates.set(i, moduleSteerStates.get(i).copy());
}
omegaRadsPerSec = kinematics.toChassisSpeeds(getModuleStates()).omegaRadiansPerSecond;
omegaRadsPerSec = kinematics.toChassisSpeeds(getModuleStates()).omega;
}
/**
@@ -444,12 +444,12 @@ public class SwerveDriveSim {
*/
protected static double getCurrentDraw(
DCMotor motor, double radiansPerSec, double inputVolts, double battVolts) {
double effVolts = inputVolts - radiansPerSec / motor.KvRadPerSecPerVolt;
double effVolts = inputVolts - radiansPerSec / motor.Kv;
// 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);
return (inputVolts / battVolts) * (effVolts / motor.R);
}
/**

8
photonlib-java-examples/aimandrange/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java
View File

@@ -88,12 +88,12 @@ public class SwerveModule {
steerMotor.setVoltage(steerPid);
// Use feedforward model to translate target drive velocities into voltages
double driveFF = kDriveFF.calculate(desiredState.speedMetersPerSecond);
double driveFF = kDriveFF.calculate(desiredState.speed);
double drivePid = 0;
if (!openLoop) {
// Perform PID feedback control to compensate for disturbances
drivePid =
drivePidController.calculate(driveEncoder.getRate(), desiredState.speedMetersPerSecond);
drivePidController.calculate(driveEncoder.getRate(), desiredState.speed);
}
driveMotor.setVoltage(driveFF + drivePid);
@@ -157,10 +157,10 @@ public class SwerveModule {
SmartDashboard.putNumber(
table + "Steer Target Degrees", Math.toDegrees(steerPidController.getSetpoint()));
SmartDashboard.putNumber(
table + "Drive Velocity Feet", Units.metersToFeet(state.speedMetersPerSecond));
table + "Drive Velocity Feet", Units.metersToFeet(state.speed));
SmartDashboard.putNumber(
table + "Drive Velocity Target Feet",
Units.metersToFeet(desiredState.speedMetersPerSecond));
Units.metersToFeet(desiredState.speed));
SmartDashboard.putNumber(table + "Drive Current", driveCurrentSim);
SmartDashboard.putNumber(table + "Steer Current", steerCurrentSim);
}