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

Auto align Consistency improvements

Browse Source
This commit is contained in:
Michael Mikovsky
2025-02-28 11:46:48 -07:00
parent a063f4b309
commit 90684ffc7b
7 changed files with 85 additions and 139 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+42 -47
View File
@@ -230,28 +230,6 @@ public final class Constants {
public static final Gains RELATIVE_LOCKED_ANGLE_GAINS = new Gains(5, 0, 0.1); // TODO: TEST
}
public static final class AutoConstants {
public static final Gains XY_GAINS = new Gains(3,0.01,0.0);
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, 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 int LIDAR_DETECT_DISTANCE = 100; // Min distance to detect pole
public static final int LIDAR_DIO_CHANNEL = 7;
public static final int LIDAR_MICROS_TO_CM = 10;
public static final int SECONDS_TO_MICROS = 1000000;
public static final double XY_TOLERANCE = 0.07; // Meters
public static final double ROT_TOLERANCE = 5; // Degrees
// public static final Pose2d targetpos = new Pose2d(new Translation2d(0.3,0), new Rotation2d());
// public static final Pose2d targetpos =
}
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.
@@ -342,6 +320,48 @@ public final class Constants {
public static final int SMARTDASHBOARD_UPDATE_FRAME = 2;
}
public static final class LiDARConstants {
public static final int LIDAR_DETECT_DISTANCE = 100; // Min distance to detect pole
public static final int LIDAR_DIO_CHANNEL = 7;
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(3,0.01,0.0);
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, 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
// X is tangent to reef side
// Y is normal to reef side
public static final double X_SCORING_POSITION_OFFSET = Units.inchesToMeters(6.5); // This is from the field
public static final double Y_SCORING_POSITION_OFFSET = Units.inchesToMeters(16);
public static final double L4_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(12);
public static final double L4_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(6);
public static final double L3_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(12);
public static final double L3_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(6);
public static final double L2_SCORE_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(1);
public static final double ALGAE_REMOVAL_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(3);
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 class VisionConstants {
public static final String LEFT_CAMERA_NAME = "CAMERA_LEFT";
public static final String RIGHT_CAMERA_NAME = "CAMERA_RIGHT";
@@ -360,31 +380,6 @@ public final class Constants {
public static final class FieldConstants {
public static final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2025Reefscape);
// public static final double HORISONTAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(9.5);
public static final double HORISONTAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(6.5);
// public static final double HORISONTAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(6.5); // Positive is Right
public static final double VERTICAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(16);
public static final double L4_DISTANCE_1 = VERTICAL_SCORING_POSITION_OFFSET + Units.inchesToMeters(6);
public static final double L4_DISTANCE_2 = VERTICAL_SCORING_POSITION_OFFSET + Units.inchesToMeters(12);
public static final double L3_DISTANCE_1 = VERTICAL_SCORING_POSITION_OFFSET + Units.inchesToMeters(6);
public static final double L3_DISTANCE_2 = VERTICAL_SCORING_POSITION_OFFSET + Units.inchesToMeters(12);
public static final double L2_ALGAE_REMOVAL = VERTICAL_SCORING_POSITION_OFFSET + Units.inchesToMeters(3);
public static final double L2_SCORE_DISTANCE = VERTICAL_SCORING_POSITION_OFFSET + Units.inchesToMeters(1);
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;
// Test april tag field layout
// public static final AprilTagFieldLayout kTagLayout = new AprilTagFieldLayout(
src/main/java/frc4388/robot/RobotContainer.java
+32 -49
View File
@@ -28,7 +28,7 @@ import frc4388.utility.controller.DeadbandedXboxController;
import frc4388.robot.Constants.FieldConstants;
import frc4388.robot.Constants.OIConstants;
import frc4388.robot.Constants.SwerveDriveConstants;
import frc4388.robot.Constants.SwerveDriveConstants.AutoConstants;
import frc4388.robot.Constants.AutoConstants;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import edu.wpi.first.wpilibj2.command.button.Trigger;
@@ -39,11 +39,10 @@ import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import edu.wpi.first.wpilibj2.command.WaitCommand;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.ConditionalCommand;
// Autos
import frc4388.utility.controller.VirtualController;
import frc4388.robot.commands.GotoLastApril;
import frc4388.robot.commands.IfCommand;
import frc4388.robot.commands.LidarAlign;
import frc4388.robot.commands.MoveForTimeCommand;
import frc4388.robot.commands.waitElevatorRefrence;
@@ -124,10 +123,10 @@ public class RobotContainer {
private Command AprilLidarAlignL4Right = new SequentialCommandGroup(
new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
// new IfCommand(() -> m_robotElevator.isL4Primed(), new SequentialCommandGroup(
new ConditionalCommand(Commands.none(), new SequentialCommandGroup(
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.RIGHT),
// )),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.RIGHT)
), () -> m_robotElevator.isL4Primed()),
// new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
// new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.RIGHT),
@@ -135,7 +134,7 @@ public class RobotContainer {
new waitEndefectorRefrence(m_robotElevator),
new waitElevatorRefrence(m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_1, Side.RIGHT),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.RIGHT),
new LidarAlign(m_robotSwerveDrive, m_lidar),
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator),
@@ -143,7 +142,7 @@ public class RobotContainer {
new waitEndefectorRefrence(m_robotElevator),
new MoveForTimeCommand(m_robotSwerveDrive,
new Translation2d(0,1), new Translation2d(), FieldConstants.L4_DRIVE_TIME, true),
new Translation2d(0,1), new Translation2d(), AutoConstants.L4_DRIVE_TIME, true),
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator),
new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
@@ -153,9 +152,11 @@ public class RobotContainer {
new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
// new IfCommand(() -> m_robotElevator.isL4Primed(), new SequentialCommandGroup(
new ConditionalCommand(Commands.none(), new SequentialCommandGroup(
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.LEFT),
// )),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.LEFT)
), () -> m_robotElevator.isL4Primed()),
// new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
// new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.LEFT),
@@ -163,7 +164,7 @@ public class RobotContainer {
new waitEndefectorRefrence(m_robotElevator),
new waitElevatorRefrence(m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_1, Side.LEFT),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.LEFT),
new LidarAlign(m_robotSwerveDrive, m_lidar),
@@ -172,7 +173,7 @@ public class RobotContainer {
new waitEndefectorRefrence(m_robotElevator),
new MoveForTimeCommand(m_robotSwerveDrive,
new Translation2d(0,1), new Translation2d(), FieldConstants.L4_DRIVE_TIME, true),
new Translation2d(0,1), new Translation2d(), AutoConstants.L4_DRIVE_TIME, true),
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator),
@@ -182,15 +183,16 @@ public class RobotContainer {
private Command AprilLidarAlignL3Left = new SequentialCommandGroup(
new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
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, FieldConstants.L3_DISTANCE_2, Side.LEFT),
// )),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_PREP, Side.LEFT)
), () -> m_robotElevator.isL3Primed()),
new waitEndefectorRefrence(m_robotElevator),
new waitElevatorRefrence(m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L3_DISTANCE_1 - Units.inchesToMeters(0), Side.LEFT),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_SCORE, Side.LEFT),
new LidarAlign(m_robotSwerveDrive, m_lidar),
// new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringThree), m_robotElevator),
new waitEndefectorRefrence(m_robotElevator),
@@ -204,18 +206,19 @@ public class RobotContainer {
private Command AprilLidarAlignL3Right = new SequentialCommandGroup(
new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
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, FieldConstants.L3_DISTANCE_2, Side.RIGHT),
// )),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_PREP, Side.RIGHT)
),() -> m_robotElevator.isL3Primed()),
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L3_DISTANCE_2, Side.RIGHT),
// new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator),
// new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L3_DISTANCE_2, Side.RIGHT),
new waitEndefectorRefrence(m_robotElevator),
new waitElevatorRefrence(m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L3_DISTANCE_1 - Units.inchesToMeters(0), Side.RIGHT),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_SCORE, Side.RIGHT),
new LidarAlign(m_robotSwerveDrive, m_lidar),
@@ -231,14 +234,14 @@ public class RobotContainer {
private Command AprilLidarAlignL2Left = new SequentialCommandGroup(
new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L2_SCORE_DISTANCE, Side.LEFT),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L2_SCORE_DISTANCE, Side.LEFT),
new LidarAlign(m_robotSwerveDrive, m_lidar),
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.L2Score);}, m_robotElevator),
new waitEndefectorRefrence(m_robotElevator),
new waitElevatorRefrence(m_robotElevator),
new MoveForTimeCommand(m_robotSwerveDrive,
new Translation2d(0,1), new Translation2d(), FieldConstants.L2_DRIVE_TIME, true),
new Translation2d(0,1), new Translation2d(), AutoConstants.L2_DRIVE_TIME, true),
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator),
new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
@@ -246,46 +249,28 @@ public class RobotContainer {
private Command AprilLidarAlignL2Right = new SequentialCommandGroup(
new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L2_SCORE_DISTANCE, Side.RIGHT),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L2_SCORE_DISTANCE, Side.RIGHT),
new LidarAlign(m_robotSwerveDrive, m_lidar),
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.L2Score);}, m_robotElevator),
new waitEndefectorRefrence(m_robotElevator),
new waitElevatorRefrence(m_robotElevator),
new MoveForTimeCommand(m_robotSwerveDrive,
new Translation2d(0,1), new Translation2d(), FieldConstants.L2_DRIVE_TIME, true),
new Translation2d(0,1), new Translation2d(), AutoConstants.L2_DRIVE_TIME, true),
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator),
new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
);
private Command placeCoral = new SequentialCommandGroup(
new InstantCommand(() -> m_robotSwerveDrive.stopModules()),
new InstantCommand(() -> System.out.println("Placing Some Coral")),
new WaitCommand(3),
new InstantCommand(() -> System.out.println("Done placing Some Coral"))
);
private Command aprilAlign = new SequentialCommandGroup(
new InstantCommand(() -> System.out.println("Aligning...")),
new WaitCommand(1)
);
private Command grabCoral = new SequentialCommandGroup(
new InstantCommand(() -> System.out.println("Yoinking some coral...")),
new WaitCommand(2),
new InstantCommand(() -> System.out.println("Done yoinking some coral..."))
);
private Command leftAlgaeRemove = new SequentialCommandGroup(
new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Primed);}, m_robotElevator),
new waitEndefectorRefrence(m_robotElevator),
new waitElevatorRefrence(m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L2_ALGAE_REMOVAL - Units.inchesToMeters(1), Side.LEFT),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE, Side.LEFT),
new LidarAlign(m_robotSwerveDrive, m_lidar),
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Go);}, m_robotElevator),
new MoveForTimeCommand(m_robotSwerveDrive,
new Translation2d(0,1), new Translation2d(), 500, true),
new Translation2d(0,1), new Translation2d(), AutoConstants.ALGAE_DRIVE_TIME, true),
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator),
new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
);
@@ -295,11 +280,11 @@ public class RobotContainer {
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL3Primed);}, m_robotElevator),
new waitEndefectorRefrence(m_robotElevator),
new waitElevatorRefrence(m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L2_ALGAE_REMOVAL - Units.inchesToMeters(1), Side.LEFT),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE, Side.LEFT),
new LidarAlign(m_robotSwerveDrive, m_lidar),
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL3Go);}, m_robotElevator),
new MoveForTimeCommand(m_robotSwerveDrive,
new Translation2d(0,1), new Translation2d(), 500, true),
new Translation2d(0,1), new Translation2d(), AutoConstants.ALGAE_DRIVE_TIME, true),
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator),
new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
);
@@ -319,12 +304,10 @@ public class RobotContainer {
NamedCommands.registerCommand("place-coral-right-l2", AprilLidarAlignL2Right);
NamedCommands.registerCommand("lineup-no-arm", new SequentialCommandGroup(
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.LEFT),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.LEFT),
new LidarAlign(m_robotSwerveDrive, m_lidar)
));
NamedCommands.registerCommand("grab-coral", grabCoral);
configureButtonBindings();
configureVirtualButtonBindings();
new DeferredBlock(() -> { // Called on first robot enable
src/main/java/frc4388/robot/commands/GotoLastApril.java
+1 -1
View File
@@ -10,7 +10,7 @@ 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.SwerveDriveConstants.AutoConstants;
import frc4388.robot.Constants.AutoConstants;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.robot.subsystems.Vision;
import frc4388.utility.Gains;
src/main/java/frc4388/robot/commands/IfCommand.java
-33
View File
@@ -1,33 +0,0 @@
package frc4388.robot.commands;
import java.time.Instant;
import java.util.function.BooleanSupplier;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.InstantCommand;
// Command that is called if something is true
public class IfCommand extends InstantCommand {
BooleanSupplier isTrue;
Command trueCommand;
Command falseCommand;
public IfCommand(BooleanSupplier isTrue, Command trueCommand, Command falseCommand){
this.isTrue = isTrue;
this.trueCommand = trueCommand;
this.falseCommand = falseCommand;
}
public IfCommand(BooleanSupplier isTrue, Command trueCommand){
this(isTrue, trueCommand, Commands.none());
}
@Override
public void execute() {
if(isTrue.getAsBoolean())
trueCommand.schedule();
else
falseCommand.schedule();
}
}
src/main/java/frc4388/robot/subsystems/Elevator.java
+1 -1
View File
@@ -20,7 +20,7 @@ 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.SwerveDriveConstants.AutoConstants;
import frc4388.robot.Constants.AutoConstants;
import frc4388.robot.subsystems.LED;
import frc4388.utility.LEDPatterns;
import frc4388.utility.TimesNegativeOne;
src/main/java/frc4388/robot/subsystems/Lidar.java
+4 -4
View File
@@ -7,7 +7,7 @@ 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.SwerveDriveConstants.AutoConstants;
import frc4388.robot.Constants.LiDARConstants;
import frc4388.utility.Status;
import frc4388.utility.Subsystem;
import frc4388.utility.Status.ReportLevel;
@@ -16,7 +16,7 @@ import frc4388.utility.Status.ReportLevel;
public class Lidar extends Subsystem {
private double distance = -1;
Counter LidarPWM = new Counter(AutoConstants.LIDAR_DIO_CHANNEL);
Counter LidarPWM = new Counter(LiDARConstants.LIDAR_DIO_CHANNEL);
public Lidar() {
LidarPWM.setMaxPeriod(1.00); //set the max period that can be measured
@@ -29,7 +29,7 @@ public class Lidar extends Subsystem {
if(LidarPWM.get() < 1)
distance = -1;
else
distance = (LidarPWM.getPeriod() * AutoConstants.SECONDS_TO_MICROS) / AutoConstants.LIDAR_MICROS_TO_CM;
distance = (LidarPWM.getPeriod() * LiDARConstants.SECONDS_TO_MICROS) / LiDARConstants.LIDAR_MICROS_TO_CM;
}
public double getDistance(){
@@ -38,7 +38,7 @@ public class Lidar extends Subsystem {
public boolean withinDistance(){
if(distance == -1) return false;
return distance < AutoConstants.LIDAR_DETECT_DISTANCE;
return distance < LiDARConstants.LIDAR_DETECT_DISTANCE;
}
ShuffleboardLayout subsystemLayout = Shuffleboard.getTab("Subsystems")
src/main/java/frc4388/utility/ReefPositionHelper.java
+3 -2
View File
@@ -7,6 +7,7 @@ import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import frc4388.robot.Constants.AutoConstants;
import frc4388.robot.Constants.FieldConstants;
public class ReefPositionHelper {
@@ -79,9 +80,9 @@ public class ReefPositionHelper {
public static double getSide(Side side){
switch(side) {
case LEFT:
return -(FieldConstants.HORISONTAL_SCORING_POSITION_OFFSET);
return -(AutoConstants.X_SCORING_POSITION_OFFSET);
case RIGHT:
return (FieldConstants.HORISONTAL_SCORING_POSITION_OFFSET);
return (AutoConstants.X_SCORING_POSITION_OFFSET);
case CENTER:
return 0;
}