From f3625c09b677fbfe372e3ab6d51995db3c41b8ca Mon Sep 17 00:00:00 2001 From: Saraansh Wadkar Date: Fri, 15 Dec 2023 21:12:59 -0500 Subject: [PATCH] auto work --- .../CustomHolonomicDriveController.kt | 3 + .../auto/mode/LimelightTestingAuto.kt | 53 +++-- .../commands/AutoAlignAndIntakeCommand.kt | 13 +- .../commands/drivetrain/DrivePathCommand.kt | 216 ++++++++++++++++-- .../commands/drivetrain/GoToAngle.kt | 6 +- .../robot2023/config/constants/Constants.kt | 2 +- .../config/constants/DrivetrainConstants.kt | 91 ++++++-- .../swervemodule/SwerveModuleIOFalcon.kt | 2 + .../subsystems/limelight/LimelightVision.kt | 27 ++- 9 files changed, 352 insertions(+), 61 deletions(-) diff --git a/src/main/kotlin/com/team4099/lib/trajectory/CustomHolonomicDriveController.kt b/src/main/kotlin/com/team4099/lib/trajectory/CustomHolonomicDriveController.kt index 2fb749d5..c6bdc7fd 100644 --- a/src/main/kotlin/com/team4099/lib/trajectory/CustomHolonomicDriveController.kt +++ b/src/main/kotlin/com/team4099/lib/trajectory/CustomHolonomicDriveController.kt @@ -5,6 +5,7 @@ import edu.wpi.first.math.geometry.Pose2d import edu.wpi.first.math.geometry.Rotation2d import edu.wpi.first.math.kinematics.ChassisSpeeds import edu.wpi.first.math.trajectory.Trajectory +import org.littletonrobotics.junction.Logger /** * This holonomic drive controller can be used to follow trajectories using a holonomic drivetrain @@ -95,6 +96,8 @@ class CustomHolonomicDriveController( val xFeedback = m_xController.calculate(currentPose.x, poseRef.x) val yFeedback = m_yController.calculate(currentPose.y, poseRef.y) val thetaFeedback = m_thetaController.calculate(currentPose.rotation.radians, angleRef.radians) + Logger.getInstance().recordOutput("Pathfollow/thetaFeedbackRadians", thetaFeedback) + Logger.getInstance().recordOutput("Pathfollow/appliedThetaFeedback", angleVelocityRefRadians + thetaFeedback) // Return next output. return ChassisSpeeds.fromFieldRelativeSpeeds( diff --git a/src/main/kotlin/com/team4099/robot2023/auto/mode/LimelightTestingAuto.kt b/src/main/kotlin/com/team4099/robot2023/auto/mode/LimelightTestingAuto.kt index a7b52005..d6aa8cf5 100644 --- a/src/main/kotlin/com/team4099/robot2023/auto/mode/LimelightTestingAuto.kt +++ b/src/main/kotlin/com/team4099/robot2023/auto/mode/LimelightTestingAuto.kt @@ -42,17 +42,27 @@ class LimelightTestingAuto(val drivetrain: Drivetrain, val superstructure: Super null, 0.0.degrees.inRotation2ds ), + Waypoint( + Translation2d( + FieldConstants.StagingLocations.translations[3]!!.x-intakeOffset.get() - 1.meters, + FieldConstants.StagingLocations.translations[3]!!.y-intakeOffset.get() + ) + .translation2d, + null, + 0.0.degrees.inRotation2ds + ), Waypoint( Translation2d( FieldConstants.StagingLocations.translations[3]!!.x-intakeOffset.get(), FieldConstants.StagingLocations.translations[3]!!.y-intakeOffset.get() ) .translation2d, - 0.0.degrees.inRotation2ds, + null, 0.0.degrees.inRotation2ds ), ) - } + }, + isAuto = true ), WaitCommand(1.5).andThen(superstructure.groundIntakeConeCommand()) ), @@ -68,7 +78,7 @@ class LimelightTestingAuto(val drivetrain: Drivetrain, val superstructure: Super FieldConstants.StagingLocations.translations[3]!!.y- intakeOffset.get() ) .translation2d, - 0.0.degrees.inRotation2ds, + null, 0.0.degrees.inRotation2ds ), Waypoint( @@ -80,7 +90,7 @@ class LimelightTestingAuto(val drivetrain: Drivetrain, val superstructure: Super Translation2d( endingPosX.get(), FieldConstants.Grids.nodeFirstY + - FieldConstants.Grids.nodeSeparationY * 7 + FieldConstants.Grids.nodeSeparationY * 5 ) .translation2d, null, @@ -88,40 +98,56 @@ class LimelightTestingAuto(val drivetrain: Drivetrain, val superstructure: Super ) ) }, - keepTrapping = true + keepTrapping = true, + isAuto = true ) ), superstructure.prepScoreCommand( Constants.Universal.GamePiece.CUBE, Constants.Universal.NodeTier.HIGH ), superstructure.score(), - WaitCommand(0.5), + WaitCommand(2.0), ParallelCommandGroup( DrivePathCommand( drivetrain, { listOf( Waypoint( - Translation2d(1.9.meters, 3.31.meters).translation2d, + Translation2d( + endingPosX.get(), + FieldConstants.Grids.nodeFirstY + + FieldConstants.Grids.nodeSeparationY * 5 + ) + .translation2d, null, - 0.0.degrees.inRotation2ds + 180.0.degrees.inRotation2ds ), Waypoint( Translation2d(2.9.meters, 4.8.meters).translation2d, null, 0.0.degrees.inRotation2ds ), + Waypoint( + Translation2d( + FieldConstants.StagingLocations.translations[2]!!.x-intakeOffset.get() - 1.meters, + FieldConstants.StagingLocations.translations[2]!!.y-intakeOffset.get() + ) + .translation2d, + null, + 0.0.degrees.inRotation2ds + ), Waypoint( Translation2d( FieldConstants.StagingLocations.translations[2]!!.x-intakeOffset.get(), FieldConstants.StagingLocations.translations[2]!!.y-intakeOffset.get() ) .translation2d, - 0.0.degrees.inRotation2ds, + null, 0.0.degrees.inRotation2ds ), ) - } + }, + isAuto = true ), WaitCommand(1.5).andThen(superstructure.groundIntakeConeCommand()) ), @@ -137,7 +163,7 @@ class LimelightTestingAuto(val drivetrain: Drivetrain, val superstructure: Super FieldConstants.StagingLocations.translations[2]!!.y- intakeOffset.get() ) .translation2d, - 0.0.degrees.inRotation2ds, + null, 0.0.degrees.inRotation2ds ), Waypoint( @@ -149,7 +175,7 @@ class LimelightTestingAuto(val drivetrain: Drivetrain, val superstructure: Super Translation2d( endingPosX.get(), FieldConstants.Grids.nodeFirstY + - FieldConstants.Grids.nodeSeparationY * 7 + FieldConstants.Grids.nodeSeparationY * 5 ) .translation2d, null, @@ -157,7 +183,8 @@ class LimelightTestingAuto(val drivetrain: Drivetrain, val superstructure: Super ) ) }, - keepTrapping = true + keepTrapping = true, + isAuto = true ) ) ) diff --git a/src/main/kotlin/com/team4099/robot2023/commands/AutoAlignAndIntakeCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/AutoAlignAndIntakeCommand.kt index 24750e17..43c98d2c 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/AutoAlignAndIntakeCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/AutoAlignAndIntakeCommand.kt @@ -9,15 +9,12 @@ 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.Command import edu.wpi.first.wpilibj2.command.Commands import edu.wpi.first.wpilibj2.command.Commands.runOnce import edu.wpi.first.wpilibj2.command.SequentialCommandGroup -import edu.wpi.first.wpilibj2.command.WaitCommand import org.littletonrobotics.junction.Logger import org.team4099.lib.controller.PIDController import org.team4099.lib.geometry.Pose2d -import org.team4099.lib.geometry.Pose3d import org.team4099.lib.geometry.Translation2d import org.team4099.lib.kinematics.ChassisAccels import org.team4099.lib.units.Velocity @@ -74,13 +71,13 @@ class AutoAlignAndIntakeCommand( val thetakP = LoggedTunableValue( "AutoAlign/alignkP", - DrivetrainConstants.PID.AUTO_THETA_PID_KP, + DrivetrainConstants.PID.TELEOP_THETA_PID_KP, Pair({ it.inDegreesPerSecondPerDegree }, { it.degrees.perSecond.perDegree }) ) val thetakI = LoggedTunableValue( "AutoAlign/alignkI", - DrivetrainConstants.PID.AUTO_THETA_PID_KI, + DrivetrainConstants.PID.TELEOP_THETA_PID_KI, Pair( { it.inDegreesPerSecondPerDegreeSeconds }, { it.degrees.perSecond.perDegreeSeconds } ) @@ -88,7 +85,7 @@ class AutoAlignAndIntakeCommand( val thetakD = LoggedTunableValue( "AutoAlign/alignkD", - DrivetrainConstants.PID.AUTO_THETA_PID_KD, + DrivetrainConstants.PID.TELEOP_THETA_PID_KD, Pair( { it.inDegreesPerSecondPerDegreesPerSecond }, { it.degrees.perSecond.perDegreePerSecond } @@ -147,7 +144,9 @@ class AutoAlignAndIntakeCommand( Logger.getInstance().recordOutput("AutoAlign/TargetPose", limelight.targetGamePiecePose?.pose3d) Logger.getInstance().recordOutput("AutoAlign/Tx", limelight.targetGamePieceTx?.inDegrees ?: 0.0) - val xFeedback = xPID.calculate(drivetrain.odometryPose.purelyTranslateBy(intakeOffsetTranslation).x, limelight.targetGamePiecePose?.x ?: 0.0.meters) + val error = limelight.targetGamePiecePose?.toPose2d()?.relativeTo(drivetrain.odometryPose.purelyTranslateBy(intakeOffsetTranslation)) + + val xFeedback = xPID.calculate(error?.translation?.magnitude?.meters ?: 0.0.meters, 0.0.meters) val yFeedback = yPID.calculate(drivetrain.odometryPose.purelyTranslateBy(intakeOffsetTranslation).y, limelight.targetGamePiecePose?.y ?: 0.0.meters) val thetaFeedback = thetaPID.calculate(limelight.targetGamePieceTx ?: 0.0.degrees, 0.0.degrees) diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt index 07d110eb..9cba8ff3 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt @@ -50,7 +50,6 @@ import org.team4099.lib.units.inDegreesPerSecond import org.team4099.lib.units.inMetersPerSecond import org.team4099.lib.units.inMetersPerSecondPerSecond import org.team4099.lib.units.inRadiansPerSecond -import org.team4099.lib.units.inRadiansPerSecondPerSecond import org.team4099.lib.units.perSecond import java.util.function.Supplier import kotlin.math.PI @@ -65,7 +64,8 @@ class DrivePathCommand( val leaveOutYAdjustment: Boolean = false, val endVelocity: Velocity2d = Velocity2d(), val tolerance: Pose2d = Pose2d(1.inches, 1.inches, 1.degrees), - val forceRobotVelocityCheck: Boolean = false + val forceRobotVelocityCheck: Boolean = false, + val isAuto: Boolean = false ) : CommandBase() { private val xPID: PIDController> private val yPID: PIDController> @@ -82,13 +82,13 @@ class DrivePathCommand( val thetakP = LoggedTunableValue( "Pathfollow/thetakP", - DrivetrainConstants.PID.AUTO_THETA_PID_KP, + DrivetrainConstants.PID.TELEOP_THETA_PID_KP, Pair({ it.inDegreesPerSecondPerDegree }, { it.degrees.perSecond.perDegree }) ) val thetakI = LoggedTunableValue( "Pathfollow/thetakI", - DrivetrainConstants.PID.AUTO_THETA_PID_KI, + DrivetrainConstants.PID.TELEOP_THETA_PID_KI, Pair( { it.inDegreesPerSecondPerDegreeSeconds }, { it.degrees.perSecond.perDegreeSeconds } ) @@ -96,7 +96,7 @@ class DrivePathCommand( val thetakD = LoggedTunableValue( "Pathfollow/thetakD", - DrivetrainConstants.PID.AUTO_THETA_PID_KD, + DrivetrainConstants.PID.TELEOP_THETA_PID_KD, Pair( { it.inDegreesPerSecondPerDegreesPerSecond }, { it.degrees.perSecond.perDegreePerSecond } @@ -111,19 +111,19 @@ class DrivePathCommand( val poskPX = LoggedTunableValue( "Pathfollow/poskPX", - DrivetrainConstants.PID.AUTO_POS_KPX, + DrivetrainConstants.PID.TELEOP_POS_KPX, Pair({ it.inMetersPerSecondPerMeter }, { it.meters.perSecond.perMeter }) ) val poskIX = LoggedTunableValue( "Pathfollow/poskIX", - DrivetrainConstants.PID.AUTO_POS_KIX, + DrivetrainConstants.PID.TELEOP_POS_KIX, Pair({ it.inMetersPerSecondPerMeterSecond }, { it.meters.perSecond.perMeterSeconds }) ) val poskDX = LoggedTunableValue( "Pathfollow/poskDX", - DrivetrainConstants.PID.AUTO_POS_KDX, + DrivetrainConstants.PID.TELEOP_POS_KDX, Pair( { it.inMetersPerSecondPerMetersPerSecond }, { it.metersPerSecondPerMetersPerSecond } ) @@ -132,19 +132,19 @@ class DrivePathCommand( val poskPY = LoggedTunableValue( "Pathfollow/poskPY", - DrivetrainConstants.PID.AUTO_POS_KPY, + DrivetrainConstants.PID.TELEOP_POS_KPY, Pair({ it.inMetersPerSecondPerMeter }, { it.meters.perSecond.perMeter }) ) val poskIY = LoggedTunableValue( "Pathfollow/poskIY", - DrivetrainConstants.PID.AUTO_POS_KIY, + DrivetrainConstants.PID.TELEOP_POS_KIY, Pair({ it.inMetersPerSecondPerMeterSecond }, { it.meters.perSecond.perMeterSeconds }) ) val poskDY = LoggedTunableValue( "Pathfollow/poskDY", - DrivetrainConstants.PID.AUTO_POS_KDY, + DrivetrainConstants.PID.TELEOP_POS_KDY, Pair( { it.inMetersPerSecondPerMetersPerSecond }, { it.metersPerSecondPerMetersPerSecond } ) @@ -169,7 +169,7 @@ class DrivePathCommand( .setEndVelocity(endVelocity.magnitude.inMetersPerSecond) .addConstraint( CentripetalAccelerationConstraint( - DrivetrainConstants.STEERING_ACCEL_MAX.inRadiansPerSecondPerSecond + DrivetrainConstants.CENTRIPETAL_ACCEL_MAX.inMetersPerSecondPerSecond ) ) .addConstraints(constraints) @@ -184,8 +184,144 @@ class DrivePathCommand( init { addRequirements(drivetrain) + if (isAuto) { + val thetakP = + LoggedTunableValue( + "Pathfollow/thetakP", + DrivetrainConstants.PID.TELEOP_THETA_PID_KP, + Pair({ it.inDegreesPerSecondPerDegree }, { it.degrees.perSecond.perDegree }) + ) + val thetakI = + LoggedTunableValue( + "Pathfollow/thetakI", + DrivetrainConstants.PID.TELEOP_THETA_PID_KI, + Pair( + { it.inDegreesPerSecondPerDegreeSeconds }, { it.degrees.perSecond.perDegreeSeconds } + ) + ) + val thetakD = + LoggedTunableValue( + "Pathfollow/thetakD", + DrivetrainConstants.PID.TELEOP_THETA_PID_KD, + Pair( + { it.inDegreesPerSecondPerDegreesPerSecond }, + { it.degrees.perSecond.perDegreePerSecond } + ) + ) + + val poskPX = + LoggedTunableValue( + "Pathfollow/poskPX", + DrivetrainConstants.PID.TELEOP_POS_KPX, + Pair({ it.inMetersPerSecondPerMeter }, { it.meters.perSecond.perMeter }) + ) + val poskIX = + LoggedTunableValue( + "Pathfollow/poskIX", + DrivetrainConstants.PID.TELEOP_POS_KIX, + Pair({ it.inMetersPerSecondPerMeterSecond }, { it.meters.perSecond.perMeterSeconds }) + ) + val poskDX = + LoggedTunableValue( + "Pathfollow/poskDX", + DrivetrainConstants.PID.TELEOP_POS_KDX, + Pair( + { it.inMetersPerSecondPerMetersPerSecond }, { it.metersPerSecondPerMetersPerSecond } + ) + ) + + val poskPY = + LoggedTunableValue( + "Pathfollow/poskPY", + DrivetrainConstants.PID.TELEOP_POS_KPY, + Pair({ it.inMetersPerSecondPerMeter }, { it.meters.perSecond.perMeter }) + ) + val poskIY = + LoggedTunableValue( + "Pathfollow/poskIY", + DrivetrainConstants.PID.TELEOP_POS_KIY, + Pair({ it.inMetersPerSecondPerMeterSecond }, { it.meters.perSecond.perMeterSeconds }) + ) + val poskDY = + LoggedTunableValue( + "Pathfollow/poskDY", + DrivetrainConstants.PID.TELEOP_POS_KDY, + Pair( + { it.inMetersPerSecondPerMetersPerSecond }, { it.metersPerSecondPerMetersPerSecond } + ) + ) + } + else { + val thetakP = + LoggedTunableValue( + "Pathfollow/thetakP", + DrivetrainConstants.PID.TELEOP_THETA_PID_KP, + Pair({ it.inDegreesPerSecondPerDegree }, { it.degrees.perSecond.perDegree }) + ) + val thetakI = + LoggedTunableValue( + "Pathfollow/thetakI", + DrivetrainConstants.PID.TELEOP_THETA_PID_KI, + Pair( + { it.inDegreesPerSecondPerDegreeSeconds }, { it.degrees.perSecond.perDegreeSeconds } + ) + ) + val thetakD = + LoggedTunableValue( + "Pathfollow/thetakD", + DrivetrainConstants.PID.TELEOP_THETA_PID_KD, + Pair( + { it.inDegreesPerSecondPerDegreesPerSecond }, + { it.degrees.perSecond.perDegreePerSecond } + ) + ) + + val poskPX = + LoggedTunableValue( + "Pathfollow/poskPX", + DrivetrainConstants.PID.TELEOP_POS_KPX, + Pair({ it.inMetersPerSecondPerMeter }, { it.meters.perSecond.perMeter }) + ) + val poskIX = + LoggedTunableValue( + "Pathfollow/poskIX", + DrivetrainConstants.PID.TELEOP_POS_KIX, + Pair({ it.inMetersPerSecondPerMeterSecond }, { it.meters.perSecond.perMeterSeconds }) + ) + val poskDX = + LoggedTunableValue( + "Pathfollow/poskDX", + DrivetrainConstants.PID.TELEOP_POS_KDX, + Pair( + { it.inMetersPerSecondPerMetersPerSecond }, { it.metersPerSecondPerMetersPerSecond } + ) + ) + + val poskPY = + LoggedTunableValue( + "Pathfollow/poskPY", + DrivetrainConstants.PID.TELEOP_POS_KPY, + Pair({ it.inMetersPerSecondPerMeter }, { it.meters.perSecond.perMeter }) + ) + val poskIY = + LoggedTunableValue( + "Pathfollow/poskIY", + DrivetrainConstants.PID.TELEOP_POS_KIY, + Pair({ it.inMetersPerSecondPerMeterSecond }, { it.meters.perSecond.perMeterSeconds }) + ) + val poskDY = + LoggedTunableValue( + "Pathfollow/poskDY", + DrivetrainConstants.PID.TELEOP_POS_KDY, + Pair( + { it.inMetersPerSecondPerMetersPerSecond }, { it.metersPerSecondPerMetersPerSecond } + ) + ) + } + xPID = PIDController(poskPX.get(), poskIX.get(), poskDX.get()) yPID = PIDController(poskPY.get(), poskIY.get(), poskDY.get()) + thetaPID = PIDController( thetakP.get(), @@ -193,6 +329,8 @@ class DrivePathCommand( thetakD.get(), ) + + thetaPID.enableContinuousInput(-PI.radians, PI.radians) swerveDriveController = @@ -204,6 +342,52 @@ class DrivePathCommand( } override fun initialize() { + + if (isAuto) { + thetakP.initDefault(DrivetrainConstants.PID.AUTO_THETA_PID_KP) + thetakI.initDefault(DrivetrainConstants.PID.AUTO_THETA_PID_KI) + thetakD.initDefault(DrivetrainConstants.PID.AUTO_THETA_PID_KD) + + poskPX.initDefault(DrivetrainConstants.PID.AUTO_POS_KPX) + poskIX.initDefault(DrivetrainConstants.PID.AUTO_POS_KIX) + poskDX.initDefault(DrivetrainConstants.PID.AUTO_POS_KDX) + + poskPY.initDefault(DrivetrainConstants.PID.AUTO_POS_KPY) + poskIY.initDefault(DrivetrainConstants.PID.AUTO_POS_KIY) + poskDY.initDefault(DrivetrainConstants.PID.AUTO_POS_KDY) + } + else { + thetakP.initDefault(DrivetrainConstants.PID.TELEOP_THETA_PID_KP) + thetakI.initDefault(DrivetrainConstants.PID.TELEOP_THETA_PID_KI) + thetakD.initDefault(DrivetrainConstants.PID.TELEOP_THETA_PID_KD) + + poskPX.initDefault(DrivetrainConstants.PID.TELEOP_POS_KPX) + poskIX.initDefault(DrivetrainConstants.PID.TELEOP_POS_KIX) + poskDX.initDefault(DrivetrainConstants.PID.TELEOP_POS_KDX) + + poskPY.initDefault(DrivetrainConstants.PID.TELEOP_POS_KPY) + poskIY.initDefault(DrivetrainConstants.PID.TELEOP_POS_KIY) + poskDY.initDefault(DrivetrainConstants.PID.TELEOP_POS_KDY) + } + + xPID.setPID( + poskPX.get(), + poskIX.get(), + poskDX.get(), + ) + + yPID.setPID( + poskPY.get(), + poskIY.get(), + poskDY.get(), + ) + + thetaPID.setPID( + thetakP.get(), + thetakI.get(), + thetakD.get(), + ) + // trajectory generation! generate(waypoints.get()) @@ -229,6 +413,8 @@ class DrivePathCommand( return } + Logger.getInstance().recordOutput("Pathfollow/thetakPinDPSPS", thetakP.get().inDegreesPerSecondPerDegree) + trajCurTime = Clock.fpgaTime - trajStartTime var desiredState = trajectory.sample(trajCurTime.inSeconds) @@ -258,6 +444,8 @@ class DrivePathCommand( yAccel = 0.0.meters.perSecond.perSecond } + Logger.getInstance().recordOutput("Pathfollow/omegaVelocity", nextDriveState.omegaRadiansPerSecond) + drivetrain.targetPose = Pose2d(Pose2dWPILIB(desiredState.poseMeters.translation, desiredRotation.position)) @@ -267,9 +455,11 @@ class DrivePathCommand( Pose2dWPILIB(desiredState.poseMeters.translation, desiredRotation.position) ) + val dtheta = (nextDriveState.omegaRadiansPerSecond.radians.perSecond - drivetrain.gyroInputs.gyroYawRate) / 0.02.seconds + drivetrain.setClosedLoop( nextDriveState, - ChassisAccels(xAccel, yAccel, 0.0.radians.perSecond.perSecond).chassisAccelsWPILIB + ChassisAccels(xAccel, yAccel, dtheta).chassisAccelsWPILIB ) Logger.getInstance() diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/GoToAngle.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/GoToAngle.kt index 16424f2f..29a8a352 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/GoToAngle.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/GoToAngle.kt @@ -60,9 +60,9 @@ class GoToAngle(val drivetrain: Drivetrain) : CommandBase() { addRequirements(drivetrain) 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) + thetakP.initDefault(DrivetrainConstants.PID.TELEOP_THETA_PID_KP) + thetakI.initDefault(DrivetrainConstants.PID.TELEOP_THETA_PID_KI) + thetakD.initDefault(DrivetrainConstants.PID.TELEOP_THETA_PID_KD) } else { thetakP.initDefault(DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KP) thetakI.initDefault(DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KI) 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 8e29be1b..9f340929 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt @@ -59,7 +59,7 @@ object Constants { object Tuning { - const val TUNING_MODE = false + const val TUNING_MODE = true const val DEBUGING_MODE = true const val SIMULATE_DRIFT = false const val DRIFT_CONSTANT = 0.001 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 dcc5913f..7d50fe15 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt @@ -47,15 +47,16 @@ object DrivetrainConstants { // cruise velocity and accel for steering motor val STEERING_VEL_MAX = 1393.2.degrees.perSecond - val STEERING_ACCEL_MAX = 13932.degrees.perSecond.perSecond + val STEERING_ACCEL_MAX = 13932.2.degrees.perSecond.perSecond + val CENTRIPETAL_ACCEL_MAX = 150.0.inches.perSecond.perSecond const val GYRO_RATE_COEFFICIENT = 0.0 // TODO: Change this value val SLOW_AUTO_VEL = 2.meters.perSecond val SLOW_AUTO_ACCEL = 2.0.meters.perSecond.perSecond - val MAX_AUTO_VEL = 3.meters.perSecond // 4 - val MAX_AUTO_ACCEL = 3.5.meters.perSecond.perSecond // 3 + val MAX_AUTO_VEL = 2.meters.perSecond // 4 + val MAX_AUTO_ACCEL = 2.5.meters.perSecond.perSecond // 3 val MAX_AUTO_BRAKE_VEL = 0.5.meters.perSecond // 4 val MAX_AUTO_BRAKE_ACCEL = 0.5.meters.perSecond.perSecond // 3 @@ -89,15 +90,15 @@ object DrivetrainConstants { val STEERING_WHEEL_INERTIA = 0.004096955.kilo.grams.meterSquared object PID { - val AUTO_POS_KPX: ProportionalGain> + val TELEOP_POS_KPX: ProportionalGain> get() { if (RobotBase.isReal()) { - return 0.25.meters.perSecond / 1.0.meters // todo:4 + return 1.meters.perSecond / 1.0.meters // todo:4 } else { return 7.0.meters.perSecond / 1.0.meters } } - val AUTO_POS_KIX: IntegralGain> + val TELEOP_POS_KIX: IntegralGain> get() { if (RobotBase.isReal()) { return 0.0.meters.perSecond / (1.0.meters * 1.0.seconds) @@ -106,7 +107,7 @@ object DrivetrainConstants { } } - val AUTO_POS_KDX: DerivativeGain> + val TELEOP_POS_KDX: DerivativeGain> get() { if (RobotBase.isReal()) { return (0.1.meters.perSecond / (1.0.meters.perSecond)) @@ -116,15 +117,15 @@ object DrivetrainConstants { } } - val AUTO_POS_KPY: ProportionalGain> + val TELEOP_POS_KPY: ProportionalGain> get() { if (RobotBase.isReal()) { - return 0.25.meters.perSecond / 1.0.meters // todo:4 + return 1.meters.perSecond / 1.0.meters // todo:4 } else { return 7.0.meters.perSecond / 1.0.meters } } - val AUTO_POS_KIY: IntegralGain> + val TELEOP_POS_KIY: IntegralGain> get() { if (RobotBase.isReal()) { return 0.0.meters.perSecond / (1.0.meters * 1.0.seconds) @@ -133,7 +134,7 @@ object DrivetrainConstants { } } - val AUTO_POS_KDY: DerivativeGain> + val TELEOP_POS_KDY: DerivativeGain> get() { if (RobotBase.isReal()) { return (0.025.meters.perSecond / (1.0.meters.perSecond)) @@ -149,7 +150,7 @@ object DrivetrainConstants { val AUTO_ALIGN_POS_KP: ProportionalGain> get() { if (RobotBase.isReal()) { - return 1.5.meters.perSecond / 1.0.meters // todo:4 + return 1.2.meters.perSecond / 1.0.meters // todo:4 } else { return 7.0.meters.perSecond / 1.0.meters } @@ -173,6 +174,61 @@ object DrivetrainConstants { } } + val AUTO_POS_KPX: ProportionalGain> + get() { + if (RobotBase.isReal()) { + return 2.meters.perSecond / 1.0.meters // todo:4 + } else { + return 7.0.meters.perSecond / 1.0.meters + } + } + val AUTO_POS_KIX: IntegralGain> + get() { + if (RobotBase.isReal()) { + return 0.0.meters.perSecond / (1.0.meters * 1.0.seconds) + } else { + return 0.0.meters.perSecond / (1.0.meters * 1.0.seconds) + } + } + + val AUTO_POS_KDX: DerivativeGain> + get() { + if (RobotBase.isReal()) { + return (0.1.meters.perSecond / (1.0.meters.perSecond)) + .metersPerSecondPerMetersPerSecond // todo: 0.25 + } else { + return (0.0.meters.perSecond / (1.0.meters.perSecond)).metersPerSecondPerMetersPerSecond + } + } + + val AUTO_POS_KPY: ProportionalGain> + get() { + if (RobotBase.isReal()) { + return 2.meters.perSecond / 1.0.meters // todo:4 + } else { + return 7.0.meters.perSecond / 1.0.meters + } + } + + val AUTO_POS_KIY: IntegralGain> + get() { + if (RobotBase.isReal()) { + return 0.0.meters.perSecond / (1.0.meters * 1.0.seconds) + } else { + return 0.0.meters.perSecond / (1.0.meters * 1.0.seconds) + } + } + + val AUTO_POS_KDY: DerivativeGain> + get() { + if (RobotBase.isReal()) { + return (0.025.meters.perSecond / (1.0.meters.perSecond)) + .metersPerSecondPerMetersPerSecond // todo: 0.25 + } else { + return (0.0.meters.perSecond / (1.0.meters.perSecond)).metersPerSecondPerMetersPerSecond + } + } + val AUTO_THETA_ALLOWED_ERROR = 3.degrees @@ -181,9 +237,9 @@ object DrivetrainConstants { 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 = + val TELEOP_THETA_PID_KP = 0.8.degrees.perSecond / 1.degrees + val TELEOP_THETA_PID_KI = 0.0.degrees.perSecond / (1.degrees * 1.seconds) + val TELEOP_THETA_PID_KD = (0.025.degrees.perSecond / (1.degrees / 1.seconds)).radiansPerSecondPerRadiansPerSecond val SIM_AUTO_THETA_PID_KP = 15.degrees.perSecond / 1.degrees @@ -191,6 +247,11 @@ object DrivetrainConstants { val SIM_AUTO_THETA_PID_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 = + (0.025.degrees.perSecond / (1.degrees / 1.seconds)).radiansPerSecondPerRadiansPerSecond + val AUTO_LEVEL_KP = 1.meters.perSecond / 1.0.degrees // tune this val AUTO_LEVEL_KI = 0.0.meters.perSecond / (1.0.degrees * 1.seconds) // tune this val AUTO_LEVEL_KD = 0.0.meters.perSecond / (1.0.degrees.perSecond) // tune this diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOFalcon.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOFalcon.kt index 80b97b44..4d42618c 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOFalcon.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOFalcon.kt @@ -28,11 +28,13 @@ import org.team4099.lib.units.derived.IntegralGain import org.team4099.lib.units.derived.ProportionalGain import org.team4099.lib.units.derived.Radian import org.team4099.lib.units.derived.Volt +import org.team4099.lib.units.derived.degrees import org.team4099.lib.units.derived.inRadians import org.team4099.lib.units.derived.inVolts import org.team4099.lib.units.derived.radians import org.team4099.lib.units.derived.volts import org.team4099.lib.units.inMetersPerSecond +import org.team4099.lib.units.perSecond import java.lang.Math.PI class SwerveModuleIOFalcon( 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 0f358f3f..863c8e49 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVision.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVision.kt @@ -8,6 +8,7 @@ import com.team4099.lib.vision.TargetCorner import com.team4099.robot2023.config.constants.Constants import com.team4099.robot2023.config.constants.FieldConstants import com.team4099.robot2023.config.constants.VisionConstants +import com.team4099.robot2023.config.constants.WaypointConstants import com.team4099.robot2023.subsystems.gameboy.objective.Objective import com.team4099.robot2023.util.FMSData import com.team4099.robot2023.util.LimelightReading @@ -61,7 +62,8 @@ class LimelightVision(val io: LimelightVisionIO) : SubsystemBase() { val vpw = (2.0 * (VisionConstants.Limelight.HORIZONTAL_FOV / 2).tan) val vph = (2.0 * (VisionConstants.Limelight.VERITCAL_FOV / 2).tan) - private val xyStdDevCoefficient = LoggedTunableNumber("LimelightVision/xystdev", 0.1) + private val xStdDevCoefficient = LoggedTunableNumber("LimelightVision/xystdev", 0.05) + private val yStdDevCoefficient = LoggedTunableNumber("LimelightVision/xystdev", 0.01) private val thetaStdDev = LoggedTunableNumber("LimelightVision/thetaStdDev", 3.0) var limelightState: LimelightStates = LimelightStates.UNINITIALIZED @@ -186,19 +188,26 @@ class LimelightVision(val io: LimelightVisionIO) : SubsystemBase() { val trueNodePoseToRobot = closestPose.transformBy(targetToCamera) // Add to vision updates - val xyStdDev = - xyStdDevCoefficient.get() * targetToCamera.translation.norm.inMeters.pow(2) + val xStdDev = + xStdDevCoefficient.get() * targetToCamera.translation.norm.inMeters.pow(2) + val yStdDev = + yStdDevCoefficient.get() * targetToCamera.translation.norm.inMeters.pow(2) val thetaStdDev = thetaStdDev.get() * targetToCamera.translation.norm.inMeters.pow(2) robotPoses.add(trueNodePoseToRobot.toPose2d()) - timestampedVisionUpdates.add( - PoseEstimator.TimestampedVisionUpdate( - inputs.timestamp, - trueNodePoseToRobot.toPose2d(), - VecBuilder.fill(xyStdDev, xyStdDev, thetaStdDev) + if (!(FieldConstants.Zones.closeCenterCommunity.containsPose(poseSupplier()) || + FieldConstants.Zones.closeLeftCommunity.containsPose(poseSupplier()) || + FieldConstants.Zones.closeRightCommunity.containsPose(poseSupplier()) + )){ + timestampedVisionUpdates.add( + PoseEstimator.TimestampedVisionUpdate( + inputs.timestamp, + trueNodePoseToRobot.toPose2d(), + VecBuilder.fill(xStdDev, yStdDev, thetaStdDev) + ) ) - ) + } } }