Create RobotPoses.java (#24)

Author: Wu-Fan-529

src/main/java/frc/robot/Constants.java

@@ -23,6 +23,8 @@
 import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
@@ -32,6 +34,7 @@
 import edu.wpi.first.units.measure.Distance;
 import edu.wpi.first.units.measure.LinearVelocity;
 import edu.wpi.first.units.measure.Mass;
+import edu.wpi.first.units.Unit;
 import edu.wpi.first.units.Units;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
@@ -42,6 +45,9 @@ public final class Constants {
    */
   public static final double MAX_VOLTAGE = 12;
 
+  public static final Transform3d ROBOT_TO_BUMPERS = new Transform3d(0, 0, Units.Meters.convertFrom(5, Units.Inches),
+      Rotation3d.kZero); // TODO: Replace with actual measurement
+
   public static class constControllers {
     public static final double DRIVER_LEFT_STICK_DEADBAND = 0.05;
     public static final boolean SILENCE_JOYSTICK_WARNINGS = true;
@@ -90,6 +96,7 @@ public static class constDrivetrain {
     public static final double TRACK_WIDTH = TRACK_WIDTH_29; // TODO: Replace with actual measurement
     // Distance between Front & Back Wheels
     public static final double WHEELBASE = WHEELBASE_29; // TODO: Replace with actual measurement
+
     // -- Pose Estimation --
     /**
      * <p>

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

@@ -21,8 +21,8 @@ public class RobotContainer {
   private final SN_XboxController conDriver = new SN_XboxController(mapControllers.DRIVER_USB);
 
   private final Drivetrain subDrivetrain = new Drivetrain();
-
   private final StateMachine subStateMachine = new StateMachine(subDrivetrain);
+  private final RobotPoses robotPose = new RobotPoses(subDrivetrain);
 
   Command TRY_NONE = Commands.deferredProxy(
       () -> subStateMachine.tryState(RobotState.NONE));

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

@@ -0,0 +1,30 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.subsystems;
+
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants;
+
+public class RobotPoses extends SubsystemBase {
+  /** Creates a new RobotPoses. */
+
+  Drivetrain subDrivetrain;
+
+  Pose3d comp0Drivetrain = Pose3d.kZero;
+  Pose3d comp1Bumpers = Pose3d.kZero.plus(Constants.ROBOT_TO_BUMPERS);
+
+  public RobotPoses(Drivetrain subDrivetrain) {
+    this.subDrivetrain = subDrivetrain;
+  }
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+
+    // Robot Positions
+    comp0Drivetrain = new Pose3d(subDrivetrain.getPose());
+  }
+}