Team4388/2025RidgeScape
1
0
Fork 0
mirror of https://github.com/Team4388/2025RidgeScape.git synced 2026-06-08 16:28:04 -06:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #42 from Team4388/advantagekit

Browse Source 
Advantagekit merged into  master
This commit is contained in:
finelygroundpowder
2025-09-09 16:45:28 -06:00
committed by GitHub
parent 06d525ade1 44286f81e7
commit a3fa7b07bd
73 changed files with 2866 additions and 2387 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.vscode/launch.json
Vendored
+9 -3
View File
@@ -4,18 +4,24 @@
// For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387
"version": "0.2.0",
"configurations": [
{
"type": "java",
"name": "Main",
"request": "launch",
"mainClass": "frc4388.robot.Main",
"projectName": "2025RidgeScape"
},
{
"type": "wpilib",
"name": "WPILib Desktop Debug",
"request": "launch",
"desktop": true,
"desktop": true
},
{
"type": "wpilib",
"name": "WPILib roboRIO Debug",
"request": "launch",
"desktop": false,
"desktop": false
}
]
}
build.gradle
+21 -1
View File
@@ -1,6 +1,7 @@
plugins {
id "java"
id "edu.wpi.first.GradleRIO" version "2025.2.1"
id "edu.wpi.first.GradleRIO" version "2025.3.2"
id "com.peterabeles.gversion" version "1.10"
}
java {
@@ -72,6 +73,9 @@ dependencies {
testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1'
testRuntimeOnly 'org.junit.platform:junit-platform-launcher'
def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text)
annotationProcessor "org.littletonrobotics.akit:akit-autolog:$akitJson.version"
}
test {
@@ -102,3 +106,19 @@ wpi.java.configureTestTasks(test)
tasks.withType(JavaCompile) {
options.compilerArgs.add '-XDstringConcat=inline'
}
task(replayWatch, type: JavaExec) {
mainClass = "org.littletonrobotics.junction.ReplayWatch"
classpath = sourceSets.main.runtimeClasspath
}
// Create version file
project.compileJava.dependsOn(createVersionFile)
gversion {
srcDir = "src/main/java/"
classPackage = "frc4388.robot.constants"
className = "BuildConstants"
dateFormat = "yyyy-MM-dd HH:mm:ss z"
timeZone = "America/Denver"
indent = " "
}
simgui-ds.json
+11 -1
View File
@@ -96,7 +96,17 @@
],
"robotJoysticks": [
{
"guid": "Keyboard0"
"guid": "030000005e040000ea0200000b050000",
"useGamepad": true
},
{
"useGamepad": true
},
{},
{},
{},
{
"useGamepad": true
}
]
}
src/main/java/frc4388/robot/Constants.java
-472
View File
@@ -1,472 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2019 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;
import static edu.wpi.first.units.Units.Inches;
import static edu.wpi.first.units.Units.Rotations;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;
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.apriltag.AprilTag;
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.geometry.Pose2d;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rotation2d;
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.trajectory.TrapezoidProfile;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.units.measure.Angle;
import edu.wpi.first.units.measure.Distance;
import frc4388.utility.CanDevice;
import frc4388.utility.Gains;
import frc4388.utility.LEDPatterns;
import frc4388.utility.ReefPositionHelper;
import frc4388.utility.Trim;
import frc4388.utility.configurable.ConfigurableDouble;
/**
* The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
* constants. This class should not be used for any other purpose. All constants should be
* declared globally (i.e. public static). Do not put anything functional in this class.
*
* <p>It is advised to statically import this class (or one of its inner classes) wherever the
* constants are needed, to reduce verbosity.
*/
public final class Constants {
public static final String CANBUS_NAME = "rio";
public static 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(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND)
).withOpenLoopRamps(
new OpenLoopRampsConfigs()
.withDutyCycleOpenLoopRampPeriod(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE)
).withClosedLoopRamps(
new ClosedLoopRampsConfigs()
.withDutyCycleClosedLoopRampPeriod(SwerveDriveConstants.Configurations.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(SwerveDriveConstants.Configurations.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???
}
// No mans land
// Beware, there be dragons.
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;
}
public static final class LiDARConstants {
public static final int REEF_LIDAR_DIO_CHANNEL = 7;
public static final int REVERSE_LIDAR_DIO_CHANNEL = 4;
public static final int HUMAN_PLAYER_STATION_DISTANCE = 40;
public static final int LIDAR_DETECT_DISTANCE = 100; // Min distance to detect pole
public static final int LIDAR_MICROS_TO_CM = 10;
public static final int SECONDS_TO_MICROS = 1000000;
}
public static final class AutoConstants {
// public static final Gains XY_GAINS = new Gains(5,0.6,0.0);
public static final Gains XY_GAINS = new Gains(8,0,0.0);
// public static final ConfigurableDouble P_XY_GAINS = new ConfigurableDouble("P_XY_GAINS", XY_GAINS.kP);
// public static final ConfigurableDouble I_XY_GAINS = new ConfigurableDouble("I_XY_GAINS", XY_GAINS.kI);
// public static final ConfigurableDouble D_XY_GAINS = new ConfigurableDouble("D_XY_GAINS", XY_GAINS.kD);
// public static final Gains XY_GAINS = new Gains(3,0.3,0.0);
// public static final Gains ROT_GAINS = new Gains(0.05,0,0.007);
// public static final Gains ROT_GAINS = new Gains(0.05,0,0.0);
public static final Trim X_OFFSET_TRIM = new Trim("X Offset Trim", Double.MAX_VALUE, -Double.MAX_VALUE,0.5, 0);
// public static final Trim Y_OFFSET_TRIM = new Trim("Y Offset Trim", Double.MAX_VALUE, -Double.MAX_VALUE, 0.5, 1.5);
public static final Trim Y_OFFSET_TRIM = new Trim("Y Offset Trim", Double.MAX_VALUE, -Double.MAX_VALUE, 0.5, 0);
public static final Trim ELEVATOR_OFFSET_TRIM = new Trim("Elevator Offset Trim", -ElevatorConstants.MAX_POSITION_ELEVATOR, ElevatorConstants.MAX_POSITION_ELEVATOR, 1, 0);
public static final Trim ARM_OFFSET_TRIM = new Trim("ARM Offset Trim", -ElevatorConstants.COMPLETLY_TOP_ENDEFFECTOR, ElevatorConstants.COMPLETLY_TOP_ENDEFFECTOR, 1, 0);
public static final double XY_TOLERANCE = 0.07; // Meters
public static final double ROT_TOLERANCE = 5; // Degrees
public static final double MIN_XY_PID_OUTPUT = 0.0;
public static final double MIN_ROT_PID_OUTPUT = 0.0;
public static final double VELOCITY_THRESHHOLD = 0.01;
// X is tangent to reef side
// Y is normal to reef side
public static final double X_SCORING_POSITION_OFFSET = Units.inchesToMeters(6.5+1); // This is from the field
public static final double Y_SCORING_POSITION_OFFSET = Units.inchesToMeters(16+1);
public static final double HALF_ROBOT_SIZE = Units.inchesToMeters(18);
public static final double L4_DISTANCE_PREP = HALF_ROBOT_SIZE + Units.inchesToMeters(15);
public static final double L4_DISTANCE_SCORE = L4_DISTANCE_PREP;
// public static final double L4_DISTANCE_SCORE = HALF_ROBOT_SIZE + Units.inchesToMeters(4.5);
public static final double L3_DISTANCE_PREP = HALF_ROBOT_SIZE + Units.inchesToMeters(15);
public static final double L3_DISTANCE_SCORE = HALF_ROBOT_SIZE + Units.inchesToMeters(5+1);
public static final double L2_PREP_DISTANCE = HALF_ROBOT_SIZE + Units.inchesToMeters(6);
public static final double L2_SCORE_DISTANCE = HALF_ROBOT_SIZE + Units.inchesToMeters(0.5-2);
public static final double ALGAE_REMOVAL_DISTANCE = HALF_ROBOT_SIZE;
// public static final double L4_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(15);
// public static final double L4_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(5.5);
// // public static final double L4_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(4.5);
// public static final double L3_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(15);
// public static final double L3_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(5+1);
// public static final double L2_PREP_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(6);
// public static final double L2_SCORE_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(0.5);
// public static final double ALGAE_REMOVAL_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(2);
public static final int L4_DRIVE_TIME = 250; //Milliseconds
// public static final int L3_DRIVE_TIME = 500;
public static final int L2_DRIVE_TIME = 250; //Milliseconds
public static final int ALGAE_DRIVE_TIME = 500;
public static final double STOP_VELOCITY = 0.0;
}
public static final class VisionConstants {
public static final String LEFT_CAMERA_NAME = "CAMERA_LEFT";
public static final String RIGHT_CAMERA_NAME = "CAMERA_RIGHT";
public static final Transform3d LEFT_CAMERA_POS = new Transform3d(new Translation3d(Units.inchesToMeters(4.547), Units.inchesToMeters(8.031), Units.inchesToMeters(8.858)), new Rotation3d(0,0.0,0.0));
public static final Transform3d RIGHT_CAMERA_POS = new Transform3d(new Translation3d(Units.inchesToMeters(4.547), -Units.inchesToMeters(8.031), Units.inchesToMeters(8.858)), new Rotation3d(0,0.0,0.0));
public static final double MIN_ESTIMATION_DISTANCE = 2; // Meters
// Photonvision thing
// The standard deviations of our vision estimated poses, which affect correction rate
// X, Y, Theta
// https://www.chiefdelphi.com/t/photonvision-finding-standard-deviations-for-swervedriveposeestimator/467802/2
public static final Matrix<N3, N1> kSingleTagStdDevs = VecBuilder.fill(0.5, 0.5, 4);
public static final Matrix<N3, N1> kMultiTagStdDevs = VecBuilder.fill(0.1, 0.1, 1);
}
public static final class FieldConstants {
public static final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2025Reefscape);
// Test april tag field layout
// public static final AprilTagFieldLayout kTagLayout = new AprilTagFieldLayout(
// Arrays.asList(new AprilTag[] {
// new AprilTag(1, new Pose3d(
// new Translation3d(0.,0.,0.26035), new Rotation3d(0.,0.,0.)
// )),
// }), 100, 100);
}
public static final class DriveConstants {
public static final int SMARTDASHBOARD_UPDATE_FRAME = 2;
}
public static final class LEDConstants {
public static final int LED_SPARK_ID = 9;
public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.FOREST_WAVES;
public static final LEDPatterns WAITING_PATTERN = LEDPatterns.SOLID_RED;
public static final LEDPatterns DOWN_PATTERN = LEDPatterns.SOLID_YELLOW;
public static final LEDPatterns READY_PATTERN = LEDPatterns.SOLID_GREEN_DARK;
public static final LEDPatterns SCORING_PATTERN = LEDPatterns.RAINBOW_RAINBOW;
public static final LEDPatterns RED_PATTERN = LEDPatterns.LAVA_WAVES;
public static final LEDPatterns BLUE_PATTERN = LEDPatterns.OCEAN_WAVES;
}
public static final class OIConstants {
public static final int XBOX_DRIVER_ID = 0;
public static final int XBOX_OPERATOR_ID = 1;
public static final int BUTTONBOX_ID = 2;
public static final int XBOX_PROGRAMMER_ID = 3;
public static final double LEFT_AXIS_DEADBAND = 0.1;
}
public static final class ElevatorConstants {
public static final CanDevice ENDEFFECTOR_ID = new CanDevice("Endeffector", 15);
public static final CanDevice ELEVATOR_ID = new CanDevice("Elevator", 16);
public static final double SAFETY_ENDEFFECTOR_MAX_TORQUE = 75;
public static final double SAFETY_ENDEFFECTOR_MIN_VELOCITY = 20;
// public static final int BASIN_LIMIT_SWITCH = 8; // TODO: FIND
public static final int BASIN_LIMIT_SWITCH = 8; // TODO: FIND
public static final int ENDEFFECTOR_LIMIT_SWITCH = 9; // TODO: FIND
public static final int INTAKE_LIMIT_SWITCH = 6; // TODO: FIND
public static final double GEAR_RATIO_ELEVATOR = -9.0;
//Max for elevator = 50%
public static final double GROUND_POSITION_ELEVATOR = 0 * GEAR_RATIO_ELEVATOR;
public static final double WAITING_POSITION_ELEVATOR = -9.5; // TODO: find 2-4 in off the pipe
public static final double HOVERING_POSITION_ELEVATOR = -7.5; // TODO: find 2-4 in off the pipe
public static final double WAITING_POSITION_BEAM_BREAK_ELEVATOR = -5; // TODO: find on the pipe
public static final double SCORING_THREE_ELEVATOR = -9.25;
public static final double DEALGAE_L2_ELEVATOR = -4;
public static final double DEALGAE_L3_ELEVATOR = -26.5;
public static final double MAX_POSITION_ELEVATOR = 4.5 * GEAR_RATIO_ELEVATOR; // TODO: find MAX position
public static final double GEAR_RATIO_ENDEFECTOR = -100.0;
public static final double ENDEFECTOR_DRIVE_SLOW = -0.08;
//Max for endefector = 60%
public static final double L2_SCORE_ELEVATOR = -5;
public static final double L2_LEAVE_ELEVATOR = -11;
public static final double L2_SCORE_ENDEFFECTOR = -19;
public static final double COMPLETLY_DOWN_ENDEFFECTOR = 0 * GEAR_RATIO_ENDEFECTOR;
public static final double DEALGAE_L2_ENDEFFECTOR = 0.22 * GEAR_RATIO_ENDEFECTOR;
public static final double COMPLETLY_MIDDLE_ENDEFFECTOR = 0.25 * GEAR_RATIO_ENDEFECTOR;
public static final double PRIMED_THREE_ENDEFFECTOR = 0.4 * GEAR_RATIO_ENDEFECTOR;
public static final double SCORING_FOUR_ENDEFFECTOR = 0.3 * GEAR_RATIO_ENDEFECTOR;
public static final double PRIMED_FOUR_ENDEFFECTOR = 0.44 * GEAR_RATIO_ENDEFECTOR;
public static final double COMPLETLY_TOP_ENDEFFECTOR = 0.5 * GEAR_RATIO_ENDEFECTOR;
public static final Slot0Configs ELEVATOR_PID = new Slot0Configs()
.withKP(1)
.withKI(0)
.withKD(0);
public static final Slot0Configs ENDEFFECTOR_PID = new Slot0Configs()
.withKP(1)
.withKI(0)
.withKD(0);
}
}
src/main/java/frc4388/robot/Robot.java
+121 -108
View File
@@ -7,28 +7,24 @@
package frc4388.robot;
import java.util.ArrayList;
import java.util.Base64;
import java.util.List;
// Advantagekit
import org.littletonrobotics.junction.LogFileUtil;
import org.littletonrobotics.junction.LoggedRobot;
import org.littletonrobotics.junction.Logger;
import org.littletonrobotics.junction.networktables.NT4Publisher;
import org.littletonrobotics.junction.wpilog.WPILOGReader;
import org.littletonrobotics.junction.wpilog.WPILOGWriter;
import com.ctre.phoenix6.CANBus;
import com.ctre.phoenix6.CANBus.CANBusStatus;
import edu.wpi.first.util.sendable.Sendable;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
import frc4388.utility.CanDevice;
import frc4388.robot.constants.BuildConstants;
import frc4388.robot.constants.Constants.SimConstants;
import frc4388.utility.DeferredBlock;
import frc4388.utility.DeferredBlockMulti;
import frc4388.utility.RobotTime;
import frc4388.utility.Status;
import frc4388.utility.Subsystem;
import frc4388.utility.Trim;
import frc4388.utility.Status.Report;
import frc4388.utility.Status.ReportLevel;
import frc4388.utility.compute.RobotTime;
import frc4388.utility.compute.Trim;
import frc4388.utility.status.FaultReporter;
//import frc4388.robot.subsystems.LED;
/**
* The VM is configured to automatically run this class, and to call the
@@ -37,7 +33,7 @@ import frc4388.utility.Status.ReportLevel;
* creating this project, you must also update the build.gradle file in the
* project.
*/
public class Robot extends TimedRobot {
public class Robot extends LoggedRobot {
Command m_autonomousCommand;
private RobotTime m_robotTime = RobotTime.getInstance();
@@ -49,32 +45,20 @@ public class Robot extends TimedRobot {
*/
@Override
public void robotInit() {
// Start logging with AdvantageKit
startLogging();
// Instantiate our RobotContainer. This will perform all our button bindings, and put our
// autonomous chooser on the dashboard.
m_robotContainer = new RobotContainer();
new Thread() {
public void run() {
try{
while(!this.isInterrupted() && this.isAlive()){
Thread.sleep(500);
for(int i=0;i<Subsystem.subsystems.size(); i++){
Subsystem.subsystems.get(i).queryStatus();
}
// System.out.println("Updated statuses!");
}
}catch(Exception e){
e.printStackTrace();
}
}
}.start();
// // Create a shuffleboard update thread, that will periodically update the values on shuffleboard
// FaultReporter.startThread();
}
/**
* This function is called every robot packet, no matter the mode. Use
* this for items like diagnostics that you want ran during disabled,
@@ -95,7 +79,6 @@ public class Robot extends TimedRobot {
// block in order for anything in the Command-based framework to work.
CommandScheduler.getInstance().run();
}
/**
* This function is called once each time the robot enters Disabled mode.
* You can use it to reset any subsystem information you want to clear when
@@ -113,7 +96,6 @@ public class Robot extends TimedRobot {
@Override
public void disabledExit() {
DeferredBlock.execute();
DeferredBlockMulti.execute();
}
/**
@@ -144,6 +126,7 @@ public class Robot extends TimedRobot {
public void autonomousPeriodic() {
}
@Override
public void teleopInit() {
m_robotContainer.stop();
@@ -156,10 +139,7 @@ public class Robot extends TimedRobot {
CommandScheduler.getInstance().cancel(m_autonomousCommand);
m_autonomousCommand.cancel();
m_autonomousCommand.end(true);
System.out.println("NOT Null!!");
} else {
System.out.println("Null!!");
}
m_robotTime.startMatchTime();
}
@@ -182,70 +162,103 @@ public class Robot extends TimedRobot {
@Override
public void testInit() {
List<String> errors = new ArrayList<>();
// Subsystems header
System.out.println(new String(Base64.getDecoder().decode("IOKWl+KWhOKWhOKWluKWl+KWliDilpfilpbilpfiloTiloTilpYgIOKWl+KWhOKWhOKWluKWl+KWliAg4paX4paW4paX4paE4paE4paW4paX4paE4paE4paE4paW4paX4paE4paE4paE4paW4paX4paWICDilpfilpYg4paX4paE4paE4paWCuKWkOKWjCAgIOKWkOKWjCDilpDilozilpDilowg4paQ4paM4paQ4paMICAgIOKWneKWmuKWnuKWmOKWkOKWjCAgICAg4paIICDilpDilowgICDilpDilpvilprilp7ilpzilozilpDilowgICAKIOKWneKWgOKWmuKWluKWkOKWjCDilpDilozilpDilpviloDilprilpYg4pad4paA4paa4paWICDilpDilowgIOKWneKWgOKWmuKWliAg4paIICDilpDilpviloDiloDilpjilpDilowgIOKWkOKWjCDilp3iloDilprilpYK4paX4paE4paE4pae4paY4pad4paa4paE4pae4paY4paQ4paZ4paE4pae4paY4paX4paE4paE4pae4paYICDilpDilowg4paX4paE4paE4pae4paYICDiloggIOKWkOKWmeKWhOKWhOKWluKWkOKWjCAg4paQ4paM4paX4paE4paE4pae4paY")));
for(int i=0;i< Subsystem.subsystems.size();i++){
Subsystem subsystem = Subsystem.subsystems.get(i);
System.out.println("** Subsystem diagnostic report for " + subsystem.getName() + ":");
Status status = subsystem.diagnosticStatus();
for(int a=0;a<status.reports.size();a++){
Report r = status.reports.get(a);
if(r.reportLevel == ReportLevel.ERROR)
errors.add(subsystem.getName() + " - " + r.toString());
subsystem.Log(r.toString());
}
}
// CAN header
System.out.println(new String(Base64.getDecoder().decode("IOKWl+KWhOKWhOKWliDilpfiloTilpYg4paX4paWICDilpfilpYK4paQ4paMICAg4paQ4paMIOKWkOKWjOKWkOKWm+KWmuKWluKWkOKWjArilpDilowgICDilpDilpviloDilpzilozilpDilowg4pad4pac4paMCuKWneKWmuKWhOKWhOKWluKWkOKWjCDilpDilozilpDilowgIOKWkOKWjCh0KQ==")));
CANBus canBus = new CANBus(Constants.CANBUS_NAME);
CANBusStatus canInfo = canBus.getStatus();
System.out.println("CANInfo BusOffCount - " + canInfo.BusOffCount);
System.out.println("CANInfo BusUtilization - " + canInfo.BusUtilization);
System.out.println("CANInfo RX Errors count - " + canInfo.REC);
System.out.println("CANInfo TX Errors count - " + canInfo.TEC);
System.out.println("CANInfo Transmit buffer full count - " + canInfo.TxFullCount);
// Broken turniary operator
ReportLevel canReportLevel = canInfo.Status.isOK() ? (canInfo.Status.isWarning() ? ReportLevel.WARNING : ReportLevel.ERROR) : ReportLevel.INFO;
String canStatus = "CAN " + canReportLevel.name() + " - " + canInfo.Status.getName() + " (" + canInfo.Status.getDescription() + ")";
if(canReportLevel == ReportLevel.ERROR) {
errors.add(canStatus);
}
System.out.println(canStatus);
for(int i=0;i<CanDevice.devices.size();i++){
CanDevice device = CanDevice.devices.get(i);
System.out.println("** CAN diagnostic report for " + device.name + ":");
Status status = device.diagnosticStatus();
for(int a=0;a<status.reports.size();a++){
Report r = status.reports.get(a);
if(r.reportLevel == ReportLevel.ERROR)
errors.add(device.getName() + " - " + r.toString());
device.Log(r.toString());
}
}
// System.out.println("Found CAN devices: " + new DeviceFinder().Find());
if(errors.size() > 0) {
// Errors header
System.out.println(new String(Base64.getDecoder().decode("4paX4paE4paE4paE4paW4paX4paE4paE4paWIOKWl+KWhOKWhOKWliAg4paX4paE4paWIOKWl+KWhOKWhOKWliAg4paX4paE4paE4paWCuKWkOKWjCAgIOKWkOKWjCDilpDilozilpDilowg4paQ4paM4paQ4paMIOKWkOKWjOKWkOKWjCDilpDilozilpDilowgICAK4paQ4pab4paA4paA4paY4paQ4pab4paA4paa4paW4paQ4pab4paA4paa4paW4paQ4paMIOKWkOKWjOKWkOKWm+KWgOKWmuKWliDilp3iloDilprilpYK4paQ4paZ4paE4paE4paW4paQ4paMIOKWkOKWjOKWkOKWjCDilpDilozilp3ilpriloTilp7ilpjilpDilowg4paQ4paM4paX4paE4paE4pae4paY")));
for(int i=0;i<errors.size(); i++){
System.out.println(errors.get(i));
}
}
FaultReporter.printReport();
}
}
// VisionSystemSim visionSim;
// RobotMapSim robotMapSim;
// @Override
// public void simulationInit() {
// visionSim = new VisionSystemSim("main");
// robotMapSim = m_robotContainer.m_robotMap.configureSim();
// // A 0.5 x 0.25 meter rectangular target
// TargetModel targetModel = new TargetModel(0.5, 0.25);
// // The pose of where the target is on the field.
// // Its rotation determines where "forward" or the target x-axis points.
// // Let's say this target is flat against the far wall center, facing the blue driver stations.
// Pose3d targetPose = new Pose3d(16, 4, 2, new Rotation3d(0, 0, Math.PI));
// // The given target model at the given pose
// VisionTargetSim visionTarget = new VisionTargetSim(targetPose, targetModel);
// // Add this vision target to the vision system simulation to make it visible
// visionSim.addVisionTargets(visionTarget);
// // The layout of AprilTags which we want to add to the vision system
// AprilTagFieldLayout tagLayout = Constants.FieldConstants.kTagLayout;
// visionSim.addAprilTags(tagLayout);
// visionSim.addCamera(robotMapSim.leftCamera, Constants.VisionConstants.LEFT_CAMERA_POS);
// visionSim.addCamera(robotMapSim.rightCamera, Constants.VisionConstants.RIGHT_CAMERA_POS);
// SmartDashboard.putData(visionSim.getDebugField());
// }
// @Override
// public void simulationPeriodic() {
// m_robotContainer.m_robotSwerveDrive.updateSim(RobotController.getBatteryVoltage());
// // visionSim.update(m_robotContainer.m_robotSwerveDrive.getPose2d());
// // m_robotContainer.m_robotSwerveDrive.
// }
// Start AdvantageKit logging / replay and record metadata
// Refrence: https://github.com/Mechanical-Advantage/AdvantageKit/blob/main/template_projects/sources/skeleton/src/main/java/frc/robot/Robot.java
public void startLogging() {
// Record metadata
Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME);
Logger.recordMetadata("BuildDate", BuildConstants.BUILD_DATE);
Logger.recordMetadata("Git+SHA", BuildConstants.GIT_SHA);
Logger.recordMetadata("GitDate", BuildConstants.GIT_DATE);
Logger.recordMetadata("GitBranch", BuildConstants.GIT_BRANCH);
switch (BuildConstants.DIRTY) {
case 0:
Logger.recordMetadata("GitDirty", "All changes committed");
break;
case 1:
Logger.recordMetadata("GitDirty", "Uncomitted changes");
break;
default:
Logger.recordMetadata("GitDirty", "Unknown");
break;
}
// Set up data receivers & replay source
switch (SimConstants.currentMode) {
case REAL:
// Running on a real robot, log to a USB stick ("/U/logs")
Logger.addDataReceiver(new WPILOGWriter());
Logger.addDataReceiver(new NT4Publisher());
break;
case SIM:
// Running a physics simulator, log to NT
Logger.addDataReceiver(new NT4Publisher());
break;
case REPLAY:
// Replaying a log, set up replay source
setUseTiming(false); // Run as fast as possible
String logPath = LogFileUtil.findReplayLog();
Logger.setReplaySource(new WPILOGReader(logPath));
Logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim")));
break;
}
// Start AdvantageKit logger
Logger.start();
}
}
src/main/java/frc4388/robot/RobotContainer.java
+83 -102
View File
@@ -11,34 +11,22 @@ package frc4388.robot;
import edu.wpi.first.wpilibj.DriverStation;
import java.io.File;
import java.util.ArrayList;
import java.util.List;
import java.util.function.BooleanSupplier;
import java.util.function.Supplier;
import edu.wpi.first.cameraserver.CameraServer;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.RobotBase;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import frc4388.utility.controller.XboxController;
import frc4388.utility.controller.ButtonBox;
import frc4388.utility.controller.DeadbandedXboxController;
import frc4388.robot.Constants.FieldConstants;
import frc4388.robot.Constants.LiDARConstants;
import frc4388.robot.Constants.OIConstants;
import frc4388.robot.Constants.SwerveDriveConstants;
import frc4388.robot.Constants.AutoConstants;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import edu.wpi.first.wpilibj2.command.button.Trigger;
// Commands
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup;
import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
@@ -47,72 +35,63 @@ import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.ConditionalCommand;
// Autos
import frc4388.utility.controller.VirtualController;
import frc4388.robot.commands.DriveUntilLiDAR;
import frc4388.robot.commands.GotoLastApril;
import frc4388.robot.commands.LidarAlign;
import frc4388.robot.commands.MoveForTimeCommand;
import frc4388.robot.commands.MoveUntilSuply;
import frc4388.robot.commands.WhileTrueCommand;
import frc4388.robot.commands.waitElevatorRefrence;
import frc4388.robot.commands.waitEndefectorRefrence;
import frc4388.robot.commands.waitFeedCoral;
import frc4388.robot.commands.waitSupplier;
import frc4388.robot.commands.Swerve.neoJoystickPlayback;
import frc4388.robot.commands.Swerve.neoJoystickRecorder;
import com.pathplanner.lib.auto.AutoBuilder;
import frc4388.robot.commands.alignment.DriveToReef;
import frc4388.robot.commands.alignment.DriveUntilLiDAR;
import frc4388.robot.commands.alignment.LidarAlign;
import frc4388.robot.commands.wait.waitElevatorRefrence;
import frc4388.robot.commands.wait.waitEndefectorRefrence;
import frc4388.robot.commands.wait.waitFeedCoral;
import frc4388.robot.commands.wait.waitSupplier;
import frc4388.robot.constants.Constants;
import frc4388.robot.constants.Constants.AutoConstants;
import frc4388.robot.constants.Constants.LiDARConstants;
import frc4388.robot.constants.Constants.OIConstants;
import frc4388.robot.constants.Constants.SimConstants.Mode;
import com.pathplanner.lib.auto.NamedCommands;
import com.pathplanner.lib.commands.PathPlannerAuto;
// Subsystems
import frc4388.robot.subsystems.LED;
import frc4388.robot.subsystems.Vision;
import frc4388.robot.subsystems.Elevator.CoordinationState;
import frc4388.robot.subsystems.Lidar;
import frc4388.robot.subsystems.Elevator;
// import frc4388.robot.subsystems.Endeffector;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.robot.subsystems.elevator.Elevator;
import frc4388.robot.subsystems.elevator.Elevator.CoordinationState;
import frc4388.robot.subsystems.lidar.LiDAR;
import frc4388.robot.subsystems.swerve.SwerveDrive;
import frc4388.robot.subsystems.vision.Vision;
// Utilites
import frc4388.utility.DeferredBlock;
import frc4388.utility.DeferredBlockMulti;
import frc4388.utility.ReefPositionHelper;
import frc4388.utility.Subsystem;
import frc4388.utility.TimesNegativeOne;
import frc4388.utility.ReefPositionHelper.Side;
import frc4388.utility.configurable.ConfigurableString;
import frc4388.utility.compute.TimesNegativeOne;
import frc4388.utility.compute.ReefPositionHelper.Side;
/**
* This class is where the bulk of the robot should be declared. Since
* Command-based is a "declarative" paradigm, very little robot logic should
* actually be handled in the {@link Robot} periodic methods (other than the
* scheduler calls). Instead, the structure of the robot (including subsystems,
* scheduler calls). Instead, the structure of the robot (2including subsystems,
* commands, and button mappings) should be declared here.
*/
public class RobotContainer {
/* RobotMap */
public final RobotMap m_robotMap = new RobotMap();
public final RobotMap m_robotMap = new RobotMap(Mode.REAL);
/* Subsystems */
public final LED m_robotLED = new LED();
public final Vision m_vision = new Vision(m_robotMap.leftCamera, m_robotMap.rightCamera);
public final Lidar m_reefLidar = new Lidar(LiDARConstants.REEF_LIDAR_DIO_CHANNEL, "Reef");
public final Lidar m_reverseLidar = new Lidar(LiDARConstants.REVERSE_LIDAR_DIO_CHANNEL, "Reverse");
public final Elevator m_robotElevator = new Elevator(m_robotMap.elevator, m_robotMap.endeffector, m_robotMap.basinLimitSwitch, m_robotMap.endeffectorLimitSwitch, m_robotMap.IRIntakeBeam, m_robotLED);
public final Elevator m_robotElevator = new Elevator(m_robotMap.elevatorIO, m_robotLED);
public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision);
// public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain);
public final LiDAR reefLidar = new LiDAR(m_robotMap.reefLidar, "Reef");
public final LiDAR reverseLidar = new LiDAR(m_robotMap.reverseLidar, "Reverse");
/* Controllers */
private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
private final ButtonBox m_buttonBox = new ButtonBox(OIConstants.BUTTONBOX_ID);
private final DeadbandedXboxController m_autoRecorderXbox = new DeadbandedXboxController(OIConstants.XBOX_PROGRAMMER_ID);
/* Virtual Controllers */
private final VirtualController m_virtualDriver = new VirtualController(0);
private final VirtualController m_virtualOperator = new VirtualController(1);
// public List<Subsystem> subsystems = new ArrayList<>();
@@ -124,7 +103,6 @@ public class RobotContainer {
}
// ! /* Autos */
private String lastAutoName = "defualt.auto";
private SendableChooser<String> autoChooser;
private Command autoCommand;
@@ -144,7 +122,7 @@ public class RobotContainer {
new ConditionalCommand(Commands.none(), new SequentialCommandGroup(
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.RIGHT, true)
new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.RIGHT, true)
), () -> m_robotElevator.isL4Primed()),
// new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
@@ -153,9 +131,9 @@ public class RobotContainer {
new waitEndefectorRefrence(m_robotElevator),
new waitElevatorRefrence(m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.RIGHT, true),
new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.RIGHT, true),
waitDebuger.asProxy(),
new LidarAlign(m_robotSwerveDrive, m_reefLidar),
new LidarAlign(m_robotSwerveDrive, reefLidar),
waitDebuger.asProxy(),
new ParallelRaceGroup(
new WaitCommand(1),
@@ -186,7 +164,7 @@ public class RobotContainer {
new ConditionalCommand(Commands.none(), new SequentialCommandGroup(
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.RIGHT, true)
new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.RIGHT, true)
), () -> m_robotElevator.isL4Primed()),
// new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
@@ -195,9 +173,9 @@ public class RobotContainer {
new waitEndefectorRefrence(m_robotElevator),
new waitElevatorRefrence(m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.RIGHT, true),
new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.RIGHT, true),
waitDebuger.asProxy(),
// new LidarAlign(m_robotSwerveDrive, m_reefLidar),
// new LidarAlign(m_robotSwerveDrive, m_robotMap.reefLidar),
// waitDebuger.asProxy(),
// new ParallelRaceGroup(
// new WaitCommand(1),
@@ -243,7 +221,7 @@ public class RobotContainer {
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.RIGHT, true),
waitDebuger.asProxy(),
new LidarAlign(m_robotSwerveDrive, m_reefLidar),
new LidarAlign(m_robotSwerveDrive, m_robotMap.reefLidar),
waitDebuger.asProxy(),
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator),
@@ -279,7 +257,7 @@ public class RobotContainer {
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.LEFT, true),
waitDebuger.asProxy(),
new LidarAlign(m_robotSwerveDrive, m_reefLidar),
new LidarAlign(m_robotSwerveDrive, m_robotMap.reefLidar),
waitDebuger.asProxy(),
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator),
@@ -307,7 +285,7 @@ public class RobotContainer {
new ConditionalCommand(Commands.none(), new SequentialCommandGroup(
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.LEFT, true)
new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.LEFT, true)
), () -> m_robotElevator.isL4Primed()),
// new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
@@ -316,9 +294,9 @@ public class RobotContainer {
new waitEndefectorRefrence(m_robotElevator),
new waitElevatorRefrence(m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.LEFT, true),
new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.LEFT, true),
waitDebuger.asProxy(),
new LidarAlign(m_robotSwerveDrive, m_reefLidar),
new LidarAlign(m_robotSwerveDrive, reefLidar),
waitDebuger.asProxy(),
new ParallelRaceGroup(
new WaitCommand(1),
@@ -354,7 +332,7 @@ public class RobotContainer {
new ConditionalCommand(Commands.none(), new SequentialCommandGroup(
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.LEFT, true)
new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.LEFT, true)
), () -> m_robotElevator.isL4Primed()),
// new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
@@ -363,9 +341,9 @@ public class RobotContainer {
new waitEndefectorRefrence(m_robotElevator),
new waitElevatorRefrence(m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.LEFT, true),
new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.LEFT, true),
waitDebuger.asProxy(),
new LidarAlign(m_robotSwerveDrive, m_reefLidar),
new LidarAlign(m_robotSwerveDrive, reefLidar),
// waitDebuger.asProxy(),
// new ParallelRaceGroup(
// new WaitCommand(1),
@@ -398,15 +376,15 @@ public class RobotContainer {
new ConditionalCommand(Commands.none(), new SequentialCommandGroup(
// new IfCommand(() -> m_robotElevator.isL3Primed(), new SequentialCommandGroup(
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_PREP, Side.LEFT, true)
new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_PREP, Side.LEFT, true)
), () -> m_robotElevator.isL3Primed()),
new waitEndefectorRefrence(m_robotElevator),
new waitElevatorRefrence(m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_SCORE-Units.inchesToMeters(1), Side.LEFT, true),
new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_SCORE-Units.inchesToMeters(1), Side.LEFT, true),
waitDebuger.asProxy(),
new LidarAlign(m_robotSwerveDrive, m_reefLidar),
new LidarAlign(m_robotSwerveDrive, reefLidar),
waitDebuger.asProxy(),
// new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringThree), m_robotElevator),
new waitEndefectorRefrence(m_robotElevator),
@@ -423,7 +401,7 @@ public class RobotContainer {
new ConditionalCommand(Commands.none(), new SequentialCommandGroup(
// new IfCommand(() -> m_robotElevator.isL3Primed(), new SequentialCommandGroup(
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_PREP, Side.RIGHT, true)
new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_PREP, Side.RIGHT, true)
),() -> m_robotElevator.isL3Primed()),
// new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator),
@@ -432,10 +410,10 @@ public class RobotContainer {
new waitEndefectorRefrence(m_robotElevator),
new waitElevatorRefrence(m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_SCORE, Side.RIGHT, true),
new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_SCORE, Side.RIGHT, true),
waitDebuger.asProxy(),
new LidarAlign(m_robotSwerveDrive, m_reefLidar),
new LidarAlign(m_robotSwerveDrive, reefLidar),
waitDebuger.asProxy(),
// new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringThree), m_robotElevator),
@@ -450,12 +428,12 @@ public class RobotContainer {
private Command AprilLidarAlignL2Left = new SequentialCommandGroup(
new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L2_PREP_DISTANCE, Side.LEFT, true),
new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L2_PREP_DISTANCE, Side.LEFT, true),
waitDebuger.asProxy(),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L2_SCORE_DISTANCE, Side.LEFT, true),
new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L2_SCORE_DISTANCE, Side.LEFT, true),
waitDebuger.asProxy(),
new LidarAlign(m_robotSwerveDrive, m_reefLidar),
new LidarAlign(m_robotSwerveDrive, reefLidar),
waitDebuger.asProxy(),
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.L2Score);}, m_robotElevator),
new waitEndefectorRefrence(m_robotElevator),
@@ -471,12 +449,12 @@ public class RobotContainer {
private Command AprilLidarAlignL2Right = new SequentialCommandGroup(
new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L2_PREP_DISTANCE, Side.RIGHT, true),
new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L2_PREP_DISTANCE, Side.RIGHT, true),
waitDebuger.asProxy(),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L2_SCORE_DISTANCE, Side.RIGHT, true),
new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L2_SCORE_DISTANCE, Side.RIGHT, true),
waitDebuger.asProxy(),
new LidarAlign(m_robotSwerveDrive, m_reefLidar),
new LidarAlign(m_robotSwerveDrive, reefLidar),
waitDebuger.asProxy(),
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.L2Score);}, m_robotElevator),
new waitEndefectorRefrence(m_robotElevator),
@@ -495,7 +473,7 @@ public class RobotContainer {
// new waitEndefectorRefrence(m_robotElevator),
// new waitElevatorRefrence(m_robotElevator),
// new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE, Side.LEFT, false),
// new LidarAlign(m_robotSwerveDrive, m_reefLidar),
// new LidarAlign(m_robotSwerveDrive, m_robotMap.reefLidar),
// new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Go);}, m_robotElevator),
// new MoveForTimeCommand(m_robotSwerveDrive,
// new Translation2d(0,1), new Translation2d(), AutoConstants.ALGAE_DRIVE_TIME, true),
@@ -506,10 +484,10 @@ public class RobotContainer {
private Command lowerAlgaeRemove = new SequentialCommandGroup(
new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Primed);}, m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE + Units.inchesToMeters(8), Side.FAR_LEFT, true),
new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE + Units.inchesToMeters(8), Side.FAR_LEFT, true),
new waitEndefectorRefrence(m_robotElevator),
new waitElevatorRefrence(m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE, Side.FAR_LEFT, true),
new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE, Side.FAR_LEFT, true),
waitDebuger.asProxy(),
// new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Go);}, m_robotElevator),
new MoveForTimeCommand(m_robotSwerveDrive,
@@ -526,7 +504,7 @@ public class RobotContainer {
// new waitEndefectorRefrence(m_robotElevator),
// new waitElevatorRefrence(m_robotElevator),
// new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE, Side.LEFT, false),
// new LidarAlign(m_robotSwerveDrive, m_reefLidar),
// new LidarAlign(m_robotSwerveDrive, m_robotMap.reefLidar),
// new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL3Go);}, m_robotElevator),
// new MoveForTimeCommand(m_robotSwerveDrive,
// new Translation2d(1,1), new Translation2d(), AutoConstants.ALGAE_DRIVE_TIME, true),
@@ -536,10 +514,10 @@ public class RobotContainer {
private Command upperAlgaeRemove = new SequentialCommandGroup(
new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL3Primed);}, m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE + Units.inchesToMeters(8), Side.FAR_LEFT, true),
new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE + Units.inchesToMeters(8), Side.FAR_LEFT, true),
new waitEndefectorRefrence(m_robotElevator),
new waitElevatorRefrence(m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE, Side.FAR_LEFT, true),
new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE, Side.FAR_LEFT, true),
waitDebuger.asProxy(),
// new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Go);}, m_robotElevator),
new MoveForTimeCommand(m_robotSwerveDrive,
@@ -584,9 +562,9 @@ public class RobotContainer {
NamedCommands.registerCommand("await-coral", new waitFeedCoral(m_robotElevator));
NamedCommands.registerCommand("feed-driveback", new DriveUntilLiDAR(m_robotSwerveDrive,
new Translation2d(-1,0), new Translation2d(), m_reverseLidar, LiDARConstants.HUMAN_PLAYER_STATION_DISTANCE, true));
new Translation2d(-1,0), new Translation2d(), reverseLidar, LiDARConstants.HUMAN_PLAYER_STATION_DISTANCE));
// NamedCommands.registerCommand("feed-driveback", Commands.none());
NamedCommands.registerCommand("stop", new waitSupplier(() -> m_robotSwerveDrive.lastOdomSpeed < AutoConstants.STOP_VELOCITY));
NamedCommands.registerCommand("stop", new waitSupplier(() -> m_robotSwerveDrive.isStopped()));
NamedCommands.registerCommand("align-feed", new SequentialCommandGroup(
new MoveForTimeCommand(m_robotSwerveDrive,
@@ -616,12 +594,14 @@ public class RobotContainer {
configureButtonBindings();
configureVirtualButtonBindings();
new DeferredBlock(() -> { // Called on first robot enable
DeferredBlock.addBlock(() -> { // Called on first robot enable
m_robotSwerveDrive.resetGyro();
});
new DeferredBlockMulti(() -> { // Called on every robot enable
}, false);
DeferredBlock.addBlock(() -> { // Called on every robot enable
TimesNegativeOne.update();
});
}, true);
DriverStation.silenceJoystickConnectionWarning(true);
// CameraServer.startAutomaticCapture();
@@ -688,7 +668,7 @@ public class RobotContainer {
private void configureButtonBindings() {
// ? /* Driver Buttons */
DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.A_BUTTON)
new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON)
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro()));
// ! /* Speed */
@@ -739,21 +719,21 @@ public class RobotContainer {
new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
// .onTrue( new DriveUntilLiDAR(m_robotSwerveDrive,
// new Translation2d(-1,0), new Translation2d(), m_reverseLidar, LiDARConstants.HUMAN_PLAYER_STATION_DISTANCE, true));
// new Translation2d(-1,0), new Translation2d(), m_robotMap.reverseLidar, LiDARConstants.HUMAN_PLAYER_STATION_DISTANCE, true));
.onTrue(WannaSeeMeDunk.asProxy());
new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
.onTrue(thrustIntake.asProxy());
new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON)
.onTrue(new InstantCommand(() -> {m_robotSwerveDrive.softStop();}, m_robotSwerveDrive, m_reefLidar));
.onTrue(new InstantCommand(() -> {m_robotSwerveDrive.softStop();}, m_robotSwerveDrive, reefLidar));
// ? /* Operator Buttons */
DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.B_BUTTON)
new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON)
.onTrue(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator));
DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.A_BUTTON)
new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON)
.onTrue(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Ready), m_robotElevator));
// Button box
@@ -914,11 +894,19 @@ public class RobotContainer {
// return new GotoPositionCommand(m_robotSwerveDrive, m_vision, AutoConstants.targetpos);
}
public boolean autoChooserUpdated = false;
public void makeAutoChooser() {
autoChooser = new SendableChooser<String>();
File dir = new File("/home/lvuser/deploy/pathplanner/autos/");
// File dir = new File("C:\\Users\\Ridgebotics\\Documents\\GitHub\\2025RidgeScape\\src\\main\\deploy\\pathplanner\\autos\\");
File dir;
if(RobotBase.isReal()) {
dir = new File("/home/lvuser/deploy/pathplanner/autos/");
} else {
// dir = new File("C:\\Users\\Ridgebotics\\Documents\\GitHub\\2025RidgeScape\\src\\main\\deploy\\pathplanner\\autos\\");
dir = new File("/home/astatin3/Documents/GitHub/2025RidgeScape/src/main/deploy/pathplanner/autos");
}
String[] autos = dir.list();
if(autos == null) return;
@@ -930,6 +918,7 @@ public class RobotContainer {
}
autoChooser.onChange((filename) -> {
autoChooserUpdated = true;
if (filename.equals("Taxi")) {
autoCommand = new SequentialCommandGroup(
new MoveForTimeCommand(m_robotSwerveDrive,
@@ -966,12 +955,4 @@ public class RobotContainer {
public ButtonBox getButtonBox() {
return this.m_buttonBox;
}
public VirtualController getVirtualDriverController() {
return m_virtualDriver;
}
public VirtualController getVirtualOperatorController() {
return m_virtualOperator;
}
}
src/main/java/frc4388/robot/RobotMap.java
+139 -32
View File
@@ -10,22 +10,31 @@ package frc4388.robot;
import com.ctre.phoenix6.hardware.TalonFX;
import org.photonvision.PhotonCamera;
import org.photonvision.simulation.PhotonCameraSim;
import org.photonvision.simulation.SimCameraProperties;
import com.ctre.phoenix6.hardware.CANcoder;
import com.ctre.phoenix6.hardware.Pigeon2;
import com.ctre.phoenix6.swerve.SwerveDrivetrain;
import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants;
import com.ctre.phoenix6.swerve.SwerveModuleConstants;
import com.ctre.phoenix6.swerve.SwerveModuleConstantsFactory;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj.DigitalInput;
import frc4388.robot.Constants.ElevatorConstants;
// import edu.wpi.first.wpilibj.motorcontrol.Spark;
// import frc4388.robot.Constants.LEDConstants;
import frc4388.robot.Constants.SwerveDriveConstants;
import frc4388.robot.Constants.VisionConstants;
// import frc4388.robot.subsystems.SwerveModule;
import frc4388.utility.RobotGyro;
import frc4388.robot.constants.Constants.ElevatorConstants;
import frc4388.robot.constants.Constants.LiDARConstants;
import frc4388.robot.constants.Constants.SimConstants;
import frc4388.robot.constants.Constants.VisionConstants;
import frc4388.robot.subsystems.elevator.ElevatorIO;
import frc4388.robot.subsystems.elevator.ElevatorReal;
import frc4388.robot.subsystems.lidar.LiDAR;
import frc4388.robot.subsystems.lidar.LidarIO;
import frc4388.robot.subsystems.lidar.LidarReal;
import frc4388.robot.subsystems.swerve.SwerveDriveConstants;
import frc4388.robot.subsystems.swerve.SwerveIO;
import frc4388.robot.subsystems.swerve.SwerveReal;
import frc4388.robot.subsystems.vision.VisionIO;
import frc4388.robot.subsystems.vision.VisionReal;
import frc4388.utility.status.FaultCANCoder;
import frc4388.utility.status.FaultPhotonCamera;
import frc4388.utility.status.FaultPidgeon2;
import frc4388.utility.status.FaultTalonFX;
/**
* Defines and holds all I/O objects on the Roborio. This is useful for unit
@@ -35,34 +44,132 @@ public class RobotMap {
// private Pigeon2 m_pigeon2 = new Pigeon2(SwerveDriveConstants.IDs.DRIVE_PIGEON.id);
// public RobotGyro gyro = new RobotGyro(m_pigeon2);
public PhotonCamera leftCamera = new PhotonCamera(VisionConstants.LEFT_CAMERA_NAME);
public PhotonCamera rightCamera = new PhotonCamera(VisionConstants.RIGHT_CAMERA_NAME);
public RobotMap() {
configureDriveMotorControllers();
}
public final VisionIO leftCamera;
public final VisionIO rightCamera;
// public final LiDAR lidar = new
public final LidarIO reefLidar;
public final LidarIO reverseLidar;
/* LED Subsystem */
// public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID);
/* Swreve Drive Subsystem */
public final SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDrivetrain = new SwerveDrivetrain<TalonFX, TalonFX, CANcoder> (TalonFX::new, TalonFX::new, CANcoder::new,
Constants.SwerveDriveConstants.DrivetrainConstants,
Constants.SwerveDriveConstants.FRONT_LEFT, Constants.SwerveDriveConstants.FRONT_RIGHT,
Constants.SwerveDriveConstants.BACK_LEFT, Constants.SwerveDriveConstants.BACK_RIGHT
);
public final SwerveIO swerveDrivetrain;
/* Elevator Subsystem */
public final TalonFX elevator = new TalonFX(ElevatorConstants.ELEVATOR_ID.id);
public final TalonFX endeffector = new TalonFX(ElevatorConstants.ENDEFFECTOR_ID.id);
public final ElevatorIO elevatorIO;
public final DigitalInput basinLimitSwitch = new DigitalInput(ElevatorConstants.BASIN_LIMIT_SWITCH);
public final DigitalInput endeffectorLimitSwitch = new DigitalInput(ElevatorConstants.ENDEFFECTOR_LIMIT_SWITCH);
public final DigitalInput IRIntakeBeam = new DigitalInput(ElevatorConstants.INTAKE_LIMIT_SWITCH);
public RobotMap(SimConstants.Mode mode) {
switch (mode) {
case REAL:
// Configure cameras
PhotonCamera leftCameraReal = new PhotonCamera(VisionConstants.LEFT_CAMERA_NAME);
PhotonCamera rightCameraReal = new PhotonCamera(VisionConstants.RIGHT_CAMERA_NAME);
void configureDriveMotorControllers() {
// endeffector.saf
leftCamera = new VisionReal(leftCameraReal, VisionConstants.LEFT_CAMERA_POS); ;
rightCamera = new VisionReal(rightCameraReal, VisionConstants.RIGHT_CAMERA_POS);
FaultPhotonCamera.addDevice(leftCameraReal, "Left Camera");
FaultPhotonCamera.addDevice(rightCameraReal , "Right Camera");
// Configure LiDAR
reefLidar = new LidarReal(LiDARConstants.REEF_LIDAR_DIO_CHANNEL);
reverseLidar = new LidarReal(LiDARConstants.REVERSE_LIDAR_DIO_CHANNEL);
// Configure swerve drive train
SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDrivetrainReal = new SwerveDrivetrain<TalonFX, TalonFX, CANcoder> (TalonFX::new, TalonFX::new, CANcoder::new,
SwerveDriveConstants.DrivetrainConstants,
SwerveDriveConstants.FRONT_LEFT, SwerveDriveConstants.FRONT_RIGHT,
SwerveDriveConstants.BACK_LEFT, SwerveDriveConstants.BACK_RIGHT
);
swerveDrivetrain = new SwerveReal(swerveDrivetrainReal);
// Configure elevator
TalonFX elevator = new TalonFX(ElevatorConstants.ELEVATOR_ID.id);
TalonFX endeffector = new TalonFX(ElevatorConstants.ENDEFFECTOR_ID.id);
DigitalInput basinLimitSwitch = new DigitalInput(ElevatorConstants.BASIN_LIMIT_SWITCH);
DigitalInput endeffectorLimitSwitch = new DigitalInput(ElevatorConstants.ENDEFFECTOR_LIMIT_SWITCH);
DigitalInput IRIntakeBeam = new DigitalInput(ElevatorConstants.INTAKE_LIMIT_SWITCH);
elevatorIO = new ElevatorReal(elevator, endeffector, basinLimitSwitch, endeffectorLimitSwitch, IRIntakeBeam);
// Fault
FaultPidgeon2.addDevice(swerveDrivetrainReal.getPigeon2(), "Gyro");
FaultTalonFX.addDevice(elevator, "Elevator");
FaultTalonFX.addDevice(endeffector, "Endeffector");
FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(0).getDriveMotor(), "Module 0 Drive");
FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(0).getSteerMotor(), "Module 0 Steer");
FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(0).getEncoder(), "Module 0 CANCoder");
FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(1).getDriveMotor(), "Module 1 Drive");
FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(1).getSteerMotor(), "Module 1 Steer");
FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(1).getEncoder(), "Module 1 CANCoder");
FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(2).getDriveMotor(), "Module 2 Drive");
FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(2).getSteerMotor(), "Module 2 Steer");
FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(2).getEncoder(), "Module 2 CANCoder");
FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(3).getDriveMotor(), "Module 3 Drive");
FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(3).getSteerMotor(), "Module 3 Steer");
FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(3).getEncoder(), "Module 3 CANCoder");
break;
// case SIM:
// break;
default:
leftCamera = new VisionIO() {};
rightCamera = new VisionIO() {};
reefLidar = new LidarIO() {};
reverseLidar = new LidarIO() {};
swerveDrivetrain = new SwerveIO() {};
elevatorIO = new ElevatorIO() {};
break;
}
}
// public class RobotMapSim {
// public PhotonCameraSim leftCamera;
// public PhotonCameraSim rightCamera;
// }
// public RobotMapSim configureSim() {
// RobotMapSim sim = new RobotMapSim();
// // The simulated camera properties
// SimCameraProperties cameraProp = new SimCameraProperties();
// // A 640 x 480 camera with a 100 degree diagonal FOV.
// cameraProp.setCalibration(640, 480, Rotation2d.fromDegrees(100));
// // Approximate detection noise with average and standard deviation error in pixels.
// cameraProp.setCalibError(0.25, 0.08);
// // Set the camera image capture framerate (Note: this is limited by robot loop rate).
// cameraProp.setFPS(20);
// // The average and standard deviation in milliseconds of image data latency.
// cameraProp.setAvgLatencyMs(35);
// cameraProp.setLatencyStdDevMs(5);
// // sim.leftCamera = new PhotonCameraSim(leftCamera, cameraProp);
// // sim.rightCamera = new PhotonCameraSim(rightCamera, cameraProp);
}
// sim.leftCamera.enableRawStream(true);
// sim.leftCamera.enableProcessedStream(true);
// sim.leftCamera.enableDrawWireframe(true);
// sim.rightCamera.enableRawStream(true);
// sim.rightCamera.enableProcessedStream(true);
// sim.rightCamera.enableDrawWireframe(true);
// return sim;
// }
}
src/main/java/frc4388/robot/commands/MoveForTimeCommand.java
+3 -3
View File
@@ -4,8 +4,7 @@ import java.time.Instant;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj2.command.Command;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.utility.TimesNegativeOne;
import frc4388.robot.subsystems.swerve.SwerveDrive;
// Command to repeat a joystick movement for a specific time.
public class MoveForTimeCommand extends Command {
@@ -23,7 +22,8 @@ public class MoveForTimeCommand extends Command {
Translation2d rightStick,
long millis,
boolean robotRelative) {
addRequirements(swerveDrive);
addRequirements(swerveDrive);
this.swerveDrive = swerveDrive;
this.leftStick = leftStick;
src/main/java/frc4388/robot/commands/MoveUntilSuply.java
+1 -3
View File
@@ -1,12 +1,10 @@
package frc4388.robot.commands;
import java.time.Instant;
import java.util.function.Supplier;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj2.command.Command;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.utility.TimesNegativeOne;
import frc4388.robot.subsystems.swerve.SwerveDrive;
// Command to repeat a joystick movement for a specific time.
public class MoveUntilSuply extends Command {
src/main/java/frc4388/robot/commands/PID.java
+1 -1
View File
@@ -5,7 +5,7 @@
package frc4388.robot.commands;
import edu.wpi.first.wpilibj2.command.Command;
import frc4388.utility.Gains;
import frc4388.utility.structs.Gains;
public abstract class PID extends Command {
protected Gains gains;
src/main/java/frc4388/robot/commands/WhileTrueCommand.java
-1
View File
@@ -8,7 +8,6 @@ import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
import edu.wpi.first.util.sendable.SendableBuilder;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
import java.util.function.BooleanSupplier;
src/main/java/frc4388/robot/commands/GotoLastApril.java → src/main/java/frc4388/robot/commands/alignment/DriveToReef.java
+11 -76
View File
@@ -1,29 +1,19 @@
package frc4388.robot.commands;
import static frc4388.robot.Constants.OIConstants.LEFT_AXIS_DEADBAND;
import java.util.Optional;
package frc4388.robot.commands.alignment;
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.util.Units;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import frc4388.robot.Constants.AutoConstants;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.robot.subsystems.Vision;
import frc4388.utility.Gains;
import frc4388.utility.ReefPositionHelper;
import frc4388.utility.TimesNegativeOne;
import frc4388.utility.ReefPositionHelper.Side;
import frc4388.utility.UtilityStructs.AutoRecordingControllerFrame;
import frc4388.utility.UtilityStructs.AutoRecordingFrame;
import frc4388.utility.controller.VirtualController;
import frc4388.robot.constants.Constants.AutoConstants;
import frc4388.robot.subsystems.swerve.SwerveDrive;
import frc4388.robot.subsystems.vision.Vision;
import frc4388.utility.compute.ReefPositionHelper;
import frc4388.utility.compute.TimesNegativeOne;
import frc4388.utility.compute.ReefPositionHelper.Side;
import frc4388.utility.structs.Gains;
public class GotoLastApril extends Command {
public class DriveToReef extends Command {
// private Translation2d translation2d= new Translation2d(14.579471999999997,0.24587199999999998);
@@ -45,7 +35,7 @@ public class GotoLastApril extends Command {
* @param SwerveDrive m_robotSwerveDrive
*/
public GotoLastApril(SwerveDrive swerveDrive, Vision vision, double distance, Side side, boolean waitVelocityZero) {
public DriveToReef(SwerveDrive swerveDrive, Vision vision, double distance, Side side, boolean waitVelocityZero) {
this.swerveDrive = swerveDrive;
this.vision = vision;
this.distance = distance;
@@ -62,23 +52,6 @@ public class GotoLastApril extends Command {
@Override
public void initialize() {
// double kP = AutoConstants.P_XY_GAINS.get();
// double kI = AutoConstants.I_XY_GAINS.get();
// double kD = AutoConstants.D_XY_GAINS.get();
// xPID = new PID(new Gains(
// kP,
// kI,
// kD),
// 0);
// yPID = new PID(new Gains(
// kP,
// kI,
// kD),
// 0);
// System.out.println("kP: "+kP);
// System.out.println("kI: "+kI);
// System.out.println("kD: "+kD);
xPID.initialize();
yPID.initialize();
this.targetpos = ReefPositionHelper.getNearestPosition(
@@ -168,33 +141,6 @@ public class GotoLastApril extends Command {
return finished;
// this statement is a boolean in and of itself
}
// @Override
// public void end(boolean interrupted) {
// }
// @Override
// public double getError() {
// return e;
// }
// @Override
// public void runWithOutput(double output) {
// // TODO Auto-generated method stub
// Translation2d leftStick = new Translation2d(Math.max(Math.min(output, 1), -1),0);
// Translation2d rightStick = new Translation2d();
// // System.out.println("Output = " + -output);
// SmartDashboard.putNumber("PID Output", output);
// swerveDrive.driveWithInput(leftStick, rightStick, true);
// }
@@ -212,17 +158,11 @@ public class GotoLastApril extends Command {
private class PID {
protected Gains gains;
private double output = 0;
private double tolerance = 0;
/** Creates a new PelvicInflammatoryDisease. */
public PID(double kp, double ki, double kd, double kf, double tolerance) {
gains = new Gains(kp, ki, kd, kf, 0);
this.tolerance = tolerance;
}
public PID(Gains gains, double tolerance) {
this.gains = gains;
this.tolerance = tolerance;
}
// Called when the command is initially scheduled.
@@ -244,10 +184,5 @@ public class GotoLastApril extends Command {
return output;
}
// // Returns true when the command should end.
// public final boolean isFinished() {
// return Math.abs(getError()) < tolerance;
// }
}
}
src/main/java/frc4388/robot/commands/DriveUntilLiDAR.java → src/main/java/frc4388/robot/commands/alignment/DriveUntilLiDAR.java
+6 -12
View File
@@ -1,29 +1,24 @@
package frc4388.robot.commands;
import java.time.Instant;
package frc4388.robot.commands.alignment;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj2.command.Command;
import frc4388.robot.subsystems.Lidar;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.utility.TimesNegativeOne;
import frc4388.robot.subsystems.lidar.LiDAR;
import frc4388.robot.subsystems.swerve.SwerveDrive;
// Command to repeat a joystick movement for a specific time.
public class DriveUntilLiDAR extends Command {
private final SwerveDrive swerveDrive;
private final Translation2d leftStick;
private final Translation2d rightStick;
private final Lidar m_lidar;
private final LiDAR m_lidar;
private final double mindistance;
private final boolean robotRelative;
public DriveUntilLiDAR(
SwerveDrive swerveDrive,
Translation2d leftStick,
Translation2d rightStick,
Lidar lidar,
double mindistance,
boolean robotRelative) {
LiDAR lidar,
double mindistance) {
addRequirements(swerveDrive);
this.swerveDrive = swerveDrive;
@@ -31,7 +26,6 @@ public class DriveUntilLiDAR extends Command {
this.rightStick = rightStick;
this.m_lidar = lidar;
this.mindistance = mindistance;
this.robotRelative = robotRelative;
}
@Override
src/main/java/frc4388/robot/commands/LidarAlign.java → src/main/java/frc4388/robot/commands/alignment/LidarAlign.java
+6 -6
View File
@@ -2,19 +2,19 @@
// 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.commands;
package frc4388.robot.commands.alignment;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import frc4388.robot.subsystems.Lidar;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.robot.subsystems.lidar.LiDAR;
import frc4388.robot.subsystems.swerve.SwerveDrive;
/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
public class LidarAlign extends Command {
private SwerveDrive swerveDrive;
private Lidar lidar;
private LiDAR lidar;
private int currentFinderTick;
// private int tickFoundPipe;
@@ -26,7 +26,7 @@ public class LidarAlign extends Command {
// private final boolean constructedHeadedRight;
/** Creates a new LidarAlign. */
public LidarAlign(SwerveDrive swerveDrive, Lidar lidar) {//, boolean headedRight) {
public LidarAlign(SwerveDrive swerveDrive, LiDAR lidar) {//, boolean headedRight) {
// Use addRequirements() here to declare subsystem dependencies.
this.swerveDrive = swerveDrive;
@@ -41,7 +41,7 @@ public class LidarAlign extends Command {
this.currentFinderTick = 0;
this.speed = 0.4; // TODO: find good speed for this
this.foundReef = false;
this.headedRight = (GotoLastApril.tagRelativeXError < 0);
this.headedRight = (DriveToReef.tagRelativeXError < 0);
this.additionalDistance = 0;
this.bounces = 0;
}
src/main/java/frc4388/robot/commands/Autos/neo AutoRecoding format.txt → src/main/java/frc4388/robot/commands/autos/neo AutoRecoding format.txt
View File
src/main/java/frc4388/robot/commands/Autos/neoPlaybackChooser.java → src/main/java/frc4388/robot/commands/autos/neoPlaybackChooser.java
+1
View File
@@ -1,3 +1,4 @@
package frc4388.robot.commands.autos;
// package frc4388.robot.commands.Autos;
// import java.io.File;
src/main/java/frc4388/robot/commands/Swerve/RotateToAngle.java → src/main/java/frc4388/robot/commands/swerve/RotateToAngle.java
+2 -2
View File
@@ -2,11 +2,11 @@
// 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.commands.Swerve;
package frc4388.robot.commands.swerve;
import edu.wpi.first.math.geometry.Translation2d;
import frc4388.robot.commands.PID;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.robot.subsystems.swerve.SwerveDrive;
public class RotateToAngle extends PID {
src/main/java/frc4388/robot/commands/Swerve/neoJoystickPlayback.java → src/main/java/frc4388/robot/commands/swerve/neoJoystickPlayback.java
+9 -9
View File
@@ -1,4 +1,4 @@
package frc4388.robot.commands.Swerve;
package frc4388.robot.commands.swerve;
import java.io.FileInputStream;
import java.util.ArrayList;
@@ -6,11 +6,11 @@ import java.util.function.Supplier;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj2.command.Command;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.utility.DataUtils;
import frc4388.utility.UtilityStructs.AutoRecordingControllerFrame;
import frc4388.utility.UtilityStructs.AutoRecordingFrame;
import frc4388.robot.subsystems.swerve.SwerveDrive;
import frc4388.utility.compute.DataUtils;
import frc4388.utility.controller.VirtualController;
import frc4388.utility.structs.UtilityStructs.AutoRecordingControllerFrame;
import frc4388.utility.structs.UtilityStructs.AutoRecordingFrame;
/**
@@ -24,8 +24,8 @@ public class neoJoystickPlayback extends Command {
private final Supplier<String> filenameGetter;
private String filename;
private int frame_index = 0;
private long startTime = 0;
private long playbackTime = 0;
// private long startTime = 0;
// private long playbackTime = 0;
private boolean m_finished = false; // ! There is no better way.
private boolean m_shouldfree = false; // should free memory on ending
@@ -150,8 +150,8 @@ public class neoJoystickPlayback extends Command {
@Override
public void initialize() {
startTime = System.currentTimeMillis();
playbackTime = 0;
// startTime = System.currentTimeMillis();
// playbackTime = 0;
frame_index = 0;
m_finished = !loadAuto();
src/main/java/frc4388/robot/commands/Swerve/neoJoystickRecorder.java → src/main/java/frc4388/robot/commands/swerve/neoJoystickRecorder.java
+5 -5
View File
@@ -1,4 +1,4 @@
package frc4388.robot.commands.Swerve;
package frc4388.robot.commands.swerve;
import java.io.FileOutputStream;
import java.util.ArrayList;
@@ -7,11 +7,11 @@ import java.util.function.Supplier;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj2.command.Command;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.utility.DataUtils;
import frc4388.utility.UtilityStructs.AutoRecordingControllerFrame;
import frc4388.utility.UtilityStructs.AutoRecordingFrame;
import frc4388.robot.subsystems.swerve.SwerveDrive;
import frc4388.utility.compute.DataUtils;
import frc4388.utility.controller.DeadbandedXboxController;
import frc4388.utility.structs.UtilityStructs.AutoRecordingControllerFrame;
import frc4388.utility.structs.UtilityStructs.AutoRecordingFrame;
/**
* The NEO autonomus recording system, designed based the old {@link JoystickRecorder} System but with {@link frc4388.utility.controller.VirtualController VirtualController}s
src/main/java/frc4388/robot/commands/waitElevatorRefrence.java → src/main/java/frc4388/robot/commands/wait/waitElevatorRefrence.java
+2 -2
View File
@@ -2,10 +2,10 @@
// 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.commands;
package frc4388.robot.commands.wait;
import edu.wpi.first.wpilibj2.command.Command;
import frc4388.robot.subsystems.Elevator;
import frc4388.robot.subsystems.elevator.Elevator;
/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
public class waitElevatorRefrence extends Command {
src/main/java/frc4388/robot/commands/waitEndefectorRefrence.java → src/main/java/frc4388/robot/commands/wait/waitEndefectorRefrence.java
+2 -2
View File
@@ -2,10 +2,10 @@
// 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.commands;
package frc4388.robot.commands.wait;
import edu.wpi.first.wpilibj2.command.Command;
import frc4388.robot.subsystems.Elevator;
import frc4388.robot.subsystems.elevator.Elevator;
/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
public class waitEndefectorRefrence extends Command {
src/main/java/frc4388/robot/commands/waitFeedCoral.java → src/main/java/frc4388/robot/commands/wait/waitFeedCoral.java
+2 -2
View File
@@ -2,10 +2,10 @@
// 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.commands;
package frc4388.robot.commands.wait;
import edu.wpi.first.wpilibj2.command.Command;
import frc4388.robot.subsystems.Elevator;
import frc4388.robot.subsystems.elevator.Elevator;
/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
public class waitFeedCoral extends Command {
src/main/java/frc4388/robot/commands/waitSupplier.java → src/main/java/frc4388/robot/commands/wait/waitSupplier.java
+1 -1
View File
@@ -2,7 +2,7 @@
// 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.commands;
package frc4388.robot.commands.wait;
import java.util.function.Supplier;
src/main/java/frc4388/robot/constants/BuildConstants.java
+19
View File
@@ -0,0 +1,19 @@
package frc4388.robot.constants;
/**
* Automatically generated file containing build version information.
*/
public final class BuildConstants {
public static final String MAVEN_GROUP = "";
public static final String MAVEN_NAME = "2025RidgeScape";
public static final String VERSION = "unspecified";
public static final int GIT_REVISION = 173;
public static final String GIT_SHA = "2b7bb1224195b9e001e60b895ee04d63abdfc513";
public static final String GIT_DATE = "2025-07-17 09:15:19 MDT";
public static final String GIT_BRANCH = "advantagekit";
public static final String BUILD_DATE = "2025-07-17 11:55:31 MDT";
public static final long BUILD_UNIX_TIME = 1752774931371L;
public static final int DIRTY = 1;
private BuildConstants(){}
}
src/main/java/frc4388/robot/constants/Constants.java
+234
View File
@@ -0,0 +1,234 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2019 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.constants;
import com.ctre.phoenix6.configs.Slot0Configs;
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.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Transform3d;
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;
import edu.wpi.first.wpilibj.RobotBase;
import frc4388.utility.compute.Trim;
import frc4388.utility.status.CanDevice;
import frc4388.utility.structs.Gains;
import frc4388.utility.structs.LEDPatterns;
/**
* The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
* constants. This class should not be used for any other purpose. All constants should be
* declared globally (i.e. public static). Do not put anything functional in this class.
*
* <p>It is advised to statically import this class (or one of its inner classes) wherever the
* constants are needed, to reduce verbosity.
*/
public final class Constants {
public static final String CANBUS_NAME = "rio";
public static final class LiDARConstants {
public static final int REEF_LIDAR_DIO_CHANNEL = 7;
public static final int REVERSE_LIDAR_DIO_CHANNEL = 4;
public static final int HUMAN_PLAYER_STATION_DISTANCE = 40;
public static final int LIDAR_DETECT_DISTANCE = 100; // Min distance to detect pole
public static final int LIDAR_MICROS_TO_CM = 10;
public static final int SECONDS_TO_MICROS = 1000000;
}
public static final class AutoConstants {
// public static final Gains XY_GAINS = new Gains(5,0.6,0.0);
public static final Gains XY_GAINS = new Gains(8,0,0.0);
// public static final ConfigurableDouble P_XY_GAINS = new ConfigurableDouble("P_XY_GAINS", XY_GAINS.kP);
// public static final ConfigurableDouble I_XY_GAINS = new ConfigurableDouble("I_XY_GAINS", XY_GAINS.kI);
// public static final ConfigurableDouble D_XY_GAINS = new ConfigurableDouble("D_XY_GAINS", XY_GAINS.kD);
// public static final Gains XY_GAINS = new Gains(3,0.3,0.0);
// public static final Gains ROT_GAINS = new Gains(0.05,0,0.007);
// public static final Gains ROT_GAINS = new Gains(0.05,0,0.0);
public static final Trim X_OFFSET_TRIM = new Trim("X Offset Trim", Double.MAX_VALUE, -Double.MAX_VALUE,0.5, 0);
// public static final Trim Y_OFFSET_TRIM = new Trim("Y Offset Trim", Double.MAX_VALUE, -Double.MAX_VALUE, 0.5, 1.5);
public static final Trim Y_OFFSET_TRIM = new Trim("Y Offset Trim", Double.MAX_VALUE, -Double.MAX_VALUE, 0.5, 0);
public static final Trim ELEVATOR_OFFSET_TRIM = new Trim("Elevator Offset Trim", -ElevatorConstants.MAX_POSITION_ELEVATOR, ElevatorConstants.MAX_POSITION_ELEVATOR, 1, 0);
public static final Trim ARM_OFFSET_TRIM = new Trim("ARM Offset Trim", -ElevatorConstants.COMPLETLY_TOP_ENDEFFECTOR, ElevatorConstants.COMPLETLY_TOP_ENDEFFECTOR, 1, 0);
public static final double XY_TOLERANCE = 0.07; // Meters
public static final double ROT_TOLERANCE = 5; // Degrees
public static final double MIN_XY_PID_OUTPUT = 0.0;
public static final double MIN_ROT_PID_OUTPUT = 0.0;
public static final double VELOCITY_THRESHHOLD = 0.01;
// X is tangent to reef side
// Y is normal to reef side
public static final double X_SCORING_POSITION_OFFSET = Units.inchesToMeters(6.5+1); // This is from the field
public static final double Y_SCORING_POSITION_OFFSET = Units.inchesToMeters(16+1);
public static final double HALF_ROBOT_SIZE = Units.inchesToMeters(18);
public static final double L4_DISTANCE_PREP = HALF_ROBOT_SIZE + Units.inchesToMeters(15);
public static final double L4_DISTANCE_SCORE = L4_DISTANCE_PREP;
// public static final double L4_DISTANCE_SCORE = HALF_ROBOT_SIZE + Units.inchesToMeters(4.5);
public static final double L3_DISTANCE_PREP = HALF_ROBOT_SIZE + Units.inchesToMeters(15);
public static final double L3_DISTANCE_SCORE = HALF_ROBOT_SIZE + Units.inchesToMeters(5+1);
public static final double L2_PREP_DISTANCE = HALF_ROBOT_SIZE + Units.inchesToMeters(6);
public static final double L2_SCORE_DISTANCE = HALF_ROBOT_SIZE + Units.inchesToMeters(0.5-2);
public static final double ALGAE_REMOVAL_DISTANCE = HALF_ROBOT_SIZE;
// public static final double L4_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(15);
// public static final double L4_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(5.5);
// // public static final double L4_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(4.5);
// public static final double L3_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(15);
// public static final double L3_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(5+1);
// public static final double L2_PREP_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(6);
// public static final double L2_SCORE_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(0.5);
// public static final double ALGAE_REMOVAL_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(2);
public static final int L4_DRIVE_TIME = 250; //Milliseconds
// public static final int L3_DRIVE_TIME = 500;
public static final int L2_DRIVE_TIME = 250; //Milliseconds
public static final int ALGAE_DRIVE_TIME = 500;
public static final double STOP_VELOCITY = 0.0;
}
public static final class VisionConstants {
public static final String LEFT_CAMERA_NAME = "CAMERA_LEFT";
public static final String RIGHT_CAMERA_NAME = "CAMERA_RIGHT";
public static final Transform3d LEFT_CAMERA_POS = new Transform3d(new Translation3d(Units.inchesToMeters(4.547), Units.inchesToMeters(8.031), Units.inchesToMeters(8.858)), new Rotation3d(0,0.0,0.0));
public static final Transform3d RIGHT_CAMERA_POS = new Transform3d(new Translation3d(Units.inchesToMeters(4.547), -Units.inchesToMeters(8.031), Units.inchesToMeters(8.858)), new Rotation3d(0,0.0,0.0));
public static final double MIN_ESTIMATION_DISTANCE = 2; // Meters
// Photonvision thing
// The standard deviations of our vision estimated poses, which affect correction rate
// X, Y, Theta
// https://www.chiefdelphi.com/t/photonvision-finding-standard-deviations-for-swervedriveposeestimator/467802/2
public static final Matrix<N3, N1> kSingleTagStdDevs = VecBuilder.fill(0.5, 0.5, 4);
public static final Matrix<N3, N1> kMultiTagStdDevs = VecBuilder.fill(0.1, 0.1, 1);
}
public static final class FieldConstants {
public static final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeWelded);
// Test april tag field layout
// public static final AprilTagFieldLayout kTagLayout = new AprilTagFieldLayout(
// Arrays.asList(new AprilTag[] {
// new AprilTag(1, new Pose3d(
// new Translation3d(0.,0.,0.26035), new Rotation3d(0.,0.,0.)
// )),
// }), 100, 100);
}
public static final class LEDConstants {
public static final int LED_SPARK_ID = 9;
public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.FOREST_WAVES;
public static final LEDPatterns WAITING_PATTERN = LEDPatterns.SOLID_RED;
public static final LEDPatterns DOWN_PATTERN = LEDPatterns.SOLID_YELLOW;
public static final LEDPatterns READY_PATTERN = LEDPatterns.SOLID_GREEN_DARK;
public static final LEDPatterns SCORING_PATTERN = LEDPatterns.RAINBOW_RAINBOW;
public static final LEDPatterns RED_PATTERN = LEDPatterns.LAVA_WAVES;
public static final LEDPatterns BLUE_PATTERN = LEDPatterns.OCEAN_WAVES;
}
public static final class OIConstants {
public static final int XBOX_DRIVER_ID = 0;
public static final int XBOX_OPERATOR_ID = 1;
public static final int BUTTONBOX_ID = 2;
public static final int XBOX_PROGRAMMER_ID = 3;
public static final double LEFT_AXIS_DEADBAND = 0.1;
}
public static final class ElevatorConstants {
public static final CanDevice ENDEFFECTOR_ID = new CanDevice("Endeffector", 15);
public static final CanDevice ELEVATOR_ID = new CanDevice("Elevator", 16);
public static final double SAFETY_ENDEFFECTOR_MAX_TORQUE = 75;
public static final double SAFETY_ENDEFFECTOR_MIN_VELOCITY = 20;
// public static final int BASIN_LIMIT_SWITCH = 8; // TODO: FIND
public static final int BASIN_LIMIT_SWITCH = 8; // TODO: FIND
public static final int ENDEFFECTOR_LIMIT_SWITCH = 9; // TODO: FIND
public static final int INTAKE_LIMIT_SWITCH = 6; // TODO: FIND
public static final double GEAR_RATIO_ELEVATOR = -9.0;
//Max for elevator = 50%
public static final double GROUND_POSITION_ELEVATOR = 0 * GEAR_RATIO_ELEVATOR;
public static final double WAITING_POSITION_ELEVATOR = -9.5; // TODO: find 2-4 in off the pipe
public static final double HOVERING_POSITION_ELEVATOR = -7.5; // TODO: find 2-4 in off the pipe
public static final double WAITING_POSITION_BEAM_BREAK_ELEVATOR = -5; // TODO: find on the pipe
public static final double SCORING_THREE_ELEVATOR = -9.25;
public static final double DEALGAE_L2_ELEVATOR = -4;
public static final double DEALGAE_L3_ELEVATOR = -26.5;
public static final double MAX_POSITION_ELEVATOR = 4.5 * GEAR_RATIO_ELEVATOR; // TODO: find MAX position
public static final double GEAR_RATIO_ENDEFECTOR = -100.0;
public static final double ENDEFECTOR_DRIVE_SLOW = -0.08;
//Max for endefector = 60%
public static final double L2_SCORE_ELEVATOR = -5;
public static final double L2_LEAVE_ELEVATOR = -11;
public static final double L2_SCORE_ENDEFFECTOR = -19;
public static final double COMPLETLY_DOWN_ENDEFFECTOR = 0 * GEAR_RATIO_ENDEFECTOR;
public static final double DEALGAE_L2_ENDEFFECTOR = 0.22 * GEAR_RATIO_ENDEFECTOR;
public static final double COMPLETLY_MIDDLE_ENDEFFECTOR = 0.25 * GEAR_RATIO_ENDEFECTOR;
public static final double PRIMED_THREE_ENDEFFECTOR = 0.4 * GEAR_RATIO_ENDEFECTOR;
public static final double SCORING_FOUR_ENDEFFECTOR = 0.3 * GEAR_RATIO_ENDEFECTOR;
public static final double PRIMED_FOUR_ENDEFFECTOR = 0.44 * GEAR_RATIO_ENDEFECTOR;
public static final double COMPLETLY_TOP_ENDEFFECTOR = 0.5 * GEAR_RATIO_ENDEFECTOR;
public static final Slot0Configs ELEVATOR_PID = new Slot0Configs()
.withKP(1)
.withKI(0)
.withKD(0);
public static final Slot0Configs ENDEFFECTOR_PID = new Slot0Configs()
.withKP(1)
.withKI(0)
.withKD(0);
}
// Logging constants
public static class SimConstants {
public static final Mode simMode = Mode.SIM;
public static final Mode currentMode = RobotBase.isReal() ? Mode.REAL : simMode;
public static enum Mode {
/** Running on a real robot. */
REAL,
/** Running a physics simulator. */
SIM,
/** Replaying from a log file. */
REPLAY
}
}
}
src/main/java/frc4388/robot/subsystems/DiffDrive.java
-108
View File
@@ -1,108 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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 com.ctre.phoenix6.controls.Follower;
import com.ctre.phoenix6.hardware.TalonFX;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import frc4388.robot.Constants.DriveConstants;
import frc4388.utility.RobotGyro;
import frc4388.utility.RobotTime;
import frc4388.utility.Status;
import frc4388.utility.Subsystem;
/**
* Add your docs here.
*/
public class DiffDrive extends Subsystem {
// Put methods for controlling this subsystem
// here. Call these from Commands.
private RobotTime m_robotTime = RobotTime.getInstance();
private TalonFX m_leftFrontMotor;
private TalonFX m_rightFrontMotor;
private TalonFX m_leftBackMotor;
private TalonFX m_rightBackMotor;
private DifferentialDrive m_driveTrain;
private RobotGyro m_gyro;
/**
* Add your docs here.
*/
public DiffDrive(TalonFX leftFrontMotor, TalonFX rightFrontMotor, TalonFX leftBackMotor,
TalonFX rightBackMotor, DifferentialDrive driveTrain, RobotGyro gyro) {
super();
m_leftFrontMotor = leftFrontMotor;
m_rightFrontMotor = rightFrontMotor;
m_leftBackMotor = leftBackMotor;
m_rightBackMotor = rightBackMotor;
m_leftBackMotor .setControl(new Follower(m_leftFrontMotor.getDeviceID(), false));
m_rightBackMotor.setControl(new Follower(m_rightBackMotor.getDeviceID(), false));
m_driveTrain = driveTrain;
m_gyro = gyro;
}
@Override
public void periodic() {
m_gyro.updatePigeonDeltas();
if (m_robotTime.m_frameNumber % DriveConstants.SMARTDASHBOARD_UPDATE_FRAME == 0) {
updateSmartDashboard();
}
}
/**
* Add your docs here.
*/
public void driveWithInput(double move, double steer) {
m_driveTrain.arcadeDrive(move, steer);
}
/**
* Add your docs here.
*/
public void tankDriveWithInput(double leftMove, double rightMove) {
m_leftFrontMotor.set(leftMove);
m_rightFrontMotor.set(rightMove);
}
/**
* Add your docs here.
*/
private void updateSmartDashboard() {
// Examples of the functionality of RobotGyro
SmartDashboard.putBoolean("Is Gyro a Pigeon?", m_gyro.m_isGyroAPigeon);
SmartDashboard.putNumber("Turn Rate", m_gyro.getRate());
SmartDashboard.putNumber("Gyro Pitch", m_gyro.getPitch());
//SmartDashboard.putData(m_gyro);
}
@Override
public String getSubsystemName() {
return "Diff Drive";
}
@Override
public void queryStatus() {
// TODO: Add Stuff
}
@Override
public Status diagnosticStatus() {
Log("Diagnostic info for this has not been inplemented!"); //TODO
return new Status();
}
}
src/main/java/frc4388/robot/subsystems/Elevator.java
-402
View File
@@ -1,402 +0,0 @@
// 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;
import java.time.Instant;
import org.opencv.ml.RTrees;
import com.ctre.phoenix6.configs.Slot0Configs;
import com.ctre.phoenix6.controls.PositionDutyCycle;
import com.ctre.phoenix6.controls.PositionVoltage;
import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.signals.NeutralModeValue;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.units.measure.Current;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.ElevatorConstants;
import frc4388.robot.Constants.LEDConstants;
import frc4388.robot.Constants.AutoConstants;
import frc4388.robot.subsystems.LED;
import frc4388.utility.LEDPatterns;
import frc4388.utility.Status;
import frc4388.utility.Subsystem;
import frc4388.utility.TimesNegativeOne;
import frc4388.utility.Status.ReportLevel;
public class Elevator extends Subsystem {
/** Creates a new Elevator. */
private TalonFX elevatorMotor;
private TalonFX endeffectorMotor;
private LED led;
private long wait = 0;
private long maxWait = 1000;
private double elevatorRefrence = 0;
private double endeffectorRefrence = 0;
private boolean elevatorManualStop = true;
private boolean endefectorManualStop = true;
private boolean disableAutoIntake = false;
private boolean seededZeroEndefector = false;
private boolean seededZeroElevator = false;
private DigitalInput basinBeamBreak;
private DigitalInput endeffectorLimitSwitch;
private DigitalInput intakeIR;
public enum CoordinationState {
Waiting, // for coral into the though
WatingBeamTripped, //once the beam break trips
Ready, // Has coral in endefector
Hovering, // Has coral in endefector
L2Score,
L2ScoreLeave,
PrimedThree, // Arm and elevator Waiting to score in the level 3 position
ScoringThree, // Arm and elevator in the level three position
PrimedFour, // Arm and elevator Waiting to score in the level 4 position
ScoringFour, // Arm and elevator in the level four position
BallRemoverL2Primed, // Arm and elevator ready to remove the ball in the level 2 reef.
BallRemoverL2Go, // Arm and elevator removing the ball in the level 2 reef.
BallRemoverL3Primed, // Arm and elevator ready to remove the ball in the level 3 reef.
BallRemoverL3Go, // Arm and elevator removing the ball in the level 3 reef.
}
private CoordinationState currentState;
// public Elevator(TalonFX elevatorTalonFX, TalonFX endeffectorTalonFX, DigitalInput basinLimitSwitch, DigitalInput endeffectorLimitSwitch, LED led) {
public Elevator(TalonFX elevatorTalonFX, TalonFX endeffectorTalonFX, DigitalInput basinLimitSwitch, DigitalInput endeffectorLimitSwitch, DigitalInput intakeDigitalInput, LED led) {
elevatorMotor = elevatorTalonFX;
endeffectorMotor = endeffectorTalonFX;
this.led = led;
this.basinBeamBreak = basinLimitSwitch;
this.endeffectorLimitSwitch = endeffectorLimitSwitch;
this.intakeIR = intakeDigitalInput;
elevatorMotor.setNeutralMode(NeutralModeValue.Brake);
endeffectorMotor.setNeutralMode(NeutralModeValue.Brake);
elevatorMotor.getConfigurator().apply(ElevatorConstants.ELEVATOR_PID);
endeffectorMotor.getConfigurator().apply(ElevatorConstants.ENDEFFECTOR_PID);
currentState = CoordinationState.Ready;
}
//PID methods
private void PIDPosition(TalonFX motor, double position) {
if (motor == elevatorMotor) elevatorRefrence = position;
else endeffectorRefrence = position;
var request = new PositionDutyCycle(position);
motor.setControl(request);
}
public void elevatorStop() {
elevatorMotor.set(0);
}
public void endeffectorStop() {
endeffectorMotor.set(0);
}
public void transitionState(CoordinationState state) {
// elevatorMotor.enable();
currentState = state;
switch (currentState) {
case Waiting: {
wait = System.currentTimeMillis() + maxWait;
PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR);
PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR + (!seededZeroEndefector ? 10 : 0));
led.setMode(LEDConstants.WAITING_PATTERN);
break;
}
case WatingBeamTripped: {
PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_BEAM_BREAK_ELEVATOR);
PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR);
led.setMode(LEDConstants.DOWN_PATTERN);
break;
}
case Ready: {
PIDPosition(elevatorMotor, ElevatorConstants.GROUND_POSITION_ELEVATOR + (!seededZeroElevator ? 10 : 0));
PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR);
led.setMode(LEDConstants.DOWN_PATTERN);
break;
}
case Hovering: {
PIDPosition(elevatorMotor, ElevatorConstants.HOVERING_POSITION_ELEVATOR);
PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR);
led.setMode(LEDConstants.READY_PATTERN);
break;
}
case L2Score: {
PIDPosition(elevatorMotor, ElevatorConstants.L2_SCORE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
PIDPosition(endeffectorMotor, ElevatorConstants.L2_SCORE_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
led.setMode(LEDConstants.SCORING_PATTERN);
break;
}
case L2ScoreLeave: {
PIDPosition(elevatorMotor, ElevatorConstants.L2_LEAVE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
PIDPosition(endeffectorMotor, ElevatorConstants.L2_SCORE_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
led.setMode(LEDConstants.SCORING_PATTERN);
break;
}
case PrimedFour: {
PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
PIDPosition(endeffectorMotor, ElevatorConstants.PRIMED_FOUR_ENDEFFECTOR);
led.setMode(LEDConstants.SCORING_PATTERN);
break;
}
case ScoringFour: {
PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
PIDPosition(endeffectorMotor, ElevatorConstants.SCORING_FOUR_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
led.setMode(LEDConstants.SCORING_PATTERN);
break;
}
case PrimedThree: {
PIDPosition(elevatorMotor, ElevatorConstants.SCORING_THREE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
PIDPosition(endeffectorMotor, ElevatorConstants.PRIMED_THREE_ENDEFFECTOR);
led.setMode(LEDConstants.SCORING_PATTERN);
break;
}
case ScoringThree: {
PIDPosition(elevatorMotor, ElevatorConstants.SCORING_THREE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
led.setMode(LEDConstants.SCORING_PATTERN);
break;
}
case BallRemoverL2Primed: {
PIDPosition(elevatorMotor, ElevatorConstants.DEALGAE_L2_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
PIDPosition(endeffectorMotor, ElevatorConstants.DEALGAE_L2_ENDEFFECTOR);
led.setMode(LEDConstants.SCORING_PATTERN);
break;
}
case BallRemoverL2Go: {
PIDPosition(elevatorMotor, ElevatorConstants.DEALGAE_L2_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
PIDPosition(endeffectorMotor, ElevatorConstants.DEALGAE_L2_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
led.setMode(LEDConstants.SCORING_PATTERN);
break;
}
case BallRemoverL3Primed: {
PIDPosition(elevatorMotor, ElevatorConstants.DEALGAE_L3_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
PIDPosition(endeffectorMotor, ElevatorConstants.DEALGAE_L2_ENDEFFECTOR);
break;
}
case BallRemoverL3Go: {
PIDPosition(elevatorMotor, ElevatorConstants.DEALGAE_L3_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
PIDPosition(endeffectorMotor, ElevatorConstants.DEALGAE_L2_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
led.setMode(LEDConstants.SCORING_PATTERN);
break;
}
default: {
assert false;
}
}
}
public void togggleAutoIntaking() {
disableAutoIntake = !disableAutoIntake;
}
public boolean elevatorAtReference() {
// double elevatorRefrence = elevatorMotor.getClosedLoopReference().getValueAsDouble();
double elevatorPosition = elevatorMotor.getPosition().getValueAsDouble();
double diffrence = elevatorRefrence - elevatorPosition;
boolean headedUp = diffrence < 0;
boolean forwardLimit = elevatorMotor.getForwardLimit().asSupplier().get().value == 0;
boolean reverseLimit = elevatorMotor.getReverseLimit().asSupplier().get().value == 0;
return (Math.abs(diffrence) <= 0.5 || (reverseLimit && headedUp) || (forwardLimit && !headedUp));
}
public boolean endeffectorAtReference() {
// double elevatorRefrence = endefectorMotor.getClosedLoopReference().getValueAsDouble();
double endeffectorPosition = endeffectorMotor.getPosition().getValueAsDouble();
double diffrence = endeffectorRefrence - endeffectorPosition;
boolean headedUp = diffrence < 0;
boolean forwardLimit = endeffectorMotor.getForwardLimit().asSupplier().get().value == 0;
boolean reverseLimit = endeffectorMotor.getReverseLimit().asSupplier().get().value == 0;
return (Math.abs(diffrence) <= 0.5 || (reverseLimit && headedUp) || (forwardLimit && !headedUp));
}
// public void driveElevatorStick(Translation2d stick) {
// if (stick.getNorm() > 0.05) {
// elevatorMotor.set(stick.getY());
// }
// }
public boolean getEndeffectorLimit() {
return endeffectorLimitSwitch.get();
}
private void periodicWaiting() {
if (!basinBeamBreak.get())
transitionState(CoordinationState.Ready);
// if(!endeffectorLimitSwitch.get())
// transitionState(CoordinationState.Hovering);
}
// private void periodicWaitingTripped() {
// if (!basinBeamBreak.get() && System.currentTimeMillis() > wait)
// transitionState(CoordinationState.Ready);
// }
private void periodicReady() {
if (elevatorAtReference() && !endeffectorLimitSwitch.get())
transitionState(CoordinationState.Hovering);
if(elevatorAtReference() && endeffectorLimitSwitch.get())
transitionState(CoordinationState.Hovering);
}
private void periodicScoring() {
if (!endeffectorLimitSwitch.get())
transitionState(CoordinationState.Waiting);
}
public void manualElevatorVel(double velocity) {
if (Math.abs(velocity) > 0.1) {
elevatorMotor.set(velocity);
elevatorManualStop = false;
return;
}
if (!elevatorManualStop) {
elevatorManualStop = true;
elevatorMotor.set(0);
}
}
public void manualEndeffectorVel(double velocity) {
if (Math.abs(velocity) > 0.1) {
endeffectorMotor.set(velocity);
endefectorManualStop = false;
return;
}
if (!endefectorManualStop) {
endefectorManualStop = true;
endeffectorMotor.set(0);
}
}
@Override
public void periodic() {
// double elevatorVelocity = elevatorMotor.getVelocity().getValueAsDouble();
// double elevatorTorque = elevatorMotor.getTorqueCurrent().getValueAsDouble();
// double endeffectorVelocity = endeffectorMotor.getVelocity().getValueAsDouble();
// double endeffectorTorque = endeffectorMotor.getTorqueCurrent().getValueAsDouble();
// if(endeffectorVelocity < ElevatorConstants.SAFETY_ENDEFFECTOR_MIN_VELOCITY && endeffectorTorque > ElevatorConstants.SAFETY_ENDEFFECTOR_MAX_TORQUE ){
// PIDPosition(endeffectorMotor, endeffectorMotor.getPosition().getValueAsDouble());
// }
// This method will be called once per scheduler run
// SmartDashboard.putNumber("Velocity Endeffector", endeffectorVelocity);
// SmartDashboard.putNumber("Torque Endeffector", endeffectorTorque);
SmartDashboard.putNumber("Basin", basinBeamBreak.get() ? 1 : 0);
SmartDashboard.putNumber("endefector", endeffectorLimitSwitch.get() ? 1 : 0);
SmartDashboard.putNumber("intake", intakeIR.get() ? 1 : 0);
SmartDashboard.putString("State", currentState.toString());
if (!seededZeroEndefector && endeffectorMotor.getForwardLimit().asSupplier().get().value == 0) {
endeffectorMotor.setPosition(0);
seededZeroEndefector = !seededZeroEndefector;
}
if (!seededZeroElevator && elevatorMotor.getForwardLimit().asSupplier().get().value == 0) {
elevatorMotor.setPosition(0);
seededZeroElevator = !seededZeroElevator;
}
if (disableAutoIntake) return;
if (currentState == CoordinationState.Waiting) {
periodicWaiting();
} else if (currentState == CoordinationState.WatingBeamTripped) {
// periodicWaitingTripped();
} else if (currentState == CoordinationState.Ready) {
periodicReady();
}
if(!intakeIR.get()){
led.setMode(LEDConstants.DOWN_PATTERN);
}
// } else if (currentState == CoordinationState.ScoringThree || currentState == CoordinationState.ScoringFour) {
// periodicScoring();
// }
}
public boolean isL4Primed() {
return currentState == CoordinationState.PrimedFour;
}
public boolean isL3Primed() {
return currentState == CoordinationState.PrimedThree;
}
public boolean hasCoral() {
return elevatorAtReference() && currentState == CoordinationState.Hovering || !endeffectorLimitSwitch.get();
}
public boolean readyToMove() {
return !intakeIR.get() || hasCoral() || !endeffectorLimitSwitch.get();
// return hasCoral();
}
public void armShuffle(){
if(!basinBeamBreak.get()){
//shuffle the coral with the arm until coral hits beam break
}
}
@Override
public String getSubsystemName() {
return "Elevator";
}
@Override
public void queryStatus() {}
@Override
public Status diagnosticStatus() {
Status status = new Status();
status.addReport(ReportLevel.INFO, "Elevator Mode: " + currentState.name());
status.diagnoseHardwareCTRE("Elevator Motor", elevatorMotor);
status.diagnoseHardwareCTRE("Endeffector Motor", endeffectorMotor);
return status;
}
}
src/main/java/frc4388/robot/subsystems/LED.java
+18 -18
View File
@@ -7,24 +7,26 @@
package frc4388.robot.subsystems;
import edu.wpi.first.wpilibj.AddressableLED;
import edu.wpi.first.wpilibj.AddressableLEDBuffer;
import org.littletonrobotics.junction.AutoLogOutput;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.motorcontrol.Spark;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.LEDConstants;
import frc4388.utility.LEDPatterns;
import frc4388.utility.Status;
import frc4388.utility.Subsystem;
import frc4388.utility.Status.ReportLevel;
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 Subsystem {
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;
@@ -47,23 +49,21 @@ public class LED extends Subsystem {
LEDController.set(mode.getValue());
}
@Override
public String getSubsystemName() {
return "LEDs";
@AutoLogOutput
public String state() {
return mode.getClass().toString();
}
@Override
public void queryStatus() {
SmartDashboard.putString("LED status", mode.name());
public String getName() {
return "LEDs";
}
@Override
public Status diagnosticStatus() {
Status status = new Status();
if(LEDController.isAlive())
status.addReport(ReportLevel.INFO, "LED is CONNECTED");
else
if(!LEDController.isAlive())
status.addReport(ReportLevel.ERROR, "LED is DISCONNECTED");
status.addReport(ReportLevel.INFO, "LED Mode: " + mode.name());
src/main/java/frc4388/robot/subsystems/Lidar.java
-92
View File
@@ -1,92 +0,0 @@
package frc4388.robot.subsystems;
import edu.wpi.first.networktables.GenericEntry;
import edu.wpi.first.wpilibj.PWM;
import edu.wpi.first.wpilibj.Counter;
import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts;
import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout;
import frc4388.robot.Constants.LiDARConstants;
import frc4388.utility.Status;
import frc4388.utility.Subsystem;
import frc4388.utility.Status.ReportLevel;
// https://girlsofsteeldocs.readthedocs.io/en/latest/technical-resources/sensors/LIDAR-Lite-Distance-Sensor.html#minimal-roborio-interface
public class Lidar extends Subsystem {
private Counter LidarPWM;
private String name = "Lidar";
private double distance = -1;
public Lidar(int port, String name) {
this.name = name;
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();
subsystemLayout = Shuffleboard.getTab("Subsystems")
.getLayout(getSubsystemName(), BuiltInLayouts.kList)
.withSize(2, 2);
sbDistance = subsystemLayout
.add("Distance", 0)
.withWidget(BuiltInWidgets.kGraph)
.getEntry();
sbWithinDistance = subsystemLayout
. add("Within Distance", 0)
.withWidget(BuiltInWidgets.kBooleanBox)
.getEntry();
}
@Override
public void periodic() {
if(LidarPWM.get() < 1)
distance = -1;
else
distance = (LidarPWM.getPeriod() * LiDARConstants.SECONDS_TO_MICROS) / LiDARConstants.LIDAR_MICROS_TO_CM;
}
public double getDistance(){
return distance;
}
public boolean withinDistance(){
if(distance == -1) return false;
return distance < LiDARConstants.LIDAR_DETECT_DISTANCE;
}
ShuffleboardLayout subsystemLayout;
GenericEntry sbDistance;
GenericEntry sbWithinDistance;
@Override
public String getSubsystemName() {
return "Lidar " + name;
}
@Override
public void queryStatus() {
sbDistance.setDouble(distance);
sbWithinDistance.setBoolean(withinDistance());
}
@Override
public Status diagnosticStatus() {
Status s = new Status();
if(distance == -1){
s.addReport(ReportLevel.ERROR, "LiDAR DISCONNECTED");
}else{
s.addReport(ReportLevel.INFO, "LiDAR Connected");
}
s.addReport(ReportLevel.INFO, "LiDAR Distance: " + distance);
return s;
}
}
src/main/java/frc4388/robot/subsystems/Vision.java
-385
View File
@@ -1,385 +0,0 @@
package frc4388.robot.subsystems;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N3;
import java.time.Instant;
import java.util.ArrayList;
import java.util.List;
import java.util.Optional;
import org.photonvision.EstimatedRobotPose;
import org.photonvision.PhotonCamera;
import org.photonvision.PhotonPoseEstimator;
import org.photonvision.PhotonPoseEstimator.PoseStrategy;
import org.photonvision.simulation.PhotonCameraSim;
import org.photonvision.simulation.SimCameraProperties;
import org.photonvision.simulation.VisionSystemSim;
import org.photonvision.targeting.MultiTargetPNPResult;
import org.photonvision.targeting.PhotonPipelineResult;
import org.photonvision.targeting.PhotonTrackedTarget;
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.networktables.GenericEntry;
import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts;
import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout;
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import frc4388.robot.Constants.FieldConstants;
import frc4388.robot.Constants.VisionConstants;
import frc4388.utility.Status;
import frc4388.utility.Subsystem;
import frc4388.utility.Status.ReportLevel;
public class Vision extends Subsystem {
// private PhotonCamera leftCamera;
// private PhotonCamera rightCamera;
private PhotonCamera[] cameras;
private PhotonPoseEstimator[] estimators;
private List<EstimatedRobotPose> poses = new ArrayList<>();
private boolean isTagDetected = false;
private boolean isTagProcessed = false;
private double lastLatency = 0;
public double getLastLatency() {
return lastLatency;
}
public Pose2d lastVisionPose = new Pose2d();
private Pose2d lastPhysOdomPose = new Pose2d();
private Matrix<N3, N1> curStdDevs;
private Field2d field = new Field2d();
ShuffleboardLayout subsystemLayout = Shuffleboard.getTab("Subsystems")
.getLayout(getSubsystemName(), BuiltInLayouts.kList)
.withSize(2, 2);
GenericEntry sbTagDetected = subsystemLayout
.add("Tag Detected", false)
.withWidget(BuiltInWidgets.kBooleanBox)
.getEntry();
GenericEntry sbTagProcessed = subsystemLayout
.add("Tag Processed", false)
.withWidget(BuiltInWidgets.kBooleanBox)
.getEntry();
GenericEntry sbLeftCamConnected = subsystemLayout
.add("Left Camera Connnected", false)
.withWidget(BuiltInWidgets.kBooleanBox)
.getEntry();
GenericEntry sbRightCamConnected = subsystemLayout
.add("Right Camera Connnected", false)
.withWidget(BuiltInWidgets.kBooleanBox)
.getEntry();
public Vision(PhotonCamera leftCamera, PhotonCamera rightCamera){
SmartDashboard.putData(field);
this.cameras = new PhotonCamera[]{leftCamera, rightCamera};
PhotonPoseEstimator photonEstimatorLeft = new PhotonPoseEstimator(FieldConstants.kTagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, VisionConstants.LEFT_CAMERA_POS);
PhotonPoseEstimator photonEstimatorRight = new PhotonPoseEstimator(FieldConstants.kTagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, VisionConstants.RIGHT_CAMERA_POS);
photonEstimatorLeft.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY);
photonEstimatorRight.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY);
this.estimators = new PhotonPoseEstimator[]{photonEstimatorLeft, photonEstimatorRight};
// resetRotations();
}
@Override
public void periodic() {
update();
field.setRobotPose(getPose2d());
// cameras[0].
}
// public int[] rotations;
// public Instant[] lastUpdateTimes;
// public void resetRotations(){
// rotations = new int[cameras.length];
// lastUpdateTimes = new Instant[cameras.length];
// }
private Instant lastVisionTime = null;
private void update() {
isTagProcessed = false;
isTagDetected = false;
Instant now = Instant.now();
int cams = 0;
// double X = 0;
// double Y = 0;
// double Yaw = 0;
double latency = 0;
// Pose2d pose = null;
poses.clear();
for(int i = 0; i < cameras.length; i++){
PhotonCamera camera = cameras[i];
PhotonPoseEstimator estimator = estimators[i];
var results = camera.getAllUnreadResults();
// If there are no more updates from the camera
if (results.size() == 0)
continue;
var result = results.get(results.size()-1);
latency += result.getTimestampSeconds();
isTagDetected = isTagDetected | result.hasTargets();
// If there are no tags
if(!result.hasTargets())
continue;
Optional<EstimatedRobotPose> estimatedRobotPose = getEstimatedGlobalPose(result, estimator);
// If the tag was failed to be processed
if(estimatedRobotPose.isEmpty())
continue;
poses.add(estimatedRobotPose.get());
// if(pose == null)
// pose = estimatedRobotPose.get().estimatedPose.toPose2d();
// else
// pose = pose.interpolate(pose, 0.5);
// X += pose.getX();
// Y += pose.getY();
// if(X > 6)
// Yaw += (pose.getRotation().getDegrees() + 180) % 360;
// cams++;
isTagProcessed = true;
}
// lastLatency = latency / cams;
// if(isTagProcessed){
// lastVisionPose = pose;
// // lastVisionPose = new Pose2d(X/cams, Y/cams, Rotation2d.fromDegrees(curAngle));
// // lastVisionPose = new Pose2d(10, 5, Rotation2d.fromDegrees(curAngle + rotations*360));
// // SmartDashboard.putNumber("curAngle", pose.getRotation().getRotations());
// // SmartDashboard.putNumber("Rotations", rotations);
// lastVisionTime = now;
// }
}
/**
* 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;
}
// /**
// * 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;
// }
// }
// }
/**
* Returns the latest standard deviations of the estimated pose from {@link
* #getEstimatedGlobalPose()}, for use with {@link
* edu.wpi.first.math.estimator.SwerveDrivePoseEstimator SwerveDrivePoseEstimator}. This should
* only be used when there are targets visible.
*/
public Matrix<N3, N1> getEstimationStdDevs() {
return curStdDevs;
}
public void setLastOdomPose(Optional<Pose2d> pose){
if(pose.isPresent())
lastPhysOdomPose = pose.get();
}
// public double getLastOdomSpeed(){
// return lastOdomSpeed;
// }
public Pose2d getPose2d() {
if(lastPhysOdomPose != null)
return lastPhysOdomPose;
return new Pose2d();
// if(isTagDetected && isTagProcessed)
// // return lastVisionPose;
// else
// return lastPhysOdomPose;
}
public static double getTime() {
return Utils.getCurrentTimeSeconds();
}
public boolean isTag(){
return isTagDetected && isTagProcessed;
}
public void addVisionMeasurement( SwerveDrivetrain<TalonFX, TalonFX, CANcoder> drivetrain){
for(EstimatedRobotPose pose : poses){
drivetrain.addVisionMeasurement(pose.estimatedPose.toPose2d(), Utils.fpgaToCurrentTime(pose.timestampSeconds));
}
}
@Override
public String getSubsystemName() {
return "Vision";
}
// GenericEntry sbShiftState = subsystemLayout
// .add("Shift State", 0)
// .withWidget(BuiltInWidgets.kNumberBar)
// .getEntry();
@Override
public void queryStatus() {
sbTagDetected.setBoolean(isTagDetected);
sbTagProcessed.setBoolean(isTagProcessed);
sbLeftCamConnected.setBoolean(cameras[0].isConnected());
sbRightCamConnected.setBoolean(cameras[1].isConnected());
// field.setRobotPose(getPose2d());
}
@Override
public Status diagnosticStatus() {
Status status = new Status();
if(cameras[0].isConnected())
status.addReport(ReportLevel.INFO, "Left Camera Connected");
else
status.addReport(ReportLevel.ERROR, "Left Camera DISCONNECTED");
if(cameras[1].isConnected())
status.addReport(ReportLevel.INFO, "Right Camera Connected");
else
status.addReport(ReportLevel.ERROR, "Right Camera DISCONNECTED");
return status;
}
}
src/main/java/frc4388/robot/subsystems/elevator/Elevator.java
+379
View File
@@ -0,0 +1,379 @@
// 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.elevator;
import org.littletonrobotics.junction.AutoLogOutput;
import org.littletonrobotics.junction.Logger;
import com.ctre.phoenix6.controls.PositionDutyCycle;
import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.signals.NeutralModeValue;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.constants.Constants.AutoConstants;
import frc4388.robot.constants.Constants.ElevatorConstants;
import frc4388.robot.constants.Constants.LEDConstants;
import frc4388.robot.subsystems.LED;
import frc4388.robot.subsystems.elevator.ElevatorIO.ElevatorState;
import frc4388.utility.status.Status;
import frc4388.utility.status.FaultReporter;
import frc4388.utility.status.Queryable;
import frc4388.utility.status.Status.ReportLevel;
public class Elevator extends SubsystemBase implements Queryable {
ElevatorIO io;
ElevatorStateAutoLogged state = new ElevatorStateAutoLogged();
/** Creates a new Elevator. */
private LED led;
@SuppressWarnings("unused")
public long wait = 0;
public long maxWait = 1000;
public boolean elevatorManualStop = true;
public boolean endefectorManualStop = true;
public boolean disableAutoIntake = false;
public boolean seededZeroEndefector = false;
public boolean seededZeroElevator = false;
// private ElevatorState state = new ElevatorState();
public enum CoordinationState {
Waiting, // for coral into the though
WatingBeamTripped, //once the beam break trips
Ready, // Has coral in endefector
Hovering, // Has coral in endefector
L2Score,
L2ScoreLeave,
PrimedThree, // Arm and elevator Waiting to score in the level 3 position
ScoringThree, // Arm and elevator in the level three position
PrimedFour, // Arm and elevator Waiting to score in the level 4 position
ScoringFour, // Arm and elevator in the level four position
BallRemoverL2Primed, // Arm and elevator ready to remove the ball in the level 2 reef.
BallRemoverL2Go, // Arm and elevator removing the ball in the level 2 reef.
BallRemoverL3Primed, // Arm and elevator ready to remove the ball in the level 3 reef.
BallRemoverL3Go, // Arm and elevator removing the ball in the level 3 reef.
}
private CoordinationState currentState;
// public Elevator(TalonFX elevatorTalonFX, TalonFX endeffectorTalonFX, DigitalInput basinLimitSwitch, DigitalInput endeffectorLimitSwitch, LED led) {
public Elevator(ElevatorIO io, LED led) {
this.io = io;
this.led = led;
currentState = CoordinationState.Ready;
FaultReporter.register(this);
}
public void transitionState(CoordinationState state) {
// elevatorMotor.enable();
currentState = state;
switch (currentState) {
case Waiting: {
wait = System.currentTimeMillis() + maxWait;
io.elevatorToPosition(ElevatorConstants.WAITING_POSITION_ELEVATOR);
io.endeffectorToPosition(ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR + (!seededZeroEndefector ? 10 : 0));
led.setMode(LEDConstants.WAITING_PATTERN);
break;
}
case WatingBeamTripped: {
io.elevatorToPosition(ElevatorConstants.WAITING_POSITION_BEAM_BREAK_ELEVATOR);
io.endeffectorToPosition(ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR);
led.setMode(LEDConstants.DOWN_PATTERN);
break;
}
case Ready: {
io.elevatorToPosition(ElevatorConstants.GROUND_POSITION_ELEVATOR + (!seededZeroElevator ? 10 : 0));
io.endeffectorToPosition(ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR);
led.setMode(LEDConstants.DOWN_PATTERN);
break;
}
case Hovering: {
io.elevatorToPosition(ElevatorConstants.HOVERING_POSITION_ELEVATOR);
io.endeffectorToPosition(ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR);
led.setMode(LEDConstants.READY_PATTERN);
break;
}
case L2Score: {
io.elevatorToPosition(ElevatorConstants.L2_SCORE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
io.endeffectorToPosition(ElevatorConstants.L2_SCORE_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
led.setMode(LEDConstants.SCORING_PATTERN);
break;
}
case L2ScoreLeave: {
io.elevatorToPosition(ElevatorConstants.L2_LEAVE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
io.endeffectorToPosition(ElevatorConstants.L2_SCORE_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
led.setMode(LEDConstants.SCORING_PATTERN);
break;
}
case PrimedFour: {
io.elevatorToPosition(ElevatorConstants.MAX_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
io.endeffectorToPosition(ElevatorConstants.PRIMED_FOUR_ENDEFFECTOR);
led.setMode(LEDConstants.SCORING_PATTERN);
break;
}
case ScoringFour: {
io.elevatorToPosition(ElevatorConstants.MAX_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
io.endeffectorToPosition(ElevatorConstants.SCORING_FOUR_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
led.setMode(LEDConstants.SCORING_PATTERN);
break;
}
case PrimedThree: {
io.elevatorToPosition(ElevatorConstants.SCORING_THREE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
io.endeffectorToPosition(ElevatorConstants.PRIMED_THREE_ENDEFFECTOR);
led.setMode(LEDConstants.SCORING_PATTERN);
break;
}
case ScoringThree: {
io.elevatorToPosition(ElevatorConstants.SCORING_THREE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
io.endeffectorToPosition(ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
led.setMode(LEDConstants.SCORING_PATTERN);
break;
}
case BallRemoverL2Primed: {
io.elevatorToPosition(ElevatorConstants.DEALGAE_L2_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
io.endeffectorToPosition(ElevatorConstants.DEALGAE_L2_ENDEFFECTOR);
led.setMode(LEDConstants.SCORING_PATTERN);
break;
}
case BallRemoverL2Go: {
io.elevatorToPosition(ElevatorConstants.DEALGAE_L2_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
io.endeffectorToPosition(ElevatorConstants.DEALGAE_L2_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
led.setMode(LEDConstants.SCORING_PATTERN);
break;
}
case BallRemoverL3Primed: {
io.elevatorToPosition(ElevatorConstants.DEALGAE_L3_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
io.endeffectorToPosition(ElevatorConstants.DEALGAE_L2_ENDEFFECTOR);
break;
}
case BallRemoverL3Go: {
io.elevatorToPosition(ElevatorConstants.DEALGAE_L3_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
io.endeffectorToPosition(ElevatorConstants.DEALGAE_L2_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
led.setMode(LEDConstants.SCORING_PATTERN);
break;
}
default: {
assert false;
}
}
}
public void togggleAutoIntaking() {
disableAutoIntake = !disableAutoIntake;
}
public boolean elevatorAtReference() {
double diffrence = state.elevatorRefrence - state.elevatorPosition;
boolean headedUp = diffrence < 0;
return (Math.abs(diffrence) <= 0.5
|| (state.elevatorReverseLimit && headedUp)
|| (state.elevatorForwardLimit && !headedUp)
);
}
public boolean endeffectorAtReference() {
double diffrence = state.endeffectorRefrence - state.endeffectorPosition;
boolean headedUp = diffrence < 0;
return (Math.abs(diffrence) <= 0.5
|| (state.elevatorReverseLimit && headedUp)
|| (state.endeffectorForwardLimit && !headedUp)
);
}
// public void driveElevatorStick(Translation2d stick) {
// if (stick.getNorm() > 0.05) {
// elevatorMotor.set(stick.getY());
// }
// }
public boolean getEndeffectorLimit() {
return state.endeffectorLimitSwitch;
}
private void periodicWaiting() {
if (!state.basinBeamBreak)
transitionState(CoordinationState.Ready);
// if(!endeffectorLimitSwitch.get())
// transitionState(CoordinationState.Hovering);
}
// private void periodicWaitingTripped() {
// if (!basinBeamBreak.get() && System.currentTimeMillis() > wait)
// transitionState(CoordinationState.Ready);
// }
private void periodicReady() {
if (elevatorAtReference() && !state.endeffectorLimitSwitch)
transitionState(CoordinationState.Hovering);
if(elevatorAtReference() && state.endeffectorLimitSwitch)
transitionState(CoordinationState.Hovering);
}
@SuppressWarnings("unused")
private void periodicScoring() {
if (!state.endeffectorLimitSwitch)
transitionState(CoordinationState.Waiting);
}
public void manualElevatorVel(double velocity) {
if (Math.abs(velocity) > 0.1) {
io.elevatorToVelocity(velocity);
elevatorManualStop = false;
return;
}
if (!elevatorManualStop) {
elevatorManualStop = true;
io.elevatorToVelocity(0);
}
}
public void manualEndeffectorVel(double velocity) {
if (Math.abs(velocity) > 0.1) {
io.endeffectorToVelocity(velocity);
endefectorManualStop = false;
return;
}
if (!endefectorManualStop) {
endefectorManualStop = true;
io.endeffectorToVelocity(0);
}
}
@Override
public void periodic() {
// double elevatorVelocity = elevatorMotor.getVelocity().getValueAsDouble();
// double elevatorTorque = elevatorMotor.getTorqueCurrent().getValueAsDouble();
// double endeffectorVelocity = endeffectorMotor.getVelocity().getValueAsDouble();
// double endeffectorTorque = endeffectorMotor.getTorqueCurrent().getValueAsDouble();
// if(endeffectorVelocity < ElevatorConstants.SAFETY_ENDEFFECTOR_MIN_VELOCITY && endeffectorTorque > ElevatorConstants.SAFETY_ENDEFFECTOR_MAX_TORQUE ){
// PIDPosition(endeffectorMotor, endeffectorMotor.getPosition().getValueAsDouble());
// }
// This method will be called once per scheduler run
// SmartDashboard.putNumber("Velocity Endeffector", endeffectorVelocity);
// SmartDashboard.putNumber("Torque Endeffector", endeffectorTorque);
// SmartDashboard.putNumber("Basin", basinBeamBreak.get() ? 1 : 0);
// SmartDashboard.putNumber("endefector", endeffectorLimitSwitch.get() ? 1 : 0);
// SmartDashboard.putNumber("intake", intakeIR.get() ? 1 : 0);
// SmartDashboard.putString("State", currentState.toString());
io.updateInputs(state);
Logger.processInputs("Elevator", state);
if (!seededZeroEndefector && state.endeffectorForwardLimit) {
io.endeffectorToPosition(0);
seededZeroEndefector = !seededZeroEndefector;
}
if (!seededZeroElevator && state.elevatorReverseLimit) {
io.endeffectorToPosition(0);
seededZeroElevator = !seededZeroElevator;
}
if (disableAutoIntake) return;
if (currentState == CoordinationState.Waiting) {
periodicWaiting();
} else if (currentState == CoordinationState.WatingBeamTripped) {
// periodicWaitingTripped();
} else if (currentState == CoordinationState.Ready) {
periodicReady();
}
if(!state.intakeIR){
led.setMode(LEDConstants.DOWN_PATTERN);
}
// } else if (currentState == CoordinationState.ScoringThree || currentState == CoordinationState.ScoringFour) {
// periodicScoring();
// }
}
@AutoLogOutput(key="Elevator/state")
public String getState() {
return currentState.toString();
}
public boolean isL4Primed() {
return currentState == CoordinationState.PrimedFour;
}
public boolean isL3Primed() {
return currentState == CoordinationState.PrimedThree;
}
public boolean hasCoral() {
return elevatorAtReference() && currentState == CoordinationState.Hovering || !state.endeffectorLimitSwitch;
}
public void elevatorStop() {
io.elevatorToVelocity(0);
}
public void endeffectorStop() {
io.endeffectorToVelocity(0);
}
public boolean readyToMove() {
return !state.intakeIR || hasCoral() || !state.endeffectorLimitSwitch;
// return hasCoral();
}
public void armShuffle(){
if(!state.basinBeamBreak){
//shuffle the coral with the arm until coral hits beam break
}
}
@Override
public String getName() {
return "Elevator";
}
// @Override
// public void queryStatus() {}
@Override
public Status diagnosticStatus() {
Status status = new Status();
status.addReport(ReportLevel.INFO, "Elevator Mode: " + currentState.name());
return status;
}
}
src/main/java/frc4388/robot/subsystems/elevator/ElevatorIO.java
+34
View File
@@ -0,0 +1,34 @@
package frc4388.robot.subsystems.elevator;
import org.littletonrobotics.junction.AutoLog;
import edu.wpi.first.wpilibj.DigitalInput;
import frc4388.robot.subsystems.lidar.LidarIO.LidarState;
public interface ElevatorIO {
@AutoLog
public class ElevatorState {
public double elevatorRefrence;
public double elevatorPosition;
public boolean elevatorForwardLimit;
public boolean elevatorReverseLimit;
public double endeffectorRefrence;
public double endeffectorPosition;
public boolean endeffectorForwardLimit;
public boolean endeffectorReverseLimit;
public boolean basinBeamBreak;
public boolean endeffectorLimitSwitch;
public boolean intakeIR;
}
public default void elevatorToPosition(double position) {}
public default void endeffectorToPosition(double position) {}
public default void elevatorToVelocity(double velocity) {}
public default void endeffectorToVelocity(double velocity) {}
public default void updateInputs(ElevatorState state) {}
}
src/main/java/frc4388/robot/subsystems/elevator/ElevatorReal.java
+80
View File
@@ -0,0 +1,80 @@
package frc4388.robot.subsystems.elevator;
import com.ctre.phoenix6.controls.PositionDutyCycle;
import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.signals.NeutralModeValue;
import edu.wpi.first.wpilibj.DigitalInput;
import frc4388.robot.constants.Constants.ElevatorConstants;
public class ElevatorReal implements ElevatorIO {
TalonFX elevatorMotor;
TalonFX endeffectorMotor;
DigitalInput basinLimitSwitch;
DigitalInput endeffectorLimitSwitch;
DigitalInput intakeIR;
double elevatorRefrence = 0;
double endeffectorRefrence = 0;
public ElevatorReal(TalonFX elevatorTalonFX, TalonFX endeffectorTalonFX, DigitalInput basinLimitSwitch, DigitalInput endeffectorLimitSwitch, DigitalInput intakeIR) {
this.elevatorMotor = elevatorTalonFX;
this.endeffectorMotor = endeffectorTalonFX;
this.basinLimitSwitch = basinLimitSwitch;
this.endeffectorLimitSwitch = endeffectorLimitSwitch;
this.intakeIR = intakeIR;
elevatorMotor.setNeutralMode(NeutralModeValue.Brake);
endeffectorMotor.setNeutralMode(NeutralModeValue.Brake);
elevatorMotor.getConfigurator().apply(ElevatorConstants.ELEVATOR_PID);
endeffectorMotor.getConfigurator().apply(ElevatorConstants.ENDEFFECTOR_PID);
}
@Override
public void updateInputs(ElevatorState state) {
state.elevatorPosition = elevatorMotor.getPosition().getValueAsDouble();
state.elevatorRefrence = elevatorRefrence;
state.elevatorForwardLimit = elevatorMotor.getForwardLimit().asSupplier().get().value == 0;
state.elevatorReverseLimit = elevatorMotor.getReverseLimit().asSupplier().get().value == 0;
state.endeffectorPosition = endeffectorMotor.getPosition().getValueAsDouble();
state.endeffectorRefrence = endeffectorRefrence;
state.endeffectorForwardLimit = endeffectorMotor.getForwardLimit().asSupplier().get().value == 0;
state.endeffectorReverseLimit = endeffectorMotor.getReverseLimit().asSupplier().get().value == 0;
state.basinBeamBreak = basinLimitSwitch.get();
state.endeffectorLimitSwitch = endeffectorLimitSwitch.get();
state.intakeIR = intakeIR.get();
}
@Override
public void elevatorToPosition(double position) {
elevatorRefrence = position;
var request = new PositionDutyCycle(position);
elevatorMotor.setControl(request);
}
@Override
public void endeffectorToPosition(double position) {
endeffectorRefrence = position;
var request = new PositionDutyCycle(position);
endeffectorMotor.setControl(request);
}
@Override
public void elevatorToVelocity(double velocity) {
elevatorMotor.set(velocity);
}
@Override
public void endeffectorToVelocity(double velocity) {
endeffectorMotor.set(velocity);
}
}
src/main/java/frc4388/robot/subsystems/lidar/LiDAR.java
+58
View File
@@ -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;
}
}
src/main/java/frc4388/robot/subsystems/lidar/LidarIO.java
+13
View File
@@ -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) {}
}
src/main/java/frc4388/robot/subsystems/lidar/LidarReal.java
+27
View File
@@ -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;
}
}
src/main/java/frc4388/robot/subsystems/SwerveDrive.java → src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java
+103 -123
View File
@@ -2,65 +2,53 @@
// 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;
package frc4388.robot.subsystems.swerve;
import java.util.Optional;
import java.util.function.DoubleSupplier;
import org.littletonrobotics.junction.AutoLogOutput;
import org.littletonrobotics.junction.Logger;
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 com.ctre.phoenix6.swerve.SwerveRequest.ForwardPerspectiveValue;
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.math.kinematics.SwerveModuleState;
import edu.wpi.first.networktables.GenericEntry;
import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts;
import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout;
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import frc4388.robot.Constants.SwerveDriveConstants;
import frc4388.robot.Constants.VisionConstants;
import frc4388.robot.commands.GotoLastApril;
import frc4388.robot.commands.LidarAlign;
import frc4388.utility.Status;
import frc4388.utility.Subsystem;
import frc4388.utility.TimesNegativeOne;
import frc4388.utility.Status.ReportLevel;
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.commands.PathPlannerAuto;
import com.pathplanner.lib.config.RobotConfig;
public class SwerveDrive extends Subsystem {
private SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain;
public class SwerveDrive extends SubsystemBase implements Queryable {
// private SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain;
private SwerveIO io;
private SwerveStateAutoLogged state;
private Vision vision;
private int gear_index = SwerveDriveConstants.STARTING_GEAR;
private boolean stopped = false;
private boolean robotKnowsWhereItIs = false;
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%
// 25%
public double lastOdomSpeed;
public Pose2d initalPose2d = null;
public Pose2d initalPose2d = new Pose2d();
public double rotTarget = 0.0;
@@ -68,12 +56,14 @@ public class SwerveDrive extends Subsystem {
public ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
/** Creates a new SwerveDrive. */
public SwerveDrive(SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain, Vision vision) {
public SwerveDrive(SwerveIO swerveDriveTrain, Vision vision) {
// public SwerveDrive(SwerveDrivetrain<TalonFX, TalonFX, CANcoder>
// swerveDriveTrain) {
super();
FaultReporter.register(this);
this.io = swerveDriveTrain;
this.state = new SwerveStateAutoLogged();
this.swerveDriveTrain = swerveDriveTrain;
this.vision = vision;
RobotConfig config;
@@ -86,12 +76,12 @@ public class SwerveDrive extends Subsystem {
// DoubleSupplier a = () -> 1.d;
AutoBuilder.configure(
() -> {
return swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(initalPose2d);
return getPose2d();
}, // Robot pose supplier
this::setOdoPose, // Method to reset odometry (will be called if your auto has a starting
// pose)
() -> swerveDriveTrain.getState().Speeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE
(speeds, feedforwards) -> swerveDriveTrain.setControl(new SwerveRequest.ApplyRobotSpeeds()
() -> 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
@@ -115,12 +105,36 @@ public class SwerveDrive extends Subsystem {
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;
swerveDriveTrain.resetPose(pose);
io.resetPose(pose);
}
// public void oneModuleTest(SwerveModule module, Translation2d leftStick,
@@ -154,11 +168,14 @@ public class SwerveDrive extends Subsystem {
// ! drift correction
if (rightStick.getNorm() > 0.05 || !SwerveDriveConstants.DRIFT_CORRECTION_ENABLED) {
rotTarget = swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(new Pose2d()).getRotation().getDegrees();
swerveDriveTrain.setControl(new SwerveRequest.FieldCentric()
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 {
@@ -171,13 +188,13 @@ public class SwerveDrive extends Subsystem {
SwerveDriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kI,
SwerveDriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kD
);
swerveDriveTrain.setControl(ctrl);
io.setControl(ctrl);
SmartDashboard.putBoolean("drift correction", true);
}
} else { // Create robot-relative speeds.
swerveDriveTrain.setControl(new SwerveRequest.RobotCentric()
io.setControl(new SwerveRequest.RobotCentric()
.withVelocityX(leftStick.getX() * speedAdjust)
.withVelocityY(-leftStick.getY() * speedAdjust)
.withRotationalRate(rightStick.getX() * rotSpeedAdjust));
@@ -188,7 +205,7 @@ public class SwerveDrive extends Subsystem {
stopped = false;
// Create robot-relative speeds.
if (rightStick.getNorm() > 0.1) rightStick = rightStick.times(0);
swerveDriveTrain.setControl(new SwerveRequest.RobotCentric()
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));
@@ -210,7 +227,7 @@ public class SwerveDrive extends Subsystem {
leftStick.rotateBy(TimesNegativeOne.ForwardOffset);
swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle()
io.setControl(new SwerveRequest.FieldCentricFacingAngle()
.withVelocityX(leftStick.getX() * speedAdjust)
.withVelocityY(leftStick.getY() * speedAdjust)
.withTargetDirection(rightStick.getAngle()));
@@ -228,7 +245,7 @@ public class SwerveDrive extends Subsystem {
SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kI,
SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kD
);
swerveDriveTrain.setControl(ctrl);
io.setControl(ctrl);
}
public void driveRelativeLockedAngle(Translation2d leftStick, Rotation2d heading) {
@@ -239,35 +256,23 @@ public class SwerveDrive extends Subsystem {
.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
// DriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kP,
// DriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kI,
// DriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kD
// );
swerveDriveTrain.setControl(ctrl);
}
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);
}
io.setControl(ctrl);
}
public void activateLuigiMode() {
setLimits(20);
io.setLimits(20);
}
public void deactivateLuigiMode() {
setLimits(SwerveDriveConstants.Configurations.SLIP_CURRENT);
io.setLimits(SwerveDriveConstants.Configurations.SLIP_CURRENT);
}
public boolean rotateToTarget(double angle) {
swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle()
io.setControl(new SwerveRequest.FieldCentricFacingAngle()
.withVelocityX(0)
.withVelocityY(0)
.withTargetDirection(Rotation2d.fromDegrees(angle)));
@@ -279,6 +284,10 @@ public class SwerveDrive extends Subsystem {
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:
@@ -289,7 +298,7 @@ public class SwerveDrive extends Subsystem {
leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset);
swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle()
io.setControl(new SwerveRequest.FieldCentricFacingAngle()
.withVelocityX(leftStick.getX() * -speedAdjust)
.withVelocityY(leftStick.getY() * speedAdjust)
.withTargetDirection(rot));
@@ -301,11 +310,13 @@ public class SwerveDrive extends Subsystem {
}
public Pose2d getPose2d() {
return swerveDriveTrain.samplePoseAt(Vision.getTime()).orElse(initalPose2d);
if(state.currentPose == null)
return initalPose2d;
return state.currentPose;
}
public void resetGyro() {
swerveDriveTrain.tareEverything();
io.tareEverything();
robotKnowsWhereItIs = false;
rotTarget = 0;
// vision.resetRotations();
@@ -314,7 +325,7 @@ public class SwerveDrive extends Subsystem {
public void softStop() {
stopped = true;
swerveDriveTrain.setControl(new SwerveRequest.FieldCentric()
io.setControl(new SwerveRequest.FieldCentric()
.withVelocityX(0)
.withVelocityY(0)
.withRotationalRate(0)
@@ -333,27 +344,21 @@ public class SwerveDrive extends Subsystem {
SmartDashboard.putNumber("Gyro", (getGyroAngle() * 180) / Math.PI);
SmartDashboard.putNumber("RotTartget", rotTarget);
double time = Vision.getTime();
double freq = swerveDriveTrain.getOdometryFrequency();
Optional<Pose2d> curpose = swerveDriveTrain.samplePoseAt(time);
Optional<Pose2d> lastPose = swerveDriveTrain.samplePoseAt(time - freq);
io.updateInputs(state);
Logger.processInputs("SwerveDrive", state);
vision.setLastOdomPose(curpose);
setLastOdomSpeed(curpose, lastPose, freq);
vision.setLastOdomPose(state.currentPose);
setLastOdomSpeed(state.currentPose, state.lastPose, state.odometryRate);
if (vision.isTag()) {
Pose2d pose = vision.getPose2d();
if (!robotKnowsWhereItIs) {
robotKnowsWhereItIs = true;
Pose2d currPose = getPose2d();
double deltaAngle = pose.getRotation().getDegrees() - currPose.getRotation().getDegrees();
rotTarget += deltaAngle;
Pose2d curPose = getPose2d();
rotTarget += pose.getRotation().getDegrees() - curPose.getRotation().getDegrees();
}
vision.addVisionMeasurement(swerveDriveTrain);
// swerveDriveTrain.addVisionMeasurement(vision.lastVisionPose, time);
//back to the ~~future~~ *past*
io.addVisionMeasurement(vision.getPosesToAdd());
}
// if(e.isPresent())
@@ -431,57 +436,32 @@ public class SwerveDrive extends Subsystem {
public void setLastOdomSpeed(Optional<Pose2d> curPose, Optional<Pose2d> lastPose, double freq){
if(curPose.isPresent() && lastPose.isPresent()){
this.lastOdomSpeed = curPose.get().getTranslation().getDistance(lastPose.get().getTranslation())/freq;
// double rotDiff = curPose.get().getRotation().getDegrees() - lastPose.get().getRotation().getDegrees();
// if(rotDiff >= 180){
// vision.subtractRotation();
// }else if(rotDiff <= -180){
// vision.addRotation();
// }
SmartDashboard.putNumber("Speed", lastOdomSpeed);
public void setLastOdomSpeed(Pose2d curPose, Pose2d lastPose, double freq){
if(curPose != null && lastPose != null){
lastOdomSpeed = curPose.getTranslation().getDistance(lastPose.getTranslation())/freq;
}
}
@Override
public String getSubsystemName() {
return "Swerve Drive Controller";
@AutoLogOutput(key="SwerveDrive/speed ")
public double getOdometrySpeed() {
return lastOdomSpeed;
}
ShuffleboardLayout subsystemLayout = Shuffleboard.getTab("Subsystems")
.getLayout(getSubsystemName(), BuiltInLayouts.kList)
.withSize(2, 2);
GenericEntry sbGyro = subsystemLayout
.add("Gyro angle", 0)
.withWidget(BuiltInWidgets.kGyro)
.getEntry();
GenericEntry sbShiftState = subsystemLayout
.add("Shift State", 0)
.withWidget(BuiltInWidgets.kNumberBar)
.getEntry();
@Override
public void queryStatus() {
sbGyro.setDouble(getGyroAngle());
sbShiftState.setDouble(this.speedAdjust);
// TODO: Add more status things
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.");
// 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;
}
}
src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java
+246
View File
@@ -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;
}
src/main/java/frc4388/robot/subsystems/swerve/SwerveIO.java
+33
View File
@@ -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) {}
}
src/main/java/frc4388/robot/subsystems/swerve/SwerveReal.java
+63
View File
@@ -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()));
}
}
}
src/main/java/frc4388/robot/subsystems/vision/Vision.java
+95
View File
@@ -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'");
}
}
src/main/java/frc4388/robot/subsystems/vision/VisionIO.java
+22
View File
@@ -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) {}
}
src/main/java/frc4388/robot/subsystems/vision/VisionReal.java
+158
View File
@@ -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;
// }
// }
// }
}
src/main/java/frc4388/utility/Alliance.java
-5
View File
@@ -1,5 +0,0 @@
package frc4388.utility;
public class Alliance {
}
src/main/java/frc4388/utility/AprilTag.java
-13
View File
@@ -1,13 +0,0 @@
package frc4388.utility;
// This is a seperate class in case I want to encode rotation or other
// information about the tag
public class AprilTag {
public final double x, y, z;
public AprilTag(double _x, double _y, double _z) {
x = _x;
y = _y;
z = _z;
}
}
src/main/java/frc4388/utility/DeferredBlock.java
+24 -6
View File
@@ -2,22 +2,40 @@ package frc4388.utility;
import java.util.ArrayList;
// Class for running code snippets whenever the robot is enabled.
public class DeferredBlock {
private static ArrayList<Runnable> m_blocks = new ArrayList<>();
private static ArrayList<Runnable> m_blocks_norerun = new ArrayList<>();
private static ArrayList<Runnable> m_blocks_rerun = new ArrayList<>();
private static boolean m_hasRun = false;
public DeferredBlock(Runnable block) {
m_blocks.add(block);
public static void addBlock(Runnable block) {
addBlock(block, false);
}
public static void addBlock(Runnable block, boolean rerun) {
if(rerun) {
m_blocks_rerun.add(block);
} else {
m_blocks_norerun.add(block);
}
}
public static void execute() {
if (m_hasRun) return;
for (Runnable block : m_blocks) {
// Run blocks that run multiple times.
for (Runnable block : m_blocks_rerun) {
block.run();
}
m_blocks.clear(); // for garbage collection
// Run blocks that only run once
if (m_hasRun) return;
for (Runnable block : m_blocks_norerun) {
block.run();
}
m_blocks_norerun.clear(); // for garbage collection
m_hasRun = true;
}
}
src/main/java/frc4388/utility/DeferredBlockMulti.java
-18
View File
@@ -1,18 +0,0 @@
package frc4388.utility;
import java.util.ArrayList;
public class DeferredBlockMulti {
private static ArrayList<Runnable> m_blocks = new ArrayList<>();
public DeferredBlockMulti(Runnable block) {
m_blocks.add(block);
}
public static void execute() {
for (Runnable block : m_blocks) {
block.run();
}
}
}
src/main/java/frc4388/utility/RobotGyro.java
-269
View File
@@ -1,269 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2019 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.utility;
// import com.ctre.phoenix.sensors.WPI_Pigeon2;
import com.ctre.phoenix6.hardware.Pigeon2;
import com.kauailabs.navx.frc.AHRS;
// import edu.wpi.first.wpilibj.GyroBase;
// import edu.wpi.first.wpilibj.interfaces.Gyro;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.geometry.Rotation2d;
/**
* Gyro class that allows for interchangeable use between a pigeon and a navX
*/
public class RobotGyro {
private RobotTime m_robotTime = RobotTime.getInstance();
private Pigeon2 m_pigeon = null;
private AHRS m_navX = null;
public boolean m_isGyroAPigeon; //true if pigeon, false if navX
private double m_lastPigeonAngle;
private double m_deltaPigeonAngle;
private double pitchZero = 0;
private double rollZero = 0;
/**
* Creates a Gyro based on a pigeon
* @param gyro the gyroscope to use for Gyro
*/
public RobotGyro(Pigeon2 gyro) {
m_pigeon = gyro;
m_isGyroAPigeon = true;
}
/**
* Creates a Gyro based on a navX
* @param gyro the gyroscope to use for Gyro
*/
public RobotGyro(AHRS gyro){
m_navX = gyro;
m_isGyroAPigeon = false;
}
/**
* Resets yaw, pitch, and roll.
*/
public void resetZeroValues() {
if (!m_isGyroAPigeon) return;
// pitchZero = m_pigeon.getPitch();
// rollZero = m_pigeon.getRoll();
}
/**
* Run in periodic if you are using a pigeon. Updates a delta angle so that it can calculate getRate(). Note
* that the getRate() method for a navX will likely be much more accurate than for a pigeon.
*/
public void updatePigeonDeltas() {
double currentPigeonAngle = getAngle();
m_deltaPigeonAngle = currentPigeonAngle - m_lastPigeonAngle;
m_lastPigeonAngle = currentPigeonAngle;
}
/**
* <p>NavX:
* <p>Calibrate the gyro by running for a number of samples and computing the center value. Then use
* the center value as the Accumulator center value for subsequent measurements. It's important to
* make sure that the robot is not moving while the centering calculations are in progress, this
* is typically done when the robot is first turned on while it's sitting at rest before the
* competition starts.
*
* <p>Pigeon:
* <p>Calibrate the gyro by collecting data at a range of tempuratures. Allow pigeon to cool, then boot
* into calibration mode. For faster calibration, use a heat lamp to heat up the pigeon. Once the pigeon
* has seen a reasonable range of tempuratures, it will exit calibration mode. It's important to
* make sure that the robot is not moving while the tempurature calculations are in progress, this
* is typically done when the robot is first turned on while it's sitting at rest before the
* competition starts.
*/
public void calibrate() {
return;
// if (m_isGyroAPigeon) {
// m_pigeon.calibrate();
// } else {
// m_navX.calibrate();
// }
}
public void reset() {
resetZeroValues();
if (m_isGyroAPigeon) {
m_pigeon.setYaw(0);
} else {
m_navX.reset();
}
}
public void reset(double val) {
resetZeroValues();
if (m_isGyroAPigeon) {
m_pigeon.setYaw(val);
} else {
m_navX.reset();
}
}
public void resetFlip() {
resetZeroValues();
if (m_isGyroAPigeon) {
m_pigeon.setYaw(180);
} else {
m_navX.reset();
}
}
public void resetNinety() {
resetZeroValues();
if (m_isGyroAPigeon) {
m_pigeon.setYaw(90);
} else {
m_navX.reset();
}
}
public void resetTwoSeventy() {
resetZeroValues();
if (m_isGyroAPigeon) {
m_pigeon.setYaw(270);
} else {
m_navX.reset();
}
}
public void resetRightSideBlue() {
resetZeroValues();
if (m_isGyroAPigeon) {
m_pigeon.setYaw(60);
} else {
m_navX.reset();
}
}
public void resetAmpSide() {
resetZeroValues();
if (m_isGyroAPigeon) {
m_pigeon.setYaw(-60);
} else {
m_navX.reset();
}
}
/**
* Get Yaw, Pitch, and Roll data.
*
* @return ypr_deg Array with yaw[0], pitch[1], and roll[2] data.
* Yaw is within [-368,640, +368,640] degrees.
* Pitch is within [-90,+90] degrees.
* Roll is within [-90,+90] degrees.
*/
private double[] getPigeonAngles() {
//m_pigeon.getAngle(); // This appeared to not do anything?
var rotation = m_pigeon.getRotation3d();
return new double[] {RobotUnits.radiansToDegrees(rotation.getX() - rollZero), RobotUnits.radiansToDegrees(rotation.getY() - pitchZero), RobotUnits.radiansToDegrees(rotation.getZ())};
}
public Rotation2d getRotation2d() {
return m_pigeon.getRotation2d();
}
public double getAngle() {
if (m_isGyroAPigeon) {
return getPigeonAngles()[2];
} else {
return m_navX.getAngle();
}
}
public double getYaw() {
return this.getAngle();
}
/**
* Gets an absolute heading of the robot
* @return heading from -180 to 180 degrees
*/
public double getHeading() {
return getHeading(getAngle());
}
/**
* Gets an absolute heading of the robot
* @return heading from -180 to 180 degrees
*/
public double getHeading(double angle) {
return Math.IEEEremainder(angle, 360);
}
/**
* Returns the current pitch value (in degrees, from -90 to 90)
* reported by the sensor. Pitch is a measure of rotation around
* the Y Axis.
* @return The current pitch value in degrees (-90 to 90).
*/
public double getPitch() {
if (m_isGyroAPigeon) {
return MathUtil.clamp(getPigeonAngles()[1], -90, 90);
} else {
return MathUtil.clamp(m_navX.getPitch(), -90, 90);
}
}
/**
* Returns the current roll value (in degrees, from -90 to 90)
* reported by the sensor. Roll is a measure of rotation around
* the X Axis.
* @return The current roll value in degrees (-90 to 90).
*/
public double getRoll() {
if (m_isGyroAPigeon) {
return MathUtil.clamp(getPigeonAngles()[2], -90, 90);
} else {
return MathUtil.clamp(m_navX.getRoll(), -90, 90);
}
}
public double getRate() {
if (m_isGyroAPigeon) {
return m_deltaPigeonAngle / m_robotTime.m_deltaTime * 1000;
} else {
return m_navX.getRate();
}
}
public Pigeon2 getPigeon(){
return m_pigeon;
}
public AHRS getNavX(){
return m_navX;
}
public void close() throws Exception {
}
}
src/main/java/frc4388/utility/Subsystem.java
-25
View File
@@ -1,25 +0,0 @@
package frc4388.utility;
import java.util.ArrayList;
import java.util.List;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
public abstract class Subsystem extends SubsystemBase {
public static List<Subsystem> subsystems = new ArrayList<>();
public Subsystem(){
subsystems.add(this);
}
public void Log(String str) {
System.out.println(getSubsystemName() + " - " + str);
}
// Get name of subsystem, for use in log.
public abstract String getSubsystemName();
// Get what the subystem is currently doing, such as "Shooter spun up". This should post to SmartDashboard
public abstract void queryStatus();
// Proactivly search for any errors in each subsystem
public abstract Status diagnosticStatus();
}
src/main/java/frc4388/utility/DataUtils.java → src/main/java/frc4388/utility/compute/DataUtils.java
+1 -1
View File
@@ -1,4 +1,4 @@
package frc4388.utility;
package frc4388.utility.compute;
import java.nio.ByteBuffer;
src/main/java/frc4388/utility/ReefPositionHelper.java → src/main/java/frc4388/utility/compute/ReefPositionHelper.java
+3 -7
View File
@@ -1,15 +1,11 @@
package frc4388.utility;
import java.util.Optional;
package frc4388.utility.compute;
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.util.Units;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import frc4388.robot.Constants.AutoConstants;
import frc4388.robot.Constants.FieldConstants;
import frc4388.robot.constants.Constants.AutoConstants;
import frc4388.robot.constants.Constants.FieldConstants;
public class ReefPositionHelper {
public enum Side {
src/main/java/frc4388/utility/RobotTime.java → src/main/java/frc4388/utility/compute/RobotTime.java
+1 -1
View File
@@ -5,7 +5,7 @@
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.utility;
package frc4388.utility.compute;
/**
* <p>Keeps track of Robot times like time passed, delta time, etc
src/main/java/frc4388/utility/RobotUnits.java → src/main/java/frc4388/utility/compute/RobotUnits.java
+1 -1
View File
@@ -2,7 +2,7 @@
// 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.utility;
package frc4388.utility.compute;
/** Aarav's good units class (better than WPILib)
* @author Aarav Shah */
src/main/java/frc4388/utility/TimesNegativeOne.java → src/main/java/frc4388/utility/compute/TimesNegativeOne.java
+2 -2
View File
@@ -1,4 +1,4 @@
package frc4388.utility;
package frc4388.utility.compute;
import java.util.Optional;
@@ -7,7 +7,7 @@ import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import frc4388.robot.Constants.SwerveDriveConstants;
import frc4388.robot.subsystems.swerve.SwerveDriveConstants;
// Class that holds weather the drivers sticks should be inverted
public class TimesNegativeOne {
src/main/java/frc4388/utility/Trim.java → src/main/java/frc4388/utility/compute/Trim.java
+45 -19
View File
@@ -2,7 +2,7 @@
// 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.utility;
package frc4388.utility.compute;
import java.io.FileInputStream;
import java.io.FileOutputStream;
@@ -27,11 +27,34 @@ public class Trim {
private boolean modified = false;
private double currentValue;
private boolean persistant = false;
private GenericEntry trimElement = null;
/**
* Creates a Trim with a given name, upper and lower bounds, step size and intial value
* Creates a variably Trim with a given name, upper and lower bounds, step size and intial value
* @param trimName please keep the trim name without special symbols
* @param upperBound the upper limit inclusive
* @param lowerBound the lower limit inclusive
* @param step the step size
* @param inital the inital value, will get overridden if the persistant trim exists on disk.
* @param persistnat Weather the trim is persistant or not
*/
public Trim(String trimName, double upperBound, double lowerBound, double step, double inital, boolean persistant) {
this.trimName = trimName;
this.upperBound = upperBound;
this.lowerBound = lowerBound;
this.step = step;
this.persistant = persistant;
currentValue = inital;
load();
trimElement = trimTab.add(trimName, currentValue).getEntry();
trims.add(this);
}
/**
* Creates a non-Trim with a given name, upper and lower bounds, step size and intial value
* @param trimName please keep the trim name without special symbols
* @param upperBound the upper limit inclusive
* @param lowerBound the lower limit inclusive
@@ -81,22 +104,25 @@ public class Trim {
}
public boolean load() {
// try (FileInputStream stream = new FileInputStream("/home/lvuser/trims/" + trimName)) {
// double fileValue = DataUtils.byteArrayToDouble(stream.readNBytes(8));
// currentValue = fileValue;
// clampModify();
// modified = false;
// if (fileValue != currentValue) {
// System.out.println("TRIMS: Loaded trim `" + trimName + "` has a value that is higher than or less than the bounds set for the trim, clamping...");
// modified = true;
// }
// return true;
// } catch (Exception e) {
// // e.printStackTrace();
// System.out.println("TRIMS: Unable to read trim file `" + trimName + "`, using current value...");
// return false;
// }
return false;
if(!persistant)
return false;
try (FileInputStream stream = new FileInputStream("/home/lvuser/trims/" + trimName)) {
double fileValue = DataUtils.byteArrayToDouble(stream.readNBytes(8));
currentValue = fileValue;
clampModify();
modified = false;
if (fileValue != currentValue) {
System.out.println("TRIMS: Loaded trim `" + trimName + "` has a value that is higher than or less than the bounds set for the trim, clamping...");
modified = true;
}
return true;
} catch (Exception e) {
// e.printStackTrace();
System.out.println("TRIMS: Unable to read trim file `" + trimName + "`, using current value...");
return false;
}
}
public void dump() {
src/main/java/frc4388/utility/controller/DeadbandedXboxController.java
+2 -1
View File
@@ -1,6 +1,7 @@
package frc4388.utility.controller;
import static frc4388.robot.Constants.OIConstants.LEFT_AXIS_DEADBAND;
import static frc4388.robot.constants.Constants.OIConstants.LEFT_AXIS_DEADBAND;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj.XboxController;
src/main/java/frc4388/utility/status/Alerts.java
+10
View File
@@ -0,0 +1,10 @@
package frc4388.utility.status;
import edu.wpi.first.wpilibj.Alert;
import edu.wpi.first.wpilibj.Alert.AlertType;
import frc4388.robot.RobotContainer;
// Class to update a series of WPILIB Alerts
public class Alerts {
public static Alert no_auto = new Alert("No auto has been selected!", AlertType.kError);
}
src/main/java/frc4388/utility/CanDevice.java → src/main/java/frc4388/utility/status/CanDevice.java
+2 -6
View File
@@ -1,13 +1,9 @@
package frc4388.utility;
package frc4388.utility.status;
import java.util.ArrayList;
import java.util.List;
import edu.wpi.first.hal.CANData;
import edu.wpi.first.hal.can.CANJNI;
import edu.wpi.first.wpilibj.CAN;
import frc4388.utility.Status.Report;
import frc4388.utility.Status.ReportLevel;
import frc4388.utility.status.Status.ReportLevel;
public class CanDevice {
public static List<CanDevice> devices = new ArrayList<>();
src/main/java/frc4388/utility/status/FaultCANCoder.java
+76
View File
@@ -0,0 +1,76 @@
package frc4388.utility.status;
import com.ctre.phoenix6.controls.EmptyControl;
import com.ctre.phoenix6.hardware.CANcoder;
import frc4388.utility.status.Status.ReportLevel;
public class FaultCANCoder implements Queryable {
private String name;
private CANcoder cancoder;
public static void addDevice(CANcoder cancoder, String name) {
FaultCANCoder p = new FaultCANCoder();
p.name = name;
p.cancoder = cancoder;
FaultReporter.register(p);
}
@Override
public String getName() {
return name;
}
@Override
public Status diagnosticStatus() {
Status s = new Status();
boolean debounceBad = !QueryUtils.isDebounceOk(cancoder.getSupplyVoltage());
boolean emptyControlBad = cancoder.setControl(new EmptyControl()).value != 0;
if(debounceBad || emptyControlBad) {
s.addReport(ReportLevel.ERROR, "device is unreachable - Failed" +
(debounceBad ? " Failed debounce test" : "") +
(emptyControlBad ? " Failed empty control test" : "")
);
}
// faults
if (cancoder.getFault_Hardware().getValue() == Boolean.TRUE) {
s.addReport(ReportLevel.ERROR, "Hardware fault detected");
}
if (cancoder.getFault_BootDuringEnable().getValue() == Boolean.TRUE) {
s.addReport(ReportLevel.ERROR, "Device booted while enabled");
}
if (cancoder.getFault_BadMagnet().getValue() == Boolean.TRUE) {
s.addReport(ReportLevel.ERROR, "Bad magnet");
}
if (cancoder.getFault_Undervoltage().getValue() == Boolean.TRUE) {
s.addReport(ReportLevel.ERROR, "Device supply voltage near brownout");
}
if (cancoder.getFault_UnlicensedFeatureInUse().getValue() == Boolean.TRUE) {
s.addReport(ReportLevel.ERROR, "Unlicensed feature in use");
}
// sticky faults
if (cancoder.getStickyFault_Hardware().getValue() == Boolean.TRUE) {
s.addReport(ReportLevel.ERROR, "[STICKY] Hardware fault detected");
}
if (cancoder.getStickyFault_BootDuringEnable().getValue() == Boolean.TRUE) {
s.addReport(ReportLevel.ERROR, "[STICKY] Device booted while enabled");
}
if (cancoder.getStickyFault_BadMagnet().getValue() == Boolean.TRUE) {
s.addReport(ReportLevel.ERROR, "[STICKY] Bad magnet");
}
if (cancoder.getStickyFault_Undervoltage().getValue() == Boolean.TRUE) {
s.addReport(ReportLevel.ERROR, "[STICKY] Device supply voltage near brownout");
}
if (cancoder.getStickyFault_UnlicensedFeatureInUse().getValue() == Boolean.TRUE) {
s.addReport(ReportLevel.ERROR, "[STICKY] Unlicensed feature in use");
}
return s;
}
}
src/main/java/frc4388/utility/status/FaultPhotonCamera.java
+35
View File
@@ -0,0 +1,35 @@
package frc4388.utility.status;
import org.photonvision.PhotonCamera;
import frc4388.utility.status.Status.ReportLevel;
public class FaultPhotonCamera implements Queryable {
private String name;
private PhotonCamera cam;
public static void addDevice(PhotonCamera cam, String name) {
FaultPhotonCamera p = new FaultPhotonCamera();
p.name = name;
p.cam = cam;
FaultReporter.register(p);
}
@Override
public String getName() {
return name;
}
@Override
public Status diagnosticStatus() {
Status s = new Status();
if(!cam.isConnected())
s.addReport(ReportLevel.ERROR, "Not Connected!");
return s;
}
}
src/main/java/frc4388/utility/status/FaultPidgeon2.java
+168
View File
@@ -0,0 +1,168 @@
package frc4388.utility.status;
import com.ctre.phoenix6.controls.EmptyControl;
import com.ctre.phoenix6.hardware.Pigeon2;
import frc4388.utility.status.Status.ReportLevel;
public class FaultPidgeon2 implements Queryable {
private String name;
private Pigeon2 pigeon2;
public static void addDevice(Pigeon2 pigeon2, String name) {
FaultPidgeon2 p = new FaultPidgeon2();
p.name = name;
p.pigeon2 = pigeon2;
FaultReporter.register(p);
}
@Override
public String getName() {
return name;
}
@Override
public Status diagnosticStatus() {
Status s = new Status();
boolean debounceBad = !QueryUtils.isDebounceOk(pigeon2.getSupplyVoltage());
boolean emptyControlBad = pigeon2.setControl(new EmptyControl()).value != 0;
if(debounceBad || emptyControlBad) {
s.addReport(ReportLevel.ERROR, "device is unreachable - Failed" +
(debounceBad ? " Failed debounce test" : "") +
(emptyControlBad ? " Failed empty control test" : "")
);
}
s.addReport(ReportLevel.INFO, "Voltage: " + pigeon2.getSupplyVoltage());
s.addReport(ReportLevel.INFO, "Pitch: " + pigeon2.getPitch());
s.addReport(ReportLevel.INFO, "Yaw: " + pigeon2.getYaw());
s.addReport(ReportLevel.INFO, "Roll: " + pigeon2.getRoll());
s.addReport(ReportLevel.INFO, "Acceleration X: " + pigeon2.getAccelerationX());
s.addReport(ReportLevel.INFO, "Acceleration Y: " + pigeon2.getAccelerationY());
s.addReport(ReportLevel.INFO, "Acceleration Z: " + pigeon2.getAccelerationZ());
s.addReport(ReportLevel.INFO, "Magnomiter X: " + pigeon2.getMagneticFieldX());
s.addReport(ReportLevel.INFO, "Magnomiter Y: " + pigeon2.getMagneticFieldY());
s.addReport(ReportLevel.INFO, "Magnomiter Z: " + pigeon2.getMagneticFieldZ());
// faults
if (pigeon2.getFault_BootDuringEnable().getValue() == Boolean.TRUE) {
s.addReport(ReportLevel.ERROR, "Device booted while enabled");
}
if (pigeon2.getFault_BootIntoMotion().getValue() == Boolean.TRUE) {
s.addReport(ReportLevel.ERROR, "Device booted while in motion");
}
if (pigeon2.getFault_BootupAccelerometer().getValue() == Boolean.TRUE) {
s.addReport(ReportLevel.ERROR, "Accelerometer fault detected");
}
if (pigeon2.getFault_BootupGyroscope().getValue() == Boolean.TRUE) {
s.addReport(ReportLevel.ERROR, "Gyro fault detected");
}
if (pigeon2.getFault_BootupMagnetometer().getValue() == Boolean.TRUE) {
s.addReport(ReportLevel.ERROR, "Magnetometer fault detected");
}
if (pigeon2.getFault_DataAcquiredLate().getValue() == Boolean.TRUE) {
s.addReport(
ReportLevel.ERROR,
"Motion stack data acquisition slower than expected");
}
if (pigeon2.getFault_Hardware().getValue() == Boolean.TRUE) {
s.addReport(ReportLevel.ERROR, "Hardware fault detected");
}
if (pigeon2.getFault_LoopTimeSlow().getValue() == Boolean.TRUE) {
s.addReport(
ReportLevel.ERROR,
"Motion stack loop time was slower than expected");
}
if (pigeon2.getFault_SaturatedAccelerometer().getValue() == Boolean.TRUE) {
s.addReport(
ReportLevel.ERROR, "Accelerometer values are saturated");
}
if (pigeon2.getFault_SaturatedGyroscope().getValue() == Boolean.TRUE) {
s.addReport(ReportLevel.ERROR, "Gyro values are saturated");
}
if (pigeon2.getFault_SaturatedMagnetometer().getValue() == Boolean.TRUE) {
s.addReport(
ReportLevel.ERROR, "Magnetometer values are saturated");
}
if (pigeon2.getFault_Undervoltage().getValue() == Boolean.TRUE) {
s.addReport(
ReportLevel.ERROR, "Device supply voltage near brownout");
}
if (pigeon2.getFault_UnlicensedFeatureInUse().getValue() == Boolean.TRUE) {
s.addReport(ReportLevel.ERROR, "Unlicensed feature in use");
}
// sticky faults
if (pigeon2.getStickyFault_BootDuringEnable().getValue() == Boolean.TRUE) {
s.addReport(
ReportLevel.ERROR, "[STICKY] Device booted while enabled");
}
if (pigeon2.getStickyFault_BootIntoMotion().getValue() == Boolean.TRUE) {
s.addReport(
ReportLevel.ERROR, "[STICKY] Device booted while in motion");
}
if (pigeon2.getStickyFault_BootupAccelerometer().getValue() == Boolean.TRUE) {
s.addReport(
ReportLevel.ERROR, "[STICKY] Accelerometer fault detected");
}
if (pigeon2.getStickyFault_BootupGyroscope().getValue() == Boolean.TRUE) {
s.addReport(ReportLevel.ERROR, "[STICKY] Gyro fault detected");
}
if (pigeon2.getStickyFault_BootupMagnetometer().getValue() == Boolean.TRUE) {
s.addReport(
ReportLevel.ERROR, "[STICKY] Magnetometer fault detected");
}
if (pigeon2.getStickyFault_DataAcquiredLate().getValue() == Boolean.TRUE) {
s.addReport(
ReportLevel.ERROR,
String.format(
"[STICKY] Motion stack data acquisition slower than expected"));
}
if (pigeon2.getStickyFault_Hardware().getValue() == Boolean.TRUE) {
s.addReport(
ReportLevel.ERROR, "[STICKY] Hardware fault detected");
}
if (pigeon2.getStickyFault_LoopTimeSlow().getValue() == Boolean.TRUE) {
s.addReport(
ReportLevel.ERROR,
String.format(
"[STICKY] Motion stack loop time was slower than expected"));
}
if (pigeon2.getStickyFault_SaturatedAccelerometer().getValue() == Boolean.TRUE) {
s.addReport(
ReportLevel.ERROR,
"[STICKY] Accelerometer values are saturated");
}
if (pigeon2.getStickyFault_SaturatedGyroscope().getValue() == Boolean.TRUE) {
s.addReport(
ReportLevel.ERROR, "[STICKY] Gyro values are saturated");
}
if (pigeon2.getStickyFault_SaturatedMagnetometer().getValue() == Boolean.TRUE) {
s.addReport(
ReportLevel.ERROR,
"[STICKY] Magnetometer values are saturated");
}
if (pigeon2.getStickyFault_Undervoltage().getValue() == Boolean.TRUE) {
s.addReport(
ReportLevel.ERROR,
"[STICKY] Device supply voltage near brownout");
}
if (pigeon2.getStickyFault_UnlicensedFeatureInUse().getValue() == Boolean.TRUE) {
s.addReport(
ReportLevel.ERROR, "[STICKY] Unlicensed feature in use");
}
return s;
}
}
src/main/java/frc4388/utility/status/FaultReporter.java
+133
View File
@@ -0,0 +1,133 @@
package frc4388.utility.status;
import java.util.ArrayList;
import java.util.List;
import com.ctre.phoenix6.CANBus;
import com.ctre.phoenix6.CANBus.CANBusStatus;
import frc4388.robot.constants.Constants;
import frc4388.utility.status.Status.Report;
import frc4388.utility.status.Status.ReportLevel;
public class FaultReporter {
private static final String REPORTS_HEADER =
"###############\n" + //
"#.............#\n" + //
"#...Reports...#\n" + //
"#.............#\n" + //
"###############\n";
private static final String CAN_HEADER =
"###############\n" + //
"#.............#\n" + //
"#....CAN(t)...#\n" + //
"#.............#\n" + //
"###############\n";
private static final String ERROR_HEADER =
"###############\n" + //
"#.............#\n" + //
"#....ERRORS...#\n" + //
"#.............#\n" + //
"###############\n";
private static List<Queryable> queryables = new ArrayList<>();
// public static void startThread() {
// new Thread() {
// public void run() {
// try{
// while(!this.isInterrupted() && this.isAlive()){
// Thread.sleep(500);
// for(int i=0;i<queryables.size(); i++){
// queryables.get(i).queryStatus();
// }
// // System.out.println("Updated statuses!");
// }
// }catch(Exception e){
// e.printStackTrace();
// }
// }
// }.start();
// }
public static void register(Queryable q) {
queryables.add(q);
}
private static void Log(Queryable q, String s){
System.out.println(q.getName() + " - " + s);
}
public static void printReport() {
List<String> errors = new ArrayList<>();
// Subsystems header
System.out.println(REPORTS_HEADER);
for(int i=0;i< queryables.size();i++){
Queryable q = queryables.get(i);
System.out.println("** Subsystem diagnostic report for " + q.getName() + ":");
Status status = q.diagnosticStatus();
for(int a=0;a<status.reports.size();a++){
Report r = status.reports.get(a);
if(r.reportLevel == ReportLevel.ERROR)
errors.add(q.getName() + " - " + r.toString());
Log(q, r.toString());
}
}
// CAN header
System.out.println(CAN_HEADER);
CANBus canBus = new CANBus(Constants.CANBUS_NAME);
CANBusStatus canInfo = canBus.getStatus();
System.out.println("CANInfo BusOffCount - " + canInfo.BusOffCount);
System.out.println("CANInfo BusUtilization - " + canInfo.BusUtilization);
System.out.println("CANInfo RX Errors count - " + canInfo.REC);
System.out.println("CANInfo TX Errors count - " + canInfo.TEC);
System.out.println("CANInfo Transmit buffer full count - " + canInfo.TxFullCount);
// Broken turniary operator
ReportLevel canReportLevel = canInfo.Status.isOK() ? (canInfo.Status.isWarning() ? ReportLevel.WARNING : ReportLevel.ERROR) : ReportLevel.INFO;
String canStatus = "CAN " + canReportLevel.name() + " - " + canInfo.Status.getName() + " (" + canInfo.Status.getDescription() + ")";
if(canReportLevel == ReportLevel.ERROR) {
errors.add(canStatus);
}
System.out.println(canStatus);
// for(int i=0;i<CanDevice.devices.size();i++){
// CanDevice device = CanDevice.devices.get(i);
// System.out.println("** CAN diagnostic report for " + device.name + ":");
// Status status = device.diagnosticStatus();
// for(int a=0;a<status.reports.size();a++){
// Report r = status.reports.get(a);
// if(r.reportLevel == ReportLevel.ERROR)
// errors.add(device.getName() + " - " + r.toString());
// device.Log(r.toString());
// }
// }
// System.out.println("Found CAN devices: " + new DeviceFinder().Find());
if(errors.size() > 0) {
// Errors header
System.out.println(ERROR_HEADER);
for(int i=0;i<errors.size(); i++){
System.out.println(errors.get(i));
}
}
}
}
src/main/java/frc4388/utility/status/FaultTalonFX.java
+154
View File
@@ -0,0 +1,154 @@
package frc4388.utility.status;
import com.ctre.phoenix6.controls.EmptyControl;
import com.ctre.phoenix6.hardware.TalonFX;
import frc4388.utility.status.Status.ReportLevel;
public class FaultTalonFX implements Queryable {
private String name;
private TalonFX motor;
public static void addDevice(TalonFX motor, String name) {
FaultTalonFX p = new FaultTalonFX();
p.name = name;
p.motor = motor;
FaultReporter.register(p);
}
@Override
public String getName() {
return name;
}
@Override
public Status diagnosticStatus() {
Status s = new Status();
boolean debounceBad = !QueryUtils.isDebounceOk(motor.getSupplyVoltage());
boolean emptyControlBad = motor.setControl(new EmptyControl()).value != 0;
if(debounceBad || emptyControlBad) {
s.addReport(ReportLevel.ERROR, "device is unreachable - Failed" +
(debounceBad ? " Failed debounce test" : "") +
(emptyControlBad ? " Failed empty control test" : "")
);
}
s.addReport(ReportLevel.INFO, "Voltage: " + motor.getSupplyVoltage());
s.addReport(ReportLevel.INFO, "Current: " + motor.getSupplyCurrent());
s.addReport(ReportLevel.INFO, "Device temp: " + motor.getDeviceTemp());
s.addReport(ReportLevel.INFO, "Processor temp: " + motor.getProcessorTemp());
s.addReport(ReportLevel.INFO, "Position: " + motor.getPosition());
s.addReport(ReportLevel.INFO, "Velocity: " + motor.getVelocity());
s.addReport(ReportLevel.INFO, "Acceleration: " + motor.getAcceleration());
// faults<
if (motor.getFault_BootDuringEnable().getValue() == Boolean.TRUE) {
s.addReport(ReportLevel.ERROR, "Device booted while enabled");
}
if (motor.getFault_BridgeBrownout().getValue() == Boolean.TRUE) {
s.addReport(ReportLevel.ERROR, "Bridge was disabled most likely due to supply voltage dropping too low");
}
if (motor.getFault_DeviceTemp().getValue() == Boolean.TRUE) {
s.addReport(ReportLevel.ERROR, "Device temperature exceeded limit");
}
if (motor.getFault_FusedSensorOutOfSync().getValue() == Boolean.TRUE) {
s.addReport(ReportLevel.ERROR, "Remote sensor is out of sync");
}
if (motor.getFault_Hardware().getValue() == Boolean.TRUE) {
s.addReport(ReportLevel.ERROR, "Hardware failure detected");
}
if (motor.getFault_MissingDifferentialFX().getValue() == Boolean.TRUE) {
s.addReport(ReportLevel.ERROR, "The remote Talon used for differential control is not present");
}
if (motor.getFault_MissingHardLimitRemote().getValue() == Boolean.TRUE) {
s.addReport(ReportLevel.ERROR,"The remote limit switch device is not present");
}
if (motor.getFault_MissingSoftLimitRemote().getValue() == Boolean.TRUE) {
s.addReport(ReportLevel.ERROR, "The remote soft limit device is not present");
}
if (motor.getFault_OverSupplyV().getValue() == Boolean.TRUE) {
s.addReport(ReportLevel.ERROR, "Supply voltage exceeded limit");
}
if (motor.getFault_ProcTemp().getValue() == Boolean.TRUE) {
s.addReport(ReportLevel.ERROR, "Processor temperature exceeded limit");
}
if (motor.getFault_RemoteSensorDataInvalid().getValue() == Boolean.TRUE) {
s.addReport(ReportLevel.ERROR, "The remote sensor's data is no longer trusted");
}
if (motor.getFault_RemoteSensorPosOverflow().getValue() == Boolean.TRUE) {
s.addReport(ReportLevel.ERROR, "remote sensor position has overflowed");
}
if (motor.getFault_RemoteSensorReset().getValue() == Boolean.TRUE) {
s.addReport(ReportLevel.ERROR, "The remote sensor has reset");
}
if (motor.getFault_Undervoltage().getValue() == Boolean.TRUE) {
s.addReport(ReportLevel.ERROR, " Device supply voltage near brownout");
}
if (motor.getFault_UnlicensedFeatureInUse().getValue() == Boolean.TRUE) {
s.addReport(ReportLevel.ERROR, "Unlicensed feature in use");
}
if (motor.getFault_UnstableSupplyV().getValue() == Boolean.TRUE) {
s.addReport(ReportLevel.ERROR, "Supply voltage is unstable");
}
// sticky faults
if (motor.getStickyFault_BootDuringEnable().getValue() == Boolean.TRUE) {
s.addReport(ReportLevel.ERROR, "[STICKY] Device booted while enabled");
}
if (motor.getStickyFault_BridgeBrownout().getValue() == Boolean.TRUE) {
s.addReport(ReportLevel.ERROR, "[STICKY] Bridge was disabled most likely due to supply voltage dropping too low");
}
if (motor.getStickyFault_DeviceTemp().getValue() == Boolean.TRUE) {
s.addReport(ReportLevel.ERROR, "[STICKY] Device temperature exceeded limit");
}
if (motor.getStickyFault_FusedSensorOutOfSync().getValue() == Boolean.TRUE) {
s.addReport(ReportLevel.ERROR, "[STICKY] Remote sensor is out of sync");
}
if (motor.getStickyFault_Hardware().getValue() == Boolean.TRUE) {
s.addReport(ReportLevel.ERROR, "[STICKY] Hardware failure detected");
}
if (motor.getStickyFault_MissingDifferentialFX().getValue() == Boolean.TRUE) {
s.addReport(ReportLevel.ERROR, "[STICKY] The remote Talon used for differential control is not present");
}
if (motor.getStickyFault_MissingHardLimitRemote().getValue() == Boolean.TRUE) {
s.addReport(ReportLevel.ERROR, "[STICKY] The remote limit switch device is not present");
}
if (motor.getStickyFault_MissingSoftLimitRemote().getValue() == Boolean.TRUE) {
s.addReport(ReportLevel.ERROR, "[STICKY] The remote soft limit device is not present");
}
if (motor.getStickyFault_OverSupplyV().getValue() == Boolean.TRUE) {
s.addReport(ReportLevel.ERROR, "[STICKY] Supply voltage exceeded limit");
}
if (motor.getStickyFault_ProcTemp().getValue() == Boolean.TRUE) {
s.addReport(ReportLevel.ERROR, "[STICKY] Processor temperature exceeded limit");
}
if (motor.getStickyFault_RemoteSensorDataInvalid().getValue() == Boolean.TRUE) {
s.addReport(ReportLevel.ERROR, "[STICKY] The remote sensor's data is no longer trusted");
}
if (motor.getStickyFault_RemoteSensorPosOverflow().getValue() == Boolean.TRUE) {
s.addReport(ReportLevel.ERROR, "[STICKY] The remote sensor position has overflowed");
}
if (motor.getStickyFault_RemoteSensorReset().getValue() == Boolean.TRUE) {
s.addReport(ReportLevel.ERROR, "[STICKY] The remote sensor has reset");
}
if (motor.getStickyFault_Undervoltage().getValue() == Boolean.TRUE) {
s.addReport(ReportLevel.ERROR, "[STICKY] Device supply voltage near brownout");
}
if (motor.getStickyFault_UnlicensedFeatureInUse().getValue() == Boolean.TRUE) {
s.addReport(ReportLevel.ERROR, "[STICKY] Unlicensed feature in use");
}
if (motor.getStickyFault_UnstableSupplyV().getValue() == Boolean.TRUE) {
s.addReport(ReportLevel.ERROR, "[STICKY] Supply voltage is unstable");
}
return s;
}
}
src/main/java/frc4388/utility/status/QueryUtils.java
+13
View File
@@ -0,0 +1,13 @@
package frc4388.utility.status;
import com.ctre.phoenix6.StatusSignal;
import edu.wpi.first.math.filter.Debouncer;
public class QueryUtils {
public static boolean isDebounceOk(@SuppressWarnings("rawtypes") StatusSignal status) {
Debouncer connectedDebounce = new Debouncer(0.5);
status.refresh();
return connectedDebounce.calculate(status.getStatus().isOK());
};
}
src/main/java/frc4388/utility/status/Queryable.java
+8
View File
@@ -0,0 +1,8 @@
package frc4388.utility.status;
public interface Queryable {
// Get name of subsystem, for use in log.
String getName();
// Proactivly search for any errors in each subsystem
Status diagnosticStatus();
}
src/main/java/frc4388/utility/Status.java → src/main/java/frc4388/utility/status/Status.java
+5 -1
View File
@@ -1,4 +1,4 @@
package frc4388.utility;
package frc4388.utility.status;
import java.util.ArrayList;
import java.util.List;
@@ -44,6 +44,10 @@ public class Status {
}
public void diagnoseHardwareCTRE(String deviceName, TalonFX motor) {
addReport(ReportLevel.ERROR, deviceName + " Motor (TalonFX) Alive?: " + (motor.isAlive() ? "Alive." : "Dead!"));
if (motor.isAlive()) addReport(ReportLevel.INFO, deviceName + " Motor (TalonFX) Alive?: Alive.");
else addReport(ReportLevel.ERROR, deviceName + " Motor (TalonFX) Alive?: Dead!");
}
src/main/java/frc4388/utility/Gains.java → src/main/java/frc4388/utility/structs/Gains.java
+1 -1
View File
@@ -2,7 +2,7 @@
// 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.utility;
package frc4388.utility.structs;
/** Add your docs here. */
public class Gains {
src/main/java/frc4388/utility/LEDPatterns.java → src/main/java/frc4388/utility/structs/LEDPatterns.java
+1 -1
View File
@@ -1,4 +1,4 @@
package frc4388.utility;
package frc4388.utility.structs;
/**
* Add your docs here.
src/main/java/frc4388/utility/UtilityStructs.java → src/main/java/frc4388/utility/structs/UtilityStructs.java
+1 -1
View File
@@ -1,4 +1,4 @@
package frc4388.utility;
package frc4388.utility.structs;
public class UtilityStructs {
public static class TimedOutput {
vendordeps/AdvantageKit.json
+35
View File
@@ -0,0 +1,35 @@
{
"fileName": "AdvantageKit.json",
"name": "AdvantageKit",
"version": "4.1.2",
"uuid": "d820cc26-74e3-11ec-90d6-0242ac120003",
"frcYear": "2025",
"mavenUrls": [
"https://frcmaven.wpi.edu/artifactory/littletonrobotics-mvn-release/"
],
"jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json",
"javaDependencies": [
{
"groupId": "org.littletonrobotics.akit",
"artifactId": "akit-java",
"version": "4.1.2"
}
],
"jniDependencies": [
{
"groupId": "org.littletonrobotics.akit",
"artifactId": "akit-wpilibio",
"version": "4.1.2",
"skipInvalidPlatforms": false,
"isJar": false,
"validPlatforms": [
"linuxathena",
"linuxx86-64",
"linuxarm64",
"osxuniversal",
"windowsx86-64"
]
}
],
"cppDependencies": []
}
vendordeps/PathplannerLib-2025.2.4.json → vendordeps/PathplannerLib-2025.2.7.json
+4 -4
View File
@@ -1,7 +1,7 @@
{
"fileName": "PathplannerLib-2025.2.4.json",
"fileName": "PathplannerLib-2025.2.7.json",
"name": "PathplannerLib",
"version": "2025.2.4",
"version": "2025.2.7",
"uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786",
"frcYear": "2025",
"mavenUrls": [
@@ -12,7 +12,7 @@
{
"groupId": "com.pathplanner.lib",
"artifactId": "PathplannerLib-java",
"version": "2025.2.4"
"version": "2025.2.7"
}
],
"jniDependencies": [],
@@ -20,7 +20,7 @@
{
"groupId": "com.pathplanner.lib",
"artifactId": "PathplannerLib-cpp",
"version": "2025.2.4",
"version": "2025.2.7",
"libName": "PathplannerLib",
"headerClassifier": "headers",
"sharedLibrary": false,
vendordeps/Phoenix6-25.1.0.json → vendordeps/Phoenix6-frc2025-latest.json
+117 -27
View File
@@ -1,7 +1,7 @@
{
"fileName": "Phoenix6-25.1.0.json",
"fileName": "Phoenix6-frc2025-latest.json",
"name": "CTRE-Phoenix (v6)",
"version": "25.1.0",
"version": "25.4.0",
"frcYear": "2025",
"uuid": "e995de00-2c64-4df5-8831-c1441420ff19",
"mavenUrls": [
@@ -19,14 +19,14 @@
{
"groupId": "com.ctre.phoenix6",
"artifactId": "wpiapi-java",
"version": "25.1.0"
"version": "25.4.0"
}
],
"jniDependencies": [
{
"groupId": "com.ctre.phoenix6",
"artifactId": "api-cpp",
"version": "25.1.0",
"version": "25.4.0",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
@@ -40,7 +40,7 @@
{
"groupId": "com.ctre.phoenix6",
"artifactId": "tools",
"version": "25.1.0",
"version": "25.4.0",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
@@ -54,7 +54,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "api-cpp-sim",
"version": "25.1.0",
"version": "25.4.0",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
@@ -68,7 +68,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "tools-sim",
"version": "25.1.0",
"version": "25.4.0",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
@@ -82,7 +82,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simTalonSRX",
"version": "25.1.0",
"version": "25.4.0",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
@@ -96,7 +96,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simVictorSPX",
"version": "25.1.0",
"version": "25.4.0",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
@@ -110,7 +110,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simPigeonIMU",
"version": "25.1.0",
"version": "25.4.0",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
@@ -124,7 +124,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simCANCoder",
"version": "25.1.0",
"version": "25.4.0",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
@@ -138,7 +138,21 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProTalonFX",
"version": "25.1.0",
"version": "25.4.0",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
"windowsx86-64",
"linuxx86-64",
"linuxarm64",
"osxuniversal"
],
"simMode": "swsim"
},
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProTalonFXS",
"version": "25.4.0",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
@@ -152,7 +166,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProCANcoder",
"version": "25.1.0",
"version": "25.4.0",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
@@ -166,7 +180,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProPigeon2",
"version": "25.1.0",
"version": "25.4.0",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
@@ -180,7 +194,35 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProCANrange",
"version": "25.1.0",
"version": "25.4.0",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
"windowsx86-64",
"linuxx86-64",
"linuxarm64",
"osxuniversal"
],
"simMode": "swsim"
},
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProCANdi",
"version": "25.4.0",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
"windowsx86-64",
"linuxx86-64",
"linuxarm64",
"osxuniversal"
],
"simMode": "swsim"
},
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProCANdle",
"version": "25.4.0",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
@@ -196,7 +238,7 @@
{
"groupId": "com.ctre.phoenix6",
"artifactId": "wpiapi-cpp",
"version": "25.1.0",
"version": "25.4.0",
"libName": "CTRE_Phoenix6_WPI",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -212,7 +254,7 @@
{
"groupId": "com.ctre.phoenix6",
"artifactId": "tools",
"version": "25.1.0",
"version": "25.4.0",
"libName": "CTRE_PhoenixTools",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -228,7 +270,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "wpiapi-cpp-sim",
"version": "25.1.0",
"version": "25.4.0",
"libName": "CTRE_Phoenix6_WPISim",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -244,7 +286,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "tools-sim",
"version": "25.1.0",
"version": "25.4.0",
"libName": "CTRE_PhoenixTools_Sim",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -260,7 +302,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simTalonSRX",
"version": "25.1.0",
"version": "25.4.0",
"libName": "CTRE_SimTalonSRX",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -276,7 +318,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simVictorSPX",
"version": "25.1.0",
"version": "25.4.0",
"libName": "CTRE_SimVictorSPX",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -292,7 +334,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simPigeonIMU",
"version": "25.1.0",
"version": "25.4.0",
"libName": "CTRE_SimPigeonIMU",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -308,7 +350,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simCANCoder",
"version": "25.1.0",
"version": "25.4.0",
"libName": "CTRE_SimCANCoder",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -324,7 +366,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProTalonFX",
"version": "25.1.0",
"version": "25.4.0",
"libName": "CTRE_SimProTalonFX",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -337,10 +379,26 @@
],
"simMode": "swsim"
},
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProTalonFXS",
"version": "25.4.0",
"libName": "CTRE_SimProTalonFXS",
"headerClassifier": "headers",
"sharedLibrary": true,
"skipInvalidPlatforms": true,
"binaryPlatforms": [
"windowsx86-64",
"linuxx86-64",
"linuxarm64",
"osxuniversal"
],
"simMode": "swsim"
},
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProCANcoder",
"version": "25.1.0",
"version": "25.4.0",
"libName": "CTRE_SimProCANcoder",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -356,7 +414,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProPigeon2",
"version": "25.1.0",
"version": "25.4.0",
"libName": "CTRE_SimProPigeon2",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -372,7 +430,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProCANrange",
"version": "25.1.0",
"version": "25.4.0",
"libName": "CTRE_SimProCANrange",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -384,6 +442,38 @@
"osxuniversal"
],
"simMode": "swsim"
},
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProCANdi",
"version": "25.4.0",
"libName": "CTRE_SimProCANdi",
"headerClassifier": "headers",
"sharedLibrary": true,
"skipInvalidPlatforms": true,
"binaryPlatforms": [
"windowsx86-64",
"linuxx86-64",
"linuxarm64",
"osxuniversal"
],
"simMode": "swsim"
},
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProCANdle",
"version": "25.4.0",
"libName": "CTRE_SimProCANdle",
"headerClassifier": "headers",
"sharedLibrary": true,
"skipInvalidPlatforms": true,
"binaryPlatforms": [
"windowsx86-64",
"linuxx86-64",
"linuxarm64",
"osxuniversal"
],
"simMode": "swsim"
}
]
}
vendordeps/photonlib.json
+6 -6
View File
@@ -1,7 +1,7 @@
{
"fileName": "photonlib.json",
"name": "photonlib",
"version": "v2025.0.0-beta-8",
"version": "v2025.3.1",
"uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004",
"frcYear": "2025",
"mavenUrls": [
@@ -13,7 +13,7 @@
{
"groupId": "org.photonvision",
"artifactId": "photontargeting-cpp",
"version": "v2025.0.0-beta-8",
"version": "v2025.3.1",
"skipInvalidPlatforms": true,
"isJar": false,
"validPlatforms": [
@@ -28,7 +28,7 @@
{
"groupId": "org.photonvision",
"artifactId": "photonlib-cpp",
"version": "v2025.0.0-beta-8",
"version": "v2025.3.1",
"libName": "photonlib",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -43,7 +43,7 @@
{
"groupId": "org.photonvision",
"artifactId": "photontargeting-cpp",
"version": "v2025.0.0-beta-8",
"version": "v2025.3.1",
"libName": "photontargeting",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -60,12 +60,12 @@
{
"groupId": "org.photonvision",
"artifactId": "photonlib-java",
"version": "v2025.0.0-beta-8"
"version": "v2025.3.1"
},
{
"groupId": "org.photonvision",
"artifactId": "photontargeting-java",
"version": "v2025.0.0-beta-8"
"version": "v2025.3.1"
}
]
}