diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 150fffa..059b974 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -113,7 +113,7 @@ public final class Constants { private static final class ModuleSpecificConstants { //2025 //Front Left - private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.22705078125+0.5); + 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; @@ -121,7 +121,7 @@ public final class Constants { private static final Distance FRONT_LEFT_YPOS = Inches.of(HALF_HEIGHT); //Front Right - private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(0.07666015625); + 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; @@ -129,7 +129,7 @@ public final class Constants { private static final Distance FRONT_RIGHT_YPOS = Inches.of(-HALF_HEIGHT); //Back Left - private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.37646484375); + 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; @@ -137,7 +137,7 @@ public final class Constants { private static final Distance BACK_LEFT_YPOS = Inches.of(HALF_HEIGHT); //Back Right - private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(-0.219970703125+0.5); + 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; @@ -287,8 +287,12 @@ public final class Constants { } 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 = 25; + 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; } @@ -331,7 +335,7 @@ public final class Constants { // 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.25; + public static final double STOP_VELOCITY = 0.0; } public static final class VisionConstants { @@ -394,8 +398,8 @@ public final class Constants { 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 = 0; - public static final double SAFETY_ENDEFFECTOR_MIN_VELOCITY = 999; + 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 diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index f2cd73f..38a76ac 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -28,6 +28,7 @@ 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; @@ -44,6 +45,7 @@ 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; @@ -91,7 +93,8 @@ 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_lidar = new Lidar(); + 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); @@ -143,7 +146,7 @@ public class RobotContainer { new waitElevatorRefrence(m_robotElevator), new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.RIGHT), - new LidarAlign(m_robotSwerveDrive, m_lidar), + new LidarAlign(m_robotSwerveDrive, m_reefLidar), new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator), @@ -178,7 +181,7 @@ public class RobotContainer { new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.LEFT), - new LidarAlign(m_robotSwerveDrive, m_lidar), + new LidarAlign(m_robotSwerveDrive, m_reefLidar), new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator), @@ -211,7 +214,7 @@ public class RobotContainer { new waitElevatorRefrence(m_robotElevator), new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_SCORE, Side.LEFT), - new LidarAlign(m_robotSwerveDrive, m_lidar), + new LidarAlign(m_robotSwerveDrive, m_reefLidar), // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringThree), m_robotElevator), new waitEndefectorRefrence(m_robotElevator), new waitElevatorRefrence(m_robotElevator), @@ -238,7 +241,7 @@ public class RobotContainer { new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_SCORE, Side.RIGHT), - new LidarAlign(m_robotSwerveDrive, m_lidar), + new LidarAlign(m_robotSwerveDrive, m_reefLidar), // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringThree), m_robotElevator), @@ -255,7 +258,7 @@ public class RobotContainer { new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L2_PREP_DISTANCE, Side.LEFT), new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L2_SCORE_DISTANCE, Side.LEFT), - new LidarAlign(m_robotSwerveDrive, m_lidar), + new LidarAlign(m_robotSwerveDrive, m_reefLidar), new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.L2Score);}, m_robotElevator), new waitEndefectorRefrence(m_robotElevator), new waitElevatorRefrence(m_robotElevator), @@ -271,7 +274,7 @@ public class RobotContainer { new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L2_PREP_DISTANCE, Side.RIGHT), new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L2_SCORE_DISTANCE, Side.RIGHT), - new LidarAlign(m_robotSwerveDrive, m_lidar), + new LidarAlign(m_robotSwerveDrive, m_reefLidar), new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.L2Score);}, m_robotElevator), new waitEndefectorRefrence(m_robotElevator), new waitElevatorRefrence(m_robotElevator), @@ -287,7 +290,7 @@ public class RobotContainer { new waitEndefectorRefrence(m_robotElevator), new waitElevatorRefrence(m_robotElevator), new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE, Side.LEFT), - new LidarAlign(m_robotSwerveDrive, m_lidar), + new LidarAlign(m_robotSwerveDrive, m_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), @@ -301,7 +304,7 @@ public class RobotContainer { new waitEndefectorRefrence(m_robotElevator), new waitElevatorRefrence(m_robotElevator), new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE, Side.LEFT), - new LidarAlign(m_robotSwerveDrive, m_lidar), + new LidarAlign(m_robotSwerveDrive, m_reefLidar), new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL3Go);}, m_robotElevator), new MoveForTimeCommand(m_robotSwerveDrive, new Translation2d(0,1), new Translation2d(), AutoConstants.ALGAE_DRIVE_TIME, true), @@ -339,7 +342,8 @@ public class RobotContainer { NamedCommands.registerCommand("grab-coral", waitFeedStation.asProxy()); NamedCommands.registerCommand("await-coral", new waitFeedCoral(m_robotElevator)); - + NamedCommands.registerCommand("feed-driveback", new DriveUntilLiDAR(m_robotSwerveDrive, + new Translation2d(0,1), new Translation2d(), m_reverseLidar, LiDARConstants.HUMAN_PLAYER_STATION_DISTANCE, false)); NamedCommands.registerCommand("stop", new waitSupplier(() -> m_robotSwerveDrive.lastOdomSpeed < AutoConstants.STOP_VELOCITY)); NamedCommands.registerCommand("align-feed", new SequentialCommandGroup( @@ -478,11 +482,14 @@ public class RobotContainer { ), m_robotSwerveDrive)) .onFalse(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive)); + new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) + .onTrue(new LidarAlign(m_robotSwerveDrive, m_reefLidar)); + new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) .onTrue(thrustIntake.asProxy()); new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON) - .onTrue(new InstantCommand(() -> {}, m_robotSwerveDrive, m_lidar)); + .onTrue(new InstantCommand(() -> {}, m_robotSwerveDrive, m_reefLidar)); // ? /* Operator Buttons */ diff --git a/src/main/java/frc4388/robot/commands/DriveUntilLiDAR.java b/src/main/java/frc4388/robot/commands/DriveUntilLiDAR.java new file mode 100644 index 0000000..029d7a6 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/DriveUntilLiDAR.java @@ -0,0 +1,50 @@ +package frc4388.robot.commands; + +import java.time.Instant; + +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 { + private final SwerveDrive swerveDrive; + private final Translation2d leftStick; + 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) { + addRequirements(swerveDrive); + + this.swerveDrive = swerveDrive; + this.leftStick = leftStick; + this.rightStick = rightStick; + this.m_lidar = lidar; + this.mindistance = mindistance; + this.robotRelative = robotRelative; + } + + @Override + public void initialize() { + } + + @Override + public void execute() { + swerveDrive.driveFine(leftStick, rightStick, 0.1); + } + + @Override + public boolean isFinished() { + return Math.abs(m_lidar.getDistance()) < mindistance; + } +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/Elevator.java b/src/main/java/frc4388/robot/subsystems/Elevator.java index e651ede..ebaf5d3 100644 --- a/src/main/java/frc4388/robot/subsystems/Elevator.java +++ b/src/main/java/frc4388/robot/subsystems/Elevator.java @@ -302,7 +302,7 @@ public class Elevator extends Subsystem { if(endeffectorVelocity < ElevatorConstants.SAFETY_ENDEFFECTOR_MIN_VELOCITY && endeffectorTorque > ElevatorConstants.SAFETY_ENDEFFECTOR_MAX_TORQUE ){ - PIDPosition(elevatorMotor, endeffectorMotor.getPosition().getValueAsDouble()); + PIDPosition(endeffectorMotor, endeffectorMotor.getPosition().getValueAsDouble()); } // This method will be called once per scheduler run diff --git a/src/main/java/frc4388/robot/subsystems/Lidar.java b/src/main/java/frc4388/robot/subsystems/Lidar.java index 3ac5cff..e660301 100644 --- a/src/main/java/frc4388/robot/subsystems/Lidar.java +++ b/src/main/java/frc4388/robot/subsystems/Lidar.java @@ -15,13 +15,31 @@ import frc4388.utility.Status.ReportLevel; // https://girlsofsteeldocs.readthedocs.io/en/latest/technical-resources/sensors/LIDAR-Lite-Distance-Sensor.html#minimal-roborio-interface public class Lidar extends Subsystem { - private double distance = -1; - Counter LidarPWM = new Counter(LiDARConstants.LIDAR_DIO_CHANNEL); + private Counter LidarPWM; + private String name = "Lidar"; - public Lidar() { + private double distance = -1; + public Lidar(int port, String name) { + this.name = name; + LidarPWM = new Counter(port); LidarPWM.setMaxPeriod(1.00); //set the max period that can be measured LidarPWM.setSemiPeriodMode(true); //Set the counter to period measurement LidarPWM.reset(); + + + subsystemLayout = Shuffleboard.getTab("Subsystems") + .getLayout(getSubsystemName(), BuiltInLayouts.kList) + .withSize(2, 2); + + sbDistance = subsystemLayout + .add("Distance", 0) + .withWidget(BuiltInWidgets.kGraph) + .getEntry(); + + sbWithinDistance = subsystemLayout + . add("Within Distance", 0) + .withWidget(BuiltInWidgets.kBooleanBox) + .getEntry(); } @Override @@ -41,23 +59,13 @@ public class Lidar extends Subsystem { return distance < LiDARConstants.LIDAR_DETECT_DISTANCE; } - ShuffleboardLayout subsystemLayout = Shuffleboard.getTab("Subsystems") - .getLayout(getSubsystemName(), BuiltInLayouts.kList) - .withSize(2, 2); - - GenericEntry sbDistance = subsystemLayout - .add("Distance", 0) - .withWidget(BuiltInWidgets.kGraph) - .getEntry(); - - GenericEntry sbWithinDistance = subsystemLayout - .add("Within Distance", 0) - .withWidget(BuiltInWidgets.kBooleanBox) - .getEntry(); + ShuffleboardLayout subsystemLayout; + GenericEntry sbDistance; + GenericEntry sbWithinDistance; @Override public String getSubsystemName() { - return "Lidar"; + return "Lidar " + name; } @Override diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index bb597f8..a7fb68f 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -237,11 +237,11 @@ public class SwerveDrive extends Subsystem { .withVelocityX(leftStick.getX() * speedAdjust) .withVelocityY(leftStick.getY() * speedAdjust) .withTargetDirection(heading); - ctrl.HeadingController.setPID( - SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kP, - SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kI, - SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kD - ); + // ctrl.HeadingController.setPID( + // SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kP, + // SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kI, + // SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kD + // ); swerveDriveTrain.setControl(ctrl); } @@ -349,7 +349,7 @@ public class SwerveDrive extends Subsystem { rotTarget += deltaAngle; } - swerveDriveTrain.addVisionMeasurement(vision.lastVisionPose, vision.getLastLatency()); + swerveDriveTrain.addVisionMeasurement(vision.lastVisionPose, time); //back to the ~~future~~ *past* }