diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 731bb76..ceeeda9 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -329,6 +329,8 @@ public final class Constants { public static final class AutoConstants { public static final Gains XY_GAINS = new Gains(3,0.3,0.0); + // 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.0); public static final Trim X_OFFSET_TRIM = new Trim("X Offset Trim", Double.MAX_VALUE, -Double.MAX_VALUE,0.5, 0); @@ -353,7 +355,7 @@ public final class Constants { 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 L2_SCORE_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(2); public static final double ALGAE_REMOVAL_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(2); @@ -445,6 +447,7 @@ public final class Constants { 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_SCORE_ENDEFFECTOR = -19; diff --git a/src/main/java/frc4388/robot/subsystems/Elevator.java b/src/main/java/frc4388/robot/subsystems/Elevator.java index 57d119c..bc41f4b 100644 --- a/src/main/java/frc4388/robot/subsystems/Elevator.java +++ b/src/main/java/frc4388/robot/subsystems/Elevator.java @@ -139,7 +139,7 @@ public class Elevator extends Subsystem { } case L2Score: { - PIDPosition(elevatorMotor, ElevatorConstants.HOVERING_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); + PIDPosition(elevatorMotor, ElevatorConstants.L2_SCORE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); PIDPosition(endeffectorMotor, ElevatorConstants.L2_SCORE_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); led.setMode(TimesNegativeOne.isRed ? LEDConstants.RED_PATTERN : LEDConstants.BLUE_PATTERN); break; diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index f8770a0..54855a9 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -326,7 +326,8 @@ public class SwerveDrive extends Subsystem { rotTarget += deltaAngle; } - swerveDriveTrain.addVisionMeasurement(vision.lastVisionPose, time); + swerveDriveTrain.addVisionMeasurement(vision.lastVisionPose, time);// - 0.020); + //back to the ~~future~~ *past* } // if(e.isPresent())