From 5a48f4a946beee93f9c7b4c313ac5b5308bd4776 Mon Sep 17 00:00:00 2001 From: AlphaPranav9102 <49816864+AlphaPranav9102@users.noreply.github.com> Date: Tue, 18 Jul 2023 23:31:00 -0400 Subject: [PATCH 1/5] auto intake mega push --- .../kotlin/com/team4099/lib/math/Zone2d.kt | 39 +++++ .../com/team4099/lib/trajectory/Waypoint.kt | 4 +- .../kotlin/com/team4099/robot2023/Robot.kt | 2 +- .../com/team4099/robot2023/RobotContainer.kt | 4 +- .../robot2023/commands/AutoIntakeCommand.kt | 86 +++++++++++ .../config/constants/FieldConstants.kt | 136 +++++++++++++++++- .../config/constants/WaypointConstants.kt | 83 +++++++++++ .../subsystems/drivetrain/drive/Drivetrain.kt | 74 ++++++++-- .../robot2023/subsystems/elevator/Elevator.kt | 6 +- .../superstructure/Superstructure.kt | 39 +++-- .../robot2023/subsystems/vision/Vision.kt | 71 ++++++--- .../robot2023/util/AllianceFlipUtil.kt | 22 ++- .../team4099/robot2023/util/PoseEstimator.kt | 32 ++++- 13 files changed, 532 insertions(+), 66 deletions(-) create mode 100644 src/main/kotlin/com/team4099/lib/math/Zone2d.kt create mode 100644 src/main/kotlin/com/team4099/robot2023/commands/AutoIntakeCommand.kt create mode 100644 src/main/kotlin/com/team4099/robot2023/config/constants/WaypointConstants.kt 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/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..5f48a4d8 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/commands/AutoIntakeCommand.kt @@ -0,0 +1,86 @@ +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/config/constants/FieldConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/FieldConstants.kt index 3f0da27b..05dddd66 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,115 @@ object FieldConstants { } } } + + object Zones { + val closeCommunity: Zone2d = + Zone2d( // done + listOf( + Translation2d(Grids.outerX, Community.rightY), + Translation2d(Community.chargingStationInnerX, Community.rightY), + Translation2d(Community.chargingStationInnerX, Community.leftY), + Translation2d(Grids.outerX, Community.leftY) + ) + ) + + val farCommunity: Zone2d = AllianceFlipUtil.apply(closeCommunity, 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( + closeCommunity, + farCommunity, + 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..60b492e1 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/WaypointConstants.kt @@ -0,0 +1,83 @@ +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.Rotation3d +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.inDegrees +import org.team4099.lib.units.derived.inRadians + +object WaypointConstants { + enum class SubstationPoints(val waypoint : Waypoint) { + CloseCommunity(Waypoint(Translation2d(2.3.meters, 4.69.meters).translation2d)), + CloseLeftLane(Waypoint(Translation2d(8.12.meters, 5.19.meters).translation2d)), + CloseLoadingZoneLane(Waypoint(Translation2d(8.12.meters, 6.75.meters).translation2d)), + CloseCenterLeftLane(Waypoint(Translation2d(8.12.meters, 3.8.meters).translation2d)), + CloseCenterRightLane(Waypoint(Translation2d(8.12.meters, 3.8.meters).translation2d)), + CloseRightLane(Waypoint(Translation2d(8.meters, 1.45.meters).translation2d)), + FarRightLane(Waypoint(Translation2d(10.75.meters, 1.45.meters).translation2d)), + FarCenterLeftLane( + Waypoint( // excess + Translation2d(10.9.meters, 3.81.meters).translation2d + ) + ), + FarCenterRightLane( + Waypoint( // excess + Translation2d(10.9.meters, 3.81.meters).translation2d + ) + ), + FarLeftLane(Waypoint(Translation2d(12.2.meters, 5.2.meters).translation2d)), + FarLoadingZoneLane(Waypoint(Translation2d(12.66.meters, 6.64.meters).translation2d)), + FarLoadingZone( + Waypoint( + Translation2d(14.25.meters, 6.69.meters).translation2d, + null, + Rotation2d(90.degrees.inRadians) + ) + ) + } + + object SubstationPaths { + var loadingZone = listOf(SubstationPoints.FarLoadingZone.waypoint) + + var farLoadingZoneLane = + listOf(SubstationPoints.FarLoadingZoneLane.waypoint) + loadingZone + var farRightLane = listOf(SubstationPoints.FarRightLane.waypoint) + farLoadingZoneLane + var excessFarLanes = listOf(SubstationPoints.FarLeftLane.waypoint) + farLoadingZoneLane + + var closeLoadingZoneLane = + listOf(SubstationPoints.CloseLoadingZoneLane.waypoint) + loadingZone + var closeLeftLane = + listOf(SubstationPoints.CloseLeftLane.waypoint) + loadingZone + var closeCenterLeftLane = + listOf(SubstationPoints.CloseCenterLeftLane.waypoint) + farLoadingZoneLane + var closeCenterRightLane = + listOf(SubstationPoints.CloseCenterRightLane.waypoint) + closeCenterLeftLane + var closeRightLane = + listOf(SubstationPoints.CloseRightLane.waypoint) + closeCenterLeftLane + + var closeCommunity = listOf(SubstationPoints.CloseCommunity.waypoint) + closeLeftLane + + fun getPath(zone: Zone2d?): List { + return when (zone) { + FieldConstants.Zones.closeCommunity -> closeCommunity + 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..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) { @@ -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 = 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..b0b27ba6 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()) { @@ -265,6 +289,7 @@ class Vision(vararg cameras: CameraIO) : SubsystemBase() { Logger.getInstance() .recordOutput("Vision/allTagPoses", *allTagPoses.map { it.pose3d }.toTypedArray()) + visionConsumer.accept(visionUpdates) Logger.getInstance() 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 } From de33e8b05b52de5fa4652ac866db6b864773ba76 Mon Sep 17 00:00:00 2001 From: Shom770 <82843611+Shom770@users.noreply.github.com> Date: Thu, 20 Jul 2023 01:40:45 -0400 Subject: [PATCH 2/5] Tuned waypoints for auto intake --- .../com/team4099/robot2022/BuildConstants.kt | 15 +++ .../robot2023/commands/AutoIntakeCommand.kt | 23 ++-- .../team4099/robot2023/config/ControlBoard.kt | 17 ++- .../config/constants/FieldConstants.kt | 52 +++++++-- .../config/constants/WaypointConstants.kt | 103 +++++++++++------- .../robot2023/subsystems/vision/Vision.kt | 1 - 6 files changed, 146 insertions(+), 65 deletions(-) create mode 100644 src/main/kotlin/com/team4099/robot2022/BuildConstants.kt 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/commands/AutoIntakeCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/AutoIntakeCommand.kt index 5f48a4d8..9d7c8ab5 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/AutoIntakeCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/AutoIntakeCommand.kt @@ -42,22 +42,25 @@ class AutoIntakeCommand(val drivetrain: Drivetrain, val superstructure: Superstr 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 + 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){ + for (vertex in FieldConstants.Zones.farLeftLane.vertices) { println("${vertex.x}, ${vertex.y}") } println("------") - for (vertex in FieldConstants.Zones.closeLeftLane.vertices){ + for (vertex in FieldConstants.Zones.closeLeftLane.vertices) { println("${vertex.x}, ${vertex.y}") } }) diff --git a/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt b/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt index 9d970e7c..4bce8511 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt @@ -62,12 +62,21 @@ object ControlBoard { val scoreOuttake = Trigger { driver.xButton } val groundIntakeCube = Trigger { driver.rightShoulderButton } - val increaseRollerVoltage = Trigger { operator.dPadUp } - val decreaseRollerVoltage = Trigger { operator.dPadDown } +<<<<<<< Updated upstream +// val increaseRollerVoltage = Trigger { operator.dPadUp } +// val decreaseRollerVoltage = Trigger { operator.dPadDown } val groundIntakeCone = Trigger { driver.yButton } - val autoScore = Trigger { driver.rightTriggerAxis > 0.5 } - val dpadDown = Trigger { driver.dPadDown } + val autoScore = Trigger { driver.dPadDown} +// val dpadDown = Trigger { driver.dPadDown } +======= + // val increaseRollerVoltage = Trigger { operator.dPadUp } + // val decreaseRollerVoltage = Trigger { operator.dPadDown } + + val groundIntakeCone = Trigger { driver.yButton } + val autoScore = Trigger { driver.dPadDown } + // val dpadDown = Trigger { driver.dPadDown } +>>>>>>> Stashed changes val ejectGamePiece = Trigger { operator.dPadRight } 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 05dddd66..eb395dbc 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/FieldConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/FieldConstants.kt @@ -435,17 +435,41 @@ object FieldConstants { } object Zones { - val closeCommunity: Zone2d = + val closeLeftCommunity: Zone2d = Zone2d( // done listOf( - Translation2d(Grids.outerX, Community.rightY), - Translation2d(Community.chargingStationInnerX, Community.rightY), + Translation2d(Grids.outerX, Community.chargingStationLeftY), + Translation2d(Community.chargingStationInnerX, Community.chargingStationLeftY), Translation2d(Community.chargingStationInnerX, Community.leftY), Translation2d(Grids.outerX, Community.leftY) ) ) - val farCommunity: Zone2d = AllianceFlipUtil.apply(closeCommunity, force=true) + 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 @@ -459,7 +483,7 @@ object FieldConstants { ) ) - val farLoadingZoneLane: Zone2d = AllianceFlipUtil.apply(closeLoadingZoneLane, force=true) + val farLoadingZoneLane: Zone2d = AllianceFlipUtil.apply(closeLoadingZoneLane, force = true) val closeLeftLane: Zone2d = Zone2d( // done @@ -471,7 +495,7 @@ object FieldConstants { ) ) - val farLeftLane: Zone2d = AllianceFlipUtil.apply(closeLeftLane, force=true) + val farLeftLane: Zone2d = AllianceFlipUtil.apply(closeLeftLane, force = true) val closeRightLane: Zone2d = Zone2d( // done @@ -483,7 +507,7 @@ object FieldConstants { ) ) - val farRightLane: Zone2d = AllianceFlipUtil.apply(closeRightLane, force=true) + val farRightLane: Zone2d = AllianceFlipUtil.apply(closeRightLane, force = true) val closeCenterLeftLane: Zone2d = Zone2d( // done @@ -500,7 +524,7 @@ object FieldConstants { ) ) - val farCenterLeftLane: Zone2d = AllianceFlipUtil.apply(closeCenterLeftLane, force=true) + val farCenterLeftLane: Zone2d = AllianceFlipUtil.apply(closeCenterLeftLane, force = true) val closeCenterRightLane: Zone2d = Zone2d( // done @@ -517,19 +541,23 @@ object FieldConstants { ) ) - val farCenterRightLane: Zone2d = AllianceFlipUtil.apply(closeCenterRightLane, force=true) + 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 closeLoadingZone: Zone2d = AllianceFlipUtil.apply(farLoadingZone, force = true) val allZones = listOf( - closeCommunity, - farCommunity, + closeLeftCommunity, + farLeftCommunity, + closeCenterCommunity, + farCenterCommunity, + closeRightCommunity, + farRightCommunity, closeLoadingZone, farLoadingZone, closeLoadingZoneLane, diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/WaypointConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/WaypointConstants.kt index 60b492e1..0f0605b8 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/WaypointConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/WaypointConstants.kt @@ -3,67 +3,94 @@ 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.Rotation3d 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.inDegrees import org.team4099.lib.units.derived.inRadians object WaypointConstants { - enum class SubstationPoints(val waypoint : Waypoint) { - CloseCommunity(Waypoint(Translation2d(2.3.meters, 4.69.meters).translation2d)), - CloseLeftLane(Waypoint(Translation2d(8.12.meters, 5.19.meters).translation2d)), - CloseLoadingZoneLane(Waypoint(Translation2d(8.12.meters, 6.75.meters).translation2d)), - CloseCenterLeftLane(Waypoint(Translation2d(8.12.meters, 3.8.meters).translation2d)), - CloseCenterRightLane(Waypoint(Translation2d(8.12.meters, 3.8.meters).translation2d)), - CloseRightLane(Waypoint(Translation2d(8.meters, 1.45.meters).translation2d)), - FarRightLane(Waypoint(Translation2d(10.75.meters, 1.45.meters).translation2d)), + 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( - Waypoint( // excess - Translation2d(10.9.meters, 3.81.meters).translation2d + listOf( + Waypoint( // excess + Translation2d(10.9.meters, 3.81.meters).translation2d + ) ) ), FarCenterRightLane( - Waypoint( // excess - Translation2d(10.9.meters, 3.81.meters).translation2d + listOf( + Waypoint( // excess + Translation2d(10.9.meters, 3.81.meters).translation2d + ) ) ), - FarLeftLane(Waypoint(Translation2d(12.2.meters, 5.2.meters).translation2d)), - FarLoadingZoneLane(Waypoint(Translation2d(12.66.meters, 6.64.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( - Waypoint( - Translation2d(14.25.meters, 6.69.meters).translation2d, - null, - Rotation2d(90.degrees.inRadians) + listOf( + Waypoint( + Translation2d(14.25.meters, 6.69.meters).translation2d, + null, + Rotation2d(90.degrees.inRadians) + ) ) ) } object SubstationPaths { - var loadingZone = listOf(SubstationPoints.FarLoadingZone.waypoint) - - var farLoadingZoneLane = - listOf(SubstationPoints.FarLoadingZoneLane.waypoint) + loadingZone - var farRightLane = listOf(SubstationPoints.FarRightLane.waypoint) + farLoadingZoneLane - var excessFarLanes = listOf(SubstationPoints.FarLeftLane.waypoint) + farLoadingZoneLane + var loadingZone = SubstationPoints.FarLoadingZone.waypoints - var closeLoadingZoneLane = - listOf(SubstationPoints.CloseLoadingZoneLane.waypoint) + loadingZone - var closeLeftLane = - listOf(SubstationPoints.CloseLeftLane.waypoint) + loadingZone - var closeCenterLeftLane = - listOf(SubstationPoints.CloseCenterLeftLane.waypoint) + farLoadingZoneLane - var closeCenterRightLane = - listOf(SubstationPoints.CloseCenterRightLane.waypoint) + closeCenterLeftLane - var closeRightLane = - listOf(SubstationPoints.CloseRightLane.waypoint) + closeCenterLeftLane + var farLoadingZoneLane = SubstationPoints.FarLoadingZoneLane.waypoints + loadingZone + var farRightLane = + SubstationPoints.FarRightLane.waypoints + + SubstationPoints.FarLeftLane.waypoints + + farLoadingZoneLane + var excessFarLanes = SubstationPoints.FarLeftLane.waypoints + farLoadingZoneLane - var closeCommunity = listOf(SubstationPoints.CloseCommunity.waypoint) + closeLeftLane + 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.closeCommunity -> closeCommunity + FieldConstants.Zones.closeLeftCommunity -> closeLeftCommunity + FieldConstants.Zones.closeCenterCommunity -> closeCenterCommunity + FieldConstants.Zones.closeRightCommunity -> closeRightCommunity FieldConstants.Zones.closeLeftLane -> closeLeftLane FieldConstants.Zones.closeCenterLeftLane -> closeCenterLeftLane FieldConstants.Zones.closeCenterRightLane -> closeCenterRightLane 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 b0b27ba6..93d1e541 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/vision/Vision.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/vision/Vision.kt @@ -289,7 +289,6 @@ class Vision(vararg cameras: CameraIO) : SubsystemBase() { Logger.getInstance() .recordOutput("Vision/allTagPoses", *allTagPoses.map { it.pose3d }.toTypedArray()) - visionConsumer.accept(visionUpdates) Logger.getInstance() From af80b6d61c41984fc41a269c0920617a96a00bf7 Mon Sep 17 00:00:00 2001 From: Shom770 <82843611+Shom770@users.noreply.github.com> Date: Thu, 20 Jul 2023 01:43:27 -0400 Subject: [PATCH 3/5] Fix control board having the wrong controls --- .../team4099/robot2023/config/ControlBoard.kt | 16 ++++------------ 1 file changed, 4 insertions(+), 12 deletions(-) diff --git a/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt b/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt index 4bce8511..bc69b056 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt @@ -62,21 +62,13 @@ object ControlBoard { val scoreOuttake = Trigger { driver.xButton } val groundIntakeCube = Trigger { driver.rightShoulderButton } -<<<<<<< Updated upstream -// val increaseRollerVoltage = Trigger { operator.dPadUp } -// val decreaseRollerVoltage = Trigger { operator.dPadDown } + val increaseRollerVoltage = Trigger { operator.dPadUp } + val decreaseRollerVoltage = Trigger { operator.dPadDown } val groundIntakeCone = Trigger { driver.yButton } - val autoScore = Trigger { driver.dPadDown} -// val dpadDown = Trigger { driver.dPadDown } -======= - // val increaseRollerVoltage = Trigger { operator.dPadUp } - // val decreaseRollerVoltage = Trigger { operator.dPadDown } + val autoScore = Trigger { driver.rightTriggerAxis > 0.5 } + val dpadDown = Trigger { driver.dPadDown } - val groundIntakeCone = Trigger { driver.yButton } - val autoScore = Trigger { driver.dPadDown } - // val dpadDown = Trigger { driver.dPadDown } ->>>>>>> Stashed changes val ejectGamePiece = Trigger { operator.dPadRight } From 4ef3ae3717fe08c270a68034feea4dc9861d51ad Mon Sep 17 00:00:00 2001 From: Shom770 <82843611+Shom770@users.noreply.github.com> Date: Sun, 23 Jul 2023 21:41:34 -0400 Subject: [PATCH 4/5] Auto intake with the functionality of double sub added via operator app -- note that it still needs to be changed since it always ground intakes a cone as of now. --- .../robot2023/commands/AutoIntakeCommand.kt | 97 +++++++++++++++++-- .../team4099/robot2023/config/ControlBoard.kt | 1 - .../robot2023/config/constants/Constants.kt | 2 +- 3 files changed, 90 insertions(+), 10 deletions(-) diff --git a/src/main/kotlin/com/team4099/robot2023/commands/AutoIntakeCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/AutoIntakeCommand.kt index 9d7c8ab5..9d6908b0 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/AutoIntakeCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/AutoIntakeCommand.kt @@ -1,8 +1,10 @@ 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 @@ -13,11 +15,15 @@ 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() { @@ -36,11 +42,11 @@ class AutoIntakeCommand(val drivetrain: Drivetrain, val superstructure: Superstr 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( @@ -56,13 +62,64 @@ class AutoIntakeCommand(val drivetrain: Drivetrain, val superstructure: Superstr ) } - 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}") - } + // 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( @@ -86,4 +143,28 @@ class AutoIntakeCommand(val drivetrain: Drivetrain, val superstructure: Superstr superstructure.groundIntakeConeCommand() ) } + + 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/ControlBoard.kt b/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt index bc69b056..9d970e7c 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt @@ -69,7 +69,6 @@ object ControlBoard { val autoScore = Trigger { driver.rightTriggerAxis > 0.5 } val dpadDown = Trigger { driver.dPadDown } - val ejectGamePiece = Trigger { operator.dPadRight } // val armCharacterization = Trigger { operator.yButton } 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 From fdaf58c63f99609e1b6c05badcc3a6ae9457cd4d Mon Sep 17 00:00:00 2001 From: Shom770 <82843611+Shom770@users.noreply.github.com> Date: Sun, 23 Jul 2023 23:12:49 -0400 Subject: [PATCH 5/5] added functionality where u can choose in oeprator app where to intake from --- .../robot2023/commands/AutoIntakeCommand.kt | 2 +- .../superstructure/Superstructure.kt | 22 +++++++++++++++++++ 2 files changed, 23 insertions(+), 1 deletion(-) diff --git a/src/main/kotlin/com/team4099/robot2023/commands/AutoIntakeCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/AutoIntakeCommand.kt index 9d6908b0..a2cb89d1 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/AutoIntakeCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/AutoIntakeCommand.kt @@ -140,7 +140,7 @@ class AutoIntakeCommand(val drivetrain: Drivetrain, val superstructure: Superstr keepTrapping = true, flipForAlliances = false ), - superstructure.groundIntakeConeCommand() + superstructure.intakeFromSubstationCommand() ) } 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 0f560a99..1991bb71 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt @@ -1180,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) {