Auto aim #36
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
|
|
@@ -5,9 +5,9 @@ | |
| "visible": false | ||
| } | ||
| }, | ||
| "RoboRIO": { | ||
| "window": { | ||
| "visible": true | ||
| } | ||
| } | ||
| }, | ||
|
|
||
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,86 @@ | ||
| package com.team4099.lib.logging | ||
|
|
||
| import edu.wpi.first.math.kinematics.SwerveModulePosition | ||
| import edu.wpi.first.math.kinematics.SwerveModuleState | ||
| import org.team4099.lib.geometry.Pose2d | ||
| import org.team4099.lib.geometry.Pose3d | ||
| import org.team4099.lib.geometry.Quaternion | ||
| import org.team4099.lib.geometry.Rotation3d | ||
| import org.team4099.lib.geometry.Transform2d | ||
| import org.team4099.lib.geometry.Transform3d | ||
| import org.team4099.lib.geometry.Translation2d | ||
| import org.team4099.lib.geometry.Translation3d | ||
| import org.team4099.lib.geometry.Twist2d | ||
| import org.team4099.lib.geometry.Twist3d | ||
| import org.team4099.lib.units.base.inMeters | ||
| import org.team4099.lib.units.derived.inRadians | ||
|
|
||
| fun Pose2d.toDoubleArray(): DoubleArray { | ||
| return doubleArrayOf(this.x.inMeters, this.y.inMeters, this.rotation.inRadians) | ||
| } | ||
|
|
||
| fun Pose3d.toDoubleArray(): DoubleArray { | ||
| return doubleArrayOf( | ||
| this.x.inMeters, | ||
| this.y.inMeters, | ||
| this.z.inMeters, | ||
| this.rotation.rotation3d.quaternion.w, | ||
| this.rotation.rotation3d.quaternion.x, | ||
| this.rotation.rotation3d.quaternion.y, | ||
| this.rotation.rotation3d.quaternion.z | ||
| ) | ||
| } | ||
|
|
||
| fun Quaternion.toDoubleArray(): Array<Double> { | ||
| return arrayOf(this.w.inRadians, this.x.inMeters, this.y.inMeters, this.z.inMeters) | ||
| } | ||
|
|
||
| fun Rotation3d.toDoubleArray(): Array<Double> { | ||
| return arrayOf(this.x.inRadians, this.y.inRadians, this.z.inRadians) | ||
| } | ||
|
|
||
| fun Transform2d.toDoubleArray(): Array<Double> { | ||
| return arrayOf(this.translation.x.inMeters, this.translation.y.inMeters, this.rotation.inRadians) | ||
| } | ||
|
|
||
| fun Transform3d.toDoubleArray(): Array<Double> { | ||
| return arrayOf( | ||
| this.translation.x.inMeters, | ||
| this.translation.y.inMeters, | ||
| this.translation.z.inMeters, | ||
| this.rotation.x.inRadians, | ||
| this.rotation.y.inRadians, | ||
| this.rotation.z.inRadians | ||
| ) | ||
| } | ||
|
|
||
| fun Translation2d.toDoubleArray(): Array<Double> { | ||
| return arrayOf(this.x.inMeters, this.y.inMeters) | ||
| } | ||
|
|
||
| fun Translation3d.toDoubleArray(): Array<Double> { | ||
| return arrayOf(this.x.inMeters, this.y.inMeters, this.z.inMeters) | ||
| } | ||
|
|
||
| fun Twist2d.toDoubleArray(): Array<Double> { | ||
| return arrayOf(this.dx.inMeters, this.dy.inMeters, this.dtheta.inRadians) | ||
| } | ||
|
|
||
| fun Twist3d.toDoubleArray(): Array<Double> { | ||
| return arrayOf( | ||
| this.dx.inMeters, | ||
| this.dy.inMeters, | ||
| this.dz.inMeters, | ||
| this.rx.inRadians, | ||
| this.ry.inRadians, | ||
| this.rz.inRadians | ||
| ) | ||
| } | ||
|
|
||
| fun SwerveModulePosition.toDoubleArray(): Array<Double> { | ||
| return arrayOf(this.distanceMeters, this.angle.radians) | ||
| } | ||
|
|
||
| fun SwerveModuleState.toDoubleArray(): Array<Double> { | ||
| return arrayOf(this.speedMetersPerSecond, this.angle.radians) | ||
| } |
|
There was a problem hiding this comment. does not belong in this pr (how is this pushing isn't it in git ignore?) |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,123 @@ | ||
| package com.team4099.robot2023.commands.drivetrain | ||
|
|
||
| import com.team4099.lib.logging.LoggedTunableValue | ||
| import com.team4099.robot2023.config.constants.DrivetrainConstants | ||
| import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain | ||
| import com.team4099.robot2023.subsystems.superstructure.Request | ||
| import edu.wpi.first.wpilibj.RobotBase | ||
| import edu.wpi.first.wpilibj2.command.Command | ||
| import org.littletonrobotics.junction.Logger | ||
| import org.team4099.lib.controller.ProfiledPIDController | ||
| import org.team4099.lib.controller.TrapezoidProfile | ||
| import org.team4099.lib.geometry.Pose2d | ||
| import org.team4099.lib.units.Velocity | ||
| import org.team4099.lib.units.base.inMeters | ||
| 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.inDegrees | ||
| import org.team4099.lib.units.derived.inDegreesPerSecondPerDegree | ||
| import org.team4099.lib.units.derived.inDegreesPerSecondPerDegreePerSecond | ||
| import org.team4099.lib.units.derived.inDegreesPerSecondPerDegreeSeconds | ||
| import org.team4099.lib.units.derived.inRadians | ||
| 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.radians | ||
| import org.team4099.lib.units.inDegreesPerSecond | ||
| import org.team4099.lib.units.perSecond | ||
| import kotlin.math.PI | ||
| import kotlin.math.atan2 | ||
|
|
||
| class TargetPoseCommand(val drivetrain: Drivetrain, val targetPose: Pose2d) : Command() { | ||
| private val thetaPID: ProfiledPIDController<Radian, Velocity<Radian>> | ||
|
|
||
| val thetakP = | ||
| LoggedTunableValue( | ||
| "Pathfollow/thetakP", | ||
| Pair({ it.inDegreesPerSecondPerDegree }, { it.degrees.perSecond.perDegree }) | ||
| ) | ||
| val thetakI = | ||
| LoggedTunableValue( | ||
| "Pathfollow/thetakI", | ||
| Pair( | ||
| { it.inDegreesPerSecondPerDegreeSeconds }, { it.degrees.perSecond.perDegreeSeconds } | ||
| ) | ||
| ) | ||
| val thetakD = | ||
| LoggedTunableValue( | ||
| "Pathfollow/thetakD", | ||
| Pair( | ||
| { it.inDegreesPerSecondPerDegreePerSecond }, | ||
| { it.degrees.perSecond.perDegreePerSecond } | ||
| ) | ||
| ) | ||
|
|
||
| val thetaMaxVel = | ||
| LoggedTunableValue("Pathfollow/thetaMaxVel", DrivetrainConstants.PID.MAX_AUTO_ANGULAR_VEL) | ||
| val thetaMaxAccel = | ||
| LoggedTunableValue("Pathfollow/thetaMaxAccel", DrivetrainConstants.PID.MAX_AUTO_ANGULAR_ACCEL) | ||
| var desiredAngle: Angle = 0.0.degrees | ||
|
|
||
| init { | ||
| addRequirements(drivetrain) | ||
|
|
||
| Logger.recordOutput("Odometry/targetedPose", doubleArrayOf(targetPose.x.inMeters, targetPose.y.inMeters, targetPose.rotation.inRadians)) | ||
|
|
||
| if (RobotBase.isReal()) { | ||
| thetakP.initDefault(DrivetrainConstants.PID.AUTO_THETA_PID_KP) | ||
| thetakI.initDefault(DrivetrainConstants.PID.AUTO_THETA_PID_KI) | ||
| thetakD.initDefault(DrivetrainConstants.PID.AUTO_THETA_PID_KD) | ||
| } else { | ||
| thetakP.initDefault(DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KP) | ||
| thetakI.initDefault(DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KI) | ||
| thetakD.initDefault(DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KD) | ||
| } | ||
|
|
||
| thetaPID = | ||
| ProfiledPIDController( | ||
| thetakP.get(), | ||
| thetakI.get(), | ||
| thetakD.get(), | ||
| TrapezoidProfile.Constraints(thetaMaxVel.get(), thetaMaxAccel.get()) | ||
| ) | ||
| thetaPID.enableContinuousInput(-PI.radians, PI.radians) | ||
| } | ||
|
|
||
| override fun initialize() { | ||
| thetaPID.reset(drivetrain.odometryPose.rotation) | ||
| } | ||
|
|
||
| override fun execute() { | ||
| Logger.recordOutput("ActiveCommands/TargetPoseCommand", true) | ||
|
|
||
| val currentPose = drivetrain.odometryPose | ||
| val relativeToRobotPose = targetPose.relativeTo(currentPose) | ||
| desiredAngle = currentPose.rotation + atan2(relativeToRobotPose.y.inMeters, relativeToRobotPose.x.inMeters).radians | ||
|
|
||
| val thetaFeedback = thetaPID.calculate(currentPose.rotation, desiredAngle) | ||
|
|
||
| drivetrain.currentRequest = | ||
| Request.DrivetrainRequest.OpenLoop( | ||
| thetaFeedback, | ||
| Pair(drivetrain.fieldVelocity.x, drivetrain.fieldVelocity.y), | ||
| fieldOriented = true | ||
| ) | ||
|
|
||
| Logger.recordOutput("AutoLevel/CurrentYawDegrees", drivetrain.odometryPose.rotation.inDegrees) | ||
| Logger.recordOutput("AutoLevel/DesiredYawDegrees", desiredAngle.inDegrees) | ||
| Logger.recordOutput("AutoLevel/thetaFeedbackDPS", thetaFeedback.inDegreesPerSecond) | ||
| } | ||
|
|
||
| override fun isFinished(): Boolean { | ||
| return (drivetrain.odometryPose.rotation - desiredAngle).absoluteValue < | ||
| DrivetrainConstants.PID.AUTO_THETA_ALLOWED_ERROR | ||
| } | ||
|
|
||
| override fun end(interrupted: Boolean) { | ||
| drivetrain.currentRequest = | ||
| Request.DrivetrainRequest.OpenLoop( | ||
| 0.0.radians.perSecond, Pair(drivetrain.fieldVelocity.x, drivetrain.fieldVelocity.y) | ||
| ) | ||
| } | ||
| } |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.
Oops, something went wrong.
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
does not belong in pr