diff --git a/src/main/deploy/pathplanner/ConeCubeAuto.path b/src/main/deploy/pathplanner/ConeCubeAuto.path new file mode 100644 index 00000000..e0617ebc --- /dev/null +++ b/src/main/deploy/pathplanner/ConeCubeAuto.path @@ -0,0 +1,74 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 1.9, + "y": 4.97 + }, + "prevControl": null, + "nextControl": { + "x": 2.5159692897510797, + "y": 6.536703073406147 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 4.513683285179071, + "y": 6.259812754000508 + }, + "prevControl": { + "x": 4.508526802268566, + "y": 6.264969236911013 + }, + "nextControl": { + "x": 4.518839768089576, + "y": 6.2546562710900035 + }, + "holonomicAngle": 179.6021606941461, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 6.706650249237999, + "y": 4.908321691643384 + }, + "prevControl": { + "x": 6.655472288809161, + "y": 4.991216760707332 + }, + "nextControl": null, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/ConeCubeAuto2.path b/src/main/deploy/pathplanner/ConeCubeAuto2.path new file mode 100644 index 00000000..2acad1cf --- /dev/null +++ b/src/main/deploy/pathplanner/ConeCubeAuto2.path @@ -0,0 +1,74 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 7.06, + "y": 4.5 + }, + "prevControl": null, + "nextControl": { + "x": 6.06, + "y": 4.5 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 3.13, + "y": 4.8 + }, + "prevControl": { + "x": 2.13, + "y": 4.8 + }, + "nextControl": { + "x": 2.13, + "y": 4.8 + }, + "holonomicAngle": 180.0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 1.9, + "y": 4.4 + }, + "prevControl": { + "x": 0.8999999999999999, + "y": 4.4 + }, + "nextControl": null, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [] +} \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/lib/math/Zone2d.kt b/src/main/kotlin/com/team4099/lib/math/Zone2d.kt new file mode 100644 index 00000000..b13d12aa --- /dev/null +++ b/src/main/kotlin/com/team4099/lib/math/Zone2d.kt @@ -0,0 +1,39 @@ +package com.team4099.lib.math + +import org.team4099.lib.geometry.Pose2d +import org.team4099.lib.geometry.Translation2d +import org.team4099.lib.units.base.inMeters +import org.team4099.lib.units.base.meters + +class Zone2d(var vertices: List) { + fun containsPose(pose: Pose2d): Boolean { + var isInside = false + val minX = vertices.minOf { it.x } + val minY = vertices.minOf { it.y } + val maxX = vertices.maxOf { it.x } + val maxY = vertices.maxOf { it.y } + + if ((pose.x !in minX..maxX) || (pose.y !in minY..maxY)) { + return false + } + + var vBIndex = vertices.size - 1 + for (vAIndex in vertices.indices) { + if ((vertices[vAIndex].y > pose.y) != (vertices[vBIndex].y > pose.y) && + pose.x < + ( + (vertices[vBIndex].x - vertices[vAIndex].x).inMeters * + (pose.y - vertices[vAIndex].y).inMeters / + (vertices[vBIndex].y - vertices[vAIndex].y).inMeters + + vertices[vAIndex].x.inMeters + ) + .meters + ) { + isInside = !isInside + } + vBIndex = vAIndex + } + + return isInside + } +} diff --git a/src/main/kotlin/com/team4099/lib/trajectory/Waypoint.kt b/src/main/kotlin/com/team4099/lib/trajectory/Waypoint.kt index b1c21f96..1bb0eab3 100644 --- a/src/main/kotlin/com/team4099/lib/trajectory/Waypoint.kt +++ b/src/main/kotlin/com/team4099/lib/trajectory/Waypoint.kt @@ -10,8 +10,8 @@ import java.util.Optional class Waypoint { /** Returns the translation component of the waypoint. */ val translation: Translation2d - private val driveRotation: Rotation2d? - private val holonomicRotation: Rotation2d? + val driveRotation: Rotation2d? + val holonomicRotation: Rotation2d? /** * Constructs a Waypoint with a translation, drive rotation, and holonomic rotation. diff --git a/src/main/kotlin/com/team4099/robot2022/BuildConstants.kt b/src/main/kotlin/com/team4099/robot2022/BuildConstants.kt new file mode 100644 index 00000000..4dd1e636 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2022/BuildConstants.kt @@ -0,0 +1,15 @@ +package com.team4099.robot2022 + +/** + * Automatically generated file containing build version information. + */ +const val MAVEN_GROUP = "" +const val MAVEN_NAME = "ChargedUp-2023" +const val VERSION = "unspecified" +const val GIT_REVISION = 11 +const val GIT_SHA = "f62076f27df05c9da57e75b873eaa1d38d512be4" +const val GIT_DATE = "2022-12-13T19:13:28Z" +const val GIT_BRANCH = "autologannotation" +const val BUILD_DATE = "2023-07-19T22:09:16Z" +const val BUILD_UNIX_TIME = 1689822556755L +const val DIRTY = 0 diff --git a/src/main/kotlin/com/team4099/robot2023/Robot.kt b/src/main/kotlin/com/team4099/robot2023/Robot.kt index a57d2854..301161fc 100644 --- a/src/main/kotlin/com/team4099/robot2023/Robot.kt +++ b/src/main/kotlin/com/team4099/robot2023/Robot.kt @@ -86,7 +86,7 @@ object Robot : LoggedRobot() { Constants.Universal.POWER_DISTRIBUTION_HUB_ID, PowerDistribution.ModuleType.kRev ) } else { - when (Constants.Universal.SIM_MODE) { + when (Constants.Tuning.SimType.SIM) { Constants.Tuning.SimType.SIM -> { logger.addDataReceiver(NTSafePublisher()) logSimulationAlert.set(true) diff --git a/src/main/kotlin/com/team4099/robot2023/commands/AutoIntakeCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/AutoIntakeCommand.kt new file mode 100644 index 00000000..9d7c8ab5 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/commands/AutoIntakeCommand.kt @@ -0,0 +1,89 @@ +package com.team4099.robot2023.commands + +import com.team4099.lib.math.Zone2d +import com.team4099.lib.trajectory.Waypoint +import com.team4099.robot2023.commands.drivetrain.DrivePathCommand +import com.team4099.robot2023.config.constants.FieldConstants +import com.team4099.robot2023.config.constants.GamePiece +import com.team4099.robot2023.config.constants.NodeTier +import com.team4099.robot2023.config.constants.WaypointConstants +import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain +import com.team4099.robot2023.subsystems.superstructure.Superstructure +import com.team4099.robot2023.util.AllianceFlipUtil +import edu.wpi.first.wpilibj2.command.Commands.runOnce +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup +import org.team4099.lib.geometry.Pose2d +import org.team4099.lib.units.base.meters +import org.team4099.lib.units.derived.Angle +import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.inRotation2ds +import org.team4099.lib.units.perSecond + +class AutoIntakeCommand(val drivetrain: Drivetrain, val superstructure: Superstructure) : + SequentialCommandGroup() { + lateinit var drivePose: Pose2d + lateinit var intermediaryWaypoints: List + var currentZone: Zone2d? = null + lateinit var finalPose: Pose2d + lateinit var postAlignPose: Pose2d + var heading: Angle = 0.0.degrees + lateinit var gamePiece: GamePiece + lateinit var nodeTier: NodeTier + + init { + val setupCommand = + runOnce({ + drivePose = drivetrain.odometryPose + heading = drivetrain.fieldVelocity.heading + currentZone = FieldConstants.determineZone(FieldConstants.Zones.allZones, drivePose) + println(currentZone) + intermediaryWaypoints = + WaypointConstants.SubstationPaths.getPath(currentZone).map { + AllianceFlipUtil.apply(it) + } + if (intermediaryWaypoints.isEmpty()) { + intermediaryWaypoints = + listOf( + Waypoint( + drivePose.pose2d.translation, + if (drivetrain.fieldVelocity.magnitude.absoluteValue < + 0.25.meters.perSecond + ) + null + else heading.inRotation2ds, + drivePose.rotation.inRotation2ds + ) + ) + } + + for (vertex in FieldConstants.Zones.farLeftLane.vertices) { + println("${vertex.x}, ${vertex.y}") + } + println("------") + for (vertex in FieldConstants.Zones.closeLeftLane.vertices) { + println("${vertex.x}, ${vertex.y}") + } + }) + + addCommands( + setupCommand, + DrivePathCommand( + drivetrain, + { + listOf( + Waypoint( + drivePose.pose2d.translation, + if (drivetrain.fieldVelocity.magnitude.absoluteValue < 0.25.meters.perSecond) + null + else heading.inRotation2ds, + drivePose.rotation.inRotation2ds + ) + ) + intermediaryWaypoints + }, + keepTrapping = true, + flipForAlliances = false + ), + superstructure.groundIntakeConeCommand() + ) + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/commands/AutoScoreCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/AutoScoreCommand.kt index 332558ac..aeaeb1cc 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/AutoScoreCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/AutoScoreCommand.kt @@ -1,11 +1,13 @@ package com.team4099.robot2023.commands +import com.team4099.lib.math.Zone2d import com.team4099.lib.trajectory.Waypoint import com.team4099.robot2023.commands.drivetrain.DrivePathCommand import com.team4099.robot2023.config.constants.Constants import com.team4099.robot2023.config.constants.FieldConstants import com.team4099.robot2023.config.constants.GamePiece import com.team4099.robot2023.config.constants.NodeTier +import com.team4099.robot2023.config.constants.WaypointConstants import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain import com.team4099.robot2023.subsystems.gameboy.objective.isConeNode import com.team4099.robot2023.subsystems.superstructure.Superstructure @@ -26,6 +28,8 @@ class AutoScoreCommand(val drivetrain: Drivetrain, val superstructure: Superstru lateinit var drivePose: Pose2d lateinit var finalPose: Pose2d lateinit var postAlignPose: Pose2d + lateinit var intermediaryWaypoints: List + var currentZone: Zone2d? = null var heading: Angle = 0.0.degrees lateinit var gamePiece: GamePiece lateinit var nodeTier: NodeTier @@ -58,6 +62,16 @@ class AutoScoreCommand(val drivetrain: Drivetrain, val superstructure: Superstru ) ) heading = drivetrain.fieldVelocity.heading + + // Determine the current zone and calculate the trajectory + currentZone = FieldConstants.determineZone(FieldConstants.Zones.allZones, drivePose) + intermediaryWaypoints = + WaypointConstants.CommunityPaths.getPath(currentZone).map { + AllianceFlipUtil.apply(it) + } + + println(currentZone == FieldConstants.Zones.closeCenterRightLane) + gamePiece = if (superstructure.objective.isConeNode()) Constants.Universal.GamePiece.CONE else GamePiece.CUBE @@ -78,11 +92,16 @@ class AutoScoreCommand(val drivetrain: Drivetrain, val superstructure: Superstru null else heading.inRotation2ds, drivePose.rotation.inRotation2ds - ), - Waypoint( - finalPose.translation.translation2d, null, finalPose.rotation.inRotation2ds ) - ) + ) + + intermediaryWaypoints + + listOf( + Waypoint( + finalPose.translation.translation2d, + null, + finalPose.rotation.inRotation2ds + ) + ) }, keepTrapping = true, flipForAlliances = false 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 99f68f1f..a69bea0f 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/FieldConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/FieldConstants.kt @@ -1,5 +1,7 @@ package com.team4099.robot2023.config.constants +import com.team4099.lib.math.Zone2d +import com.team4099.robot2023.util.AllianceFlipUtil import edu.wpi.first.wpilibj.DriverStation import edu.wpi.first.wpilibj.DriverStation.Alliance import org.team4099.lib.apriltag.AprilTag @@ -13,10 +15,8 @@ import org.team4099.lib.units.base.feet import org.team4099.lib.units.base.inMeters import org.team4099.lib.units.base.inches import org.team4099.lib.units.base.meters -import org.team4099.lib.units.centi import org.team4099.lib.units.derived.Angle import org.team4099.lib.units.derived.cos -import org.team4099.lib.units.derived.degrees import org.team4099.lib.units.derived.radians import org.team4099.lib.units.derived.sin @@ -38,43 +38,44 @@ object FieldConstants { val homeAprilTags: List = listOf( AprilTag( - 4, + 1, Pose3d( - (103.5).inches, - (-2.5).inches, - (51.5).centi.meters, - Rotation3d(0.0.radians, 0.0.radians, 90.degrees) + 40.inches, + (104.125).inches, + (18.22).inches, + Rotation3d(0.0.radians, 0.0.radians, 0.0.radians) ) ), AprilTag(5, Pose3d((1).centi.meters, (87.5).inches, (89.3).centi.meters, Rotation3d())), AprilTag( - 2, + 3, Pose3d( - (1).centi.meters, - (87.5).inches + 91.6.centi.meters, - (89.1).centi.meters, - Rotation3d() + (42.875).inches, + (113.25).inches, + (18.22).inches, + Rotation3d(0.0.radians, 0.0.radians, 0.0.radians) ) ), AprilTag( - 6, + 4, Pose3d( - (1).centi.meters, - (87.5).inches + - 183.3.centi.meters, // FIRST's diagram has a typo (it says 147.19) // 91.8 - (89.9).centi.meters, - Rotation3d() + (409.5).inches, + (85.5).inches, + (27.833).inches, + Rotation3d(0.0.radians, 0.0.radians, Math.PI.radians) ) ), AprilTag( - 0, + 6, Pose3d( - (1).centi.meters, - (87.5).inches + 274.8.centi.meters, - (91.5).centi.meters, + (40.45).inches, + (174.19).inches, // FIRST's diagram has a typo (it says 147.19) + (18.22).inches, Rotation3d() ) - ) + ), + AprilTag(7, Pose3d((40.45).inches, (108.19).inches, (18.22).inches, Rotation3d())), + AprilTag(8, Pose3d((40.45).inches, (42.19).inches, (18.22).inches, Rotation3d())) ) // val homeAprilTags: List = @@ -229,6 +230,24 @@ object FieldConstants { } } + fun allianceFlip(zone: Zone2d): Zone2d { + return if (DriverStation.getAlliance() == Alliance.Red) { + Zone2d(zone.vertices.map { allianceFlip(it) }) + } else { + zone + } + } + + fun determineZone(zones: List, pose: Pose2d): Zone2d? { + for (zone in zones) { + if (AllianceFlipUtil.apply(zone).containsPose(pose)) { + return zone + } + } + + return null + } + fun getTagPose(id: Int): Pose3d? { return if (Constants.Universal.REAL_FIELD) aprilTags.firstOrNull { it.id == id }?.pose else homeAprilTags.firstOrNull { it.id == id }?.pose @@ -397,4 +416,143 @@ object FieldConstants { } } } + + object Zones { + val closeLeftCommunity: Zone2d = + Zone2d( // done + listOf( + Translation2d(Grids.outerX, Community.chargingStationLeftY), + Translation2d(Community.chargingStationInnerX, Community.chargingStationLeftY), + Translation2d(Community.chargingStationInnerX, Community.leftY), + Translation2d(Grids.outerX, Community.leftY) + ) + ) + + val farLeftCommunity: Zone2d = AllianceFlipUtil.apply(closeLeftCommunity, force = true) + + val closeCenterCommunity: Zone2d = + Zone2d( // done + listOf( + Translation2d(Grids.outerX, Community.chargingStationRightY), + Translation2d(Community.chargingStationInnerX, Community.chargingStationRightY), + Translation2d(Community.chargingStationInnerX, Community.chargingStationLeftY), + Translation2d(Grids.outerX, Community.chargingStationLeftY) + ) + ) + + val farCenterCommunity: Zone2d = AllianceFlipUtil.apply(closeCenterCommunity, force = true) + + val closeRightCommunity: Zone2d = + Zone2d( // done + listOf( + Translation2d(Grids.outerX, Community.rightY), + Translation2d(Community.chargingStationInnerX, Community.rightY), + Translation2d(Community.chargingStationInnerX, Community.chargingStationRightY), + Translation2d(Grids.outerX, Community.chargingStationRightY) + ) + ) + + val farRightCommunity: Zone2d = AllianceFlipUtil.apply(closeRightCommunity, force = true) + + val closeLoadingZoneLane: Zone2d = + Zone2d( // done + listOf( + Translation2d((132.25).inches, LoadingZone.rightY), + Translation2d(fieldLength / 2, LoadingZone.rightY), + Translation2d(fieldLength / 2, fieldWidth), + Translation2d((264.25).inches, fieldWidth), + Translation2d((264.25).inches, LoadingZone.midY), + Translation2d((132.25).inches, LoadingZone.midY) + ) + ) + + val farLoadingZoneLane: Zone2d = AllianceFlipUtil.apply(closeLoadingZoneLane, force = true) + + val closeLeftLane: Zone2d = + Zone2d( // done + listOf( + Translation2d(Community.chargingStationInnerX, Community.chargingStationLeftY), + Translation2d(fieldLength / 2, Community.chargingStationLeftY), + Translation2d(fieldLength / 2, Community.leftY), + Translation2d(Community.chargingStationInnerX, Community.leftY) + ) + ) + + val farLeftLane: Zone2d = AllianceFlipUtil.apply(closeLeftLane, force = true) + + val closeRightLane: Zone2d = + Zone2d( // done + listOf( + Translation2d(Community.chargingStationInnerX, 0.meters), + Translation2d(fieldLength / 2, 0.meters), + Translation2d(fieldLength / 2, Community.chargingStationRightY), + Translation2d(Community.chargingStationInnerX, Community.chargingStationRightY) + ) + ) + + val farRightLane: Zone2d = AllianceFlipUtil.apply(closeRightLane, force = true) + + val closeCenterLeftLane: Zone2d = + Zone2d( // done + listOf( + Translation2d( + Community.chargingStationOuterX, + Community.rightY + Community.chargingStationWidth / 2 + ), + Translation2d( + fieldLength / 2, Community.rightY + Community.chargingStationWidth / 2 + ), + Translation2d(fieldLength / 2, Community.chargingStationLeftY), + Translation2d(Community.chargingStationOuterX, Community.chargingStationLeftY) + ) + ) + + val farCenterLeftLane: Zone2d = AllianceFlipUtil.apply(closeCenterLeftLane, force = true) + + val closeCenterRightLane: Zone2d = + Zone2d( // done + listOf( + Translation2d( + Community.chargingStationOuterX, + Community.rightY + Community.chargingStationWidth / 2 + ), + Translation2d( + fieldLength / 2, Community.rightY + Community.chargingStationWidth / 2 + ), + Translation2d(fieldLength / 2, Community.chargingStationRightY), + Translation2d(Community.chargingStationOuterX, Community.chargingStationRightY) + ) + ) + + val farCenterRightLane: Zone2d = AllianceFlipUtil.apply(closeCenterRightLane, force = true) + + val farLoadingZone: Zone2d = + Zone2d( // done + LoadingZone.regionCorners.toList() + ) + + val closeLoadingZone: Zone2d = AllianceFlipUtil.apply(farLoadingZone, force = true) + + val allZones = + listOf( + closeLeftCommunity, + farLeftCommunity, + closeCenterCommunity, + farCenterCommunity, + closeRightCommunity, + farRightCommunity, + closeLoadingZone, + farLoadingZone, + closeLoadingZoneLane, + farLoadingZoneLane, + closeLeftLane, + farLeftLane, + closeCenterLeftLane, + farCenterLeftLane, + closeCenterRightLane, + farCenterRightLane, + closeRightLane, + farRightLane + ) + } } diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/WaypointConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/WaypointConstants.kt new file mode 100644 index 00000000..e8bdaecf --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/WaypointConstants.kt @@ -0,0 +1,217 @@ +package com.team4099.robot2023.config.constants + +import com.team4099.lib.math.Zone2d +import com.team4099.lib.trajectory.Waypoint +import edu.wpi.first.math.geometry.Rotation2d +import org.team4099.lib.geometry.Translation2d +import org.team4099.lib.units.base.meters +import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.inRadians + +object WaypointConstants { + enum class SubstationPoints(val waypoints: List) { + CloseLeftCommunity( + listOf( + Waypoint( + Translation2d(3.meters, 4.64.meters).translation2d, + holonomicRotation = Rotation2d(180.degrees.inRadians) + ) + ) + ), + CloseCenterCommunity( + listOf( + Waypoint( + Translation2d(2.01.meters, 3.48.meters).translation2d, + holonomicRotation = Rotation2d(180.degrees.inRadians) + ) + ) + ), + CloseRightCommunity( + listOf( + Waypoint( + Translation2d(2.01.meters, 1.47.meters).translation2d, + holonomicRotation = Rotation2d(180.degrees.inRadians) + ) + ) + ), + CloseLeftLane(listOf(Waypoint(Translation2d(8.12.meters, 5.19.meters).translation2d))), + CloseLoadingZoneLane(listOf(Waypoint(Translation2d(8.12.meters, 6.75.meters).translation2d))), + CloseCenterLeftLane(listOf(Waypoint(Translation2d(8.12.meters, 3.8.meters).translation2d))), + CloseCenterRightLane(listOf(Waypoint(Translation2d(8.12.meters, 3.8.meters).translation2d))), + CloseRightLane(listOf(Waypoint(Translation2d(6.meters, 0.85.meters).translation2d))), + FarRightLane(listOf(Waypoint(Translation2d(10.16.meters, 2.meters).translation2d))), + FarCenterLeftLane( + listOf( + Waypoint( // excess + Translation2d(10.9.meters, 3.81.meters).translation2d + ) + ) + ), + FarCenterRightLane( + listOf( + Waypoint( // excess + Translation2d(10.9.meters, 3.81.meters).translation2d + ) + ) + ), + FarLeftLane(listOf(Waypoint(Translation2d(10.9.meters, 4.69.meters).translation2d))), + FarLoadingZoneLane(listOf(Waypoint(Translation2d(12.66.meters, 6.64.meters).translation2d))), + FarLoadingZone( + listOf( + Waypoint( + Translation2d(14.25.meters, 6.69.meters).translation2d, + null, + Rotation2d(90.degrees.inRadians) + ) + ) + ) + } + + enum class CommunityPoints(val waypoints: List) { + CloseLeftCommunity( + listOf( + Waypoint( + Translation2d(2.01.meters, 4.meters).translation2d, + holonomicRotation = Rotation2d(180.degrees.inRadians) + ) + ) + ), + CloseRightCommunity( + listOf( + Waypoint( + Translation2d(2.01.meters, 0.85.meters).translation2d, + holonomicRotation = Rotation2d(180.degrees.inRadians) + ) + ) + ), + CloseLeftLane(listOf(Waypoint(Translation2d(3.85.meters, 4.79.meters).translation2d))), + CloseLoadingZoneLane(listOf(Waypoint(Translation2d(5.79.meters, 5.93.meters).translation2d))), + CloseCenterLeftLane(listOf(Waypoint(Translation2d(5.79.meters, 4.79.meters).translation2d))), + CloseCenterRightLane(listOf(Waypoint(Translation2d(5.79.meters, 0.85.meters).translation2d))), + CloseRightLane(listOf(Waypoint(Translation2d(3.15.meters, 0.85.meters).translation2d))), + FarRightLane(listOf(Waypoint(Translation2d(10.16.meters, 0.85.meters).translation2d))), + FarCenterLeftLane( + listOf( + Waypoint( // excess + Translation2d(10.9.meters, 3.81.meters).translation2d + ) + ) + ), + FarCenterRightLane( + listOf( + Waypoint( // excess + Translation2d(10.9.meters, 3.81.meters).translation2d + ) + ) + ), + FarLeftLane(listOf(Waypoint(Translation2d(8.69.meters, 4.69.meters).translation2d))), + FarLoadingZoneLane(listOf(Waypoint(Translation2d(8.55.meters, 5.66.meters).translation2d))), + FarLoadingZone( + listOf( + Waypoint( + Translation2d(10.2.meters, 7.15.meters).translation2d, + ) + ) + ) + } + + object SubstationPaths { + var loadingZone = SubstationPoints.FarLoadingZone.waypoints + + var farLoadingZoneLane = SubstationPoints.FarLoadingZoneLane.waypoints + loadingZone + var farRightLane = + SubstationPoints.FarRightLane.waypoints + + SubstationPoints.FarLeftLane.waypoints + + farLoadingZoneLane + var excessFarLanes = SubstationPoints.FarLeftLane.waypoints + farLoadingZoneLane + + var closeLoadingZoneLane = SubstationPoints.CloseLoadingZoneLane.waypoints + loadingZone + var closeLeftLane = SubstationPoints.CloseLeftLane.waypoints + loadingZone + var closeCenterLeftLane = SubstationPoints.CloseCenterLeftLane.waypoints + farLoadingZoneLane + var closeCenterRightLane = SubstationPoints.CloseCenterRightLane.waypoints + closeCenterLeftLane + var closeRightLane = SubstationPoints.CloseRightLane.waypoints + closeCenterLeftLane + var closeLeftCommunity = SubstationPoints.CloseLeftCommunity.waypoints + closeLeftLane + var closeCenterCommunity = SubstationPoints.CloseCenterCommunity.waypoints + closeLeftCommunity + var closeRightCommunity = SubstationPoints.CloseRightCommunity.waypoints + closeCenterCommunity + + fun getPath(zone: Zone2d?): List { + return when (zone) { + FieldConstants.Zones.closeLeftCommunity -> closeLeftCommunity + FieldConstants.Zones.closeCenterCommunity -> closeCenterCommunity + FieldConstants.Zones.closeRightCommunity -> closeRightCommunity + FieldConstants.Zones.closeLeftLane -> closeLeftLane + FieldConstants.Zones.closeCenterLeftLane -> closeCenterLeftLane + FieldConstants.Zones.closeCenterRightLane -> closeCenterRightLane + FieldConstants.Zones.closeRightLane -> closeRightLane + FieldConstants.Zones.closeLoadingZoneLane -> closeLoadingZoneLane + FieldConstants.Zones.farLoadingZone -> loadingZone + FieldConstants.Zones.farLeftLane -> excessFarLanes + FieldConstants.Zones.farRightLane -> farRightLane + FieldConstants.Zones.farLoadingZoneLane -> farLoadingZoneLane + FieldConstants.Zones.farCenterLeftLane -> excessFarLanes + FieldConstants.Zones.farCenterRightLane -> excessFarLanes + null -> listOf() + else -> listOf() + } + } + } + + object CommunityPaths { + var closeLeftCommunity = CommunityPoints.CloseLeftCommunity.waypoints + var closeRightCommunity = CommunityPoints.CloseRightCommunity.waypoints + + var closeLeftLane = CommunityPoints.CloseLeftLane.waypoints + closeLeftCommunity + var closeRightLane = CommunityPoints.CloseRightLane.waypoints + closeRightCommunity + + var closeCenterLeftLane = CommunityPoints.CloseCenterLeftLane.waypoints + closeLeftLane + var closeCenterRightLane = CommunityPoints.CloseCenterRightLane.waypoints + closeRightLane + var closeLoadingZoneLane = CommunityPoints.CloseLoadingZoneLane.waypoints + closeLeftLane + + var farLoadingZoneLane = CommunityPoints.FarLoadingZoneLane.waypoints + closeLeftLane + var farRightLane = CommunityPoints.FarRightLane.waypoints + closeRightLane + var farLeftLane = CommunityPoints.FarLeftLane.waypoints + closeLeftLane + var loadingZone = CommunityPoints.FarLoadingZone.waypoints + farLoadingZoneLane + + // var loadingZone = SubstationPoints.FarLoadingZone.waypoints + // + // var farLoadingZoneLane = SubstationPoints.FarLoadingZoneLane.waypoints + loadingZone + // var farRightLane = + // SubstationPoints.FarRightLane.waypoints + + // SubstationPoints.FarLeftLane.waypoints + + // farLoadingZoneLane + // var excessFarLanes = SubstationPoints.FarLeftLane.waypoints + farLoadingZoneLane + // + // var closeLoadingZoneLane = SubstationPoints.CloseLoadingZoneLane.waypoints + loadingZone + // var closeLeftLane = SubstationPoints.CloseLeftLane.waypoints + loadingZone + // var closeCenterLeftLane = SubstationPoints.CloseCenterLeftLane.waypoints + + // farLoadingZoneLane + // var closeCenterRightLane = SubstationPoints.CloseCenterRightLane.waypoints + + // closeCenterLeftLane + // var closeRightLane = SubstationPoints.CloseRightLane.waypoints + closeCenterLeftLane + // var closeLeftCommunity = SubstationPoints.CloseLeftCommunity.waypoints + closeLeftLane + // var closeCenterCommunity = SubstationPoints.CloseCenterCommunity.waypoints + + // closeLeftCommunity + // var closeRightCommunity = SubstationPoints.CloseRightCommunity.waypoints + + // closeCenterCommunity + + fun getPath(zone: Zone2d?): List { + return when (zone) { + FieldConstants.Zones.closeLeftCommunity -> closeLeftCommunity + FieldConstants.Zones.closeRightCommunity -> closeRightCommunity + FieldConstants.Zones.closeLeftLane -> closeLeftLane + FieldConstants.Zones.closeCenterLeftLane -> closeCenterLeftLane + FieldConstants.Zones.closeCenterRightLane -> closeCenterRightLane + FieldConstants.Zones.closeRightLane -> closeRightLane + FieldConstants.Zones.closeLoadingZoneLane -> closeLoadingZoneLane + FieldConstants.Zones.farLoadingZone -> loadingZone + FieldConstants.Zones.farLeftLane -> farLeftLane + FieldConstants.Zones.farRightLane -> farRightLane + FieldConstants.Zones.farLoadingZoneLane -> farLoadingZoneLane + FieldConstants.Zones.farCenterLeftLane -> farLeftLane + FieldConstants.Zones.farCenterRightLane -> farRightLane + null -> listOf() + else -> listOf() + } + } + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt index 1fc40a07..0f560a99 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt @@ -747,7 +747,7 @@ class Superstructure( } } else { if (scoringConeWithoutLoweringGroundIntake) { // previously scored without lowering ground - // intake so we need to retract manipulator + // intake, so we need to retract manipulator // and lower elevator val rollerCommandedVoltage = when (usingGamePiece) { diff --git a/src/main/kotlin/com/team4099/robot2023/util/AllianceFlipUtil.kt b/src/main/kotlin/com/team4099/robot2023/util/AllianceFlipUtil.kt index 6db2c5c1..54994922 100644 --- a/src/main/kotlin/com/team4099/robot2023/util/AllianceFlipUtil.kt +++ b/src/main/kotlin/com/team4099/robot2023/util/AllianceFlipUtil.kt @@ -1,6 +1,8 @@ package com.team4099.robot2023.util +import com.team4099.lib.math.Zone2d import com.team4099.lib.trajectory.RotationSequence +import com.team4099.lib.trajectory.Waypoint import com.team4099.robot2023.config.constants.FieldConstants import edu.wpi.first.math.geometry.Rotation2d import edu.wpi.first.math.trajectory.Trajectory @@ -23,8 +25,8 @@ import org.team4099.lib.units.derived.sin */ object AllianceFlipUtil { /** Flips a translation to the correct side of the field based on the current alliance color. */ - fun apply(translation: Translation2d): Translation2d { - return if (shouldFlip()) { + fun apply(translation: Translation2d, force: Boolean = false): Translation2d { + return if (shouldFlip() || force) { Translation2d(FieldConstants.fieldLength - translation.x, translation.y) } else { translation @@ -68,6 +70,14 @@ object AllianceFlipUtil { } } + fun apply(waypoint: Waypoint): Waypoint { + return if (shouldFlip()) { + Waypoint(apply(waypoint.translation), waypoint.driveRotation, waypoint.holonomicRotation) + } else { + waypoint + } + } + /** * Flips a trajectory state to the correct side of the field based on the current alliance color. */ @@ -90,6 +100,14 @@ object AllianceFlipUtil { } } + fun apply(zone: Zone2d, force: Boolean = false): Zone2d { + return if (shouldFlip() || force) { + Zone2d(zone.vertices.map { AllianceFlipUtil.apply(it, force) }) + } else { + zone + } + } + /** Flips a rotation sequence state based on the current alliance color. */ fun apply(state: RotationSequence.State): RotationSequence.State { return if (shouldFlip()) { diff --git a/src/main/kotlin/com/team4099/robot2023/util/PoseEstimator.kt b/src/main/kotlin/com/team4099/robot2023/util/PoseEstimator.kt index 87843c3b..27e8112b 100644 --- a/src/main/kotlin/com/team4099/robot2023/util/PoseEstimator.kt +++ b/src/main/kotlin/com/team4099/robot2023/util/PoseEstimator.kt @@ -6,6 +6,7 @@ import edu.wpi.first.math.VecBuilder import edu.wpi.first.math.numbers.N1 import edu.wpi.first.math.numbers.N3 import edu.wpi.first.wpilibj.Timer +import org.littletonrobotics.junction.Logger import org.team4099.lib.geometry.Pose2d import org.team4099.lib.geometry.Twist2d import org.team4099.lib.units.base.Time @@ -45,7 +46,12 @@ class PoseEstimator(stateStdDevs: Matrix) { fun addVisionData(visionData: List) { for (timestampedVisionUpdate in visionData) { val timestamp: Double = timestampedVisionUpdate.timestamp.inSeconds - val visionUpdate = VisionUpdate(timestampedVisionUpdate.pose, timestampedVisionUpdate.stdDevs) + val visionUpdate = + VisionUpdate( + timestampedVisionUpdate.pose, + timestampedVisionUpdate.stdDevs, + timestampedVisionUpdate.fromVision + ) if (updates.containsKey(timestamp)) { // There was already an update at this timestamp, add to it val oldVisionUpdates: ArrayList = updates[timestamp]!!.visionUpdates @@ -98,6 +104,17 @@ class PoseEstimator(stateStdDevs: Matrix) { for (updateEntry in updates.entries) { latestPose = updateEntry.value.apply(latestPose, q) } + + for (update in updates) { + if (update.value.visionUpdates.size > 0 && update.value.visionUpdates[0].fromVision) { + Logger.getInstance().recordOutput("Vision/Buffer/Vision", update.key) + + Logger.getInstance() + .recordOutput("Vision/Buffer/VisionPose", update.value.visionUpdates[0].pose.pose2d) + } else { + Logger.getInstance().recordOutput("Vision/Buffer/Drivetrain", update.key) + } + } } /** @@ -154,7 +171,11 @@ class PoseEstimator(stateStdDevs: Matrix) { } /** Represents a single vision pose with associated standard deviations. */ - class VisionUpdate(val pose: Pose2d, val stdDevs: Matrix) { + class VisionUpdate( + val pose: Pose2d, + val stdDevs: Matrix, + val fromVision: Boolean = false + ) { companion object { val compareDescStdDev = Comparator { a: VisionUpdate, b: VisionUpdate -> -(a.stdDevs.get(0, 0) + a.stdDevs.get(1, 0)).compareTo( @@ -165,7 +186,12 @@ class PoseEstimator(stateStdDevs: Matrix) { } /** Represents a single vision pose with a timestamp and associated standard deviations. */ - class TimestampedVisionUpdate(val timestamp: Time, val pose: Pose2d, val stdDevs: Matrix) + class TimestampedVisionUpdate( + val timestamp: Time, + val pose: Pose2d, + val stdDevs: Matrix, + val fromVision: Boolean = false + ) companion object { private const val historyLengthSecs = 0.3 }