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

2
.gitignore vendored
View File

@@ -161,3 +161,5 @@ photon-client/playwright/.auth/
shell.nix
flake.nix
flake.lock
/workspace

10
photonlib-cpp-examples/aimandrange/src/main/cpp/subsystems/SwerveDrive.cpp
View File

@@ -32,7 +32,7 @@
SwerveDrive::SwerveDrive()
: poseEstimator(kinematics, GetGyroYaw(), GetModulePositions(),
frc::Pose2d{}, {0.1, 0.1, 0.1}, {1.0, 1.0, 1.0}),
gyroSim(gyro),
// gyroSim(gyro),
swerveDriveSim(constants::Swerve::kDriveFF, frc::DCMotor::Falcon500(1),
constants::Swerve::kDriveGearRatio,
constants::Swerve::kWheelDiameter / 2,
@@ -82,8 +82,8 @@ void SwerveDrive::ResetPose(const frc::Pose2d& pose, bool resetSimPose) {
for (int i = 0; i < swerveMods.size(); i++) {
swerveMods[i].SimulationUpdate(0_m, 0_mps, 0_A, 0_rad, 0_rad_per_s, 0_A);
}
gyroSim.SetAngle(-pose.Rotation().Degrees());
gyroSim.SetRate(0_rad_per_s);
// gyroSim.SetAngle(-pose.Rotation().Degrees());
// gyroSim.SetRate(0_rad_per_s);
}
poseEstimator.ResetPosition(GetGyroYaw(), GetModulePositions(), pose);
@@ -190,8 +190,8 @@ void SwerveDrive::SimulationPeriodic() {
swerveMods[i].SimulationUpdate(drivePos, driveRate, driveCurrents[i],
steerPos, steerRate, steerCurrents[i]);
}
gyroSim.SetRate(-swerveDriveSim.GetOmega());
gyroSim.SetAngle(-swerveDriveSim.GetPose().Rotation().Degrees());
// gyroSim.SetRate(-swerveDriveSim.GetOmega());
// gyroSim.SetAngle(-swerveDriveSim.GetPose().Rotation().Degrees());
}
frc::Pose2d SwerveDrive::GetSimPose() const { return swerveDriveSim.GetPose(); }

9
photonlib-cpp-examples/aimandrange/src/main/include/subsystems/SwerveDrive.h
View File

@@ -24,11 +24,9 @@
#pragma once
#include <frc/ADXRS450_Gyro.h>
#include <frc/SPI.h>
#include <frc/estimator/SwerveDrivePoseEstimator.h>
#include <frc/kinematics/SwerveDriveKinematics.h>
#include <frc/simulation/ADXRS450_GyroSim.h>
#include <frc/OboardIMU.h>
#include "SwerveDriveSim.h"
#include "SwerveModule.h"
@@ -70,11 +68,12 @@ class SwerveDrive {
swerveMods[2].GetModuleConstants().centerOffset,
swerveMods[3].GetModuleConstants().centerOffset,
};
frc::ADXRS450_Gyro gyro{frc::SPI::Port::kOnboardCS0};
frc::OnboardIMU gyro{MountOrientation::kFlat};
frc::SwerveDrivePoseEstimator<4> poseEstimator;
frc::ChassisSpeeds targetChassisSpeeds{};
frc::sim::ADXRS450_GyroSim gyroSim;
//TODO(Jade) onboard imu doesn't have sim yet
// frc::sim::ADXRS450_GyroSim gyroSim;
SwerveDriveSim swerveDriveSim;
units::ampere_t totalCurrentDraw{0};
};

2
photonlib-java-examples/aimandrange/build.gradle
View File

@@ -78,7 +78,7 @@ dependencies {
nativeRelease wpi.java.vendor.jniRelease(wpi.platforms.desktop)
simulationRelease wpi.sim.enableRelease()
testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1'
testImplementation 'org.junit.jupiter:junit-jupiter:5.12.2'
testRuntimeOnly 'org.junit.platform:junit-platform-launcher'
}

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);
}

2
photonlib-java-examples/aimattarget/build.gradle
View File

@@ -74,7 +74,7 @@ dependencies {
nativeRelease wpi.java.vendor.jniRelease(wpi.platforms.desktop)
simulationRelease wpi.sim.enableRelease()
testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1'
testImplementation 'org.junit.jupiter:junit-jupiter:5.12.2'
testRuntimeOnly 'org.junit.platform:junit-platform-launcher'
}

42
photonlib-java-examples/aimattarget/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) OnboardIMU doesn't 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/aimattarget/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/aimattarget/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);
}

2
photonlib-java-examples/poseest/build.gradle
View File

@@ -74,7 +74,7 @@ dependencies {
nativeRelease wpi.java.vendor.jniRelease(wpi.platforms.desktop)
simulationRelease wpi.sim.enableRelease()
testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1'
testImplementation 'org.junit.jupiter:junit-jupiter:5.12.2'
testRuntimeOnly 'org.junit.platform:junit-platform-launcher'
}

4
photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/GamepieceLauncher.java
View File

@@ -51,7 +51,7 @@ public class GamepieceLauncher {
public void periodic() {
double maxRPM =
Units.radiansPerSecondToRotationsPerMinute(DCMotor.getFalcon500(1).freeSpeedRadPerSec);
Units.radiansPerSecondToRotationsPerMinute(DCMotor.getFalcon500(1).freeSpeed);
curMotorCmd = curDesSpd / maxRPM;
curMotorCmd = MathUtil.clamp(curMotorCmd, 0.0, 1.0);
motor.set(curMotorCmd);
@@ -76,7 +76,7 @@ public class GamepieceLauncher {
public void simulationPeriodic() {
launcherSim.setInputVoltage(curMotorCmd * RobotController.getBatteryVoltage());
launcherSim.update(0.02);
var spd = launcherSim.getAngularVelocityRPM();
var spd = launcherSim.getAngularVelocity();
SmartDashboard.putNumber("GPLauncher Act Spd (RPM)", spd);
}
}

42
photonlib-java-examples/poseest/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) WPILib doesn't have onboard IMU 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/poseest/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/poseest/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);
}