it gives values don't know if they are right

.OutlineViewer/outlineviewer.json

@@ -1,13 +1,40 @@
 {
+  "Clients": {
+    "open": true
+  },
   "NetworkTables Settings": {
     "mode": "Client (NT4)"
   },
+  "client@2": {
+    "open": true
+  },
+  "client@4": {
+    "Publishers": {
+      "open": true
+    },
+    "open": true
+  },
+  "outlineviewer@2": {
+    "Publishers": {
+      "open": true
+    },
+    "open": true
+  },
+  "outlineviewer@3": {
+    "open": true
+  },
+  "shuffleboard@1": {
+    "open": true
+  },
   "transitory": {
     "Shuffleboard": {
       "Sensors Tab": {
         "open": true
       },
       "open": true
+    },
+    "orange_Fiducial": {
+      "open": true
     }
   }
 }

src/main/java/frc/robot/RobotContainer.java

@@ -63,7 +63,7 @@ public class RobotContainer {
 
 		drivetrain = new Drivetrain();
 
-		vision = new Vision(drivetrain::getGyroValue);
+		vision = new Vision();
 
 		elevator = new Elevator();
 		//elevator = new ElevatorSysID();
@@ -109,8 +109,8 @@ public class RobotContainer {
 
 		drivetrain.setDefaultCommand(
 			drivetrain.drive(
-				() -> Math.pow(driver.getLeftY(), 3),
-				() -> Math.pow(driver.getLeftX(), 3),
+				() -> driver.getLeftY() * Math.pow(Math.abs(driver.getLeftY()) + Math.abs(driver.getLeftX()), 3),
+				() -> driver.getLeftX() * Math.pow(Math.abs(driver.getLeftY()) + Math.abs(driver.getLeftX()), 3),
 				driver::getRightX, //Math.signum(driver.getRightX()) * Math.pow(driver.getRightX(), 3)
 				() -> true
 			)
@@ -143,7 +143,7 @@ public class RobotContainer {
 		);
 
 		driver.leftTrigger().whileTrue( 
-			manipulator.runUntilCollected(() -> 0.75).andThen(manipulator.retractCommand(() -> 0.25))
+			manipulator.runUntilCollected(() -> 0.75)
 		);
 
 		driver.start().and(driver.back()).onTrue(
@@ -153,6 +153,8 @@ public class RobotContainer {
 		driver.y().whileTrue(drivetrain.zeroHeading());
 		
 		driver.a().whileTrue(manipulator.runManipulator(() -> -0.5, false));
+
+		driver.b().whileTrue(manipulator.runManipulator(() -> -0.35, true));
 		
 		/* 
 		driver.rightBumper().whileTrue(
@@ -201,26 +203,28 @@ public class RobotContainer {
 			)
 		);
 
-		operator.start().toggleOnTrue(climberPivot.runPivot(() -> operator.getRightY()*0.5).alongWith(climberRollers.runRoller(() -> operator.getLeftY()*0.5)));
+		operator.back().onTrue(elevator.homeCommand());
+
+		operator.start().toggleOnTrue(climberPivot.runPivot(() -> -operator.getRightY()*0.5).alongWith(climberRollers.runRoller(() -> operator.getLeftY()*0.5)));
 
 		operator.a().onTrue(
-			safeMoveManipulator(ElevatorConstants.kL1Position, 0.0)
+			safeMoveManipulator(ElevatorConstants.kL1Position, ManipulatorPivotConstants.kStartingPosition)
 		);
 
 		operator.x().onTrue(
 			safeMoveManipulator(ElevatorConstants.kL2AlgaePosition, ManipulatorPivotConstants.kL2AlgaePosition)
-				.alongWith(manipulator.runManipulator(() -> 0.5, false))
+				.alongWith(manipulator.runManipulator(() -> 0.85, false))
 				.until(() -> driver.a().getAsBoolean())
 		);
 
 		operator.b().onTrue(
 			safeMoveManipulator(ElevatorConstants.kL3AlgaePosition, ManipulatorPivotConstants.kL3AlgaePosition)
-				.alongWith(manipulator.runManipulator(() -> 0.5, false))
+				.alongWith(manipulator.runManipulator(() -> 0.85, false))
 				.until(() -> driver.a().getAsBoolean())
 		);
 
 		operator.y().onTrue(moveWithAlgae(ElevatorConstants.kProcessorPosition, ManipulatorPivotConstants.kProcessorPosition)
-			.alongWith(manipulator.runManipulator(() -> 0.5, false))
+			.alongWith(manipulator.runManipulator(() -> 0.85, false))
 			.until(() -> driver.a().getAsBoolean())
 		);
 
@@ -241,6 +245,7 @@ public class RobotContainer {
     private void configureShuffleboard() {
 		ShuffleboardTab autoTab = Shuffleboard.getTab(OIConstants.kAutoTab);
     	ShuffleboardTab sensorTab = Shuffleboard.getTab(OIConstants.kSensorsTab);
+		ShuffleboardTab apriltagTab = Shuffleboard.getTab(OIConstants.kApriltagTab);
 
 		Shuffleboard.selectTab(OIConstants.kAutoTab);
 
@@ -316,6 +321,17 @@ public class RobotContainer {
 
 		//sensorTab.add("odometry", drivetrain::getPose);
 
+		apriltagTab.addDouble("Orange ID", vision::getOrangeClosestTag);
+		apriltagTab.addDouble("Orange tx", vision::getOrangeTX);
+		apriltagTab.addDouble("Orange ty", vision::getOrangeTY);
+		apriltagTab.addDouble("Orange dist", vision::getOrangeDist);
+		apriltagTab.addDouble("global x", () -> vision.getOrangeGlobalPose(drivetrain.getGyroBuffer()).getX());
+		apriltagTab.addDouble("global y", () -> vision.getOrangeGlobalPose(drivetrain.getGyroBuffer()).getY());
+
+
+//		sensorTab.addDouble(" ID", vision::getOrangeClosestTag);
+
+
  	}
 
 	public Command getAutonomousCommand() {

src/main/java/frc/robot/constants/ElevatorConstants.java

@@ -43,8 +43,8 @@ public class ElevatorConstants {
 
     public static final double kCoralIntakePosition = 0;
     public static final double kL1Position = 0;
-    public static final double kL2Position = 9;
-    public static final double kL3Position = 23.0;
+    public static final double kL2Position = 11;
+    public static final double kL3Position = 27;
     public static final double kL4Position = 50.5;
     public static final double kL4TransitionPosition = 40.0;
     public static final double kL2AlgaePosition = 23.0;

src/main/java/frc/robot/constants/ManipulatorPivotConstants.java

@@ -29,26 +29,27 @@ public class ManipulatorPivotConstants {
     public static final double kFeedForwardG = (0.3+0.19) / 2; // calculated value 0.41
     public static final double kFeedForwardV = 0.68; //calculated value 0.68
 
-    public static final double kFFGravityOffset = Units.degreesToRadians(135.0);
+    public static final double kFFGravityOffset = Units.degreesToRadians(135.0+90);
 
     public static final double kMaxAcceleration = Units.degreesToRadians(1000.0); // degrees per second^2 calculated max = 2100
     public static final double kMaxVelocity = Units.degreesToRadians(100.0); // degrees per second calculated max = 168
 
-    public static final double kEncoderOffset = 0.780;
+    public static final double kEncoderOffset = 0.78-0.25;
 
-    public static final double kCoralIntakePosition = Units.degreesToRadians(175.0);
-    public static final double kL1Position = Units.degreesToRadians(0.0);
-    public static final double kL2Position = Units.degreesToRadians(22.0);
-    public static final double kL3Position = Units.degreesToRadians(22.0);
-    public static final double kL4Position = Units.degreesToRadians(45.0);
-    public static final double kL2AlgaePosition = Units.degreesToRadians(175.0);
-    public static final double kL3AlgaePosition = Units.degreesToRadians(175.0);
-    public static final double kProcessorPosition = Units.degreesToRadians(175.0);
-    public static final double kNetPosition = Units.degreesToRadians(175.0);
+    public static final double kStartingPosition = Units.degreesToRadians(90);
+    public static final double kCoralIntakePosition = Units.degreesToRadians(175.0+90);
+    public static final double kL1Position = Units.degreesToRadians(0.0+90);
+    public static final double kL2Position = Units.degreesToRadians(22.0+90);
+    public static final double kL3Position = Units.degreesToRadians(22.0+90);
+    public static final double kL4Position = Units.degreesToRadians(45.0+90);
+    public static final double kL2AlgaePosition = Units.degreesToRadians(175.0+90);
+    public static final double kL3AlgaePosition = Units.degreesToRadians(175.0+90);
+    public static final double kProcessorPosition = Units.degreesToRadians(175.0+90);
+    public static final double kNetPosition = Units.degreesToRadians(175.0+90);
     /**The closest position to the elevator brace without hitting it */
-    public static final double kPivotSafeStowPosition = Units.degreesToRadians(71.0);
+    public static final double kPivotSafeStowPosition = Units.degreesToRadians(71.0+90);
     /**The forward rotation limit of the pivot */
-    public static final double kRotationLimit = Units.degreesToRadians(175.0);
+    public static final double kRotationLimit = Units.degreesToRadians(175.0+90);
 
     public static final double kSysIDRampRate = 1;
     public static final double kSysIDStepVolts = 7;

src/main/java/frc/robot/constants/OIConstants.java

@@ -4,8 +4,9 @@ public class OIConstants {
     public static final int kDriverControllerPort = 0;
     public static final int kOperatorControllerPort = 1;
     
-    public static final double kDriveDeadband = 0.05;
+    public static final double kDriveDeadband = Math.pow(0.05, 3);
 
     public static final String kAutoTab = "Auto Tab";
     public static final String kSensorsTab = "Sensors Tab";
+    public static final String kApriltagTab = "Apriltag Tab";
 }

src/main/java/frc/robot/subsystems/Drivetrain.java

@@ -19,6 +19,7 @@ import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
@@ -45,6 +46,8 @@ public class Drivetrain extends SubsystemBase {
 
     // Odometry class for tracking robot pose
     private SwerveDrivePoseEstimator m_estimator;
+    
+    private TimeInterpolatableBuffer<Double> gyroBuffer = TimeInterpolatableBuffer.createDoubleBuffer(2.0);
 
     public Orchestra m_orchestra = new Orchestra();
     private Timer musicTimer = new Timer();
@@ -144,6 +147,8 @@ public class Drivetrain extends SubsystemBase {
                 m_rearRight.getPosition()
         });
 
+        gyroBuffer.addSample(getGyroValue(), Timer.getTimestamp());
+
 
         // if the detected tags match your alliances reef tags use their pose estimates 
         /* 
@@ -351,6 +356,10 @@ public class Drivetrain extends SubsystemBase {
         return Rotation2d.fromDegrees(getGyroValue()).getDegrees();
     }
 
+    public TimeInterpolatableBuffer<Double> getGyroBuffer(){
+        return gyroBuffer;
+    }
+
     /**
      * Returns the turn rate of the robot.
      *

src/main/java/frc/robot/subsystems/Elevator.java

@@ -174,6 +174,14 @@ public class Elevator extends SubsystemBase {
         
     }
 
+    public Command homeCommand(){
+        return run(() -> {
+            elevatorMotor1.setVoltage(0.5);
+        })
+        .until(() -> elevatorMotor1.getOutputCurrent() > 5)
+        .andThen(run(() -> encoder.setPosition(0)));
+    }
+
     /**
      * Moves the elevator to a target destination (setpoint). 
      * 
@@ -208,7 +216,7 @@ public class Elevator extends SubsystemBase {
                     }
                 
                 }).until(() -> pidControllerUp.atSetpoint() || pidControllerDown.atSetpoint())
-                    .andThen(runManualElevator(() -> -.1)
+                    .andThen(runManualElevator(() -> -.5)
                     .until(() -> encoder.getPosition() == 0));
 
         } else {

src/main/java/frc/robot/subsystems/ManipulatorPivot.java

@@ -47,7 +47,7 @@ public class ManipulatorPivot extends SubsystemBase {
         );
         pidController.setSetpoint(0);
 
-        pidController.enableContinuousInput(0, 180);
+        pidController.enableContinuousInput(0, 280);
         
         feedForward = new ArmFeedforward(
             ManipulatorPivotConstants.kFeedForwardS,

src/main/java/frc/robot/subsystems/Vision.java

@@ -2,14 +2,18 @@ package frc.robot.subsystems;
 
 import java.util.function.DoubleSupplier;
 
+import edu.wpi.first.math.estimator.PoseEstimator;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Transform2d;
 import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.networktables.BooleanSubscriber;
 import edu.wpi.first.networktables.DoubleSubscriber;
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.Timer;
 import frc.robot.constants.VisionConstants;
 
 public class Vision{
@@ -23,18 +27,19 @@ public class Vision{
 
     private DoubleSubscriber blackFramerate;
 
-    private DoubleSubscriber orangeRobotRelativeX;
-    private DoubleSubscriber orangeRobotRelativeY;
-    private DoubleSubscriber orangeRobotRelativeZ;
+    private DoubleSubscriber orange_tx;
+    private DoubleSubscriber orange_ty;
+    private DoubleSubscriber orange_dist;
 
     private DoubleSubscriber orangeClosestTag;
     private BooleanSubscriber orangeTagDetected;
 
     private DoubleSubscriber orangeFramerate;
 
-    private DoubleSupplier gyroAngle;
+    private double[] orangeCamPose = {0,0,0,0,0};
+    private double[] blackCamPose = {0,0,0,0,0};
 
-    public Vision(DoubleSupplier gyroAngle){
+    public Vision(){
         NetworkTableInstance inst = NetworkTableInstance.getDefault();
 
         NetworkTable blackVisionTable = inst.getTable("black_Fiducial");
@@ -49,33 +54,58 @@ public class Vision{
 
         blackFramerate = blackVisionTable.getDoubleTopic("blackFPS").subscribe(0.0);
 
-        orangeRobotRelativeX = orangeVisionTable.getDoubleTopic("orangeRelativeX").subscribe(0.0);
-        orangeRobotRelativeY = orangeVisionTable.getDoubleTopic("orangeRelativeY").subscribe(0.0);
-        orangeRobotRelativeZ = orangeVisionTable.getDoubleTopic("orangeRelativeZ").subscribe(0.0);
+        orange_tx = orangeVisionTable.getDoubleTopic("tx").subscribe(0.0);
+        orange_ty = orangeVisionTable.getDoubleTopic("ty").subscribe(0.0);
+        orange_dist = orangeVisionTable.getDoubleTopic("totalDist").subscribe(0.0);
 
         orangeClosestTag = orangeVisionTable.getDoubleTopic("orangeClosestTag").subscribe(0.0);
         orangeTagDetected = orangeVisionTable.getBooleanTopic("orangeTagDetected").subscribe(false);
 
         orangeFramerate = orangeVisionTable.getDoubleTopic("orangeFPS").subscribe(0.0);
+    
     }
     
-    public Pose2d relativeToGlobalPose2d(int tagID, Translation2d relativeCoords){
+    public Pose2d relativeToGlobalPose2d(int tagID, Translation2d relativeCoords, double timestamp, TimeInterpolatableBuffer<Double> gyroBuffer){
         Pose2d tag2dPose = new Pose2d(VisionConstants.globalTagCoords[tagID][0],
             VisionConstants.globalTagCoords[tagID][1],
             new Rotation2d());
 
-        Pose2d relative = new Pose2d(relativeCoords, new Rotation2d(gyroAngle.getAsDouble()));
+        Pose2d relative = new Pose2d(relativeCoords, new Rotation2d(gyroBuffer.getSample(timestamp).get()));
 
         Transform2d relative2dTransformation = new Transform2d(relative.getTranslation(), relative.getRotation());
 
         Pose2d globalPose = tag2dPose.transformBy(relative2dTransformation.inverse());
 
-        return new Pose2d(globalPose.getTranslation(), new Rotation2d(gyroAngle.getAsDouble()));
+        return new Pose2d(globalPose.getTranslation(), new Rotation2d(gyroBuffer.getSample(timestamp).get()));
     }
 
-    public Pose2d getBlackGlobalPose(){
-        return relativeToGlobalPose2d(getBlackClosestTag(), 
-            new Translation2d(getBlackRelativeX(), getBlackRelativeY()));
+    public Pose2d CameraToGlobalPose2d(int tagID, double totalDist, double tx, double ty, double timestamp, TimeInterpolatableBuffer<Double> gyroBuffer){
+
+        System.out.println(gyroBuffer.getSample(timestamp));
+
+        double distance2d = totalDist * Math.cos(-orangeCamPose[1] - ty);
+
+        Rotation2d camToTagRotation = new Rotation2d(gyroBuffer.getSample(timestamp).get()).plus(new Rotation2d(orangeCamPose[0] - tx));
+
+        Pose2d tagPose2d = new Pose2d(VisionConstants.globalTagCoords[tagID][0],
+            VisionConstants.globalTagCoords[tagID][1],
+            new Rotation2d());
+
+        Translation2d fieldToCameraTranslation = new Pose2d(tagPose2d.getTranslation(), camToTagRotation.plus(Rotation2d.kPi))
+            .transformBy(new Transform2d(distance2d, 0.0, new Rotation2d()))
+            .getTranslation();
+        Pose2d robotPose = new Pose2d(fieldToCameraTranslation,
+            new Rotation2d(gyroBuffer.getSample(timestamp).get()).plus(new Rotation2d(orangeCamPose[0])))
+            .transformBy(new Transform2d(new Pose2d(new Translation2d(), new Rotation2d(orangeCamPose[0])), Pose2d.kZero));
+
+        robotPose = new Pose2d(robotPose.getTranslation(), new Rotation2d( gyroBuffer.getSample(timestamp).get()));
+
+        return robotPose;
+    }
+
+    public Pose2d getBlackGlobalPose(TimeInterpolatableBuffer<Double> gyroBuffer){
+        return CameraToGlobalPose2d(getBlackClosestTag(), orange_dist.get(),
+            getBlackRelativeX(), getBlackRelativeY(), Timer.getTimestamp(), gyroBuffer);
     }
 
     public double getBlackRelativeX(){
@@ -106,21 +136,26 @@ public class Vision{
         return blackFramerate.get();
     }
 
-    public Pose2d getOrangeGlobalPose(){
-        return relativeToGlobalPose2d(getOrangeClosestTag(), 
-            new Translation2d(getOrangeRelativeX(), getOrangeRelativeY()));
+    public Pose2d getOrangeGlobalPose(TimeInterpolatableBuffer<Double> gyroBuffer){
+        if(getOrangeClosestTag() >= 1 && getOrangeClosestTag() <= 22){
+            return CameraToGlobalPose2d(getOrangeClosestTag(), orange_dist.get(),
+                orange_tx.get(), orange_ty.get(), Timer.getTimestamp(), gyroBuffer
+                );
+        }else{
+            return new Pose2d();
+        }
     }
     
-    public double getOrangeRelativeX(){
-        return orangeRobotRelativeX.get();
+    public double getOrangeTX(){
+        return orange_tx.get();
     }
 
-    public double getOrangeRelativeY(){
-        return orangeRobotRelativeY.get();
+    public double getOrangeTY(){
+        return orange_ty.get();
     }
 
-    public double getOrangeRelativeZ(){
-        return orangeRobotRelativeZ.get();
+    public double getOrangeDist(){
+        return orange_dist.get();
     }
 
     public int getOrangeClosestTag(){
@@ -128,7 +163,7 @@ public class Vision{
     }
 
     public double getOrangeTimeStamp(){
-        return orangeRobotRelativeX.getLastChange();
+        return orange_tx.getLastChange();
     }
 
     public boolean getOrangeTagDetected(){