diff --git a/src/main/kotlin/com/team4099/robot2023/Robot.kt b/src/main/kotlin/com/team4099/robot2023/Robot.kt index 5f8ec0d9..39adaf01 100644 --- a/src/main/kotlin/com/team4099/robot2023/Robot.kt +++ b/src/main/kotlin/com/team4099/robot2023/Robot.kt @@ -5,6 +5,7 @@ import com.team4099.robot2023.auto.AutonomousSelector import com.team4099.robot2023.auto.PathStore import com.team4099.robot2023.config.ControlBoard import com.team4099.robot2023.config.constants.Constants +import com.team4099.robot2023.config.constants.FieldConstants import com.team4099.robot2023.subsystems.falconspin.MotorChecker import com.team4099.robot2023.util.Alert import com.team4099.robot2023.util.Alert.AlertType @@ -157,6 +158,7 @@ object Robot : LoggedRobot() { } override fun robotPeriodic() { + // RobotContainer.measurementsWithTimestamps.forEach { // RobotContainer.addVisionMeasurement(it) } @@ -192,6 +194,9 @@ object Robot : LoggedRobot() { Logger.getInstance().recordOutput("LoggedRobot/port3", port3.voltage) */ + + Logger.getInstance().recordOutput("AprilTagPose/0", FieldConstants.homeAprilTags[0].pose.pose3d) + Logger.getInstance().recordOutput("AprilTagPose/1", FieldConstants.homeAprilTags[1].pose.pose3d) } override fun teleopInit() { diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt index d2ef7602..842d7df1 100644 --- a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt +++ b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt @@ -7,6 +7,7 @@ import com.team4099.robot2023.commands.drivetrain.ResetGyroYawCommand import com.team4099.robot2023.commands.drivetrain.TeleopDriveCommand import com.team4099.robot2023.config.ControlBoard import com.team4099.robot2023.config.constants.Constants +import com.team4099.robot2023.config.constants.FieldConstants import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain import com.team4099.robot2023.subsystems.drivetrain.drive.DrivetrainIOReal import com.team4099.robot2023.subsystems.drivetrain.drive.DrivetrainIOSim @@ -101,6 +102,8 @@ object RobotContainer { drivetrain.elevatorHeightSupplier = Supplier { superstructure.elevatorInputs.elevatorPosition } drivetrain.objectiveSupplier = Supplier { superstructure.objective } limelight.gamePieceToLookFor = { superstructure.objective } + + } fun mapDefaultCommands() { diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/FieldConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/FieldConstants.kt index 30730ae1..e3c38d72 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/FieldConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/FieldConstants.kt @@ -163,7 +163,6 @@ object FieldConstants { // church tags val homeAprilTags: List = listOf( - AprilTag(1, Pose3d((40.45).inches, (108.19).inches, (45).centi.meters, Rotation3d())), AprilTag( 0, Pose3d( @@ -172,7 +171,8 @@ object FieldConstants { (46).centi.meters, Rotation3d() ) - ) + ), + AprilTag(1, Pose3d((40.45).inches, (108.19).inches, (45).centi.meters, Rotation3d())) ) // AprilTag locations (do not flip for red alliance)