Skip to content

Commit

Permalink
initial sim shot tuning
Browse files Browse the repository at this point in the history
MatthewChoulas committed Feb 20, 2024

Verified

This commit was created on GitHub.com and signed with GitHub’s verified signature.
1 parent 851ecf8 commit 3db431b
Showing 11 changed files with 272 additions and 27 deletions.
10 changes: 5 additions & 5 deletions src/main/kotlin/com/team4099/robot2023/BuildConstants.kt
Original file line number Diff line number Diff line change
@@ -6,10 +6,10 @@ package com.team4099.robot2023
const val MAVEN_GROUP = ""
const val MAVEN_NAME = "Crescendo-2024"
const val VERSION = "unspecified"
const val GIT_REVISION = 259
const val GIT_SHA = "b6fcecccf23ea4432c8c5f69a63678444ca119f7"
const val GIT_DATE = "2024-02-15T21:20:33Z"
const val GIT_REVISION = 260
const val GIT_SHA = "851ecf84ee0e858110dc9ab517f2015eb3654d88"
const val GIT_DATE = "2024-02-19T15:19:59Z"
const val GIT_BRANCH = "AutoAim"
const val BUILD_DATE = "2024-02-19T14:17:46Z"
const val BUILD_UNIX_TIME = 1708370266917L
const val BUILD_DATE = "2024-02-20T01:25:57Z"
const val BUILD_UNIX_TIME = 1708410357413L
const val DIRTY = 1
26 changes: 26 additions & 0 deletions src/main/kotlin/com/team4099/robot2023/RobotContainer.kt
Original file line number Diff line number Diff line change
@@ -36,7 +36,14 @@ import org.team4099.lib.smoothDeadband
import org.team4099.lib.units.derived.Angle
import org.team4099.lib.units.derived.degrees
import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest
import FieldConstants
import com.team4099.robot2023.commands.drivetrain.TargetPoseCommand
import com.team4099.robot2023.subsystems.wrist.WristIO
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup
import org.team4099.lib.geometry.Pose2d
import org.team4099.lib.geometry.Pose3d
import org.team4099.lib.geometry.Rotation3d
import javax.swing.GroupLayout

object RobotContainer {
private val drivetrain: Drivetrain
@@ -148,6 +155,24 @@ object RobotContainer {

ControlBoard.resetGyro.whileTrue(ResetGyroYawCommand(drivetrain, toAngle = 180.degrees))

ControlBoard.intake.whileTrue(superstructure.groundIntakeCommand())
ControlBoard.ampPrep.whileTrue(superstructure.prepAmpCommand())
ControlBoard.scoreAmp.whileTrue(superstructure.scoreAmpCommand())
ControlBoard.autoAim.whileTrue(
ParallelCommandGroup(
TargetPoseCommand(
drivetrain,
Pose2d(
FieldConstants.Speaker.centerSpeakerOpening.x,
FieldConstants.Speaker.centerSpeakerOpening.y,
0.0.degrees
)
)
, superstructure.autoAimCommand())
)
ControlBoard.scoreSpeaker.whileTrue(superstructure.scoreSpeakerLowCommand())

/*
ControlBoard.runGroundIntake.whileTrue(superstructure.groundIntakeCommand())
ControlBoard.ejectGamePiece.whileTrue(superstructure.ejectGamePieceCommand())
ControlBoard.prepAmpScore.whileTrue(superstructure.prepAmpCommand())
@@ -158,6 +183,7 @@ object RobotContainer {
ControlBoard.climbExtend.whileTrue(superstructure.climbExtendCommand())
ControlBoard.climbRetract.whileTrue(superstructure.climbRetractCommand())
ControlBoard.requestIdle.whileTrue(superstructure.requestIdleCommand())
*/

/*
TUNING COMMANDS
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)
)
}
}
10 changes: 10 additions & 0 deletions src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt
Original file line number Diff line number Diff line change
@@ -34,6 +34,7 @@ object ControlBoard {

val resetGyro = Trigger { driver.startButton && driver.selectButton }

/*
val shooterUp = Trigger { driver.bButton }
val shooterDown = Trigger { driver.xButton }
val wristTestUp = Trigger { driver.yButton }
@@ -64,4 +65,13 @@ object ControlBoard {
val testWrist = Trigger { operator.aButton }
val testElevator = Trigger { operator.bButton }
val setTuningMode = Trigger { driver.rightShoulderButton }
*/

val intake = Trigger {driver.aButton}
val scoreAmp = Trigger {driver.bButton}
val scoreSpeaker = Trigger {driver.xButton}

val autoAim = Trigger {operator.aButton}
val ampPrep = Trigger {operator.bButton}
}
Original file line number Diff line number Diff line change
@@ -1,7 +1,10 @@
import org.team4099.lib.apriltag.AprilTagFieldLayout
import org.team4099.lib.geometry.Pose2d
import org.team4099.lib.geometry.Pose3d
import org.team4099.lib.geometry.Translation2d
import org.team4099.lib.geometry.Translation3d
import org.team4099.lib.units.base.inches
import org.team4099.lib.units.derived.degrees

// Copyright (c) 2024 FRC 6328
// http://github.com/Mechanical-Advantage
@@ -88,5 +91,6 @@ object FieldConstants {

/** Center of the speaker opening (blue alliance) */
var centerSpeakerOpening: Translation3d = (bottomLeftSpeaker + topRightSpeaker) / 2.0
var speakerTargetPose = Pose2d(centerSpeakerOpening.x, centerSpeakerOpening.y, 0.0.degrees)
}
}
Original file line number Diff line number Diff line change
@@ -1,5 +1,6 @@
package com.team4099.robot2023.config.constants

import org.team4099.lib.units.base.inches
import org.team4099.lib.units.base.meters
import org.team4099.lib.units.derived.degrees
import org.team4099.lib.units.derived.rotations
@@ -8,22 +9,24 @@ import org.team4099.lib.units.perMinute
object SuperstructureConstants {

val distanceFlywheelSpeedTable = listOf(
Pair(1.meters, 2000.rotations.perMinute),
Pair(2.meters, 2000.rotations.perMinute),
Pair(3.meters, 3000.rotations.perMinute),
Pair(4.meters, 3000.rotations.perMinute),
Pair(5.meters, 4000.rotations.perMinute),
Pair(6.meters, 4000.rotations.perMinute),
Pair(7.meters, 5000.rotations.perMinute)
Pair(1.meters, 3000.rotations.perMinute),
Pair(2.meters, 3500.rotations.perMinute),
Pair(3.meters, 4000.rotations.perMinute),
Pair(4.meters, 4500.rotations.perMinute),
Pair(5.meters, 5000.rotations.perMinute),
Pair(6.meters, 5500.rotations.perMinute),
Pair(7.meters, 6000.rotations.perMinute),
Pair(500.inches, 8000.rotations.perMinute)
)

val distanceWristAngleTable = listOf(
Pair(1.meters, -35.degrees),
Pair(2.meters, -30.degrees),
Pair(3.meters, -25.degrees),
Pair(4.meters, -20.degrees),
Pair(5.meters, -15.degrees),
Pair(6.meters, -10.degrees),
Pair(7.meters, -5.degrees)
Pair(46.5.inches, -35.degrees),
Pair(75.inches, -22.degrees),
Pair(117.inches, -11.5.degrees),
Pair(149.inches, -6.5.degrees),
Pair(191.inches, -2.degrees),
Pair(253.inches, 1.5.degrees),
Pair(321.inches, 4.degrees),
Pair(500.inches , 10.degrees)
)
}
Original file line number Diff line number Diff line change
@@ -1,13 +1,13 @@
package com.team4099.robot2023.subsystems.superstructure

import FieldConstants
import com.ctre.phoenix6.controls.VelocityDutyCycle
import com.team4099.lib.logging.LoggedTunableValue
import com.team4099.robot2023.config.constants.SuperstructureConstants
import com.team4099.robot2023.util.PoseEstimator
import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap
import edu.wpi.first.math.interpolation.InterpolatingTreeMap
import edu.wpi.first.math.interpolation.Interpolator
import edu.wpi.first.units.Angle
import java.util.function.Consumer
import org.littletonrobotics.junction.Logger
import org.team4099.lib.geometry.Pose2d
@@ -17,6 +17,7 @@ import org.team4099.lib.units.base.inInches
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.Radian
import org.team4099.lib.units.derived.degrees
import org.team4099.lib.units.derived.inDegrees
@@ -61,7 +62,7 @@ class AutoAim {

for (point in tunableWristInterpolationTable) {
if (point.first.hasChanged() || point.second.hasChanged()) {
updateFlywheelInterpolationTable()
updateWristInterpolationTable()
break
}
}
@@ -71,6 +72,20 @@ class AutoAim {
Logger.recordOutput("AutoAim/InterpolatedWristAngle", wristAngleDegreesInterpolationTable.get(interpolationTestDistance.get().inMeters))
}


fun calculateDistanceFromSpeaker(): Length {
val distance = (poseSupplier() - FieldConstants.Speaker.speakerTargetPose).translation.magnitude.meters
Logger.recordOutput("AutoAim/currentDistanceInhces", distance.inInches)
return distance
}
fun calculateFlywheelSpeed(): AngularVelocity {
return flywheelSpeedRPMInterpolationTable.get(calculateDistanceFromSpeaker().inMeters).rotations.perMinute
}

fun calculateWristAngle(): Angle {
return wristAngleDegreesInterpolationTable.get(calculateDistanceFromSpeaker().inMeters).degrees
}

fun updateFlywheelInterpolationTable() {
flywheelSpeedRPMInterpolationTable.clear()
tunableFlywheelInterpolationTable.forEach {
Original file line number Diff line number Diff line change
@@ -15,6 +15,8 @@ sealed interface Request {

class GroundIntake() : SuperstructureRequest

class AutoAim() : SuperstructureRequest

class EjectGamePiece() : SuperstructureRequest

class PrepScoreAmp() : SuperstructureRequest
Loading

0 comments on commit 3db431b

Please sign in to comment.