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/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt index 9e1a7d1d..7196a2cc 100644 --- a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt +++ b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt @@ -1,7 +1,7 @@ package com.team4099.robot2023 import com.team4099.robot2023.auto.AutonomousSelector -import com.team4099.robot2023.commands.AutoScoreCommand +import com.team4099.robot2023.commands.AutoIntakeCommand import com.team4099.robot2023.commands.drivetrain.ResetGyroYawCommand import com.team4099.robot2023.commands.drivetrain.TeleopDriveCommand import com.team4099.robot2023.config.ControlBoard @@ -202,7 +202,7 @@ object RobotContainer { ) ControlBoard.groundIntakeCone.whileTrue(superstructure.groundIntakeConeCommand()) - ControlBoard.autoScore.whileTrue(AutoScoreCommand(drivetrain, superstructure)) + ControlBoard.autoScore.whileTrue(AutoIntakeCommand(drivetrain, superstructure)) ControlBoard.ejectGamePiece.whileTrue(superstructure.ejectGamePieceCommand()) // ControlBoard.dpadDown.whileTrue(PickupFromSubstationCommand(drivetrain, superstructure)) 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..a2cb89d1 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/commands/AutoIntakeCommand.kt @@ -0,0 +1,170 @@ +package com.team4099.robot2023.commands + +import com.team4099.lib.logging.LoggedTunableValue +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.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.geometry.Translation2d +import org.team4099.lib.units.base.inInches +import org.team4099.lib.units.base.inches +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 +import com.team4099.robot2023.config.constants.Constants.Universal.Substation as Substation + +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) + 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 + ) + ) + } + + // Replace last waypoint with the correct waypoint based on operator app + val finalPose = + AllianceFlipUtil.apply( + when (superstructure.objective.substation) { + Substation.DOUBLE_SUBSTATION_LEFT -> { + Pose2d( + FieldConstants.getTagPose( + Constants.AprilTagIds.BLUE_DOUBLE_SUBSTATION_ID + )!! + .toPose2d() + .translation + + Translation2d( + -doubleSubstationXoffset.get(), doubleSubstationYoffset.get() + ), + 0.0.degrees + ) + } + Substation.DOUBLE_SUBSTATION_RIGHT -> { + Pose2d( + FieldConstants.getTagPose( + Constants.AprilTagIds.BLUE_DOUBLE_SUBSTATION_ID + )!! + .toPose2d() + .translation + + Translation2d( + -doubleSubstationXoffset.get(), -doubleSubstationYoffset.get() + ), + 0.0.degrees + ) + } + Substation.SINGLE_SUBSTATION -> { + Pose2d( + FieldConstants.LoadingZone.singleSubstationTranslation + + Translation2d( + singleSubstationXoffset.get(), -singleSubstationYoffset.get() + ), + 90.0.degrees + ) + } + Substation.NONE -> { + Pose2d( + FieldConstants.LoadingZone.singleSubstationTranslation + + Translation2d( + singleSubstationXoffset.get(), -singleSubstationYoffset.get() + ), + 90.0.degrees + ) + } // Return single substation by default + } + ) + + intermediaryWaypoints = + intermediaryWaypoints.mapIndexed { index, waypoint -> + waypoint.takeUnless { index == intermediaryWaypoints.size - 1 } + ?: Waypoint( + finalPose.pose2d.translation, holonomicRotation = finalPose.pose2d.rotation + ) + } + }) + + 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.intakeFromSubstationCommand() + ) + } + + companion object { + val doubleSubstationXoffset = + LoggedTunableValue( + "Drivetrain/doubleSubstationXOffsetInches", + 1.0.meters, + Pair({ it.inInches }, { it.inches }) + ) + val doubleSubstationYoffset = + LoggedTunableValue( + "Drivetrain/doubleSubstationYOffsetInches", + 0.735.meters, + Pair({ it.inInches }, { it.inches }) + ) + + val singleSubstationXoffset = + LoggedTunableValue( + "Drivetrain/singleSubstationXOffset", 0.0.meters, Pair({ it.inInches }, { it.inches }) + ) + val singleSubstationYoffset = + LoggedTunableValue( + "Drivetrain/singleSubstationYOffset", 1.0.meters, Pair({ it.inInches }, { it.inches }) + ) + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt index 4cfa010b..181272be 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt @@ -12,7 +12,7 @@ typealias NodeTier = Constants.Universal.NodeTier object Constants { object Universal { val SIM_MODE = Tuning.SimType.SIM - const val REAL_FIELD = false + const val REAL_FIELD = true const val CTRE_CONFIG_TIMEOUT = 0 const val EPSILON = 1E-9 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 3f0da27b..eb395dbc 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 @@ -12,6 +14,7 @@ import org.team4099.lib.geometry.Translation3d 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.derived.Angle import org.team4099.lib.units.derived.cos import org.team4099.lib.units.derived.radians @@ -62,7 +65,7 @@ object FieldConstants { ) ), AprilTag( - 4, + 5, Pose3d( (43.125).inches, (173.375).inches, @@ -71,7 +74,7 @@ object FieldConstants { ) ), AprilTag( - 5, + 4, Pose3d( (409.5).inches, (85.5).inches, @@ -244,6 +247,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 @@ -412,4 +433,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..0f0605b8 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/WaypointConstants.kt @@ -0,0 +1,110 @@ +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) + ) + ) + ) + } + + 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() + } + } + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt index 86deff06..86e2cb94 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt @@ -12,16 +12,19 @@ import com.team4099.robot2023.util.FMSData import com.team4099.robot2023.util.PoseEstimator import com.team4099.robot2023.util.Velocity2d import edu.wpi.first.math.VecBuilder +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator import edu.wpi.first.math.kinematics.SwerveDriveKinematics import edu.wpi.first.math.kinematics.SwerveDriveOdometry import edu.wpi.first.math.kinematics.SwerveModulePosition import edu.wpi.first.math.kinematics.SwerveModuleState +import edu.wpi.first.networktables.NetworkTablesJNI import edu.wpi.first.wpilibj.DriverStation import edu.wpi.first.wpilibj.RobotBase import edu.wpi.first.wpilibj2.command.SubsystemBase import org.littletonrobotics.junction.Logger import org.team4099.lib.geometry.Pose2d import org.team4099.lib.geometry.Pose3d +import org.team4099.lib.geometry.Rotation2dWPILIB import org.team4099.lib.geometry.Rotation3d import org.team4099.lib.geometry.Transform2d import org.team4099.lib.geometry.Translation2d @@ -39,7 +42,6 @@ import org.team4099.lib.units.base.inMilliseconds import org.team4099.lib.units.base.inSeconds 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.degrees import org.team4099.lib.units.derived.inRadians @@ -116,7 +118,16 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB backRightWheelLocation.translation2d ) - var swerveDrivePoseEstimator = PoseEstimator(VecBuilder.fill(0.003, 0.003, 0.00001)) + // var swerveDrivePoseEstimator = PoseEstimator(VecBuilder.fill(0.003, 0.003, 0.00001)) + var swerveDrivePoseEstimator = + SwerveDrivePoseEstimator( + swerveDriveKinematics, + gyroInputs.gyroYaw.inRotation2ds, + generateSwerveModuleStates(), + Pose2d().pose2d, + VecBuilder.fill(0.003, 0.003, 0.00001), + VecBuilder.fill(0.07, 0.07, 0.75) + ) var swerveDriveOdometry = SwerveDriveOdometry( @@ -131,16 +142,18 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB ) var odometryPose: Pose2d - //get() = swerveDrivePoseEstimator.getLatestPose() - get() { - return Pose2d( - 42.875.inches + 79.centi.meters + 14.inches + 2.75.inches, - 113.25.inches + 1.inches, - 180.degrees - ) - } + get() = Pose2d(swerveDrivePoseEstimator.estimatedPosition) + // get() { + // return Pose2d( + // 42.875.inches + 79.centi.meters + 14.inches + 2.75.inches, + // 113.25.inches + 1.inches, + // 180.degrees + // ) + // } set(value) { - swerveDrivePoseEstimator.resetPose(value) + swerveDrivePoseEstimator.resetPosition( + Rotation2dWPILIB(value.rotation.inRadians), generateSwerveModuleStates(), value.pose2d + ) if (RobotBase.isReal()) {} else { undriftedPose = odometryPose @@ -181,6 +194,8 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB var lastGyroYaw = 0.0.radians override fun periodic() { + Logger.getInstance().recordOutput("Drivetrain/timestamp", NetworkTablesJNI.now()) + val startTime = Clock.realTimestamp gyroNotConnectedAlert.set(!gyroInputs.gyroConnected) gyroIO.updateInputs(gyroInputs) @@ -325,7 +340,13 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB Logger.getInstance().recordOutput(VisionConstants.SIM_POSE_TOPIC_NAME, undriftedPose.pose2d) } - swerveDrivePoseEstimator.addDriveData(Clock.fpgaTime.inSeconds, Twist2d(driveTwist)) + swerveDrivePoseEstimator.updateWithTime( + Clock.fpgaTime.inSeconds, + gyroInputs.gyroYaw.inRotation2ds, + generateSwerveModuleStates(), + ) + + Logger.getInstance().recordOutput("Vision/drivetrainTimestamp", Clock.fpgaTime.inSeconds) } /** @@ -546,7 +567,7 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB fun zeroGyroYaw(toAngle: Angle = 0.degrees) { gyroIO.zeroGyroYaw(toAngle) - swerveDrivePoseEstimator.resetPose(Pose2d(odometryPose.x, odometryPose.y, gyroInputs.gyroYaw)) + odometryPose = Pose2d(odometryPose.x, odometryPose.y, gyroInputs.gyroYaw) if (RobotBase.isSimulation()) { swerveDriveOdometry.resetPosition( @@ -579,7 +600,32 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB swerveModules.forEach { it.zeroDrive() } } + private fun generateSwerveModuleStates(): Array { + var moduleStates = mutableListOf() + for (module in swerveModules) { + moduleStates.add( + SwerveModulePosition( + module.inputs.drivePosition.inMeters, + Rotation2dWPILIB(module.inputs.steeringPosition.inRadians) + ) + ) + } + + return moduleStates.toTypedArray() + } + fun addVisionData(visionData: List) { - swerveDrivePoseEstimator.addVisionData(visionData) + var count = 0.0 + var time = mutableListOf() + for (poseEstimate in visionData) { + Logger.getInstance() + .recordOutput("Vision/glitch_instance/${count}_instance", poseEstimate.pose.pose2d) + swerveDrivePoseEstimator.addVisionMeasurement( + poseEstimate.pose.pose2d, poseEstimate.timestamp.inSeconds + ) + count += 1 + time.add(poseEstimate.timestamp.inSeconds) + } + Logger.getInstance().recordOutput("Vision/glitch_instance/total_count", count) } } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt index e12f5d93..402277ab 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt @@ -337,9 +337,9 @@ class Elevator(val io: ElevatorIO) { get() = ( currentRequest is ElevatorRequest.TargetingPosition && - (inputs.elevatorPosition - elevatorPositionTarget).absoluteValue <= elevatorContinueBuffer - ) || - (TunableElevatorHeights.enableElevator.get() != 1.0) + (inputs.elevatorPosition - elevatorPositionTarget).absoluteValue <= + elevatorContinueBuffer + ) || (TunableElevatorHeights.enableElevator.get() != 1.0) init { TunableElevatorHeights 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 73e59d9a..1991bb71 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) { @@ -781,16 +781,23 @@ class Superstructure( } } - if ((manipulator.isAtTargetedPosition && - (elevator.isAtTargetedPosition || (NodeTier.HIGH == nodeTier && ( - (elevator.inputs.elevatorPosition - 48.inches).absoluteValue.inInches < 8 - ))) && - ( - groundIntake.isAtTargetedPosition || - groundIntake - .canContinueSafely - )) - ) { // because manipulator and elevator will move while we're + if (( + manipulator.isAtTargetedPosition && + ( + elevator.isAtTargetedPosition || + ( + NodeTier.HIGH == nodeTier && + ((elevator.inputs.elevatorPosition - 48.inches).absoluteValue.inInches < 8) + ) + ) && + ( + groundIntake.isAtTargetedPosition || + groundIntake + .canContinueSafely + ) + ) + ) { // because manipulator and elevator will move while + // we're // switching between different cone tiers val rollerCommandedVoltage = when (usingGamePiece) { @@ -846,10 +853,14 @@ class Superstructure( elevator.currentRequest = Request.ElevatorRequest.TargetingPosition(scoreHeight) - Logger.getInstance().recordOutput("Superstructure/PlsBruh", (elevator.inputs.elevatorPosition - scoreHeight).absoluteValue.inInches) - - if ((elevator.inputs.elevatorPosition - scoreHeight).absoluteValue.inInches < 8 || elevator.isAtTargetedPosition + Logger.getInstance() + .recordOutput( + "Superstructure/PlsBruh", + (elevator.inputs.elevatorPosition - scoreHeight).absoluteValue.inInches + ) + if ((elevator.inputs.elevatorPosition - scoreHeight).absoluteValue.inInches < 8 || + elevator.isAtTargetedPosition ) { val extension = @@ -1169,6 +1180,28 @@ class Superstructure( return returnCommand } + fun intakeFromSubstationCommand(): CommandBase { + val returnCommand = + if (objective.substation in + setOf( + Constants.Universal.Substation.DOUBLE_SUBSTATION_LEFT, + Constants.Universal.Substation.DOUBLE_SUBSTATION_RIGHT + ) + ) { + runOnce { + currentRequest = SuperstructureRequest.DoubleSubstationIntakePrep(GamePiece.CONE) + } + .until { currentState == SuperstructureStates.DOUBLE_SUBSTATION_CLEANUP } + } else { + runOnce { currentRequest = SuperstructureRequest.GroundIntakeCone() }.until { + isAtRequestedState && currentState == SuperstructureStates.GROUND_INTAKE_CONE + } + } + + returnCommand.name = "IntakeFromSubstationCommand" + return returnCommand + } + fun score(): CommandBase { var stateToBeChecked: SuperstructureStates if (usingGamePiece == GamePiece.CUBE) { diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/vision/Vision.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/vision/Vision.kt index dd3c802a..93d1e541 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/vision/Vision.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/vision/Vision.kt @@ -7,6 +7,7 @@ import com.team4099.robot2023.config.constants.VisionConstants import com.team4099.robot2023.subsystems.vision.camera.CameraIO import com.team4099.robot2023.util.PoseEstimator import edu.wpi.first.math.VecBuilder +import edu.wpi.first.wpilibj.DriverStation import edu.wpi.first.wpilibj2.command.SubsystemBase import org.littletonrobotics.junction.Logger import org.team4099.lib.geometry.Pose2d @@ -29,17 +30,17 @@ class Vision(vararg cameras: CameraIO) : SubsystemBase() { val inputs = List(io.size) { CameraIO.CameraInputs() } companion object { - val ambiguityThreshold = 0.15 + val ambiguityThreshold = 1.6 val targetLogTime = 0.05.seconds val cameraPoses = VisionConstants.CAMERA_TRANSFORMS - val xyStdDevCoeffecient = 0.01 + val xyStdDevCoeffecient = 0.1 val thetaStdDevCoefficient = 0.75 } - private val xyStdDevCoefficient = TunableNumber("Vision/xystdev", 0.1) + private val xyStdDevCoefficient = TunableNumber("Vision/xystdev", Vision.xyStdDevCoeffecient) - private val thetaStdDev = TunableNumber("Vision/thetaStdDev", 0.75) + private val thetaStdDev = TunableNumber("Vision/thetaStdDev", Vision.thetaStdDevCoefficient) private var poseSupplier = Supplier { Pose2d() } private var visionConsumer: Consumer> = Consumer {} @@ -86,7 +87,6 @@ class Vision(vararg cameras: CameraIO) : SubsystemBase() { val visionUpdates = mutableListOf() for (instance in io.indices) { - for (frameIndex in inputs[instance].timestamps.indices) { lastFrameTimes[instance] = Clock.fpgaTime val timestamp = inputs[instance].timestamps[frameIndex] @@ -185,6 +185,19 @@ class Vision(vararg cameras: CameraIO) : SubsystemBase() { } } + val rejectPose: Boolean = + ( + robotPose != null && + ( + ( + (currentPose - robotPose).translation.magnitude.meters > 2.meters && + DriverStation.isEnabled() + ) + ) + ) + + Logger.getInstance().recordOutput("Vision/rejectPose", rejectPose) + if (cameraPose == null || robotPose == null) { continue } @@ -212,30 +225,41 @@ class Vision(vararg cameras: CameraIO) : SubsystemBase() { val xyStdDev = xyStdDevCoefficient.get() * averageDistance.inMeters.pow(2) / tagPoses.size val thetaStdDev = thetaStdDev.get() * averageDistance.inMeters.pow(2) / tagPoses.size + Logger.getInstance().recordOutput("Vision/timestamp", (timestamp).inMilliseconds) + visionUpdates.add( PoseEstimator.TimestampedVisionUpdate( - timestamp, robotPose, VecBuilder.fill(xyStdDev, xyStdDev, thetaStdDev) + timestamp, robotPose, VecBuilder.fill(xyStdDev, xyStdDev, thetaStdDev), true ) ) - robotPoses.add(robotPose) - - Logger.getInstance() - .recordOutput( - "Vision/${VisionConstants.CAMERA_NAMES[instance]}/latencyMS", - (Clock.fpgaTime - timestamp).inMilliseconds - ) - Logger.getInstance() - .recordOutput( - "Vision/${VisionConstants.CAMERA_NAMES[instance]}/estimatedRobotPose", - robotPose.pose2d - ) + robotPoses.add(robotPose) - Logger.getInstance() - .recordOutput( - "Vision/${VisionConstants.CAMERA_NAMES[instance]}/tagPoses", - *tagPoses.map { it.pose3d }.toTypedArray() - ) + if (!DriverStation.isEStopped()) { + Logger.getInstance() + .recordOutput( + "Vision/${VisionConstants.CAMERA_NAMES[instance]}/latencyMS", + (Clock.fpgaTime - timestamp).inMilliseconds + ) + + Logger.getInstance() + .recordOutput( + "Vision/${VisionConstants.CAMERA_NAMES[instance]}/estimatedRobotPose", + robotPose.pose2d + ) + + Logger.getInstance() + .recordOutput( + "Vision/${VisionConstants.CAMERA_NAMES[instance]}/estimatedRobotPoseDistance", + robotPose.x.inMeters + ) + + Logger.getInstance() + .recordOutput( + "Vision/${VisionConstants.CAMERA_NAMES[instance]}/tagPoses", + *tagPoses.map { it.pose3d }.toTypedArray() + ) + } } if (inputs[instance].timestamps.isEmpty()) { 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 }