mirror of
https://github.com/Team4388/2026KPopRobotHunters.git
synced 2026-06-09 00:38:03 -06:00
Initial commit
This commit is contained in:
mirage54321
@@ -0,0 +1,75 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2018 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package frc4388.robot.subsystems;
|
||||
|
||||
import org.littletonrobotics.junction.AutoLogOutput;
|
||||
|
||||
import edu.wpi.first.wpilibj.DriverStation;
|
||||
import edu.wpi.first.wpilibj.motorcontrol.Spark;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc4388.robot.constants.Constants.LEDConstants;
|
||||
import frc4388.utility.status.Status;
|
||||
import frc4388.utility.status.FaultReporter;
|
||||
import frc4388.utility.status.Queryable;
|
||||
import frc4388.utility.status.Status.ReportLevel;
|
||||
import frc4388.utility.structs.LEDPatterns;
|
||||
|
||||
/**
|
||||
* Allows for the control of a 5v LED Strip using a Rev Robotics Blinkin LED
|
||||
* Driver
|
||||
*/
|
||||
public class LED extends SubsystemBase implements Queryable {
|
||||
public LED() {
|
||||
FaultReporter.register(this);
|
||||
}
|
||||
|
||||
private static Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID);
|
||||
private LEDPatterns mode = LEDConstants.DEFAULT_PATTERN;
|
||||
|
||||
public void setMode(LEDPatterns pattern){
|
||||
this.mode = pattern;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
update();
|
||||
}
|
||||
|
||||
public void update() {
|
||||
if(!LEDController.isAlive() || LEDController.isSafetyEnabled()) return;
|
||||
|
||||
if(DriverStation.isDisabled()){
|
||||
LEDController.set(LEDConstants.DEFAULT_PATTERN.getValue());
|
||||
}else
|
||||
LEDController.set(mode.getValue());
|
||||
}
|
||||
|
||||
@AutoLogOutput
|
||||
public String state() {
|
||||
return mode.getClass().toString();
|
||||
}
|
||||
|
||||
@Override
|
||||
public String getName() {
|
||||
return "LEDs";
|
||||
}
|
||||
|
||||
@Override
|
||||
public Status diagnosticStatus() {
|
||||
Status status = new Status();
|
||||
|
||||
if(!LEDController.isAlive())
|
||||
status.addReport(ReportLevel.ERROR, "LED is DISCONNECTED");
|
||||
|
||||
status.addReport(ReportLevel.INFO, "LED Mode: " + mode.name());
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
@@ -0,0 +1,58 @@
|
||||
package frc4388.robot.subsystems.lidar;
|
||||
|
||||
import org.littletonrobotics.junction.Logger;
|
||||
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc4388.robot.constants.Constants.LiDARConstants;
|
||||
import frc4388.utility.status.Status;
|
||||
import frc4388.utility.status.FaultReporter;
|
||||
import frc4388.utility.status.Queryable;
|
||||
import frc4388.utility.status.Status.ReportLevel;
|
||||
|
||||
public class LiDAR extends SubsystemBase implements Queryable {
|
||||
LidarIO io;
|
||||
LidarStateAutoLogged state = new LidarStateAutoLogged();
|
||||
|
||||
private String name = "Lidar";
|
||||
|
||||
public LiDAR(LidarIO device, String name) {
|
||||
FaultReporter.register(this);
|
||||
|
||||
this.io = device;
|
||||
this.name = name;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
io.updateInputs(state);
|
||||
Logger.processInputs("LiDAR/"+name, state);
|
||||
}
|
||||
|
||||
// @AutoLogOutput(key = "Lidar/{name}")
|
||||
public double getDistance(){
|
||||
return state.distance;
|
||||
}
|
||||
|
||||
public boolean withinDistance(){
|
||||
if(state.distance == -1) return false;
|
||||
return state.distance < LiDARConstants.LIDAR_DETECT_DISTANCE;
|
||||
}
|
||||
|
||||
@Override
|
||||
public String getName() {
|
||||
return "Lidar " + name;
|
||||
}
|
||||
|
||||
@Override
|
||||
public Status diagnosticStatus() {
|
||||
Status s = new Status();
|
||||
|
||||
if(state.distance == -1)
|
||||
s.addReport(ReportLevel.ERROR, "LiDAR DISCONNECTED");
|
||||
|
||||
s.addReport(ReportLevel.INFO, "LiDAR Distance: " + state.distance);
|
||||
|
||||
return s;
|
||||
}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,13 @@
|
||||
package frc4388.robot.subsystems.lidar;
|
||||
|
||||
import org.littletonrobotics.junction.AutoLog;
|
||||
|
||||
public interface LidarIO {
|
||||
@AutoLog
|
||||
public class LidarState {
|
||||
public boolean connected;
|
||||
public double distance;
|
||||
}
|
||||
|
||||
public default void updateInputs(LidarState state) {}
|
||||
}
|
||||
@@ -0,0 +1,27 @@
|
||||
package frc4388.robot.subsystems.lidar;
|
||||
|
||||
import edu.wpi.first.wpilibj.Counter;
|
||||
import frc4388.robot.constants.Constants.LiDARConstants;
|
||||
|
||||
// https://girlsofsteeldocs.readthedocs.io/en/latest/technical-resources/sensors/LIDAR-Lite-Distance-Sensor.html#minimal-roborio-interface
|
||||
public class LidarReal implements LidarIO {
|
||||
|
||||
|
||||
private Counter LidarPWM;
|
||||
|
||||
public LidarReal(int port) {
|
||||
LidarPWM = new Counter(port);
|
||||
LidarPWM.setMaxPeriod(1.00); //set the max period that can be measured
|
||||
LidarPWM.setSemiPeriodMode(true); //Set the counter to period measurement
|
||||
LidarPWM.reset();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void updateInputs(LidarState state) {
|
||||
|
||||
if(LidarPWM.get() < 1)
|
||||
state.distance = -1;
|
||||
else
|
||||
state.distance = (LidarPWM.getPeriod() * LiDARConstants.SECONDS_TO_MICROS) / LiDARConstants.LIDAR_MICROS_TO_CM;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,467 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package frc4388.robot.subsystems.swerve;
|
||||
|
||||
import org.littletonrobotics.junction.AutoLogOutput;
|
||||
import org.littletonrobotics.junction.Logger;
|
||||
|
||||
import com.ctre.phoenix6.swerve.SwerveRequest;
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.math.kinematics.ChassisSpeeds;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc4388.robot.constants.Constants.AutoConstants;
|
||||
import frc4388.robot.subsystems.vision.Vision;
|
||||
import frc4388.utility.compute.TimesNegativeOne;
|
||||
import frc4388.utility.status.Status;
|
||||
import frc4388.utility.status.FaultReporter;
|
||||
import frc4388.utility.status.Queryable;
|
||||
|
||||
import com.pathplanner.lib.controllers.PPHolonomicDriveController;
|
||||
import com.pathplanner.lib.util.PathPlannerLogging;
|
||||
import com.pathplanner.lib.config.PIDConstants;
|
||||
import com.pathplanner.lib.auto.AutoBuilder;
|
||||
import com.pathplanner.lib.config.RobotConfig;
|
||||
|
||||
public class SwerveDrive extends SubsystemBase implements Queryable {
|
||||
// private SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain;
|
||||
|
||||
private SwerveIO io;
|
||||
private SwerveStateAutoLogged state;
|
||||
|
||||
private Vision vision;
|
||||
|
||||
|
||||
|
||||
public int gear_index = SwerveDriveConstants.STARTING_GEAR;
|
||||
public boolean stopped = false;
|
||||
public boolean robotKnowsWhereItIs = false;
|
||||
|
||||
public double speedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * SwerveDriveConstants.GEARS[gear_index];
|
||||
public double rotSpeedAdjust = SwerveDriveConstants.MAX_ROT_SPEED;
|
||||
public double autoSpeedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * 0.25; // cap auto performance to
|
||||
// 25%
|
||||
|
||||
public double lastOdomSpeed;
|
||||
|
||||
public Pose2d initalPose2d = new Pose2d();
|
||||
|
||||
|
||||
public double rotTarget = 0.0;
|
||||
public Rotation2d orientRotTarget = new Rotation2d();
|
||||
public ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
|
||||
|
||||
/** Creates a new SwerveDrive. */
|
||||
public SwerveDrive(SwerveIO swerveDriveTrain, Vision vision) {
|
||||
// public SwerveDrive(SwerveDrivetrain<TalonFX, TalonFX, CANcoder>
|
||||
// swerveDriveTrain) {
|
||||
FaultReporter.register(this);
|
||||
|
||||
this.io = swerveDriveTrain;
|
||||
this.state = new SwerveStateAutoLogged();
|
||||
|
||||
this.vision = vision;
|
||||
|
||||
RobotConfig config;
|
||||
try {
|
||||
config = RobotConfig.fromGUISettings();
|
||||
} catch (Exception e) {
|
||||
// Handle exception as needed
|
||||
config = null;
|
||||
}
|
||||
// DoubleSupplier a = () -> 1.d;
|
||||
AutoBuilder.configure(
|
||||
() -> {
|
||||
return getPose2d();
|
||||
}, // Robot pose supplier
|
||||
this::setOdoPose, // Method to reset odometry (will be called if your auto has a starting
|
||||
// pose)
|
||||
() -> state.speeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE
|
||||
(speeds, feedforwards) -> io.setControl(new SwerveRequest.ApplyRobotSpeeds()
|
||||
.withSpeeds(speeds)), // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds.
|
||||
// Also optionally outputs individual module feedforwards
|
||||
new PPHolonomicDriveController( // PPHolonomicController is the built in path following controller for
|
||||
// holonomic drive trains
|
||||
new PIDConstants(5.0, 0.0, 0.0), // Translation PID constants
|
||||
new PIDConstants(5.0, 0.0, 0.0) // Rotation PID constants
|
||||
),
|
||||
config, // The robot configuration
|
||||
() -> {
|
||||
// Boolean supplier that controls when the path will be mirrored for the red
|
||||
// alliance
|
||||
// This will flip the path being followed to the red side of the field.
|
||||
// THE ORIGIN WILL REMAIN ON THE BLUE SIDE
|
||||
|
||||
// var alliance = DriverStation.getAlliance();
|
||||
// if (alliance.isPresent()) {
|
||||
// return alliance.get() == DriverStation.Alliance.Red;
|
||||
// }
|
||||
return TimesNegativeOne.isRed;
|
||||
},
|
||||
this // Reference to this subsystem to set requirements
|
||||
);
|
||||
|
||||
PathPlannerLogging.setLogActivePathCallback(
|
||||
(activePath) -> {
|
||||
Logger.recordOutput(
|
||||
"Odometry/Trajectory", activePath.toArray(new Pose2d[activePath.size()]));
|
||||
});
|
||||
|
||||
PathPlannerLogging.setLogTargetPoseCallback(
|
||||
(targetPose) -> {
|
||||
Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose);
|
||||
});
|
||||
|
||||
|
||||
|
||||
// // Configure SysId
|
||||
// sysId =
|
||||
// new SysIdRoutine(
|
||||
// new SysIdRoutine.Config(
|
||||
// null,
|
||||
// null,
|
||||
// null,
|
||||
// (state) -> Logger.recordOutput("Drive/SysIdState", toString())),
|
||||
// new SysIdRoutine.Mechanism(
|
||||
// (voltage) -> runCharacterization(voltage.in(Volts)), null, this));
|
||||
|
||||
}
|
||||
|
||||
public void setOdoPose(Pose2d pose) {
|
||||
if (pose == null) return;
|
||||
initalPose2d = pose;
|
||||
io.resetPose(pose);
|
||||
}
|
||||
|
||||
// public void oneModuleTest(SwerveModule module, Translation2d leftStick,
|
||||
// Translation2d rightStick){
|
||||
// // double ang = Math.atan2(rightStick.getY(), rightStick.getX());
|
||||
// // rightStick.getAngle()
|
||||
// double speed = Math.sqrt(Math.pow(leftStick.getX(), 2) +
|
||||
// Math.pow(leftStick.getY(), 2));
|
||||
// // System.out.println(ang);
|
||||
// // module.go(ang);
|
||||
// // Rotation2d rot = Rotation2d.fromRadians(ang);
|
||||
// Rotation2d rot = new Rotation2d(rightStick.getX(), rightStick.getY());
|
||||
// SwerveModuleState state = new SwerveModuleState(speed, rot);
|
||||
// module.setDesiredState(state);
|
||||
// }
|
||||
|
||||
public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
|
||||
if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the swerve drive is still going:
|
||||
stopModules(); // stop the swerve
|
||||
|
||||
if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05) // if no imput
|
||||
return; // don't bother doing swerve drive math and return early.
|
||||
|
||||
leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset);
|
||||
|
||||
stopped = false;
|
||||
if (fieldRelative) {
|
||||
|
||||
leftStick = TimesNegativeOne.invert(leftStick, TimesNegativeOne.XAxis, TimesNegativeOne.YAxis);
|
||||
rightStick = TimesNegativeOne.invert(rightStick, TimesNegativeOne.RotAxis);
|
||||
|
||||
// ! drift correction
|
||||
if (rightStick.getNorm() > 0.05 || !SwerveDriveConstants.DRIFT_CORRECTION_ENABLED) {
|
||||
|
||||
rotTarget = state.currentPose.getRotation().getDegrees();
|
||||
|
||||
io.setControl(new SwerveRequest.FieldCentric()
|
||||
.withVelocityX(leftStick.getX() * speedAdjust)
|
||||
.withVelocityY(leftStick.getY() * speedAdjust)
|
||||
.withRotationalRate(rightStick.getX() * rotSpeedAdjust));
|
||||
|
||||
// .withForwardPerspective(ForwardPerspectiveValue.OperatorPerspective));
|
||||
SmartDashboard.putBoolean("drift correction", false);
|
||||
} else {
|
||||
var ctrl = new SwerveRequest.FieldCentricFacingAngle()
|
||||
.withVelocityX(leftStick.getX() * speedAdjust)
|
||||
.withVelocityY(leftStick.getY() * speedAdjust)
|
||||
.withTargetDirection(Rotation2d.fromDegrees(rotTarget));
|
||||
ctrl.HeadingController.setPID(
|
||||
SwerveDriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kP,
|
||||
SwerveDriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kI,
|
||||
SwerveDriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kD
|
||||
);
|
||||
io.setControl(ctrl);
|
||||
SmartDashboard.putBoolean("drift correction", true);
|
||||
}
|
||||
|
||||
|
||||
} else { // Create robot-relative speeds.
|
||||
io.setControl(new SwerveRequest.RobotCentric()
|
||||
.withVelocityX(leftStick.getX() * speedAdjust)
|
||||
.withVelocityY(-leftStick.getY() * speedAdjust)
|
||||
.withRotationalRate(rightStick.getX() * rotSpeedAdjust));
|
||||
}
|
||||
}
|
||||
|
||||
public void driveFine(Translation2d leftStick, Translation2d rightStick, double percentOutput) {
|
||||
stopped = false;
|
||||
// Create robot-relative speeds.
|
||||
if (rightStick.getNorm() > 0.1) rightStick = rightStick.times(0);
|
||||
io.setControl(new SwerveRequest.RobotCentric()
|
||||
.withVelocityX(leftStick.getX() * SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * percentOutput)
|
||||
.withVelocityY(-leftStick.getY() * SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * percentOutput)
|
||||
.withRotationalRate(rightStick.getX() * rotSpeedAdjust));
|
||||
|
||||
}
|
||||
|
||||
|
||||
public void driveWithInputOrientation(Translation2d leftStick, Translation2d rightStick) { // there is no practical
|
||||
// reason to have a robot
|
||||
// relitive version of
|
||||
// this, and no pre
|
||||
// provided version
|
||||
if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the swerve
|
||||
// drive is still going:
|
||||
stopModules(); // stop the swerve
|
||||
|
||||
if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05) // if no imput
|
||||
return; // don't bother doing swerve drive math and return early.
|
||||
|
||||
leftStick.rotateBy(TimesNegativeOne.ForwardOffset);
|
||||
|
||||
io.setControl(new SwerveRequest.FieldCentricFacingAngle()
|
||||
.withVelocityX(leftStick.getX() * speedAdjust)
|
||||
.withVelocityY(leftStick.getY() * speedAdjust)
|
||||
.withTargetDirection(rightStick.getAngle()));
|
||||
}
|
||||
|
||||
public void driveRelativeAngle(Translation2d leftStick, Rotation2d heading) {
|
||||
leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset);
|
||||
leftStick = TimesNegativeOne.invert(leftStick, TimesNegativeOne.XAxis, TimesNegativeOne.YAxis);
|
||||
var ctrl = new SwerveRequest.FieldCentricFacingAngle()
|
||||
.withVelocityX(leftStick.getX() * speedAdjust)
|
||||
.withVelocityY(leftStick.getY() * speedAdjust)
|
||||
.withTargetDirection(heading);
|
||||
ctrl.HeadingController.setPID(
|
||||
SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kP,
|
||||
SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kI,
|
||||
SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kD
|
||||
);
|
||||
io.setControl(ctrl);
|
||||
}
|
||||
|
||||
public void driveRelativeLockedAngle(Translation2d leftStick, Rotation2d heading) {
|
||||
leftStick = leftStick.rotateBy(heading);
|
||||
|
||||
var ctrl = new SwerveRequest.FieldCentricFacingAngle()
|
||||
.withVelocityX(leftStick.getX() * speedAdjust)
|
||||
.withVelocityY(leftStick.getY() * speedAdjust)
|
||||
.withTargetDirection(heading);
|
||||
// ctrl.HeadingController.setPID(
|
||||
// DriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kP,
|
||||
// DriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kI,
|
||||
// DriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kD
|
||||
// );
|
||||
io.setControl(ctrl);
|
||||
}
|
||||
|
||||
public void activateLuigiMode() {
|
||||
io.setLimits(20);
|
||||
}
|
||||
|
||||
public void deactivateLuigiMode() {
|
||||
io.setLimits(SwerveDriveConstants.Configurations.SLIP_CURRENT);
|
||||
}
|
||||
|
||||
public boolean rotateToTarget(double angle) {
|
||||
io.setControl(new SwerveRequest.FieldCentricFacingAngle()
|
||||
.withVelocityX(0)
|
||||
.withVelocityY(0)
|
||||
.withTargetDirection(Rotation2d.fromDegrees(angle)));
|
||||
|
||||
if (Math.abs(angle - getGyroAngle()) < 5.0) {
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
public boolean isStopped() {
|
||||
return lastOdomSpeed < AutoConstants.STOP_VELOCITY;
|
||||
}
|
||||
|
||||
public void driveWithInputRotation(Translation2d leftStick, Rotation2d rot) {
|
||||
// if (leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the
|
||||
// swerve drive is still going:
|
||||
// stopModules(); // stop the swerve
|
||||
|
||||
// if (leftStick.getNorm() < 0.05) //if no imput
|
||||
// return; // don't bother doing swerve drive math and return early.
|
||||
|
||||
leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset);
|
||||
|
||||
io.setControl(new SwerveRequest.FieldCentricFacingAngle()
|
||||
.withVelocityX(leftStick.getX() * -speedAdjust)
|
||||
.withVelocityY(leftStick.getY() * speedAdjust)
|
||||
.withTargetDirection(rot));
|
||||
// double
|
||||
}
|
||||
|
||||
public double getGyroAngle() {
|
||||
return getPose2d().getRotation().getRadians();
|
||||
}
|
||||
|
||||
public Pose2d getPose2d() {
|
||||
if(state.currentPose == null)
|
||||
return initalPose2d;
|
||||
return state.currentPose;
|
||||
}
|
||||
|
||||
public void resetGyro() {
|
||||
io.tareEverything();
|
||||
robotKnowsWhereItIs = false;
|
||||
rotTarget = 0;
|
||||
// vision.resetRotations();
|
||||
}
|
||||
|
||||
|
||||
public void softStop() {
|
||||
stopped = true;
|
||||
io.setControl(new SwerveRequest.FieldCentric()
|
||||
.withVelocityX(0)
|
||||
.withVelocityY(0)
|
||||
.withRotationalRate(0)
|
||||
); // stop the modules without breaking
|
||||
}
|
||||
|
||||
public void stopModules() {
|
||||
// stopped = true;
|
||||
// swerveDriveTrain.setControl(new SwerveRequest.SwerveDriveBrake());
|
||||
softStop();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
// This method will be called once per scheduler run\
|
||||
SmartDashboard.putNumber("Gyro", (getGyroAngle() * 180) / Math.PI);
|
||||
SmartDashboard.putNumber("RotTartget", rotTarget);
|
||||
|
||||
io.updateInputs(state);
|
||||
Logger.processInputs("SwerveDrive", state);
|
||||
|
||||
vision.setLastOdomPose(state.currentPose);
|
||||
setLastOdomSpeed(state.currentPose, state.lastPose, state.odometryRate);
|
||||
|
||||
if (vision.isTag()) {
|
||||
Pose2d pose = vision.getPose2d();
|
||||
if (!robotKnowsWhereItIs) {
|
||||
robotKnowsWhereItIs = true;
|
||||
Pose2d curPose = getPose2d();
|
||||
rotTarget += pose.getRotation().getDegrees() - curPose.getRotation().getDegrees();
|
||||
}
|
||||
|
||||
io.addVisionMeasurement(vision.getPosesToAdd());
|
||||
}
|
||||
|
||||
// if(e.isPresent())
|
||||
}
|
||||
|
||||
private void reset_index() {
|
||||
gear_index = SwerveDriveConstants.STARTING_GEAR; // however we wish to initialize the gear (What gear does the
|
||||
// robot start in?)
|
||||
}
|
||||
|
||||
public void shiftDown() {
|
||||
if (gear_index == -1 || gear_index >= SwerveDriveConstants.GEARS.length)
|
||||
reset_index(); // If outof bounds: reset index
|
||||
int i = gear_index - 1;
|
||||
if (i == -1)
|
||||
i = 0;
|
||||
setPercentOutput(SwerveDriveConstants.GEARS[i]);
|
||||
gear_index = i;
|
||||
}
|
||||
|
||||
public void shiftUp() {
|
||||
if (gear_index == -1 || gear_index >= SwerveDriveConstants.GEARS.length)
|
||||
reset_index(); // If outof bounds: reset index
|
||||
int i = gear_index + 1;
|
||||
if (i == SwerveDriveConstants.GEARS.length)
|
||||
i = SwerveDriveConstants.GEARS.length - 1;
|
||||
setPercentOutput(SwerveDriveConstants.GEARS[i]);
|
||||
gear_index = i;
|
||||
}
|
||||
|
||||
public void setPercentOutput(double speed) {
|
||||
speedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * speed;
|
||||
gear_index = -1;
|
||||
}
|
||||
|
||||
public void setToSlow() {
|
||||
setPercentOutput(SwerveDriveConstants.SLOW_SPEED);
|
||||
gear_index = 0;
|
||||
}
|
||||
|
||||
public void setToFast() {
|
||||
setPercentOutput(SwerveDriveConstants.FAST_SPEED);
|
||||
gear_index = 1;
|
||||
}
|
||||
|
||||
public void setToTurbo() {
|
||||
setPercentOutput(SwerveDriveConstants.TURBO_SPEED);
|
||||
gear_index = 2;
|
||||
}
|
||||
|
||||
public void shiftUpRot() {
|
||||
rotSpeedAdjust = SwerveDriveConstants.ROTATION_SPEED;
|
||||
}
|
||||
|
||||
public void shiftDownRot() {
|
||||
rotSpeedAdjust = SwerveDriveConstants.MIN_ROT_SPEED;
|
||||
}
|
||||
|
||||
private int tmp_gear_index = SwerveDriveConstants.STARTING_GEAR;
|
||||
|
||||
public void startSlowPeriod() {
|
||||
tmp_gear_index = gear_index;
|
||||
setToSlow();
|
||||
}
|
||||
|
||||
public void startTurboPeriod() {
|
||||
tmp_gear_index = gear_index;
|
||||
setToTurbo();
|
||||
}
|
||||
|
||||
public void endSlowPeriod() {
|
||||
setPercentOutput(SwerveDriveConstants.GEARS[tmp_gear_index]);
|
||||
gear_index = tmp_gear_index;
|
||||
}
|
||||
|
||||
|
||||
|
||||
public void setLastOdomSpeed(Pose2d curPose, Pose2d lastPose, double freq){
|
||||
if(curPose != null && lastPose != null){
|
||||
lastOdomSpeed = curPose.getTranslation().getDistance(lastPose.getTranslation())/freq;
|
||||
}
|
||||
}
|
||||
|
||||
@AutoLogOutput(key="SwerveDrive/speed ")
|
||||
public double getOdometrySpeed() {
|
||||
return lastOdomSpeed;
|
||||
}
|
||||
|
||||
|
||||
|
||||
@Override
|
||||
public String getName() {
|
||||
return "Swerve Drive Controller";
|
||||
}
|
||||
|
||||
@Override
|
||||
public Status diagnosticStatus() {
|
||||
Status status = new Status();
|
||||
|
||||
// status.addReport(ReportLevel.INFO,
|
||||
// "Don't know how to diganose new CTRE swerve systems. please check under the CAN(t) section for more detailed information about the swerves there.");
|
||||
|
||||
return status;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,246 @@
|
||||
package frc4388.robot.subsystems.swerve;
|
||||
|
||||
import static edu.wpi.first.units.Units.Inches;
|
||||
import static edu.wpi.first.units.Units.Rotations;
|
||||
|
||||
import com.ctre.phoenix6.configs.CANcoderConfiguration;
|
||||
import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs;
|
||||
import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
|
||||
import com.ctre.phoenix6.configs.MotorOutputConfigs;
|
||||
import com.ctre.phoenix6.configs.OpenLoopRampsConfigs;
|
||||
import com.ctre.phoenix6.configs.Slot0Configs;
|
||||
import com.ctre.phoenix6.configs.TalonFXConfiguration;
|
||||
import com.ctre.phoenix6.signals.NeutralModeValue;
|
||||
import com.ctre.phoenix6.signals.StaticFeedforwardSignValue;
|
||||
import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants;
|
||||
import com.ctre.phoenix6.swerve.SwerveModuleConstants;
|
||||
import com.ctre.phoenix6.swerve.SwerveModuleConstants.ClosedLoopOutputType;
|
||||
import com.ctre.phoenix6.swerve.SwerveModuleConstants.DriveMotorArrangement;
|
||||
import com.ctre.phoenix6.swerve.SwerveModuleConstants.SteerFeedbackType;
|
||||
import com.ctre.phoenix6.swerve.SwerveModuleConstants.SteerMotorArrangement;
|
||||
import com.ctre.phoenix6.swerve.SwerveModuleConstantsFactory;
|
||||
|
||||
import edu.wpi.first.units.measure.Angle;
|
||||
import edu.wpi.first.units.measure.Distance;
|
||||
import frc4388.utility.status.CanDevice;
|
||||
import frc4388.utility.structs.Gains;
|
||||
|
||||
|
||||
// No mans land
|
||||
// Beware, there be dragons.
|
||||
public final class SwerveDriveConstants {
|
||||
public static final double MAX_ROT_SPEED = Math.PI * 2;
|
||||
public static final double AUTO_MAX_ROT_SPEED = 1.5;
|
||||
public static final double MIN_ROT_SPEED = 1.0;
|
||||
public static double ROTATION_SPEED = MAX_ROT_SPEED;
|
||||
public static double PLAYBACK_ROTATION_SPEED = AUTO_MAX_ROT_SPEED;
|
||||
public static double ROT_CORRECTION_SPEED = 10; // MIN_ROT_SPEED;
|
||||
|
||||
public static final double CORRECTION_MIN = 10;
|
||||
public static final double CORRECTION_MAX = 50;
|
||||
|
||||
public static final double SLOW_SPEED = 0.25;
|
||||
public static final double FAST_SPEED = 0.5;
|
||||
public static final double TURBO_SPEED = 1.0;
|
||||
|
||||
public static final double[] GEARS = {SLOW_SPEED, FAST_SPEED, TURBO_SPEED};
|
||||
public static final int STARTING_GEAR = 0;
|
||||
// Dimensions
|
||||
public static final double WIDTH = 18.5; // TODO: validate
|
||||
public static final double HEIGHT = 18.5; // TODO: validate
|
||||
public static final double HALF_WIDTH = WIDTH / 2.d;
|
||||
public static final double HALF_HEIGHT = HEIGHT / 2.d;
|
||||
|
||||
// Mechanics
|
||||
private static final double COUPLE_RATIO = 3; //todo: find
|
||||
private static final double DRIVE_RATIO = 6.12;
|
||||
private static final double STEER_RATIO = (150/7);
|
||||
private static final Distance WHEEL_RADIUS = Inches.of(2);
|
||||
|
||||
public static final double MAX_SPEED_MEETERS_PER_SEC = 6.22; // TODO: Validate
|
||||
|
||||
public static final double MAX_SPEED_FEET_PER_SECOND = MAX_SPEED_MEETERS_PER_SEC * 3.28084;
|
||||
public static final double MAX_ANGULAR_SPEED_FEET_PER_SECOND = 2 * 2 * Math.PI;
|
||||
|
||||
// Operation
|
||||
public static final double FORWARD_OFFSET = 90; // 0, 90, 180, 270
|
||||
|
||||
public static final boolean DRIFT_CORRECTION_ENABLED = true;
|
||||
public static final boolean INVERT_X = false;
|
||||
public static final boolean INVERT_Y = true;
|
||||
public static final boolean INVERT_ROTATION = false;
|
||||
|
||||
// public static final Trim POINTLESS_TRIM = new Trim("Pointless Trim", Double.MAX_VALUE, Double.MIN_VALUE, 0.1, 0);
|
||||
|
||||
private static final class ModuleSpecificConstants { //2025
|
||||
//Front Left
|
||||
private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(-0.368896484375);
|
||||
private static final boolean FRONT_LEFT_DRIVE_MOTOR_INVERTED = false;
|
||||
private static final boolean FRONT_LEFT_STEER_MOTOR_INVERTED = true;
|
||||
private static final boolean FRONT_LEFT_ENCODER_INVERTED = false;
|
||||
private static final Distance FRONT_LEFT_XPOS = Inches.of(HALF_WIDTH);
|
||||
private static final Distance FRONT_LEFT_YPOS = Inches.of(HALF_HEIGHT);
|
||||
|
||||
//Front Right
|
||||
private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(-0.011474609375);
|
||||
private static final boolean FRONT_RIGHT_DRIVE_MOTOR_INVERTED = true;
|
||||
private static final boolean FRONT_RIGHT_STEER_MOTOR_INVERTED = true;
|
||||
private static final boolean FRONT_RIGHT_ENCODER_INVERTED = false;
|
||||
private static final Distance FRONT_RIGHT_XPOS = Inches.of(HALF_WIDTH);
|
||||
private static final Distance FRONT_RIGHT_YPOS = Inches.of(-HALF_HEIGHT);
|
||||
|
||||
//Back Left
|
||||
private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.333251953125+0.5);
|
||||
private static final boolean BACK_LEFT_DRIVE_MOTOR_INVERTED = false;
|
||||
private static final boolean BACK_LEFT_STEER_MOTOR_INVERTED = true;
|
||||
private static final boolean BACK_LEFT_ENCODER_INVERTED = false;
|
||||
private static final Distance BACK_LEFT_XPOS = Inches.of(-HALF_WIDTH);
|
||||
private static final Distance BACK_LEFT_YPOS = Inches.of(HALF_HEIGHT);
|
||||
|
||||
//Back Right
|
||||
private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(0.4306640625+0.5);
|
||||
private static final boolean BACK_RIGHT_DRIVE_MOTOR_INVERTED = false;
|
||||
private static final boolean BACK_RIGHT_STEER_MOTOR_INVERTED = true;
|
||||
private static final boolean BACK_RIGHT_ENCODER_INVERTED = false;
|
||||
private static final Distance BACK_RIGHT_XPOS = Inches.of(-HALF_WIDTH);
|
||||
private static final Distance BACK_RIGHT_YPOS = Inches.of(-HALF_HEIGHT);
|
||||
}
|
||||
|
||||
public static final class IDs {
|
||||
public static final CanDevice RIGHT_FRONT_WHEEL = new CanDevice("RIGHT_FRONT_WHEEL", 4);
|
||||
public static final CanDevice RIGHT_FRONT_STEER = new CanDevice("RIGHT_FRONT_STEER", 5);
|
||||
public static final CanDevice RIGHT_FRONT_ENCODER = new CanDevice("RIGHT_FRONT_ENCODER", 11);
|
||||
|
||||
public static final CanDevice LEFT_FRONT_WHEEL = new CanDevice("LEFT_FRONT_WHEEL", 2);
|
||||
public static final CanDevice LEFT_FRONT_STEER = new CanDevice("LEFT_FRONT_STEER", 3);
|
||||
public static final CanDevice LEFT_FRONT_ENCODER = new CanDevice("LEFT_FRONT_ENCODER", 10);
|
||||
|
||||
public static final CanDevice LEFT_BACK_WHEEL = new CanDevice("LEFT_BACK_WHEEL", 6);
|
||||
public static final CanDevice LEFT_BACK_STEER = new CanDevice("LEFT_BACK_STEER", 7);
|
||||
public static final CanDevice LEFT_BACK_ENCODER = new CanDevice("LEFT_BACK_ENCODER", 12);
|
||||
|
||||
public static final CanDevice RIGHT_BACK_WHEEL = new CanDevice("RIGHT_BACK_WHEEL", 8);
|
||||
public static final CanDevice RIGHT_BACK_STEER = new CanDevice("RIGHT_BACK_STEER", 9);
|
||||
public static final CanDevice RIGHT_BACK_ENCODER = new CanDevice("RIGHT_BACK_ENCODER", 13);
|
||||
|
||||
public static final CanDevice DRIVE_PIGEON = new CanDevice("DRIVE_PIGEON", 14);
|
||||
}
|
||||
|
||||
public static final class PIDConstants {
|
||||
public static final int SWERVE_SLOT_IDX = 0;
|
||||
public static final int SWERVE_PID_LOOP_IDX = 1;
|
||||
public static final Gains SWERVE_GAINS = new Gains(50, 0.0, 0.32, 0.0, 0, 0.0);
|
||||
|
||||
public static final Slot0Configs CURRENT_SWERVE_ROT_GAINS = new Slot0Configs()
|
||||
.withKP(50).withKI(0).withKD(0.32)
|
||||
.withKS(0).withKV(0).withKA(0);
|
||||
|
||||
public static final Slot0Configs TEST_SWERVE_ROT_GAINS = new Slot0Configs()
|
||||
.withKP(10).withKI(0).withKD(0.3)
|
||||
.withKS(0).withKV(0).withKA(0);
|
||||
|
||||
public static final Gains TEST_SWERVE_GAINS = new Gains(1.2, 0.0, 0.0, 0.0, 0, 0.0);
|
||||
|
||||
// The steer motor uses any SwerveModule.SteerRequestType control request with the
|
||||
// output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput
|
||||
public static final Slot0Configs PREPROVIDED_STEER_GAINS = new Slot0Configs()
|
||||
.withKP(100).withKI(0).withKD(0.6)
|
||||
.withKS(0.1).withKV(1.91).withKA(0)
|
||||
.withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign);
|
||||
// When using closed-loop control, the drive motor uses the control
|
||||
// output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput
|
||||
public static final Slot0Configs PREPROVIDED_DRIVE_GAINS = new Slot0Configs()
|
||||
.withKP(0.1).withKI(0).withKD(0)
|
||||
.withKS(0).withKV(0.124);
|
||||
|
||||
public static final Gains DRIFT_CORRECTION_GAINS = new Gains(2.5, 0, 0.1);
|
||||
public static final Gains RELATIVE_LOCKED_ANGLE_GAINS = new Gains(10, 0, 1);
|
||||
}
|
||||
|
||||
public static final class Configurations {
|
||||
public static final double OPEN_LOOP_RAMP_RATE = 0.4; // Todo: Test. think this will help.
|
||||
public static final double CLOSED_LOOP_RAMP_RATE = 0.4; // Todo: Test. think this will help.
|
||||
public static final double NEUTRAL_DEADBAND = 0.04;
|
||||
|
||||
// POWER! (limiting)
|
||||
private static final TalonFXConfiguration DRIVE_MOTOR_CONFIGS = new TalonFXConfiguration()
|
||||
.withMotorOutput(
|
||||
new MotorOutputConfigs()
|
||||
.withNeutralMode(NeutralModeValue.Brake)
|
||||
.withDutyCycleNeutralDeadband(NEUTRAL_DEADBAND)
|
||||
).withOpenLoopRamps(
|
||||
new OpenLoopRampsConfigs()
|
||||
.withDutyCycleOpenLoopRampPeriod(OPEN_LOOP_RAMP_RATE)
|
||||
).withClosedLoopRamps(
|
||||
new ClosedLoopRampsConfigs()
|
||||
.withDutyCycleClosedLoopRampPeriod(CLOSED_LOOP_RAMP_RATE)
|
||||
);
|
||||
private static final TalonFXConfiguration STEER_MOTOR_CONFIGS = new TalonFXConfiguration()
|
||||
.withCurrentLimits(
|
||||
new CurrentLimitsConfigs()
|
||||
.withStatorCurrentLimit(40) // TODO: tune???
|
||||
.withStatorCurrentLimitEnable(true)
|
||||
).withMotorOutput(
|
||||
new MotorOutputConfigs()
|
||||
.withNeutralMode(NeutralModeValue.Brake)
|
||||
.withDutyCycleNeutralDeadband(NEUTRAL_DEADBAND)
|
||||
// ).withOpenLoopRamps(
|
||||
// new OpenLoopRampsConfigs()
|
||||
// .withDutyCycleOpenLoopRampPeriod(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE)
|
||||
// ).withClosedLoopRamps(
|
||||
// new ClosedLoopRampsConfigs()
|
||||
// .withDutyCycleClosedLoopRampPeriod(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE)
|
||||
);
|
||||
public static final double SLIP_CURRENT = 60; // TODO: Tune???
|
||||
}
|
||||
|
||||
public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants()
|
||||
.withPigeon2Id(IDs.DRIVE_PIGEON.id);
|
||||
|
||||
private static final SwerveModuleConstantsFactory<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> ConstantCreator =
|
||||
new SwerveModuleConstantsFactory<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>() // holy verbosity batman.
|
||||
.withDriveMotorGearRatio(DRIVE_RATIO)
|
||||
.withSteerMotorGearRatio(STEER_RATIO)
|
||||
.withCouplingGearRatio(COUPLE_RATIO)
|
||||
.withWheelRadius(WHEEL_RADIUS)
|
||||
.withSteerMotorGains(PIDConstants.PREPROVIDED_STEER_GAINS) // ?
|
||||
.withDriveMotorGains(PIDConstants.PREPROVIDED_DRIVE_GAINS) // ?
|
||||
.withSteerMotorClosedLoopOutput(ClosedLoopOutputType.Voltage)
|
||||
.withDriveMotorClosedLoopOutput(ClosedLoopOutputType.Voltage)
|
||||
.withSlipCurrent(Configurations.SLIP_CURRENT)
|
||||
.withSpeedAt12Volts(MAX_SPEED_MEETERS_PER_SEC)
|
||||
.withDriveMotorType(DriveMotorArrangement.TalonFX_Integrated)
|
||||
.withSteerMotorType(SteerMotorArrangement.TalonFX_Integrated)
|
||||
.withFeedbackSource(SteerFeedbackType.RemoteCANcoder)
|
||||
.withDriveMotorInitialConfigs(Configurations.DRIVE_MOTOR_CONFIGS)
|
||||
.withSteerMotorInitialConfigs(Configurations.STEER_MOTOR_CONFIGS);
|
||||
|
||||
public static final SwerveModuleConstants<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> FRONT_LEFT =
|
||||
ConstantCreator.createModuleConstants(
|
||||
IDs.LEFT_FRONT_STEER.id, IDs.LEFT_FRONT_WHEEL.id, IDs.LEFT_FRONT_ENCODER.id, ModuleSpecificConstants.FRONT_LEFT_ENCODER_OFFSET,
|
||||
ModuleSpecificConstants.FRONT_LEFT_XPOS, ModuleSpecificConstants.FRONT_LEFT_YPOS,
|
||||
ModuleSpecificConstants.FRONT_LEFT_DRIVE_MOTOR_INVERTED, ModuleSpecificConstants.FRONT_LEFT_STEER_MOTOR_INVERTED, ModuleSpecificConstants.FRONT_LEFT_ENCODER_INVERTED
|
||||
);
|
||||
public static final SwerveModuleConstants<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> FRONT_RIGHT =
|
||||
ConstantCreator.createModuleConstants(
|
||||
IDs.RIGHT_FRONT_STEER.id, IDs.RIGHT_FRONT_WHEEL.id, IDs.RIGHT_FRONT_ENCODER.id, ModuleSpecificConstants.FRONT_RIGHT_ENCODER_OFFSET,
|
||||
ModuleSpecificConstants.FRONT_RIGHT_XPOS, ModuleSpecificConstants.FRONT_RIGHT_YPOS,
|
||||
ModuleSpecificConstants.FRONT_RIGHT_DRIVE_MOTOR_INVERTED, ModuleSpecificConstants.FRONT_RIGHT_STEER_MOTOR_INVERTED, ModuleSpecificConstants.FRONT_RIGHT_ENCODER_INVERTED
|
||||
);
|
||||
public static final SwerveModuleConstants<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> BACK_LEFT =
|
||||
ConstantCreator.createModuleConstants(
|
||||
IDs.LEFT_BACK_STEER.id, IDs.LEFT_BACK_WHEEL.id, IDs.LEFT_BACK_ENCODER.id, ModuleSpecificConstants.BACK_LEFT_ENCODER_OFFSET,
|
||||
ModuleSpecificConstants.BACK_LEFT_XPOS, ModuleSpecificConstants.BACK_LEFT_YPOS,
|
||||
ModuleSpecificConstants.BACK_LEFT_DRIVE_MOTOR_INVERTED, ModuleSpecificConstants.BACK_LEFT_STEER_MOTOR_INVERTED, ModuleSpecificConstants.BACK_LEFT_ENCODER_INVERTED
|
||||
);
|
||||
public static final SwerveModuleConstants<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> BACK_RIGHT =
|
||||
ConstantCreator.createModuleConstants(
|
||||
IDs.RIGHT_BACK_STEER.id, IDs.RIGHT_BACK_WHEEL.id, IDs.RIGHT_BACK_ENCODER.id, ModuleSpecificConstants.BACK_RIGHT_ENCODER_OFFSET,
|
||||
ModuleSpecificConstants.BACK_RIGHT_XPOS, ModuleSpecificConstants.BACK_RIGHT_YPOS,
|
||||
ModuleSpecificConstants.BACK_RIGHT_DRIVE_MOTOR_INVERTED, ModuleSpecificConstants.BACK_RIGHT_STEER_MOTOR_INVERTED, ModuleSpecificConstants.BACK_RIGHT_ENCODER_INVERTED
|
||||
);
|
||||
|
||||
// misc
|
||||
public static final int TIMEOUT_MS = 30;
|
||||
public static final int SMARTDASHBOARD_UPDATE_FRAME = 2;
|
||||
}
|
||||
@@ -0,0 +1,33 @@
|
||||
package frc4388.robot.subsystems.swerve;
|
||||
|
||||
import java.util.List;
|
||||
|
||||
import org.littletonrobotics.junction.AutoLog;
|
||||
|
||||
import com.ctre.phoenix6.swerve.SwerveRequest;
|
||||
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.math.kinematics.ChassisSpeeds;
|
||||
import frc4388.robot.subsystems.vision.VisionIO.PoseObservation;
|
||||
|
||||
public interface SwerveIO {
|
||||
@AutoLog
|
||||
public class SwerveState {
|
||||
public Pose2d currentPose = null;
|
||||
public Pose2d lastPose = null;
|
||||
public ChassisSpeeds speeds = null;
|
||||
public double odometryRate = 1;
|
||||
}
|
||||
|
||||
public default void setControl(SwerveRequest ctrl) {}
|
||||
|
||||
public default void setLimits(double limitInAmps) {}
|
||||
|
||||
public default void tareEverything() {}
|
||||
|
||||
public default void resetPose(Pose2d pose) {}
|
||||
|
||||
public default void addVisionMeasurement(List<PoseObservation> poses) {}
|
||||
|
||||
public default void updateInputs(SwerveState state) {}
|
||||
}
|
||||
@@ -0,0 +1,63 @@
|
||||
package frc4388.robot.subsystems.swerve;
|
||||
|
||||
import java.util.List;
|
||||
|
||||
import com.ctre.phoenix6.Utils;
|
||||
import com.ctre.phoenix6.configs.TalonFXConfiguration;
|
||||
import com.ctre.phoenix6.hardware.CANcoder;
|
||||
import com.ctre.phoenix6.hardware.TalonFX;
|
||||
import com.ctre.phoenix6.swerve.SwerveDrivetrain;
|
||||
import com.ctre.phoenix6.swerve.SwerveModule;
|
||||
import com.ctre.phoenix6.swerve.SwerveRequest;
|
||||
|
||||
import frc4388.robot.subsystems.vision.Vision;
|
||||
import frc4388.robot.subsystems.vision.VisionIO.PoseObservation;
|
||||
|
||||
public class SwerveReal implements SwerveIO {
|
||||
SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain;
|
||||
|
||||
public SwerveReal(SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain) {
|
||||
this.swerveDriveTrain = swerveDriveTrain;
|
||||
swerveDriveTrain.getOdometryFrequency();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void updateInputs(SwerveState state) {
|
||||
double time = Vision.getTime();
|
||||
state.odometryRate = 1 / swerveDriveTrain.getOdometryFrequency();
|
||||
state.currentPose = swerveDriveTrain.samplePoseAt(time).orElse(null);
|
||||
state.lastPose = swerveDriveTrain.samplePoseAt(time - state.odometryRate).orElse(null);
|
||||
state.speeds = swerveDriveTrain.getState().Speeds;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void setControl(SwerveRequest ctrl) {
|
||||
swerveDriveTrain.setControl(ctrl);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void tareEverything() {
|
||||
swerveDriveTrain.tareEverything();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void setLimits(double limitInAmps) {
|
||||
for (SwerveModule<TalonFX, TalonFX, CANcoder> module : swerveDriveTrain.getModules()) {
|
||||
var talonFXConfigurator = module.getDriveMotor().getConfigurator();
|
||||
var talonFXConfigs = new TalonFXConfiguration();
|
||||
|
||||
talonFXConfigurator.refresh(talonFXConfigs);
|
||||
talonFXConfigs.CurrentLimits.StatorCurrentLimit = limitInAmps;
|
||||
talonFXConfigs.CurrentLimits.SupplyCurrentLimit = limitInAmps+10;
|
||||
talonFXConfigurator.apply(talonFXConfigs);
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public void addVisionMeasurement(List<PoseObservation> poses) {
|
||||
for(PoseObservation pose : poses) {
|
||||
swerveDriveTrain.addVisionMeasurement(pose.pose(), Utils.fpgaToCurrentTime(pose.timestamp()));
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,95 @@
|
||||
package frc4388.robot.subsystems.vision;
|
||||
|
||||
import java.util.ArrayList;
|
||||
import java.util.List;
|
||||
import java.util.Optional;
|
||||
|
||||
import org.littletonrobotics.junction.AutoLogOutput;
|
||||
import org.littletonrobotics.junction.Logger;
|
||||
|
||||
import com.ctre.phoenix6.Utils;
|
||||
import com.ctre.phoenix6.hardware.CANcoder;
|
||||
import com.ctre.phoenix6.hardware.TalonFX;
|
||||
import com.ctre.phoenix6.swerve.SwerveDrivetrain;
|
||||
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc4388.robot.subsystems.vision.VisionIO.PoseObservation;
|
||||
import frc4388.utility.status.FaultReporter;
|
||||
import frc4388.utility.status.Queryable;
|
||||
import frc4388.utility.status.Status;
|
||||
|
||||
public class Vision extends SubsystemBase implements Queryable {
|
||||
VisionIO[] io;
|
||||
VisionStateAutoLogged[] state;
|
||||
|
||||
|
||||
public Pose2d lastVisionPose = new Pose2d();
|
||||
public Pose2d lastPhysOdomPose = new Pose2d();
|
||||
|
||||
public Vision(VisionIO... devices) {
|
||||
FaultReporter.register(this);
|
||||
io = devices;
|
||||
state = new VisionStateAutoLogged[io.length];
|
||||
|
||||
for(int i = 0; i < io.length; i++) {
|
||||
state[i] = new VisionStateAutoLogged();
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
for(int i = 0; i < io.length; i++) {
|
||||
io[i].updateInputs(state[i]);
|
||||
Logger.processInputs("Vision/Camera" + i , state[i]);
|
||||
}
|
||||
}
|
||||
|
||||
public List<PoseObservation> getPosesToAdd(){
|
||||
List<PoseObservation> poses = new ArrayList<>();
|
||||
for(int i = 0; i < state.length; i++) {
|
||||
if(state[i].lastEstimatedPose != null) {
|
||||
poses.add(state[i].lastEstimatedPose);
|
||||
}
|
||||
}
|
||||
|
||||
return poses;
|
||||
}
|
||||
|
||||
public void setLastOdomPose(Pose2d pose){
|
||||
if(pose != null)
|
||||
lastPhysOdomPose = pose;
|
||||
}
|
||||
|
||||
public boolean isTag(){
|
||||
for(int i = 0; i < state.length; i++){
|
||||
if(state[i].isTagDetected && state[i].isTagProcessed)
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
@AutoLogOutput
|
||||
public Pose2d getPose2d() {
|
||||
if(lastPhysOdomPose != null)
|
||||
return lastPhysOdomPose;
|
||||
|
||||
// if(lastVisionPose != null)
|
||||
// return lastVisionPose;
|
||||
return new Pose2d();
|
||||
|
||||
}
|
||||
|
||||
public static double getTime() {
|
||||
return Utils.getCurrentTimeSeconds();
|
||||
}
|
||||
|
||||
|
||||
@Override
|
||||
public Status diagnosticStatus() {
|
||||
return new Status();
|
||||
// // TODO Auto-generated method stub
|
||||
// throw new UnsupportedOperationException("Unimplemented method 'diagnosticStatus'");
|
||||
}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,22 @@
|
||||
package frc4388.robot.subsystems.vision;
|
||||
|
||||
import org.littletonrobotics.junction.AutoLog;
|
||||
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
|
||||
public interface VisionIO {
|
||||
@AutoLog
|
||||
public class VisionState {
|
||||
public boolean isTagDetected = false;
|
||||
public boolean isTagProcessed = false;
|
||||
// public double latency = 0;
|
||||
public PoseObservation lastEstimatedPose = null;
|
||||
}
|
||||
|
||||
public static record PoseObservation(
|
||||
Pose2d pose,
|
||||
double timestamp
|
||||
) {}
|
||||
|
||||
public default void updateInputs(VisionState state) {}
|
||||
}
|
||||
@@ -0,0 +1,158 @@
|
||||
package frc4388.robot.subsystems.vision;
|
||||
|
||||
import edu.wpi.first.math.geometry.Transform3d;
|
||||
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 frc4388.robot.constants.Constants.FieldConstants;
|
||||
import frc4388.robot.constants.Constants.VisionConstants;
|
||||
|
||||
public class VisionReal implements VisionIO {
|
||||
// private PhotonCamera[] cameras;
|
||||
// private PhotonPoseEstimator[] estimators;
|
||||
|
||||
private PhotonCamera camera;
|
||||
private PhotonPoseEstimator estimator;
|
||||
|
||||
// public List<EstimatedRobotPose> poses = new ArrayList<>();
|
||||
|
||||
|
||||
public VisionReal(PhotonCamera camera, Transform3d position){
|
||||
this.camera = camera;
|
||||
estimator = new PhotonPoseEstimator(FieldConstants.kTagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, position);
|
||||
estimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY);
|
||||
}
|
||||
|
||||
// private Instant lastVisionTime = null;
|
||||
|
||||
|
||||
public void updateInputs(VisionState state) {
|
||||
state.isTagProcessed = false;
|
||||
state.isTagDetected = false;
|
||||
|
||||
state.lastEstimatedPose = null;
|
||||
|
||||
var results = camera.getAllUnreadResults();
|
||||
|
||||
// If there are no more updates from the camera
|
||||
if (results.size() == 0)
|
||||
return;
|
||||
|
||||
|
||||
var result = results.get(results.size()-1);
|
||||
|
||||
state.isTagDetected = state.isTagDetected | result.hasTargets();
|
||||
|
||||
// If there are no tags
|
||||
if(!result.hasTargets())
|
||||
return;
|
||||
|
||||
Optional<EstimatedRobotPose> estimatedRobotPose = getEstimatedGlobalPose(result, estimator);
|
||||
|
||||
// If the tag was failed to be processed
|
||||
if(estimatedRobotPose.isEmpty())
|
||||
return;
|
||||
|
||||
EstimatedRobotPose pose = estimatedRobotPose.get();
|
||||
|
||||
state.lastEstimatedPose = new PoseObservation(pose.estimatedPose.toPose2d(), pose.timestampSeconds);
|
||||
|
||||
state.isTagProcessed = true;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* The latest estimated robot pose on the field from vision data. This may be empty. This should
|
||||
* only be called once per loop.
|
||||
*
|
||||
* <p>Also includes updates for the standard deviations, which can (optionally) be retrieved with
|
||||
* {@link getEstimationStdDevs}
|
||||
*
|
||||
* @return An {@link EstimatedRobotPose} with an estimated pose, estimate timestamp, and targets
|
||||
* used for estimation.
|
||||
*/
|
||||
public Optional<EstimatedRobotPose> getEstimatedGlobalPose(PhotonPipelineResult change, PhotonPoseEstimator estimator) {
|
||||
Optional<EstimatedRobotPose> visionEst = Optional.empty();
|
||||
|
||||
var targets = change.getTargets();
|
||||
for(int i = targets.size()-1; i >= 0; i--){
|
||||
Transform3d pos = targets.get(i).getBestCameraToTarget();
|
||||
double distance = Math.sqrt(Math.pow(pos.getX(),2) + Math.pow(pos.getY(),2) + Math.pow(pos.getZ(),2));
|
||||
if (distance > VisionConstants.MIN_ESTIMATION_DISTANCE) {
|
||||
change.targets.remove(i);
|
||||
}
|
||||
}
|
||||
|
||||
if(targets.size() <= 0)
|
||||
return visionEst; // Will be empty
|
||||
|
||||
visionEst = estimator.update(change);
|
||||
// updateEstimationStdDevs(visionEst, change.getTargets(), estimator);
|
||||
|
||||
return visionEst;
|
||||
}
|
||||
|
||||
public String getName() {
|
||||
return camera.getName();
|
||||
}
|
||||
|
||||
|
||||
// /**
|
||||
// * Calculates new standard deviations This algorithm is a heuristic that creates dynamic standard
|
||||
// * deviations based on number of tags, estimation strategy, and distance from the tags.
|
||||
// *
|
||||
// * @param estimatedPose The estimated pose to guess standard deviations for.
|
||||
// * @param targets All targets in this camera frame
|
||||
// */
|
||||
// private void updateEstimationStdDevs(
|
||||
// Optional<EstimatedRobotPose> estimatedPose,
|
||||
// List<PhotonTrackedTarget> targets,
|
||||
// PhotonPoseEstimator estimator) {
|
||||
// if (estimatedPose.isEmpty()) {
|
||||
// // No pose input. Default to single-tag std devs
|
||||
// curStdDevs = VisionConstants.kSingleTagStdDevs;
|
||||
|
||||
// } else {
|
||||
// // Pose present. Start running Heuristic
|
||||
// var estStdDevs = VisionConstants.kSingleTagStdDevs;
|
||||
// int numTags = 0;
|
||||
// double avgDist = 0;
|
||||
|
||||
// // Precalculation - see how many tags we found, and calculate an average-distance metric
|
||||
// for (var tgt : targets) {
|
||||
// var tagPose = estimator.getFieldTags().getTagPose(tgt.getFiducialId());
|
||||
// if (tagPose.isEmpty()) continue;
|
||||
|
||||
// double distance = tagPose
|
||||
// .get()
|
||||
// .toPose2d()
|
||||
// .getTranslation()
|
||||
// .getDistance(estimatedPose.get().estimatedPose.toPose2d().getTranslation());
|
||||
|
||||
// if (distance < VisionConstants.MIN_ESTIMATION_DISTANCE) {
|
||||
// numTags++;
|
||||
// avgDist += distance;
|
||||
// }
|
||||
// }
|
||||
|
||||
// if (numTags == 0) {
|
||||
// // No tags visible. Default to single-tag std devs
|
||||
// curStdDevs = VisionConstants.kSingleTagStdDevs;
|
||||
// } else {
|
||||
// // One or more tags visible, run the full heuristic.
|
||||
// avgDist /= numTags;
|
||||
// // Decrease std devs if multiple targets are visible
|
||||
// if (numTags > 1) estStdDevs = VisionConstants.kMultiTagStdDevs;
|
||||
// // Increase std devs based on (average) distance
|
||||
// if (numTags == 1 && avgDist > 4)
|
||||
// estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE);
|
||||
// else estStdDevs = estStdDevs.times(1 + (avgDist * avgDist / 30));
|
||||
// curStdDevs = estStdDevs;
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
}
|
||||
Reference in New Issue
Block a user