Team4388/2025RidgeScape
1
0
Fork 0
mirror of https://github.com/Team4388/2025RidgeScape.git synced 2026-06-09 00:38:02 -06:00
Code Issues Packages Projects Releases Wiki Activity

The great code restructuring

Browse Source
This commit is contained in:
Michael Mikovsky
2025-07-11 14:07:53 -06:00
parent 06d525ade1
commit bf4da44338
52 changed files with 1142 additions and 970 deletions
Download Patch File Download Diff File Expand all files Collapse all files
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
+118 -14
View File
@@ -11,24 +11,31 @@ 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.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
import frc4388.utility.CanDevice;
import frc4388.robot.constants.BuildConstants;
import frc4388.robot.constants.Constants;
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.status.CanDevice;
import frc4388.utility.status.Status;
import frc4388.utility.status.Subsystem;
import frc4388.utility.status.Status.Report;
import frc4388.utility.status.Status.ReportLevel;
//import frc4388.robot.subsystems.LED;
/**
* The VM is configured to automatically run this class, and to call the
@@ -37,7 +44,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();
@@ -55,7 +62,7 @@ public class Robot extends TimedRobot {
m_robotContainer = new RobotContainer();
// Create a shuffleboard update thread, that will periodically update the values on shuffleboard
new Thread() {
public void run() {
try{
@@ -69,7 +76,7 @@ public class Robot extends TimedRobot {
}
}catch(Exception e){
e.printStackTrace();
e.printStackTrace();
}
}
}.start();
@@ -248,4 +255,101 @@ public class Robot extends TimedRobot {
}
}
}
// 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("GitSHA", 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
+51 -85
View File
@@ -11,34 +11,21 @@ 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.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,20 +34,19 @@ 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 com.pathplanner.lib.auto.NamedCommands;
import com.pathplanner.lib.commands.PathPlannerAuto;
@@ -68,7 +54,6 @@ import com.pathplanner.lib.commands.PathPlannerAuto;
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;
@@ -76,11 +61,8 @@ import frc4388.robot.subsystems.SwerveDrive;
// 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
@@ -97,8 +79,6 @@ public class RobotContainer {
/* 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 SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision);
// public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain);
@@ -108,11 +88,6 @@ public class RobotContainer {
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 +99,6 @@ public class RobotContainer {
}
// ! /* Autos */
private String lastAutoName = "defualt.auto";
private SendableChooser<String> autoChooser;
private Command autoCommand;
@@ -144,7 +118,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 +127,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),
@@ -186,7 +160,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 +169,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 +217,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 +253,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 +281,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 +290,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, m_robotMap.reefLidar),
waitDebuger.asProxy(),
new ParallelRaceGroup(
new WaitCommand(1),
@@ -354,7 +328,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 +337,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, m_robotMap.reefLidar),
// waitDebuger.asProxy(),
// new ParallelRaceGroup(
// new WaitCommand(1),
@@ -398,15 +372,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, m_robotMap.reefLidar),
waitDebuger.asProxy(),
// new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringThree), m_robotElevator),
new waitEndefectorRefrence(m_robotElevator),
@@ -423,7 +397,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 +406,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, m_robotMap.reefLidar),
waitDebuger.asProxy(),
// new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringThree), m_robotElevator),
@@ -450,12 +424,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, m_robotMap.reefLidar),
waitDebuger.asProxy(),
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.L2Score);}, m_robotElevator),
new waitEndefectorRefrence(m_robotElevator),
@@ -471,12 +445,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, m_robotMap.reefLidar),
waitDebuger.asProxy(),
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.L2Score);}, m_robotElevator),
new waitEndefectorRefrence(m_robotElevator),
@@ -495,7 +469,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 +480,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 +500,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 +510,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,7 +558,7 @@ 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(), m_robotMap.reverseLidar, LiDARConstants.HUMAN_PLAYER_STATION_DISTANCE));
// NamedCommands.registerCommand("feed-driveback", Commands.none());
NamedCommands.registerCommand("stop", new waitSupplier(() -> m_robotSwerveDrive.lastOdomSpeed < AutoConstants.STOP_VELOCITY));
@@ -688,7 +662,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 +713,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, m_robotMap.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
@@ -966,12 +940,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
+56 -18
View File
@@ -10,22 +10,18 @@ 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.VisionConstants;
import frc4388.robot.constants.DriveConstants;
import frc4388.robot.subsystems.Lidar;
/**
* Defines and holds all I/O objects on the Roborio. This is useful for unit
@@ -35,8 +31,12 @@ 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 final PhotonCamera leftCamera = new PhotonCamera(VisionConstants.LEFT_CAMERA_NAME);
public final PhotonCamera rightCamera = new PhotonCamera(VisionConstants.RIGHT_CAMERA_NAME);
public final Lidar reefLidar = new Lidar(LiDARConstants.REEF_LIDAR_DIO_CHANNEL, "Reef");
public final Lidar reverseLidar = new Lidar(LiDARConstants.REVERSE_LIDAR_DIO_CHANNEL, "Reverse");
public RobotMap() {
configureDriveMotorControllers();
@@ -47,9 +47,9 @@ public class RobotMap {
/* 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
DriveConstants.DrivetrainConstants,
DriveConstants.FRONT_LEFT, DriveConstants.FRONT_RIGHT,
DriveConstants.BACK_LEFT, DriveConstants.BACK_RIGHT
);
/* Elevator Subsystem */
@@ -64,5 +64,43 @@ public class RobotMap {
void configureDriveMotorControllers() {
// endeffector.saf
}
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
+2 -2
View File
@@ -5,7 +5,6 @@ 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;
// 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
-2
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;
// 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
+9 -74
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.constants.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.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
+2 -8
View File
@@ -1,12 +1,9 @@
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;
// Command to repeat a joystick movement for a specific time.
public class DriveUntilLiDAR extends Command {
@@ -15,15 +12,13 @@ public class DriveUntilLiDAR extends Command {
private final Translation2d rightStick;
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) {
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
+2 -2
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.alignment;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
@@ -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
+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.Swerve;
package frc4388.robot.commands.swerve;
import edu.wpi.first.math.geometry.Translation2d;
import frc4388.robot.commands.PID;
src/main/java/frc4388/robot/commands/Swerve/neoJoystickPlayback.java → src/main/java/frc4388/robot/commands/swerve/neoJoystickPlayback.java
+8 -8
View File
@@ -1,4 +1,4 @@
package frc4388.robot.commands.Swerve;
package frc4388.robot.commands.swerve;
import java.io.FileInputStream;
import java.util.ArrayList;
@@ -7,10 +7,10 @@ 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.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
+4 -4
View File
@@ -1,4 +1,4 @@
package frc4388.robot.commands.Swerve;
package frc4388.robot.commands.swerve;
import java.io.FileOutputStream;
import java.util.ArrayList;
@@ -8,10 +8,10 @@ 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.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
+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 edu.wpi.first.wpilibj2.command.Command;
import frc4388.robot.subsystems.Elevator;
src/main/java/frc4388/robot/commands/waitEndefectorRefrence.java → src/main/java/frc4388/robot/commands/wait/waitEndefectorRefrence.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 edu.wpi.first.wpilibj2.command.Command;
import frc4388.robot.subsystems.Elevator;
src/main/java/frc4388/robot/commands/waitFeedCoral.java → src/main/java/frc4388/robot/commands/wait/waitFeedCoral.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 edu.wpi.first.wpilibj2.command.Command;
import frc4388.robot.subsystems.Elevator;
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 = 164;
public static final String GIT_SHA = "06d525ade1118ae193383bc86066ea5563d86a1f";
public static final String GIT_DATE = "2025-04-22 18:21:00 EDT";
public static final String GIT_BRANCH = "advantagekit";
public static final String BUILD_DATE = "2025-07-11 15:07:31 EDT";
public static final long BUILD_UNIX_TIME = 1752260851324L;
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.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/constants/DriveConstants.java
+247
View File
@@ -0,0 +1,247 @@
package frc4388.robot.constants;
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 DriveConstants {
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/DiffDrive.java
+4 -5
View File
@@ -12,12 +12,11 @@ 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.robot.constants.DriveConstants;
import frc4388.utility.RobotGyro;
import frc4388.utility.RobotTime;
import frc4388.utility.Status;
import frc4388.utility.Subsystem;
import frc4388.utility.compute.RobotTime;
import frc4388.utility.status.Status;
import frc4388.utility.status.Subsystem;
/**
* Add your docs here.
src/main/java/frc4388/robot/subsystems/Elevator.java
+8 -20
View File
@@ -4,32 +4,18 @@
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;
import frc4388.robot.constants.Constants.AutoConstants;
import frc4388.robot.constants.Constants.ElevatorConstants;
import frc4388.robot.constants.Constants.LEDConstants;
import frc4388.utility.status.Status;
import frc4388.utility.status.Subsystem;
import frc4388.utility.status.Status.ReportLevel;
public class Elevator extends Subsystem {
/** Creates a new Elevator. */
@@ -37,6 +23,7 @@ public class Elevator extends Subsystem {
private TalonFX endeffectorMotor;
private LED led;
@SuppressWarnings("unused")
private long wait = 0;
private long maxWait = 1000;
@@ -278,6 +265,7 @@ public class Elevator extends Subsystem {
transitionState(CoordinationState.Hovering);
}
@SuppressWarnings("unused")
private void periodicScoring() {
if (!endeffectorLimitSwitch.get())
transitionState(CoordinationState.Waiting);
src/main/java/frc4388/robot/subsystems/LED.java
+5 -9
View File
@@ -7,18 +7,14 @@
package frc4388.robot.subsystems;
import edu.wpi.first.wpilibj.AddressableLED;
import edu.wpi.first.wpilibj.AddressableLEDBuffer;
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.Subsystem;
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
src/main/java/frc4388/robot/subsystems/Lidar.java
+4 -5
View File
@@ -1,16 +1,15 @@
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;
import frc4388.robot.constants.Constants.LiDARConstants;
import frc4388.utility.status.Status;
import frc4388.utility.status.Subsystem;
import frc4388.utility.status.Status.ReportLevel;
// https://girlsofsteeldocs.readthedocs.io/en/latest/technical-resources/sensors/LIDAR-Lite-Distance-Sensor.html#minimal-roborio-interface
public class Lidar extends Subsystem {
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+74 -68
View File
@@ -5,7 +5,8 @@
package frc4388.robot.subsystems;
import java.util.Optional;
import java.util.function.DoubleSupplier;
import org.littletonrobotics.junction.Logger;
import com.ctre.phoenix6.Utils;
import com.ctre.phoenix6.configs.TalonFXConfiguration;
@@ -14,34 +15,26 @@ 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 frc4388.robot.constants.DriveConstants;
import frc4388.utility.compute.TimesNegativeOne;
import frc4388.utility.status.Status;
import frc4388.utility.status.Subsystem;
import frc4388.utility.status.Status.ReportLevel;
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 {
@@ -49,13 +42,14 @@ public class SwerveDrive extends Subsystem {
private Vision vision;
private int gear_index = SwerveDriveConstants.STARTING_GEAR;
private int gear_index = DriveConstants.STARTING_GEAR;
private boolean stopped = false;
@SuppressWarnings("unused")
private 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
public double speedAdjust = DriveConstants.MAX_SPEED_MEETERS_PER_SEC * DriveConstants.GEARS[gear_index];
public double rotSpeedAdjust = DriveConstants.MAX_ROT_SPEED;
public double autoSpeedAdjust = DriveConstants.MAX_SPEED_MEETERS_PER_SEC * 0.25; // cap auto performance to
// 25%
public double lastOdomSpeed;
@@ -115,6 +109,30 @@ 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", state.toString())),
// new SysIdRoutine.Mechanism(
// (voltage) -> runCharacterization(voltage.in(Volts)), null, this));
}
public void setOdoPose(Pose2d pose) {
@@ -153,7 +171,7 @@ public class SwerveDrive extends Subsystem {
rightStick = TimesNegativeOne.invert(rightStick, TimesNegativeOne.RotAxis);
// ! drift correction
if (rightStick.getNorm() > 0.05 || !SwerveDriveConstants.DRIFT_CORRECTION_ENABLED) {
if (rightStick.getNorm() > 0.05 || !DriveConstants.DRIFT_CORRECTION_ENABLED) {
rotTarget = swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(new Pose2d()).getRotation().getDegrees();
swerveDriveTrain.setControl(new SwerveRequest.FieldCentric()
.withVelocityX(leftStick.getX() * speedAdjust)
@@ -167,9 +185,9 @@ public class SwerveDrive extends Subsystem {
.withVelocityY(leftStick.getY() * speedAdjust)
.withTargetDirection(Rotation2d.fromDegrees(rotTarget));
ctrl.HeadingController.setPID(
SwerveDriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kP,
SwerveDriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kI,
SwerveDriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kD
DriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kP,
DriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kI,
DriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kD
);
swerveDriveTrain.setControl(ctrl);
SmartDashboard.putBoolean("drift correction", true);
@@ -189,8 +207,8 @@ public class SwerveDrive extends Subsystem {
// Create robot-relative speeds.
if (rightStick.getNorm() > 0.1) rightStick = rightStick.times(0);
swerveDriveTrain.setControl(new SwerveRequest.RobotCentric()
.withVelocityX(leftStick.getX() * SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * percentOutput)
.withVelocityY(-leftStick.getY() * SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * percentOutput)
.withVelocityX(leftStick.getX() * DriveConstants.MAX_SPEED_MEETERS_PER_SEC * percentOutput)
.withVelocityY(-leftStick.getY() * DriveConstants.MAX_SPEED_MEETERS_PER_SEC * percentOutput)
.withRotationalRate(rightStick.getX() * rotSpeedAdjust));
}
@@ -224,9 +242,9 @@ 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);
}
@@ -239,9 +257,9 @@ 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);
}
@@ -263,7 +281,7 @@ public class SwerveDrive extends Subsystem {
}
public void deactivateLuigiMode() {
setLimits(SwerveDriveConstants.Configurations.SLIP_CURRENT);
setLimits(DriveConstants.Configurations.SLIP_CURRENT);
}
public boolean rotateToTarget(double angle) {
@@ -342,77 +360,65 @@ public class SwerveDrive extends Subsystem {
vision.setLastOdomPose(curpose);
setLastOdomSpeed(curpose, lastPose, freq);
if (vision.isTag()) {
Pose2d pose = vision.getPose2d();
if (!robotKnowsWhereItIs) {
robotKnowsWhereItIs = true;
Pose2d currPose = getPose2d();
double deltaAngle = pose.getRotation().getDegrees() - currPose.getRotation().getDegrees();
rotTarget += deltaAngle;
}
vision.addVisionMeasurement(swerveDriveTrain);
// swerveDriveTrain.addVisionMeasurement(vision.lastVisionPose, time);
//back to the ~~future~~ *past*
}
// if (vision.isTag()) {5
// if(e.isPresent())
}
private void reset_index() {
gear_index = SwerveDriveConstants.STARTING_GEAR; // however we wish to initialize the gear (What gear does the
gear_index = DriveConstants.STARTING_GEAR; // however we wish to initialize the gear (What gear does the
// robot start in?)
}
public void shiftDown() {
if (gear_index == -1 || gear_index >= SwerveDriveConstants.GEARS.length)
if (gear_index == -1 || gear_index >= DriveConstants.GEARS.length)
reset_index(); // If outof bounds: reset index
int i = gear_index - 1;
if (i == -1)
i = 0;
setPercentOutput(SwerveDriveConstants.GEARS[i]);
setPercentOutput(DriveConstants.GEARS[i]);
gear_index = i;
}
public void shiftUp() {
if (gear_index == -1 || gear_index >= SwerveDriveConstants.GEARS.length)
if (gear_index == -1 || gear_index >= DriveConstants.GEARS.length)
reset_index(); // If outof bounds: reset index
int i = gear_index + 1;
if (i == SwerveDriveConstants.GEARS.length)
i = SwerveDriveConstants.GEARS.length - 1;
setPercentOutput(SwerveDriveConstants.GEARS[i]);
if (i == DriveConstants.GEARS.length)
i = DriveConstants.GEARS.length - 1;
setPercentOutput(DriveConstants.GEARS[i]);
gear_index = i;
}
public void setPercentOutput(double speed) {
speedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * speed;
speedAdjust = DriveConstants.MAX_SPEED_MEETERS_PER_SEC * speed;
gear_index = -1;
}
public void setToSlow() {
setPercentOutput(SwerveDriveConstants.SLOW_SPEED);
setPercentOutput(DriveConstants.SLOW_SPEED);
gear_index = 0;
}
public void setToFast() {
setPercentOutput(SwerveDriveConstants.FAST_SPEED);
setPercentOutput(DriveConstants.FAST_SPEED);
gear_index = 1;
}
public void setToTurbo() {
setPercentOutput(SwerveDriveConstants.TURBO_SPEED);
setPercentOutput(DriveConstants.TURBO_SPEED);
gear_index = 2;
}
public void shiftUpRot() {
rotSpeedAdjust = SwerveDriveConstants.ROTATION_SPEED;
rotSpeedAdjust = DriveConstants.ROTATION_SPEED;
}
public void shiftDownRot() {
rotSpeedAdjust = SwerveDriveConstants.MIN_ROT_SPEED;
rotSpeedAdjust = DriveConstants.MIN_ROT_SPEED;
}
private int tmp_gear_index = SwerveDriveConstants.STARTING_GEAR;
private int tmp_gear_index = DriveConstants.STARTING_GEAR;
public void startSlowPeriod() {
tmp_gear_index = gear_index;
@@ -425,7 +431,7 @@ public class SwerveDrive extends Subsystem {
}
public void endSlowPeriod() {
setPercentOutput(SwerveDriveConstants.GEARS[tmp_gear_index]);
setPercentOutput(DriveConstants.GEARS[tmp_gear_index]);
gear_index = tmp_gear_index;
}
@@ -435,17 +441,10 @@ public class SwerveDrive extends Subsystem {
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);
}
}
@Override
@@ -484,4 +483,11 @@ public class SwerveDrive extends Subsystem {
return status;
}
// Update CTRE simulation, if used.
public void updateSim(double voltage) {
swerveDriveTrain.updateSimState(0.02, voltage);
}
}
src/main/java/frc4388/robot/subsystems/Vision.java
+14 -52
View File
@@ -1,15 +1,11 @@
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;
@@ -18,13 +14,7 @@ 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;
@@ -37,11 +27,11 @@ 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;
import frc4388.robot.constants.Constants.FieldConstants;
import frc4388.robot.constants.Constants.VisionConstants;
import frc4388.utility.status.Status;
import frc4388.utility.status.Subsystem;
import frc4388.utility.status.Status.ReportLevel;
public class Vision extends Subsystem {
@@ -104,39 +94,26 @@ public class Vision extends Subsystem {
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 Instant lastVisionTime = null;
private void update() {
isTagProcessed = false;
isTagDetected = false;
Instant now = Instant.now();
// Instant now = Instant.now();
int cams = 0;
// int cams = 0;
// double X = 0;
// double Y = 0;
// double Yaw = 0;
double latency = 0;
// double latency = 0;
// Pose2d pose = null;
poses.clear();
@@ -153,7 +130,7 @@ public class Vision extends Subsystem {
var result = results.get(results.size()-1);
latency += result.getTimestampSeconds();
// latency += result.getTimestampSeconds();
isTagDetected = isTagDetected | result.hasTargets();
@@ -185,21 +162,6 @@ public class Vision extends Subsystem {
}
// 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;
// }
}
@@ -315,11 +277,11 @@ public class Vision extends Subsystem {
public Pose2d getPose2d() {
if(lastPhysOdomPose != null)
return lastPhysOdomPose;
// if(lastVisionPose != null)
// return lastVisionPose;
return new Pose2d();
// if(isTagDetected && isTagProcessed)
// // return lastVisionPose;
// else
// return lastPhysOdomPose;
}
public static double getTime() {
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/RobotGyro.java
+2
View File
@@ -15,6 +15,8 @@ import com.kauailabs.navx.frc.AHRS;
// import edu.wpi.first.wpilibj.interfaces.Gyro;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.geometry.Rotation2d;
import frc4388.utility.compute.RobotTime;
import frc4388.utility.compute.RobotUnits;
/**
* Gyro class that allows for interchangeable use between a pigeon and a navX
src/main/java/frc4388/utility/Trim.java
+45 -18
View File
@@ -11,6 +11,7 @@ import java.util.ArrayList;
import edu.wpi.first.networktables.GenericEntry;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
import frc4388.utility.compute.DataUtils;
/**
* Reboot persistant Trims.
@@ -27,11 +28,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 +105,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/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
+10 -10
View File
@@ -1,4 +1,4 @@
package frc4388.utility;
package frc4388.utility.compute;
import java.util.Optional;
@@ -7,16 +7,16 @@ 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.constants.DriveConstants;
// Class that holds weather the drivers sticks should be inverted
public class TimesNegativeOne {
public static boolean XAxis = SwerveDriveConstants.INVERT_X;
public static boolean YAxis = SwerveDriveConstants.INVERT_Y;
public static boolean RotAxis = SwerveDriveConstants.INVERT_ROTATION;
public static boolean XAxis = DriveConstants.INVERT_X;
public static boolean YAxis = DriveConstants.INVERT_Y;
public static boolean RotAxis = DriveConstants.INVERT_ROTATION;
public static boolean isRed = false;
public static Rotation2d ForwardOffset = Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET);
public static Rotation2d ForwardOffset = Rotation2d.fromDegrees(DriveConstants.FORWARD_OFFSET);
private static boolean isRed() {
Optional<Alliance> alliance = DriverStation.getAlliance();
@@ -26,10 +26,10 @@ public class TimesNegativeOne {
public static void update(){
isRed = isRed();
XAxis = SwerveDriveConstants.INVERT_X ^ isRed;
YAxis = SwerveDriveConstants.INVERT_Y ^ isRed;
RotAxis = SwerveDriveConstants.INVERT_ROTATION;
ForwardOffset = Rotation2d.fromDegrees((SwerveDriveConstants.FORWARD_OFFSET + (isRed ? 0 : 0)));
XAxis = DriveConstants.INVERT_X ^ isRed;
YAxis = DriveConstants.INVERT_Y ^ isRed;
RotAxis = DriveConstants.INVERT_ROTATION;
ForwardOffset = Rotation2d.fromDegrees((DriveConstants.FORWARD_OFFSET + (isRed ? 0 : 0)));
SmartDashboard.putBoolean("Is red alliance", isRed);
}
src/main/java/frc4388/utility/controller/DeadbandedXboxController.java
+1 -1
View File
@@ -1,6 +1,6 @@
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/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.java → src/main/java/frc4388/utility/status/Status.java
+1 -1
View File
@@ -1,4 +1,4 @@
package frc4388.utility;
package frc4388.utility.status;
import java.util.ArrayList;
import java.util.List;
src/main/java/frc4388/utility/Subsystem.java → src/main/java/frc4388/utility/status/Subsystem.java
+1 -1
View File
@@ -1,4 +1,4 @@
package frc4388.utility;
package frc4388.utility.status;
import java.util.ArrayList;
import java.util.List;
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 {