diff --git a/src/main/deploy/pathplanner/autos/3 Coral Cage 5.auto b/src/main/deploy/pathplanner/autos/3 Coral Cage 5.auto index b9b9e6b..f0d0c95 100644 --- a/src/main/deploy/pathplanner/autos/3 Coral Cage 5.auto +++ b/src/main/deploy/pathplanner/autos/3 Coral Cage 5.auto @@ -22,6 +22,12 @@ "name": "place-coral-left-l4" } }, + { + "type": "named", + "data": { + "name": "stop" + } + }, { "type": "path", "data": { diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index a37693b..150fffa 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -331,6 +331,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 class VisionConstants { @@ -393,6 +394,9 @@ 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 int BASIN_LIMIT_SWITCH = 8; // TODO: FIND 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 5eed240..f2cd73f 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -14,6 +14,7 @@ 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; @@ -338,6 +339,9 @@ public class RobotContainer { NamedCommands.registerCommand("grab-coral", waitFeedStation.asProxy()); NamedCommands.registerCommand("await-coral", new waitFeedCoral(m_robotElevator)); + + NamedCommands.registerCommand("stop", new waitSupplier(() -> m_robotSwerveDrive.lastOdomSpeed < AutoConstants.STOP_VELOCITY)); + NamedCommands.registerCommand("align-feed", new SequentialCommandGroup( new MoveForTimeCommand(m_robotSwerveDrive, new Translation2d(0, 1), diff --git a/src/main/java/frc4388/robot/subsystems/Elevator.java b/src/main/java/frc4388/robot/subsystems/Elevator.java index 49b253f..e651ede 100644 --- a/src/main/java/frc4388/robot/subsystems/Elevator.java +++ b/src/main/java/frc4388/robot/subsystems/Elevator.java @@ -295,7 +295,19 @@ public class Elevator extends Subsystem { @Override public void periodic() { + // double elevatorVelocity = elevatorMotor.getVelocity().getValueAsDouble(); + // double elevatorTorque = elevatorMotor.getTorqueCurrent().getValueAsDouble(); + double endeffectorVelocity = endeffectorMotor.getVelocity().getValueAsDouble(); + double endeffectorTorque = endeffectorMotor.getTorqueCurrent().getValueAsDouble(); + + + if(endeffectorVelocity < ElevatorConstants.SAFETY_ENDEFFECTOR_MIN_VELOCITY && endeffectorTorque > ElevatorConstants.SAFETY_ENDEFFECTOR_MAX_TORQUE ){ + PIDPosition(elevatorMotor, endeffectorMotor.getPosition().getValueAsDouble()); + } + // This method will be called once per scheduler run + SmartDashboard.putNumber("Velocity Endeffector", endeffectorVelocity); + SmartDashboard.putNumber("Torque Endeffector", endeffectorTorque); SmartDashboard.putNumber("Basin", basinBeamBreak.get() ? 1 : 0); SmartDashboard.putNumber("endefector", endeffectorLimitSwitch.get() ? 1 : 0); SmartDashboard.putNumber("intake", intakeIR.get() ? 1 : 0); @@ -314,6 +326,8 @@ public class Elevator extends Subsystem { if(!intakeIR.get()){ led.setMode(LEDConstants.DOWN_PATTERN); } + + // } else if (currentState == CoordinationState.ScoringThree || currentState == CoordinationState.ScoringFour) { // periodicScoring(); // } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 254204d..bb597f8 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -58,6 +58,8 @@ public class SwerveDrive extends Subsystem { public double autoSpeedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * 0.25; // cap auto performance to // 25% + public double lastOdomSpeed; + public Pose2d initalPose2d = null; @@ -336,7 +338,7 @@ public class SwerveDrive extends Subsystem { Optional lastPose = swerveDriveTrain.samplePoseAt(time - freq); vision.setLastOdomPose(curpose); - vision.setLastOdomSpeed(curpose, lastPose, freq); + setLastOdomSpeed(curpose, lastPose, freq); if (vision.isTag()) { Pose2d pose = vision.getPose2d(); @@ -347,7 +349,7 @@ public class SwerveDrive extends Subsystem { rotTarget += deltaAngle; } - swerveDriveTrain.addVisionMeasurement(vision.lastVisionPose, time);// - 0.020); + swerveDriveTrain.addVisionMeasurement(vision.lastVisionPose, vision.getLastLatency()); //back to the ~~future~~ *past* } @@ -424,6 +426,14 @@ public class SwerveDrive extends Subsystem { gear_index = tmp_gear_index; } + + + public void setLastOdomSpeed(Optional curPose, Optional lastPose, double freq){ + if(curPose.isPresent() && lastPose.isPresent()) + this.lastOdomSpeed = curPose.get().getTranslation().getDistance(lastPose.get().getTranslation())/freq; + } + + @Override public String getSubsystemName() { return "Swerve Drive Controller"; diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java index b7cd090..c0e4305 100644 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -52,9 +52,14 @@ public class Vision extends Subsystem { private boolean isTagDetected = false; private boolean isTagProcessed = false; + private double lastLatency = 0; + + public double getLastLatency() { + return lastLatency; + } + public Pose2d lastVisionPose = new Pose2d(); private Pose2d lastPhysOdomPose = new Pose2d(); - private double lastOdomSpeed; private Matrix curStdDevs; @@ -102,6 +107,7 @@ public class Vision extends Subsystem { public void periodic() { update(); field.setRobotPose(getPose2d()); + // cameras[0]. } @@ -115,6 +121,7 @@ public class Vision extends Subsystem { double X = 0; double Y = 0; double Yaw = 0; + double latency = 0; for(int i = 0; i < cameras.length; i++){ PhotonCamera camera = cameras[i]; @@ -128,6 +135,7 @@ public class Vision extends Subsystem { var result = results.get(results.size()-1); + latency += result.getTimestampSeconds(); isTagDetected = isTagDetected | result.hasTargets(); @@ -151,6 +159,8 @@ public class Vision extends Subsystem { } + lastLatency = latency / cams; + if(isTagProcessed){ lastVisionPose = new Pose2d(X/cams, Y/cams, Rotation2d.fromDegrees(Yaw/cams)); } @@ -262,14 +272,9 @@ public class Vision extends Subsystem { lastPhysOdomPose = pose.get(); } - public void setLastOdomSpeed(Optional curPose, Optional lastPose, double freq){ - if(curPose.isPresent() && lastPose.isPresent()) - this.lastOdomSpeed = curPose.get().getTranslation().getDistance(lastPose.get().getTranslation())/freq; - } - - public double getLastOdomSpeed(){ - return lastOdomSpeed; - } + // public double getLastOdomSpeed(){ + // return lastOdomSpeed; + // } public Pose2d getPose2d() { if(isTagDetected && isTagProcessed)