From 5ff3327823fa7e4f4890f611299bfd7329d75742 Mon Sep 17 00:00:00 2001 From: Matthew Choulas Date: Mon, 4 Dec 2023 06:34:26 -0500 Subject: [PATCH] finish auto align and intake --- .../com/team4099/robot2023/RobotContainer.kt | 4 +- .../commands/AutoAlignAndIntakeCommand.kt | 186 ++++++++++++++++++ .../robot2023/config/constants/Constants.kt | 11 +- .../config/constants/DrivetrainConstants.kt | 5 + .../subsystems/limelight/LimelightVision.kt | 169 ++++++++++------ 5 files changed, 308 insertions(+), 67 deletions(-) create mode 100644 src/main/kotlin/com/team4099/robot2023/commands/AutoAlignAndIntakeCommand.kt diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt index 0c8c4783..0e75eaf9 100644 --- a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt +++ b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt @@ -95,10 +95,10 @@ object RobotContainer { } vision.setDataInterfaces({ drivetrain.odometryPose }, { drivetrain.addVisionData(it) }) + limelight.setDataInterfaces({ drivetrain.odometryPose }, { drivetrain.addVisionData(it) }) drivetrain.elevatorHeightSupplier = Supplier { superstructure.elevatorInputs.elevatorPosition } drivetrain.objectiveSupplier = Supplier { superstructure.objective } - limelight.poseSupplier = { drivetrain.odometryPose } - limelight.nodeToLookFor = { superstructure.objective } + limelight.gamePieceToLookFor = { superstructure.objective } } fun mapDefaultCommands() { diff --git a/src/main/kotlin/com/team4099/robot2023/commands/AutoAlignAndIntakeCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/AutoAlignAndIntakeCommand.kt new file mode 100644 index 00000000..cd2e6d7b --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/commands/AutoAlignAndIntakeCommand.kt @@ -0,0 +1,186 @@ +package com.team4099.robot2023.commands + +import com.team4099.lib.logging.LoggedTunableValue +import com.team4099.robot2023.config.constants.Constants +import com.team4099.robot2023.config.constants.DrivetrainConstants +import com.team4099.robot2023.config.constants.GamePiece +import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain +import com.team4099.robot2023.subsystems.limelight.LimelightVision +import com.team4099.robot2023.subsystems.superstructure.Superstructure +import edu.wpi.first.math.kinematics.ChassisSpeeds +import edu.wpi.first.wpilibj2.command.Commands +import edu.wpi.first.wpilibj2.command.Commands.runOnce +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup +import org.littletonrobotics.junction.Logger +import org.team4099.lib.controller.PIDController +import org.team4099.lib.geometry.Pose2d +import org.team4099.lib.kinematics.ChassisAccels +import org.team4099.lib.units.Velocity +import org.team4099.lib.units.base.Meter +import org.team4099.lib.units.base.meters +import org.team4099.lib.units.derived.Angle +import org.team4099.lib.units.derived.Radian +import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.inDegreesPerSecondPerDegree +import org.team4099.lib.units.derived.inDegreesPerSecondPerDegreeSeconds +import org.team4099.lib.units.derived.inDegreesPerSecondPerDegreesPerSecond +import org.team4099.lib.units.derived.inMetersPerSecondPerMeter +import org.team4099.lib.units.derived.inMetersPerSecondPerMeterSecond +import org.team4099.lib.units.derived.inMetersPerSecondPerMetersPerSecond +import org.team4099.lib.units.derived.inRotation2ds +import org.team4099.lib.units.derived.metersPerSecondPerMetersPerSecond +import org.team4099.lib.units.derived.perDegree +import org.team4099.lib.units.derived.perDegreePerSecond +import org.team4099.lib.units.derived.perDegreeSeconds +import org.team4099.lib.units.derived.perMeter +import org.team4099.lib.units.derived.perMeterSeconds +import org.team4099.lib.units.derived.radians +import org.team4099.lib.units.inMetersPerSecond +import org.team4099.lib.units.inRadiansPerSecond +import org.team4099.lib.units.perSecond + +class AutoAlignAndIntakeCommand( + val drivetrain: Drivetrain, + val superstructure: Superstructure, + val limelight: LimelightVision, + val gamePiece: Constants.Universal.GamePiece +) : SequentialCommandGroup() { + lateinit var drivePose: Pose2d + var heading: Angle = 0.0.degrees + + private val alignPID: PIDController> + + private val xPID: PIDController> + private val yPID: PIDController> + + val alignkP = + LoggedTunableValue( + "AutoAlign/alignkP", + DrivetrainConstants.PID.AUTO_ALIGN_KP, + Pair({ it.inDegreesPerSecondPerDegree }, { it.degrees.perSecond.perDegree }) + ) + val alignkI = + LoggedTunableValue( + "AutoAlign/alignkI", + DrivetrainConstants.PID.AUTO_ALIGN_KI, + Pair( + { it.inDegreesPerSecondPerDegreeSeconds }, { it.degrees.perSecond.perDegreeSeconds } + ) + ) + val alignkD = + LoggedTunableValue( + "AutoAlign/alignkD", + DrivetrainConstants.PID.AUTO_ALIGN_KD, + Pair( + { it.inDegreesPerSecondPerDegreesPerSecond }, + { it.degrees.perSecond.perDegreePerSecond } + ) + ) + + val poskPX = + LoggedTunableValue( + "Pathfollow/poskPX", + DrivetrainConstants.PID.AUTO_POS_KPX, + Pair({ it.inMetersPerSecondPerMeter }, { it.meters.perSecond.perMeter }) + ) + val poskIX = + LoggedTunableValue( + "Pathfollow/poskIX", + DrivetrainConstants.PID.AUTO_POS_KIX, + Pair({ it.inMetersPerSecondPerMeterSecond }, { it.meters.perSecond.perMeterSeconds }) + ) + val poskDX = + LoggedTunableValue( + "Pathfollow/poskDX", + DrivetrainConstants.PID.AUTO_POS_KDX, + Pair( + { it.inMetersPerSecondPerMetersPerSecond }, { it.metersPerSecondPerMetersPerSecond } + ) + ) + + val poskPY = + LoggedTunableValue( + "Pathfollow/poskPY", + DrivetrainConstants.PID.AUTO_POS_KPY, + Pair({ it.inMetersPerSecondPerMeter }, { it.meters.perSecond.perMeter }) + ) + val poskIY = + LoggedTunableValue( + "Pathfollow/poskIY", + DrivetrainConstants.PID.AUTO_POS_KIY, + Pair({ it.inMetersPerSecondPerMeterSecond }, { it.meters.perSecond.perMeterSeconds }) + ) + val poskDY = + LoggedTunableValue( + "Pathfollow/poskDY", + DrivetrainConstants.PID.AUTO_POS_KDY, + Pair( + { it.inMetersPerSecondPerMetersPerSecond }, { it.metersPerSecondPerMetersPerSecond } + ) + ) + + init { + + alignPID = PIDController(alignkP.get(), alignkI.get(), alignkD.get()) + alignPID.errorTolerance = DrivetrainConstants.ALLOWED_STEERING_ANGLE_ERROR + + xPID = PIDController(poskPX.get(), poskIX.get(), poskDX.get()) + yPID = PIDController(poskPY.get(), poskIY.get(), poskDY.get()) + + val setupCommand = + runOnce({ + Logger.getInstance().recordOutput("Auto/isAutoDriving", true) + superstructure.objective.gamePiece = gamePiece + alignPID + }) + + val alignmentCommand = + Commands.run({ + val angle = + limelight.angleYawFromTarget(drivetrain.odometryPose, limelight.targetGamePiecePose) + val rotationFeedback = alignPID.calculate(angle, 0.radians) + + drivetrain.setClosedLoop( + ChassisSpeeds(0.0, 0.0, rotationFeedback.inRadiansPerSecond), + ChassisAccels( + 0.0.meters.perSecond.perSecond, + 0.0.meters.perSecond.perSecond, + 0.0.radians.perSecond.perSecond + ) + .chassisAccelsWPILIB + ) + if (alignPID.isAtSetpoint) { + this.end(false) + } + }) + + val intakeCommand = + Commands.run({ + val angle = + limelight.angleYawFromTarget(drivetrain.odometryPose, limelight.targetGamePiecePose) + + val rotationFeedback = alignPID.calculate(angle, 0.radians) + val xFeedback = xPID.calculate(drivetrain.odometryPose.x, limelight.targetGamePiecePose.x) + val yFeedback = yPID.calculate(drivetrain.odometryPose.y, limelight.targetGamePiecePose.y) + + drivetrain.setClosedLoop( + ChassisSpeeds.fromFieldRelativeSpeeds( + xFeedback.inMetersPerSecond, + yFeedback.inMetersPerSecond, + rotationFeedback.inRadiansPerSecond, + drivetrain.odometryPose.rotation.inRotation2ds + ), + ChassisAccels( + 0.0.meters.perSecond.perSecond, + 0.0.meters.perSecond.perSecond, + 0.0.radians.perSecond.perSecond + ) + .chassisAccelsWPILIB + ) + }) + + addCommands( + setupCommand, alignmentCommand, superstructure.groundIntakeConeCommand(), intakeCommand + ) + } +} 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 5e42743a..63b1efb8 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt @@ -27,7 +27,15 @@ object Constants { enum class GamePiece { CUBE, CONE, - NONE + NONE; + + inline fun toClassName(): String { + return when (this) { + CUBE -> "cube" + CONE -> "cone" + else -> "none" + } + } } enum class NodeTier { @@ -43,7 +51,6 @@ object Constants { SINGLE_SUBSTATION, NONE, } - } object AprilTagIds { diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt index 9d11e466..98cb55e7 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt @@ -145,6 +145,11 @@ object DrivetrainConstants { val AUTO_THETA_ALLOWED_ERROR = 3.degrees + val AUTO_ALIGN_KP = 0.0.degrees.perSecond / 1.degrees + val AUTO_ALIGN_KI = 0.0.degrees.perSecond / (1.degrees * 1.seconds) + val AUTO_ALIGN_KD = + (0.0.degrees.perSecond / (1.degrees / 1.seconds)).radiansPerSecondPerRadiansPerSecond + val AUTO_THETA_PID_KP = 0.8.degrees.perSecond / 1.degrees val AUTO_THETA_PID_KI = 0.0.degrees.perSecond / (1.degrees * 1.seconds) val AUTO_THETA_PID_KD = diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVision.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVision.kt index d548602b..1b5e9257 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVision.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVision.kt @@ -57,12 +57,21 @@ class LimelightVision(val io: LimelightVisionIO) : SubsystemBase() { private val xyStdDevCoefficient = TunableNumber("LimelightVision/xystdev", 0.05) private val thetaStdDev = TunableNumber("LimelightVision/thetaStdDev", 0.75) - init { - for (locationIndex in listOf(0, 1, 2, 3) ) { + val limelightState: LimelightStates = LimelightStates.AUTO_POSE_ESTIMATION + + var targetGamePiecePose = Pose3d() + + enum class LimelightStates { + AUTO_POSE_ESTIMATION, + TELEOP_GAME_PIECE_DETECTION + } + + init { + for (locationIndex in listOf(0, 1, 2, 3)) { conePoses.add( Pose3d( FieldConstants.StagingLocations.positionX, - FieldConstants.StagingLocations.translations[locationIndex].y, + FieldConstants.StagingLocations.translations[locationIndex]?.y ?: 0.meters, VisionConstants.Limelight.CONE_HEIGHT, Rotation3d() ) @@ -71,7 +80,7 @@ class LimelightVision(val io: LimelightVisionIO) : SubsystemBase() { cubePoses.add( Pose3d( FieldConstants.StagingLocations.positionX, - FieldConstants.StagingLocations.translations[locationIndex].y, + FieldConstants.StagingLocations.translations[locationIndex]?.y ?: 0.meters, VisionConstants.Limelight.CUBE_HEIGHT, Rotation3d() ) @@ -89,89 +98,120 @@ class LimelightVision(val io: LimelightVisionIO) : SubsystemBase() { val visibleGamePieces = mutableListOf() + val timestampedVisionUpdates = mutableListOf() + // mathematically figure out which game pieces we're looking at based on limelight neural // network detector // process transform outputs from LL and get the target corners - // calculating true game piece position - val gamePieceX = FieldConstants.StagingLocations.positionX + if (limelightState == LimelightStates.AUTO_POSE_ESTIMATION) { + // calculating true game piece position + val gamePieceX = FieldConstants.StagingLocations.positionX - val gamePieceLocationIndex = if (FMSData.isBlue) gamePieceToLookFor.invoke().autoStagingLocation - else 3 - gamePieceToLookFor.invoke().autoStagingLocation + val gamePieceLocationIndex = + if (FMSData.isBlue) gamePieceToLookFor.invoke().autoStagingLocation + else 3 - gamePieceToLookFor.invoke().autoStagingLocation - val gamePieceY = FieldConstants.StagingLocations.firstY + FieldConstants.StagingLocations.separationY * gamePieceLocationIndex + val gamePieceY = + FieldConstants.StagingLocations.firstY + + FieldConstants.StagingLocations.separationY * gamePieceLocationIndex - val gamePieceZ = - when (gamePieceToLookFor.invoke().gamePiece) { - Constants.Universal.GamePiece.CONE -> VisionConstants.Limelight.CONE_HEIGHT / 2 - Constants.Universal.GamePiece.CUBE -> VisionConstants.Limelight.CUBE_HEIGHT / 2 - else -> -1337.meters - } + val gamePieceZ = + when (gamePieceToLookFor.invoke().gamePiece) { + Constants.Universal.GamePiece.CONE -> VisionConstants.Limelight.CONE_HEIGHT / 2 + Constants.Universal.GamePiece.CUBE -> VisionConstants.Limelight.CUBE_HEIGHT / 2 + else -> -1337.meters + } - val gamePieceRotation = Rotation3d(0.0.radians, 0.0.radians, if (FMSData.isBlue) 0.0.degrees else 180.degrees) + val gamePieceRotation = + Rotation3d(0.0.radians, 0.0.radians, if (FMSData.isBlue) 0.0.degrees else 180.degrees) - val gamePiecePose = Pose3d(gamePieceX, gamePieceY, gamePieceZ, gamePieceRotation) + val gamePiecePose = Pose3d(gamePieceX, gamePieceY, gamePieceZ, gamePieceRotation) - val robotPoses = mutableListOf() + val robotPoses = mutableListOf() - if (inputs.validReading) { + if (inputs.validReading) { - for (target in inputs.gamePieceTargets) { - visibleGamePieces.add( - solveTargetPoseFromAngle( - currentPose, - target, - ( - if (target.className == "cone") VisionConstants.Limelight.CONE_HEIGHT / 2 - else VisionConstants.Limelight.CUBE_HEIGHT / 2 - ) + for (target in inputs.gamePieceTargets) { + visibleGamePieces.add( + solveTargetPoseFromAngle( + currentPose, + target, + ( + if (target.className == "cone") VisionConstants.Limelight.CONE_HEIGHT / 2 + else VisionConstants.Limelight.CUBE_HEIGHT / 2 + ) + ) ) - ) + } + // this is adding where we think they are,, not where they actually are } - // this is adding where we think they are,, not where they actually are - } - val timestampedVisionUpdates = mutableListOf() + val trueGamePieces = mutableListOf() - val trueGamePieces = mutableListOf() + for ((index, gamePiecePose) in visibleGamePieces.withIndex()) { + val searchList = + if (inputs.gamePieceTargets[index].className == "cone") conePoses else cubePoses + val closestPose = gamePiecePose.findClosestPose(*searchList.toTypedArray()) - for ((index, gamePiecePose) in visibleGamePieces.withIndex()) { - val searchList = if (inputs.gamePieceTargets[index].className == "cone") conePoses else cubePoses - val closestPose = gamePiecePose.findClosestPose(*searchList.toTypedArray()) + trueGamePieces.add(closestPose) - trueGamePieces.add(closestPose) + // find inverse translation from the detected pose to robot + val targetToCamera = + gamePiecePose + .relativeTo( + currentPose.toPose3d().transformBy(VisionConstants.Limelight.LL_TRANSFORM) + ) + .toTransform3d() + .inverse() - // find inverse translation from the detected pose to robot - val targetToCamera = - gamePiecePose - .relativeTo( - currentPose.toPose3d().transformBy(VisionConstants.Limelight.LL_TRANSFORM) - ) - .toTransform3d() - .inverse() + val trueNodePoseToRobot = closestPose.transformBy(targetToCamera) - val trueNodePoseToRobot = closestPose.transformBy(targetToCamera) + // Add to vision updates + val xyStdDev = xyStdDevCoefficient.get() * targetToCamera.translation.norm.inMeters.pow(2) + val thetaStdDev = thetaStdDev.get() * targetToCamera.translation.norm.inMeters.pow(2) - // Add to vision updates - val xyStdDev = xyStdDevCoefficient.get() * targetToCamera.translation.norm.inMeters.pow(2) - val thetaStdDev = thetaStdDev.get() * targetToCamera.translation.norm.inMeters.pow(2) + robotPoses.add(trueNodePoseToRobot.toPose2d()) - robotPoses.add(trueNodePoseToRobot.toPose2d()) + timestampedVisionUpdates.add( + PoseEstimator.TimestampedVisionUpdate( + inputs.timestamp, + trueNodePoseToRobot.toPose2d(), + VecBuilder.fill(xyStdDev, xyStdDev, thetaStdDev) + ) + ) + } - timestampedVisionUpdates.add( - PoseEstimator.TimestampedVisionUpdate( - inputs.timestamp, - trueNodePoseToRobot.toPose2d(), - VecBuilder.fill(xyStdDev, xyStdDev, thetaStdDev) + Logger.getInstance() + .recordOutput( + "LimelightVision/estimatedRobotPoses", *robotPoses.map { it.pose2d }.toTypedArray() ) - ) - } - Logger.getInstance() - .recordOutput( - "LimelightVision/estimatedRobotPoses", *robotPoses.map { it.pose2d }.toTypedArray() - ) + visionConsumer.accept(timestampedVisionUpdates) + } else if (limelightState == LimelightStates.TELEOP_GAME_PIECE_DETECTION) { + if (inputs.validReading) { + for (target in inputs.gamePieceTargets) { + visibleGamePieces.add( + solveTargetPoseFromAngle( + currentPose, + target, + ( + if (target.className == "cone") VisionConstants.Limelight.CONE_HEIGHT / 2 + else VisionConstants.Limelight.CUBE_HEIGHT / 2 + ) + ) + ) + } + + val searchList = + visibleGamePieces.filterIndexed({ index, pose -> + inputs.gamePieceTargets[index].className == + gamePieceToLookFor.invoke().gamePiece.toClassName() + }) + targetGamePiecePose = currentPose.toPose3d().findClosestPose(*searchList.toTypedArray()) + } + } Logger.getInstance() .recordOutput( @@ -190,8 +230,6 @@ class LimelightVision(val io: LimelightVisionIO) : SubsystemBase() { "LoggedRobot/Subsystems/LimelightLoopTimeMS", (Clock.realTimestamp - startTime).inMilliseconds ) - - // visionConsumer.accept(timestampedVisionUpdates) } fun solveTargetPoseFromAngle( @@ -259,6 +297,11 @@ class LimelightVision(val io: LimelightVisionIO) : SubsystemBase() { return targetToCameraHeight / tan(angleToGoal) } + fun angleYawFromTarget(currentPose: Pose2d, targetPose: Pose3d): Angle { + val robotToTarget = targetPose.toPose2d().relativeTo(currentPose) + return Math.asin((robotToTarget.y.inMeters / robotToTarget.translation.magnitude)).radians + } + // based off of angles fun solveTargetPositionFromAngularOutput( tx: Angle,