From 4d50becea4e0791140d878ab6908b7e2042a0b57 Mon Sep 17 00:00:00 2001 From: nbhog <146785661+nbhog@users.noreply.github.com> Date: Sat, 20 Jan 2024 20:36:41 -0500 Subject: [PATCH] fixed some programming issues --- .../team4099/lib/phoenix6/PositionVoltage.kt | 101 +- .../team4099/lib/phoenix6/VelocityVoltage.kt | 98 +- .../com/team4099/robot2023/RobotContainer.kt | 5 +- .../drivetrain/DriveBrakeModeCommand.kt | 5 +- .../commands/drivetrain/DrivePathCommand.kt | 23 +- .../commands/drivetrain/GoToAngle.kt | 24 +- .../commands/drivetrain/GyroAutoLevel.kt | 25 +- .../drivetrain/OpenLoopReverseCommand.kt | 5 +- .../commands/drivetrain/TeleopDriveCommand.kt | 2 +- .../commands/drivetrain/ZeroSensorsCommand.kt | 2 +- .../team4099/robot2023/config/ControlBoard.kt | 8 +- .../config/constants/DrivetrainConstants.kt | 2 +- .../config/constants/ElevatorConstants.kt | 140 ++- .../subsystems/drivetrain/drive/Drivetrain.kt | 1085 +++++++++-------- .../drivetrain/drive/DrivetrainIO.kt | 42 +- .../drivetrain/drive/DrivetrainIOSim.kt | 18 +- .../subsystems/drivetrain/gyro/GyroIO.kt | 90 +- .../subsystems/drivetrain/gyro/GyroIONavx.kt | 126 +- .../drivetrain/servemodule/SwerveModule.kt | 495 ++++---- .../drivetrain/servemodule/SwerveModuleIO.kt | 211 ++-- .../servemodule/SwerveModuleIOSim.kt | 442 +++---- .../robot2023/subsystems/elevator/Elevator.kt | 647 +++++----- .../subsystems/elevator/ElevatorIO.kt | 152 ++- .../subsystems/elevator/ElevatorIOKraken.kt | 271 ++-- .../subsystems/elevator/ElevatorIONEO.kt | 318 ++--- .../subsystems/elevator/ElevatorIOSim.kt | 252 ++-- .../subsystems/superstructure/Request.kt | 11 +- 27 files changed, 2389 insertions(+), 2211 deletions(-) diff --git a/src/main/kotlin/com/team4099/lib/phoenix6/PositionVoltage.kt b/src/main/kotlin/com/team4099/lib/phoenix6/PositionVoltage.kt index 5f7c1054..939bf44c 100644 --- a/src/main/kotlin/com/team4099/lib/phoenix6/PositionVoltage.kt +++ b/src/main/kotlin/com/team4099/lib/phoenix6/PositionVoltage.kt @@ -12,50 +12,61 @@ import org.team4099.lib.units.perSecond import com.ctre.phoenix6.controls.PositionVoltage as PositionVoltagePhoenix6 class PositionVoltage( - private var position: Angle, // Assuming an AngularPosition type exists similar to AngularVelocity - private var enableFOC: Boolean = true, - private var feedforward: ElectricalPotential = 0.0.volts, - private var slot: Int = 0, - private var overrideBrakeDurNeutral: Boolean = false, - private var limitForwardMotion: Boolean = false, - private var limitReverseMotion: Boolean = false, - var velocity: AngularVelocity = 0.0.degrees.perSecond, + private var position: + Angle, // Assuming an AngularPosition type exists similar to AngularVelocity + private var enableFOC: Boolean = true, + private var feedforward: ElectricalPotential = 0.0.volts, + private var slot: Int = 0, + private var overrideBrakeDurNeutral: Boolean = false, + private var limitForwardMotion: Boolean = false, + private var limitReverseMotion: Boolean = false, + var velocity: AngularVelocity = 0.0.degrees.perSecond, ) { - val positionVoltagePhoenix6 = PositionVoltagePhoenix6(position.inRotations, velocity.inRotationsPerSecond, enableFOC, feedforward.inVolts, slot, overrideBrakeDurNeutral, limitForwardMotion, limitReverseMotion) - - fun setPosition(new_position: Angle) { - position = new_position - positionVoltagePhoenix6.Position = new_position.inRotations - } - - fun setEnableFOC(new_enableFOC: Boolean) { - enableFOC = new_enableFOC - positionVoltagePhoenix6.EnableFOC = new_enableFOC - } - - fun setFeedforward(new_feedforward: ElectricalPotential) { - feedforward = new_feedforward - positionVoltagePhoenix6.FeedForward = new_feedforward.inVolts - } - - fun setSlot(new_slot: Int) { - slot = new_slot - positionVoltagePhoenix6.Slot = new_slot - } - - fun setOverrideBrakeDurNeutral(new_override: Boolean) { - overrideBrakeDurNeutral = new_override - positionVoltagePhoenix6.OverrideBrakeDurNeutral = new_override - } - - fun setLimitForwardMotion(new_limitForward: Boolean) { - limitForwardMotion = new_limitForward - positionVoltagePhoenix6.LimitForwardMotion = new_limitForward - } - - fun setLimitReverseMotion(new_limitReverse: Boolean) { - limitReverseMotion = new_limitReverse - positionVoltagePhoenix6.LimitReverseMotion = new_limitReverse - } -} \ No newline at end of file + val positionVoltagePhoenix6 = + PositionVoltagePhoenix6( + position.inRotations, + velocity.inRotationsPerSecond, + enableFOC, + feedforward.inVolts, + slot, + overrideBrakeDurNeutral, + limitForwardMotion, + limitReverseMotion + ) + + fun setPosition(new_position: Angle) { + position = new_position + positionVoltagePhoenix6.Position = new_position.inRotations + } + + fun setEnableFOC(new_enableFOC: Boolean) { + enableFOC = new_enableFOC + positionVoltagePhoenix6.EnableFOC = new_enableFOC + } + + fun setFeedforward(new_feedforward: ElectricalPotential) { + feedforward = new_feedforward + positionVoltagePhoenix6.FeedForward = new_feedforward.inVolts + } + + fun setSlot(new_slot: Int) { + slot = new_slot + positionVoltagePhoenix6.Slot = new_slot + } + + fun setOverrideBrakeDurNeutral(new_override: Boolean) { + overrideBrakeDurNeutral = new_override + positionVoltagePhoenix6.OverrideBrakeDurNeutral = new_override + } + + fun setLimitForwardMotion(new_limitForward: Boolean) { + limitForwardMotion = new_limitForward + positionVoltagePhoenix6.LimitForwardMotion = new_limitForward + } + + fun setLimitReverseMotion(new_limitReverse: Boolean) { + limitReverseMotion = new_limitReverse + positionVoltagePhoenix6.LimitReverseMotion = new_limitReverse + } +} diff --git a/src/main/kotlin/com/team4099/lib/phoenix6/VelocityVoltage.kt b/src/main/kotlin/com/team4099/lib/phoenix6/VelocityVoltage.kt index 91339c15..dde287dc 100644 --- a/src/main/kotlin/com/team4099/lib/phoenix6/VelocityVoltage.kt +++ b/src/main/kotlin/com/team4099/lib/phoenix6/VelocityVoltage.kt @@ -1,6 +1,5 @@ package com.team4099.lib.phoenix6 -import com.ctre.phoenix6.controls.VelocityVoltage as VelocityVoltagePhoenix6 import org.team4099.lib.units.AngularAcceleration import org.team4099.lib.units.AngularVelocity import org.team4099.lib.units.derived.ElectricalPotential @@ -10,55 +9,68 @@ import org.team4099.lib.units.derived.volts import org.team4099.lib.units.inRotationsPerSecond import org.team4099.lib.units.inRotationsPerSecondPerSecond import org.team4099.lib.units.perSecond +import com.ctre.phoenix6.controls.VelocityVoltage as VelocityVoltagePhoenix6 -class VelocityVoltage(private var velocity: AngularVelocity, - private var acceleration: AngularAcceleration = 0.0.degrees.perSecond.perSecond, - private var enableFOC:Boolean = true, - private var feedforward: ElectricalPotential = 0.0.volts, - private var slot:Int = 0, - private var overrideBrakeDurNeutral: Boolean = false, - private var limitForwardMotion: Boolean = false, - private var limitReverseMotion: Boolean = false){ +class VelocityVoltage( + private var velocity: AngularVelocity, + private var acceleration: AngularAcceleration = 0.0.degrees.perSecond.perSecond, + private var enableFOC: Boolean = true, + private var feedforward: ElectricalPotential = 0.0.volts, + private var slot: Int = 0, + private var overrideBrakeDurNeutral: Boolean = false, + private var limitForwardMotion: Boolean = false, + private var limitReverseMotion: Boolean = false +) { - val velocityVoltagePhoenix6 = VelocityVoltagePhoenix6(velocity.inRotationsPerSecond, acceleration.inRotationsPerSecondPerSecond, enableFOC, feedforward.inVolts, slot, overrideBrakeDurNeutral, limitForwardMotion, limitReverseMotion) + val velocityVoltagePhoenix6 = + VelocityVoltagePhoenix6( + velocity.inRotationsPerSecond, + acceleration.inRotationsPerSecondPerSecond, + enableFOC, + feedforward.inVolts, + slot, + overrideBrakeDurNeutral, + limitForwardMotion, + limitReverseMotion + ) - fun setVelocity(new_velocity: AngularVelocity) { - velocity = new_velocity - velocityVoltagePhoenix6.Velocity = velocity.inRotationsPerSecond - } + fun setVelocity(new_velocity: AngularVelocity) { + velocity = new_velocity + velocityVoltagePhoenix6.Velocity = velocity.inRotationsPerSecond + } - fun setAcceleration(new_accel: AngularAcceleration) { - acceleration = new_accel - velocityVoltagePhoenix6.Acceleration = acceleration.inRotationsPerSecondPerSecond - } + fun setAcceleration(new_accel: AngularAcceleration) { + acceleration = new_accel + velocityVoltagePhoenix6.Acceleration = acceleration.inRotationsPerSecondPerSecond + } - fun setEnableFOC(new_enableFOC: Boolean) { - enableFOC = new_enableFOC - velocityVoltagePhoenix6.EnableFOC = new_enableFOC - } + fun setEnableFOC(new_enableFOC: Boolean) { + enableFOC = new_enableFOC + velocityVoltagePhoenix6.EnableFOC = new_enableFOC + } - fun setFeedforward(new_feedforward: ElectricalPotential) { - feedforward = new_feedforward - velocityVoltagePhoenix6.FeedForward = new_feedforward.inVolts - } + fun setFeedforward(new_feedforward: ElectricalPotential) { + feedforward = new_feedforward + velocityVoltagePhoenix6.FeedForward = new_feedforward.inVolts + } - fun setSlot(new_slot: Int) { - slot = new_slot - velocityVoltagePhoenix6.Slot = new_slot - } + fun setSlot(new_slot: Int) { + slot = new_slot + velocityVoltagePhoenix6.Slot = new_slot + } - fun setOverrideBrakeDurNeutral(new_override: Boolean) { - overrideBrakeDurNeutral = new_override - velocityVoltagePhoenix6.OverrideBrakeDurNeutral = new_override - } + fun setOverrideBrakeDurNeutral(new_override: Boolean) { + overrideBrakeDurNeutral = new_override + velocityVoltagePhoenix6.OverrideBrakeDurNeutral = new_override + } - fun setLimitForwardMotion(new_limitForward: Boolean) { - limitForwardMotion = new_limitForward - velocityVoltagePhoenix6.LimitForwardMotion = new_limitForward - } + fun setLimitForwardMotion(new_limitForward: Boolean) { + limitForwardMotion = new_limitForward + velocityVoltagePhoenix6.LimitForwardMotion = new_limitForward + } - fun setLimitReverseMotion(new_limitReverse: Boolean) { - limitReverseMotion = new_limitReverse - velocityVoltagePhoenix6.LimitReverseMotion = new_limitReverse - } -} \ No newline at end of file + fun setLimitReverseMotion(new_limitReverse: Boolean) { + limitReverseMotion = new_limitReverse + velocityVoltagePhoenix6.LimitReverseMotion = new_limitReverse + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt index 598a72ab..60ca3cb5 100644 --- a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt +++ b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt @@ -14,16 +14,14 @@ import com.team4099.robot2023.subsystems.elevator.ElevatorIONEO import com.team4099.robot2023.subsystems.elevator.ElevatorIOSim import com.team4099.robot2023.subsystems.limelight.LimelightVision import com.team4099.robot2023.subsystems.limelight.LimelightVisionIO -import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest import com.team4099.robot2023.subsystems.vision.Vision import com.team4099.robot2023.subsystems.vision.camera.CameraIONorthstar import com.team4099.robot2023.util.driver.Ryan import edu.wpi.first.wpilibj.RobotBase -import org.team4099.lib.geometry.Pose2d import org.team4099.lib.smoothDeadband -import org.team4099.lib.units.base.feet import org.team4099.lib.units.derived.Angle import org.team4099.lib.units.derived.degrees +import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest object RobotContainer { private val drivetrain: Drivetrain @@ -140,7 +138,6 @@ object RobotContainer { ControlBoard.elevatorOpenLoopRetract.whileTrue(elevator.testElevatorOpenLoopRetractCommand()) ControlBoard.elevatorClosedLoopHigh.whileTrue(elevator.testElevatorClosedLoopExtendCommand()) ControlBoard.elevatorClosedLoopLow.whileTrue(elevator.testElevatorClosedLoopExtendCommand()) - } fun mapTestControls() {} diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DriveBrakeModeCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DriveBrakeModeCommand.kt index 97472978..3eb85b2e 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DriveBrakeModeCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DriveBrakeModeCommand.kt @@ -1,11 +1,11 @@ package com.team4099.robot2023.commands.drivetrain import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain -import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest import edu.wpi.first.wpilibj2.command.Command import org.team4099.lib.units.base.meters import org.team4099.lib.units.derived.radians import org.team4099.lib.units.perSecond +import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest class DriveBrakeModeCommand(val drivetrain: Drivetrain) : Command() { init { @@ -13,7 +13,8 @@ class DriveBrakeModeCommand(val drivetrain: Drivetrain) : Command() { } override fun execute() { - drivetrain.currentRequest = DrivetrainRequest.OpenLoop( + drivetrain.currentRequest = + DrivetrainRequest.OpenLoop( 0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond) ) drivetrain.swerveModules.forEach() { it.setDriveBrakeMode(true) } 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 4284a536..12a8841c 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt @@ -6,7 +6,6 @@ import com.team4099.lib.trajectory.CustomTrajectoryGenerator import com.team4099.lib.trajectory.Waypoint import com.team4099.robot2023.config.constants.DrivetrainConstants import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain -import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest import com.team4099.robot2023.util.AllianceFlipUtil import com.team4099.robot2023.util.Velocity2d import edu.wpi.first.math.kinematics.ChassisSpeeds @@ -22,8 +21,6 @@ import org.team4099.lib.geometry.Pose2d import org.team4099.lib.geometry.Pose2dWPILIB import org.team4099.lib.hal.Clock import org.team4099.lib.kinematics.ChassisAccels -import java.util.function.Supplier -import kotlin.math.PI import org.team4099.lib.units.Velocity import org.team4099.lib.units.base.Meter import org.team4099.lib.units.base.inMeters @@ -55,6 +52,9 @@ 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 +import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest class DrivePathCommand( val drivetrain: Drivetrain, @@ -244,7 +244,8 @@ class DrivePathCommand( Pose2dWPILIB(desiredState.poseMeters.translation, desiredRotation.position) ) - drivetrain.currentRequest = DrivetrainRequest.ClosedLoop( + drivetrain.currentRequest = + DrivetrainRequest.ClosedLoop( nextDriveState, ChassisAccels(xAccel, yAccel, 0.0.radians.perSecond.perSecond).chassisAccelsWPILIB ) @@ -309,16 +310,18 @@ class DrivePathCommand( Logger.recordOutput("ActiveCommands/DrivePathCommand", false) if (interrupted) { // Stop where we are if interrupted - drivetrain.currentRequest = DrivetrainRequest.OpenLoop( - 0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond) - ) + drivetrain.currentRequest = + DrivetrainRequest.OpenLoop( + 0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond) + ) } else { // Execute one last time to end up in the final state of the trajectory // Since we weren't interrupted, we know curTime > endTime execute() - drivetrain.currentRequest = DrivetrainRequest.OpenLoop( - 0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond) - ) + drivetrain.currentRequest = + DrivetrainRequest.OpenLoop( + 0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond) + ) } } } 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 30d1a3f2..f48ddc33 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/GoToAngle.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/GoToAngle.kt @@ -3,7 +3,6 @@ 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.DrivetrainRequest as DrivetrainRequest import edu.wpi.first.wpilibj.RobotBase import edu.wpi.first.wpilibj2.command.Command import org.littletonrobotics.junction.Logger @@ -11,10 +10,21 @@ import org.team4099.lib.controller.ProfiledPIDController import org.team4099.lib.controller.TrapezoidProfile import org.team4099.lib.units.Velocity import org.team4099.lib.units.base.meters -import org.team4099.lib.units.derived.* +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.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 com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest class GoToAngle(val drivetrain: Drivetrain) : Command() { private val thetaPID: ProfiledPIDController> @@ -82,7 +92,8 @@ class GoToAngle(val drivetrain: Drivetrain) : Command() { val thetaFeedback = thetaPID.calculate(drivetrain.odometryPose.rotation, desiredAngle) - drivetrain.currentRequest = DrivetrainRequest.OpenLoop( + drivetrain.currentRequest = + DrivetrainRequest.OpenLoop( thetaFeedback, Pair(0.0.meters.perSecond, 0.0.meters.perSecond), fieldOriented = true ) @@ -98,8 +109,9 @@ class GoToAngle(val drivetrain: Drivetrain) : Command() { } override fun end(interrupted: Boolean) { - drivetrain.currentRequest = DrivetrainRequest.OpenLoop( - 0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond) - ) + drivetrain.currentRequest = + DrivetrainRequest.OpenLoop( + 0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond) + ) } } diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/GyroAutoLevel.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/GyroAutoLevel.kt index e49f994f..43260231 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/GyroAutoLevel.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/GyroAutoLevel.kt @@ -4,16 +4,23 @@ import com.team4099.lib.hal.Clock 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.DrivetrainRequest as DrivetrainRequest 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.units.* +import org.team4099.lib.units.Value +import org.team4099.lib.units.Velocity import org.team4099.lib.units.base.Meter import org.team4099.lib.units.base.Second import org.team4099.lib.units.base.meters -import org.team4099.lib.units.derived.* +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.radians +import org.team4099.lib.units.inMetersPerSecond +import org.team4099.lib.units.perSecond +import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest class GyroAutoLevel(val drivetrain: Drivetrain) : Command() { private val gyroPID: ProfiledPIDController> @@ -92,9 +99,8 @@ class GyroAutoLevel(val drivetrain: Drivetrain) : Command() { override fun execute() { Logger.recordOutput("ActiveCommands/AutoLevelCommand", true) - drivetrain.currentRequest = DrivetrainRequest.OpenLoop( - 0.0.radians.perSecond, gyroFeedback, fieldOriented = true - ) + drivetrain.currentRequest = + DrivetrainRequest.OpenLoop(0.0.radians.perSecond, gyroFeedback, fieldOriented = true) Logger.recordOutput( "AutoLevel/DesiredPitchDegrees", DrivetrainConstants.DOCKING_GYRO_SETPOINT.inDegrees @@ -120,8 +126,9 @@ class GyroAutoLevel(val drivetrain: Drivetrain) : Command() { } override fun end(interrupted: Boolean) { - drivetrain.currentRequest = DrivetrainRequest.OpenLoop( - 0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond) - ) + drivetrain.currentRequest = + DrivetrainRequest.OpenLoop( + 0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond) + ) } } diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/OpenLoopReverseCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/OpenLoopReverseCommand.kt index 6f4fa148..257be010 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/OpenLoopReverseCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/OpenLoopReverseCommand.kt @@ -1,11 +1,11 @@ package com.team4099.robot2023.commands.drivetrain import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain -import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest import edu.wpi.first.wpilibj2.command.Command import org.team4099.lib.units.base.feet import org.team4099.lib.units.derived.degrees import org.team4099.lib.units.perSecond +import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest class OpenLoopReverseCommand(val drivetrain: Drivetrain) : Command() { init { @@ -13,7 +13,8 @@ class OpenLoopReverseCommand(val drivetrain: Drivetrain) : Command() { } override fun execute() { - drivetrain.currentRequest = DrivetrainRequest.OpenLoop( + drivetrain.currentRequest = + DrivetrainRequest.OpenLoop( 0.degrees.perSecond, Pair(-5.0.feet.perSecond, 0.0.feet.perSecond), fieldOriented = false diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TeleopDriveCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TeleopDriveCommand.kt index 18f554e4..6a09a062 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TeleopDriveCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TeleopDriveCommand.kt @@ -1,10 +1,10 @@ package com.team4099.robot2023.commands.drivetrain import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain -import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest import com.team4099.robot2023.util.driver.DriverProfile import edu.wpi.first.wpilibj2.command.Command import org.littletonrobotics.junction.Logger +import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest class TeleopDriveCommand( val driver: DriverProfile, diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ZeroSensorsCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ZeroSensorsCommand.kt index d3b6d267..e8510fb3 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ZeroSensorsCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ZeroSensorsCommand.kt @@ -1,9 +1,9 @@ package com.team4099.robot2023.commands.drivetrain import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain -import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest import edu.wpi.first.wpilibj2.command.Command import org.littletonrobotics.junction.Logger +import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest class ZeroSensorsCommand(val drivetrain: Drivetrain) : Command() { diff --git a/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt b/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt index 87aeed9c..7ace6cad 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt @@ -98,8 +98,8 @@ object ControlBoard { // for testing val setArmCommand = Trigger { technician.yButton } - val elevatorOpenLoopExtend = Trigger {driver.aButton} - val elevatorOpenLoopRetract = Trigger {driver.bButton} - val elevatorClosedLoopHigh = Trigger {driver.xButton} - val elevatorClosedLoopLow = Trigger { driver.yButton} + val elevatorOpenLoopExtend = Trigger { driver.aButton } + val elevatorOpenLoopRetract = Trigger { driver.bButton } + val elevatorClosedLoopHigh = Trigger { driver.xButton } + val elevatorClosedLoopLow = Trigger { driver.yButton } } 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 280c2a36..13aac5e5 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt @@ -170,4 +170,4 @@ object DrivetrainConstants { val SIM_STEERING_KI = 0.0.volts.perDegreeSeconds val SIM_STEERING_KD = 0.0.volts.perDegreePerSecond } -} \ No newline at end of file +} diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt index ef75cbf8..5fba2d33 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt @@ -1,70 +1,84 @@ package com.team4099.robot2023.config.constants -import org.team4099.lib.units.base.* +import org.team4099.lib.units.base.Meter +import org.team4099.lib.units.base.amps +import org.team4099.lib.units.base.inches +import org.team4099.lib.units.base.meters +import org.team4099.lib.units.base.percent +import org.team4099.lib.units.base.pounds +import org.team4099.lib.units.base.seconds +import org.team4099.lib.units.derived.DerivativeGain +import org.team4099.lib.units.derived.IntegralGain +import org.team4099.lib.units.derived.ProportionalGain +import org.team4099.lib.units.derived.Volt import org.team4099.lib.units.derived.degrees -import org.team4099.lib.units.derived.perInch -import org.team4099.lib.units.derived.rotations import org.team4099.lib.units.derived.volts -import org.team4099.lib.units.perMinute import org.team4099.lib.units.perSecond object ElevatorConstants { - // TODO: Change values later based on CAD - val REAL_KP = 0.0.volts / 1.inches - val REAL_KI = 0.0.volts / (1.inches * 1.seconds) - val REAL_KD = 0.0.volts / (1.inches.perSecond) - - val RAMP_RATE = 0.5.percent.perSecond - val CARRIAGE_MASS = 20.pounds - - val ELEVATOR_MAX_RETRACTION = 0.0.inches - val ELEVATOR_MAX_EXTENSION = 18.0.inches - - val LEADER_INVERTED = false - val FOLLOWER_INVERTED = true - - val SIM_KP = 0.0.volts / 1.inches - val SIM_KI = 0.0.volts / (1.inches * 1.seconds) - val SIM_KD = 0.0.volts / (1.inches.perSecond) - - val ELEVATOR_KS = 0.0.volts - val ELEVATOR_KG = 0.0.volts - val ELEVATOR_KV = 0.0.volts/0.0.inches.perSecond - val ELEVATOR_KA = 0.0.volts/0.0.inches.perSecond.perSecond - - val ENABLE_ELEVATOR = 1.0 - val ELEVATOR_IDLE_HEIGHT = 0.0.inches - val ELEVATOR_SOFT_LIMIT_EXTENSION = 0.0.inches - val ELEVATOR_SOFT_LIMIT_RETRACTION = 0.0.inches - val ELEVATOR_OPEN_LOOP_SOFT_LIMIT_EXTENSION = 0.0.inches - val ELEVATOR_OPEN_LOOP_SOFT_LIMIT_RETRACTION = 0.0.inches - - val ELEVATOR_TOLERANCE = 0.0.inches - - val MAX_VELOCITY = 0.0.meters.perSecond - val MAX_ACCELERATION = 0.0.meters.perSecond.perSecond - - val SHOOT_SPEAKER_POSITION = 0.0.inches - val SHOOT_AMP_POSITION = 0.0.inches - val SOURCE_NOTE_OFFSET = 0.0.inches - val ELEVATOR_THETA_POS = 0.0.degrees - val HOMING_STATOR_CURRENT = 0.0.amps - val HOMING_STALL_TIME_THRESHOLD = 0.0.seconds - val HOMING_APPLIED_VOLTAGE = 0.0.volts - val ELEVATOR_GROUND_OFFSET = 0.0.inches - - - val VOLTAGE_COMPENSATION = 12.0.volts - val GEAR_RATIO = 4.0 / 1 * 4.0 / 1 - val SENSOR_CPR = 0 - val SPOOL_DIAMETER = 1.5.inches - - val LEADER_SUPPLY_CURRENT_LIMIT = 0.0.amps - val LEADER_THRESHOLD_CURRENT_LIMIT = 0.0.amps - val LEADER_SUPPLY_TIME_THRESHOLD = 0.0.seconds - val LEADER_STATOR_CURRENT_LIMIT = 0.0.amps - - val FOLLOWER_SUPPLY_TIME_THRESHOLD = 0.0.seconds - val FOLLOWER_STATOR_CURRENT_LIMIT = 0.0.amps - -} \ No newline at end of file + // TODO: Change values later based on CAD + val REAL_KP = 0.0.volts / 1.inches + val REAL_KI = 0.0.volts / (1.inches * 1.seconds) + val REAL_KD = 0.0.volts / (1.inches.perSecond) + + val CARRIAGE_MASS = 20.pounds + + val ELEVATOR_MAX_RETRACTION = 0.0.inches + val ELEVATOR_MAX_EXTENSION = 18.0.inches + + val LEADER_INVERTED = false + val FOLLOWER_INVERTED = true + + val LEADER_KP: ProportionalGain = 0.0.volts / 1.inches + val LEADER_KI: IntegralGain = 0.0.volts / (1.inches * 1.seconds) + val LEADER_KD: DerivativeGain = 0.0.volts / (1.inches.perSecond) + + val FOLLOWER_KP: ProportionalGain = 0.0.volts / 1.inches + val FOLLOWER_KI: IntegralGain = 0.0.volts / (1.inches * 1.seconds) + val FOLLOWER_KD: DerivativeGain = 0.0.volts / (1.inches.perSecond) + + val SIM_KP = 0.0.volts / 1.inches + val SIM_KI = 0.0.volts / (1.inches * 1.seconds) + val SIM_KD = 0.0.volts / (1.inches.perSecond) + + val ELEVATOR_KS = 0.0.volts + val ELEVATOR_KG = 0.0.volts + val ELEVATOR_KV = 0.0.volts / 0.0.inches.perSecond + val ELEVATOR_KA = 0.0.volts / 0.0.inches.perSecond.perSecond + + val ENABLE_ELEVATOR = 1.0 + val ELEVATOR_IDLE_HEIGHT = 0.0.inches + val ELEVATOR_SOFT_LIMIT_EXTENSION = 0.0.inches + val ELEVATOR_SOFT_LIMIT_RETRACTION = 0.0.inches + val ELEVATOR_OPEN_LOOP_SOFT_LIMIT_EXTENSION = 0.0.inches + val ELEVATOR_OPEN_LOOP_SOFT_LIMIT_RETRACTION = 0.0.inches + + val ELEVATOR_TOLERANCE = 0.0.inches + + val MAX_VELOCITY = 0.0.meters.perSecond + val MAX_ACCELERATION = 0.0.meters.perSecond.perSecond + + val SHOOT_SPEAKER_POSITION = 0.0.inches + val SHOOT_AMP_POSITION = 0.0.inches + val SOURCE_NOTE_OFFSET = 0.0.inches + val ELEVATOR_THETA_POS = 0.0.degrees + val HOMING_STATOR_CURRENT = 0.0.amps + val HOMING_STALL_TIME_THRESHOLD = 0.0.seconds + val HOMING_APPLIED_VOLTAGE = 0.0.volts + val ELEVATOR_GROUND_OFFSET = 0.0.inches + + val VOLTAGE_COMPENSATION = 12.0.volts + val GEAR_RATIO = 4.0 / 1 * 4.0 / 1 + val SENSOR_CPR = 0 + val SPOOL_DIAMETER = 1.5.inches + + val LEADER_SUPPLY_CURRENT_LIMIT = 0.0.amps + val LEADER_THRESHOLD_CURRENT_LIMIT = 0.0.amps + val LEADER_SUPPLY_TIME_THRESHOLD = 0.0.seconds + val LEADER_STATOR_CURRENT_LIMIT = 0.0.amps + + val FOLLOWER_SUPPLY_TIME_THRESHOLD = 0.0.seconds + val FOLLOWER_STATOR_CURRENT_LIMIT = 0.0.amps + val FOLLOWER_SUPPLY_CURRENT_LIMIT = 0.0.amps + val FOLLOWER_THRESHOLD_CURRENT_LIMIT = 0.0.amps +} 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 870ddbdc..b94811e9 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 @@ -5,7 +5,6 @@ import com.team4099.robot2023.config.constants.Constants import com.team4099.robot2023.config.constants.DrivetrainConstants import com.team4099.robot2023.config.constants.VisionConstants import com.team4099.robot2023.subsystems.drivetrain.gyro.GyroIO -import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest import com.team4099.robot2023.util.Alert import com.team4099.robot2023.util.FMSData import com.team4099.robot2023.util.PoseEstimator @@ -46,610 +45,616 @@ import org.team4099.lib.units.derived.radians import org.team4099.lib.units.inMetersPerSecond import org.team4099.lib.units.perSecond import java.util.function.Supplier +import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemBase() { - val gyroNotConnectedAlert = - Alert( - "Gyro is not connected, field relative driving will be significantly worse.", - Alert.AlertType.ERROR - ) - - val gyroInputs = GyroIO.GyroIOInputs() - val swerveModules = swerveModuleIOs.getSwerveModules() // FL, FR, BL, BR - - var gyroYawOffset = 0.0.radians - - val closestAlignmentAngle: Angle - get() { - for (angle in -180..90 step 90) { - if ((odometryPose.rotation - angle.degrees).absoluteValue <= 45.degrees) { - return angle.degrees - } - } - return 0.0.degrees + val gyroNotConnectedAlert = + Alert( + "Gyro is not connected, field relative driving will be significantly worse.", + Alert.AlertType.ERROR + ) + + val gyroInputs = GyroIO.GyroIOInputs() + val swerveModules = swerveModuleIOs.getSwerveModules() // FL, FR, BL, BR + + var gyroYawOffset = 0.0.radians + + val closestAlignmentAngle: Angle + get() { + for (angle in -180..90 step 90) { + if ((odometryPose.rotation - angle.degrees).absoluteValue <= 45.degrees) { + return angle.degrees } + } + return 0.0.degrees + } - var canMoveSafely = Supplier { false } - - var elevatorHeightSupplier = Supplier { 0.0.inches } - - init { - - // Wheel speeds - zeroSteering() + var canMoveSafely = Supplier { false } + + var elevatorHeightSupplier = Supplier { 0.0.inches } + + init { + + // Wheel speeds + zeroSteering() + } + + private val frontLeftWheelLocation = + Translation2d( + DrivetrainConstants.DRIVETRAIN_LENGTH / 2, DrivetrainConstants.DRIVETRAIN_WIDTH / 2 + ) + private val frontRightWheelLocation = + Translation2d( + DrivetrainConstants.DRIVETRAIN_LENGTH / 2, -DrivetrainConstants.DRIVETRAIN_WIDTH / 2 + ) + private val backLeftWheelLocation = + Translation2d( + -DrivetrainConstants.DRIVETRAIN_LENGTH / 2, DrivetrainConstants.DRIVETRAIN_WIDTH / 2 + ) + private val backRightWheelLocation = + Translation2d( + -DrivetrainConstants.DRIVETRAIN_LENGTH / 2, -DrivetrainConstants.DRIVETRAIN_WIDTH / 2 + ) + + val moduleTranslations = + listOf( + frontLeftWheelLocation, + frontRightWheelLocation, + backLeftWheelLocation, + backRightWheelLocation + ) + + val swerveDriveKinematics = + SwerveDriveKinematics( + frontLeftWheelLocation.translation2d, + frontRightWheelLocation.translation2d, + backLeftWheelLocation.translation2d, + backRightWheelLocation.translation2d + ) + + var swerveDrivePoseEstimator = PoseEstimator(VecBuilder.fill(0.003, 0.003, 0.00001)) + + var swerveDriveOdometry = + SwerveDriveOdometry( + swerveDriveKinematics, + gyroInputs.gyroYaw.inRotation2ds, + swerveModules.map { it.modulePosition }.toTypedArray() + ) + + var setPointStates = + mutableListOf( + SwerveModuleState(), SwerveModuleState(), SwerveModuleState(), SwerveModuleState() + ) + + var odometryPose: Pose2d + get() = swerveDrivePoseEstimator.getLatestPose() + set(value) { + swerveDrivePoseEstimator.resetPose(value) + + if (RobotBase.isReal()) {} else { + undriftedPose = odometryPose + swerveModules.map { it.inputs.drift = 0.0.meters } // resetting drift to 0 + } } - private val frontLeftWheelLocation = - Translation2d( - DrivetrainConstants.DRIVETRAIN_LENGTH / 2, DrivetrainConstants.DRIVETRAIN_WIDTH / 2 - ) - private val frontRightWheelLocation = - Translation2d( - DrivetrainConstants.DRIVETRAIN_LENGTH / 2, -DrivetrainConstants.DRIVETRAIN_WIDTH / 2 - ) - private val backLeftWheelLocation = - Translation2d( - -DrivetrainConstants.DRIVETRAIN_LENGTH / 2, DrivetrainConstants.DRIVETRAIN_WIDTH / 2 - ) - private val backRightWheelLocation = - Translation2d( - -DrivetrainConstants.DRIVETRAIN_LENGTH / 2, -DrivetrainConstants.DRIVETRAIN_WIDTH / 2 - ) + var rawGyroAngle = odometryPose.rotation - val moduleTranslations = - listOf( - frontLeftWheelLocation, - frontRightWheelLocation, - backLeftWheelLocation, - backRightWheelLocation - ) + var undriftedPose: Pose2d + get() = Pose2d(swerveDriveOdometry.poseMeters) + set(value) { + swerveDriveOdometry.resetPosition( + gyroInputs.gyroYaw.inRotation2ds, + swerveModules.map { it.modulePosition }.toTypedArray(), + value.pose2d + ) + } - val swerveDriveKinematics = - SwerveDriveKinematics( - frontLeftWheelLocation.translation2d, - frontRightWheelLocation.translation2d, - backLeftWheelLocation.translation2d, - backRightWheelLocation.translation2d - ) + var targetPose: Pose2d = Pose2d(0.0.meters, 0.0.meters, 0.0.radians) - var swerveDrivePoseEstimator = PoseEstimator(VecBuilder.fill(0.003, 0.003, 0.00001)) + var drift: Transform2d = Transform2d(Translation2d(), 0.0.radians) - var swerveDriveOdometry = - SwerveDriveOdometry( - swerveDriveKinematics, - gyroInputs.gyroYaw.inRotation2ds, - swerveModules.map { it.modulePosition }.toTypedArray() - ) + var fieldVelocity = Velocity2d(0.0.meters.perSecond, 0.0.meters.perSecond) - var setPointStates = - mutableListOf( - SwerveModuleState(), SwerveModuleState(), SwerveModuleState(), SwerveModuleState() - ) + var robotVelocity = Pair(0.0.meters.perSecond, 0.0.meters.perSecond) - var odometryPose: Pose2d - get() = swerveDrivePoseEstimator.getLatestPose() - set(value) { - swerveDrivePoseEstimator.resetPose(value) + var omegaVelocity = 0.0.radians.perSecond - if (RobotBase.isReal()) {} else { - undriftedPose = odometryPose - swerveModules.map { it.inputs.drift = 0.0.meters } // resetting drift to 0 - } - } + var lastModulePositions = + mutableListOf( + SwerveModulePosition(), + SwerveModulePosition(), + SwerveModulePosition(), + SwerveModulePosition() + ) - var rawGyroAngle = odometryPose.rotation + var lastGyroYaw = 0.0.radians - var undriftedPose: Pose2d - get() = Pose2d(swerveDriveOdometry.poseMeters) - set(value) { - swerveDriveOdometry.resetPosition( - gyroInputs.gyroYaw.inRotation2ds, - swerveModules.map { it.modulePosition }.toTypedArray(), - value.pose2d - ) - } + var velocityTarget = 0.degrees.perSecond - var targetPose: Pose2d = Pose2d(0.0.meters, 0.0.meters, 0.0.radians) + var targetedDriveVector = Pair(0.meters.perSecond, 0.meters.perSecond) - var drift: Transform2d = Transform2d(Translation2d(), 0.0.radians) + var fieldOrientation = true - var fieldVelocity = Velocity2d(0.0.meters.perSecond, 0.0.meters.perSecond) + var targetedChassisSpeeds = edu.wpi.first.math.kinematics.ChassisSpeeds(0.0, 0.0, 0.0) - var robotVelocity = Pair(0.0.meters.perSecond, 0.0.meters.perSecond) + var targetedChassisAccels = edu.wpi.first.math.kinematics.ChassisSpeeds(0.0, 0.0, 0.0) - var omegaVelocity = 0.0.radians.perSecond - - var lastModulePositions = - mutableListOf( - SwerveModulePosition(), - SwerveModulePosition(), - SwerveModulePosition(), - SwerveModulePosition() - ) - - var lastGyroYaw = 0.0.radians - - var velocityTarget = 0.degrees.perSecond + var currentState: DrivetrainState = DrivetrainState.UNINITIALIZED + var currentRequest: DrivetrainRequest = DrivetrainRequest.ZeroSensors() + set(value) { + when (value) { + is DrivetrainRequest.OpenLoop -> { + velocityTarget = value.angularVelocity + targetedDriveVector = value.driveVector + fieldOrientation = value.fieldOriented + } + is DrivetrainRequest.ClosedLoop -> { + targetedChassisSpeeds = value.chassisSpeeds + targetedChassisAccels = value.chassisAccels + } + else -> {} + } + field = value + } - var targetedDriveVector = Pair(0.meters.perSecond, 0.meters.perSecond) + init { + zeroSteering() + } - var fieldOrientation = true + override fun periodic() { + val startTime = Clock.realTimestamp + gyroNotConnectedAlert.set(!gyroInputs.gyroConnected) + gyroIO.updateInputs(gyroInputs) - var targetedChassisSpeeds = edu.wpi.first.math.kinematics.ChassisSpeeds(0.0, 0.0, 0.0) + swerveModules.forEach { it.periodic() } - var targetedChassisAccels = edu.wpi.first.math.kinematics.ChassisSpeeds(0.0, 0.0, 0.0) + // TODO: add logic to reduce setpoint based on elevator/arm position + DrivetrainConstants.DRIVE_SETPOINT_MAX = 15.feet.perSecond - var currentState: DrivetrainState = DrivetrainState.UNINITIALIZED - var currentRequest: DrivetrainRequest = DrivetrainRequest.ZeroSensors() - set(value) { - when (value) { - is DrivetrainRequest.OpenLoop -> { - velocityTarget = value.angularVelocity - targetedDriveVector = value.driveVector - fieldOrientation = value.fieldOriented - } - is DrivetrainRequest.ClosedLoop -> { - targetedChassisSpeeds = value.chassisSpeeds - targetedChassisAccels = value.chassisAccels - } - else -> {} - } - field = value - } + Logger.recordOutput( + "Drivetrain/maxSetpointMetersPerSecond", + DrivetrainConstants.DRIVE_SETPOINT_MAX.inMetersPerSecond + ) - init { - zeroSteering() + // Update field velocity + val measuredStates = arrayOfNulls(4) + for (i in 0..3) { + measuredStates[i] = + SwerveModuleState( + swerveModules[i].inputs.driveVelocity.inMetersPerSecond, + swerveModules[i].inputs.steeringPosition.inRotation2ds + ) + } + val chassisState: ChassisSpeeds = + ChassisSpeeds(swerveDriveKinematics.toChassisSpeeds(*measuredStates)) + val fieldVelCalculated = + Translation2d( + chassisState.vx.inMetersPerSecond.meters, chassisState.vy.inMetersPerSecond.meters + ) + .rotateBy(odometryPose.rotation) // we don't use this but it's there if you want it ig + + robotVelocity = Pair(chassisState.vx, chassisState.vy) + fieldVelocity = Velocity2d(fieldVelCalculated.x.perSecond, fieldVelCalculated.y.perSecond) + + omegaVelocity = chassisState.omega + if (!gyroInputs.gyroConnected) { + gyroInputs.gyroYawRate = omegaVelocity + rawGyroAngle += Constants.Universal.LOOP_PERIOD_TIME * gyroInputs.gyroYawRate + gyroInputs.gyroYaw = rawGyroAngle + gyroYawOffset } - override fun periodic() { - val startTime = Clock.realTimestamp - gyroNotConnectedAlert.set(!gyroInputs.gyroConnected) - gyroIO.updateInputs(gyroInputs) + // updating odometry every loop cycle + updateOdometry() + + Logger.recordOutput("Drivetrain/xVelocityMetersPerSecond", fieldVelocity.x.inMetersPerSecond) + Logger.recordOutput("Drivetrain/yVelocityMetersPerSecond", fieldVelocity.y.inMetersPerSecond) + + Logger.processInputs("Drivetrain/Gyro", gyroInputs) + Logger.recordOutput("Drivetrain/ModuleStates", *measuredStates) + Logger.recordOutput("Drivetrain/SetPointStates", *setPointStates.toTypedArray()) + + Logger.recordOutput( + VisionConstants.POSE_TOPIC_NAME, + doubleArrayOf( + odometryPose.x.inMeters, odometryPose.y.inMeters, odometryPose.rotation.inRadians + ) + ) + Logger.recordOutput( + "Odometry/pose3d", + Pose3d( + odometryPose.x, + odometryPose.y, + 1.0.meters, + Rotation3d(gyroInputs.gyroRoll, gyroInputs.gyroPitch, gyroInputs.gyroYaw) + ) + .pose3d + ) + Logger.recordOutput( + "Odometry/targetPose", + doubleArrayOf(targetPose.x.inMeters, targetPose.y.inMeters, targetPose.rotation.inRadians) + ) + + Logger.recordOutput( + "LoggedRobot/Subsystems/DrivetrainLoopTimeMS", + (Clock.realTimestamp - startTime).inMilliseconds + ) + + var nextState = currentState + + when (currentState) { + DrivetrainState.UNINITIALIZED -> { + // Transitions + nextState = DrivetrainState.ZEROING_SENSORS + } + DrivetrainState.ZEROING_SENSORS -> { + zeroSensors() + + // Transitions + currentRequest = DrivetrainRequest.Idle() + nextState = fromRequestToState(currentRequest) + } + DrivetrainState.OPEN_LOOP -> { + // Outputs + setOpenLoop(velocityTarget, targetedDriveVector, fieldOrientation) + + // Transitions + nextState = fromRequestToState(currentRequest) + } + DrivetrainState.CLOSED_LOOP -> { + // Outputs + setClosedLoop(targetedChassisSpeeds, targetedChassisAccels) + + // Transitions + nextState = fromRequestToState(currentRequest) + } + DrivetrainState.IDLE -> { + nextState = fromRequestToState(currentRequest) + } + } - swerveModules.forEach { it.periodic() } + currentState = nextState + + // Log the current state + Logger.recordOutput("Drivetrain/requestedState", currentState.toString()) + } + + private fun updateOdometry() { + val wheelDeltas = + mutableListOf( + SwerveModulePosition(), + SwerveModulePosition(), + SwerveModulePosition(), + SwerveModulePosition() + ) + for (i in 0 until 4) { + wheelDeltas[i] = + SwerveModulePosition( + swerveModules[i].inputs.drivePosition.inMeters - + lastModulePositions[i].distanceMeters, + swerveModules[i].inputs.steeringPosition.inRotation2ds + ) + lastModulePositions[i] = + SwerveModulePosition( + swerveModules[i].inputs.drivePosition.inMeters, + swerveModules[i].inputs.steeringPosition.inRotation2ds + ) + } - // TODO: add logic to reduce setpoint based on elevator/arm position - DrivetrainConstants.DRIVE_SETPOINT_MAX = 15.feet.perSecond + var driveTwist = swerveDriveKinematics.toTwist2d(*wheelDeltas.toTypedArray()) - Logger.recordOutput( - "Drivetrain/maxSetpointMetersPerSecond", - DrivetrainConstants.DRIVE_SETPOINT_MAX.inMetersPerSecond + if (gyroInputs.gyroConnected) { + driveTwist = + edu.wpi.first.math.geometry.Twist2d( + driveTwist.dx, driveTwist.dy, (gyroInputs.rawGyroYaw - lastGyroYaw).inRadians ) + lastGyroYaw = gyroInputs.rawGyroYaw + } - // Update field velocity - val measuredStates = arrayOfNulls(4) - for (i in 0..3) { - measuredStates[i] = - SwerveModuleState( - swerveModules[i].inputs.driveVelocity.inMetersPerSecond, - swerveModules[i].inputs.steeringPosition.inRotation2ds - ) - } - val chassisState: ChassisSpeeds = - ChassisSpeeds(swerveDriveKinematics.toChassisSpeeds(*measuredStates)) - val fieldVelCalculated = - Translation2d( - chassisState.vx.inMetersPerSecond.meters, chassisState.vy.inMetersPerSecond.meters - ) - .rotateBy(odometryPose.rotation) // we don't use this but it's there if you want it ig - - robotVelocity = Pair(chassisState.vx, chassisState.vy) - fieldVelocity = Velocity2d(fieldVelCalculated.x.perSecond, fieldVelCalculated.y.perSecond) - - omegaVelocity = chassisState.omega - if (!gyroInputs.gyroConnected) { - gyroInputs.gyroYawRate = omegaVelocity - rawGyroAngle += Constants.Universal.LOOP_PERIOD_TIME * gyroInputs.gyroYawRate - gyroInputs.gyroYaw = rawGyroAngle + gyroYawOffset - } + // reversing the drift to store the ground truth pose + if (RobotBase.isSimulation() && Constants.Tuning.SIMULATE_DRIFT) { + val undriftedModules = arrayOfNulls(4) + for (i in 0 until 4) { + undriftedModules[i] = + SwerveModulePosition( + ( + swerveModules[i].modulePosition.distanceMeters.meters - + swerveModules[i].inputs.drift + ) + .inMeters, + swerveModules[i].modulePosition.angle + ) + } + swerveDriveOdometry.update(gyroInputs.gyroYaw.inRotation2ds, undriftedModules) + + drift = undriftedPose.minus(odometryPose) + + Logger.recordOutput(VisionConstants.SIM_POSE_TOPIC_NAME, undriftedPose.pose2d) + } - // updating odometry every loop cycle - updateOdometry() - - Logger.recordOutput("Drivetrain/xVelocityMetersPerSecond", fieldVelocity.x.inMetersPerSecond) - Logger.recordOutput("Drivetrain/yVelocityMetersPerSecond", fieldVelocity.y.inMetersPerSecond) - - Logger.processInputs("Drivetrain/Gyro", gyroInputs) - Logger.recordOutput("Drivetrain/ModuleStates", *measuredStates) - Logger.recordOutput("Drivetrain/SetPointStates", *setPointStates.toTypedArray()) - - Logger.recordOutput(VisionConstants.POSE_TOPIC_NAME, doubleArrayOf(odometryPose.x.inMeters, odometryPose.y.inMeters, odometryPose.rotation.inRadians)) - Logger.recordOutput( - "Odometry/pose3d", - Pose3d( - odometryPose.x, - odometryPose.y, - 1.0.meters, - Rotation3d(gyroInputs.gyroRoll, gyroInputs.gyroPitch, gyroInputs.gyroYaw) - ) - .pose3d + swerveDrivePoseEstimator.addDriveData(Clock.fpgaTime.inSeconds, Twist2d(driveTwist)) + } + + /** + * @param angularVelocity Represents the angular velocity of the chassis + * @param driveVector Pair of linear velocities: First is X vel, second is Y vel + * @param fieldOriented Are the chassis speeds driving relative to field (aka use gyro or not) + */ + fun setOpenLoop( + angularVelocity: AngularVelocity, + driveVector: Pair, + fieldOriented: Boolean = true + ) { + val flipDrive = if (FMSData.allianceColor == DriverStation.Alliance.Red) -1 else 1 + val allianceFlippedDriveVector = + Pair(driveVector.first * flipDrive, driveVector.second * flipDrive) + + val swerveModuleStates: Array + var desiredChassisSpeeds: ChassisSpeeds + + if (fieldOriented) { + Logger.recordOutput("drivetrain/isFieldOriented", true) + desiredChassisSpeeds = + ChassisSpeeds.fromFieldRelativeSpeeds( + allianceFlippedDriveVector.first, + allianceFlippedDriveVector.second, + angularVelocity, + odometryPose.rotation ) - Logger.recordOutput( - "Odometry/targetPose", - doubleArrayOf(targetPose.x.inMeters, targetPose.y.inMeters, targetPose.rotation.inRadians) + } else { + desiredChassisSpeeds = + ChassisSpeeds( + allianceFlippedDriveVector.first, + allianceFlippedDriveVector.second, + angularVelocity, ) + } - Logger.recordOutput( - "LoggedRobot/Subsystems/DrivetrainLoopTimeMS", - (Clock.realTimestamp - startTime).inMilliseconds + if (DrivetrainConstants.MINIMIZE_SKEW) { + val velocityTransform = + Transform2d( + Translation2d( + Constants.Universal.LOOP_PERIOD_TIME * desiredChassisSpeeds.vx, + Constants.Universal.LOOP_PERIOD_TIME * desiredChassisSpeeds.vy + ), + Constants.Universal.LOOP_PERIOD_TIME * desiredChassisSpeeds.omega ) - var nextState = currentState - - when (currentState) { - DrivetrainState.UNINITIALIZED -> { - // Transitions - nextState = DrivetrainState.ZEROING_SENSORS - } - DrivetrainState.ZEROING_SENSORS -> { - zeroSensors() - - // Transitions - currentRequest = DrivetrainRequest.Idle() - nextState = fromRequestToState(currentRequest) - } - DrivetrainState.OPEN_LOOP -> { - // Outputs - setOpenLoop(velocityTarget, targetedDriveVector, fieldOrientation) - - // Transitions - nextState = fromRequestToState(currentRequest) - } - DrivetrainState.CLOSED_LOOP -> { - // Outputs - setClosedLoop(targetedChassisSpeeds, targetedChassisAccels) - - // Transitions - nextState = fromRequestToState(currentRequest) - } - DrivetrainState.IDLE -> { - nextState = fromRequestToState(currentRequest) - } - } + val twistToNextPose: Twist2d = velocityTransform.log() - currentState = nextState - - // Log the current state - Logger.recordOutput("Drivetrain/requestedState", currentState.toString()) + desiredChassisSpeeds = + ChassisSpeeds( + (twistToNextPose.dx / Constants.Universal.LOOP_PERIOD_TIME), + (twistToNextPose.dy / Constants.Universal.LOOP_PERIOD_TIME), + (twistToNextPose.dtheta / Constants.Universal.LOOP_PERIOD_TIME) + ) } - private fun updateOdometry() { - val wheelDeltas = - mutableListOf( - SwerveModulePosition(), - SwerveModulePosition(), - SwerveModulePosition(), - SwerveModulePosition() - ) - for (i in 0 until 4) { - wheelDeltas[i] = - SwerveModulePosition( - swerveModules[i].inputs.drivePosition.inMeters - - lastModulePositions[i].distanceMeters, - swerveModules[i].inputs.steeringPosition.inRotation2ds - ) - lastModulePositions[i] = - SwerveModulePosition( - swerveModules[i].inputs.drivePosition.inMeters, - swerveModules[i].inputs.steeringPosition.inRotation2ds - ) - } + swerveModuleStates = + swerveDriveKinematics.toSwerveModuleStates(desiredChassisSpeeds.chassisSpeedsWPILIB) - var driveTwist = swerveDriveKinematics.toTwist2d(*wheelDeltas.toTypedArray()) - - if (gyroInputs.gyroConnected) { - driveTwist = - edu.wpi.first.math.geometry.Twist2d( - driveTwist.dx, driveTwist.dy, (gyroInputs.rawGyroYaw - lastGyroYaw).inRadians - ) - lastGyroYaw = gyroInputs.rawGyroYaw - } + SwerveDriveKinematics.desaturateWheelSpeeds( + swerveModuleStates, DrivetrainConstants.DRIVE_SETPOINT_MAX.inMetersPerSecond + ) - // reversing the drift to store the ground truth pose - if (RobotBase.isSimulation() && Constants.Tuning.SIMULATE_DRIFT) { - val undriftedModules = arrayOfNulls(4) - for (i in 0 until 4) { - undriftedModules[i] = - SwerveModulePosition( - ( - swerveModules[i].modulePosition.distanceMeters.meters - - swerveModules[i].inputs.drift - ) - .inMeters, - swerveModules[i].modulePosition.angle - ) - } - swerveDriveOdometry.update(gyroInputs.gyroYaw.inRotation2ds, undriftedModules) - - drift = undriftedPose.minus(odometryPose) - - Logger.recordOutput(VisionConstants.SIM_POSE_TOPIC_NAME, undriftedPose.pose2d) - } + setPointStates = swerveModuleStates.toMutableList() - swerveDrivePoseEstimator.addDriveData(Clock.fpgaTime.inSeconds, Twist2d(driveTwist)) + for (moduleIndex in 0 until DrivetrainConstants.WHEEL_COUNT) { + swerveModules[moduleIndex].setPositionOpenLoop(swerveModuleStates[moduleIndex]) } - - /** - * @param angularVelocity Represents the angular velocity of the chassis - * @param driveVector Pair of linear velocities: First is X vel, second is Y vel - * @param fieldOriented Are the chassis speeds driving relative to field (aka use gyro or not) - */ - fun setOpenLoop( - angularVelocity: AngularVelocity, - driveVector: Pair, - fieldOriented: Boolean = true - ) { - val flipDrive = if (FMSData.allianceColor == DriverStation.Alliance.Red) -1 else 1 - val allianceFlippedDriveVector = - Pair(driveVector.first * flipDrive, driveVector.second * flipDrive) - - val swerveModuleStates: Array - var desiredChassisSpeeds: ChassisSpeeds - - if (fieldOriented) { - Logger.recordOutput("drivetrain/isFieldOriented", true) - desiredChassisSpeeds = - ChassisSpeeds.fromFieldRelativeSpeeds( - allianceFlippedDriveVector.first, - allianceFlippedDriveVector.second, - angularVelocity, - odometryPose.rotation - ) - } else { - desiredChassisSpeeds = - ChassisSpeeds( - allianceFlippedDriveVector.first, - allianceFlippedDriveVector.second, - angularVelocity, - ) - } - - if (DrivetrainConstants.MINIMIZE_SKEW) { - val velocityTransform = - Transform2d( - Translation2d( - Constants.Universal.LOOP_PERIOD_TIME * desiredChassisSpeeds.vx, - Constants.Universal.LOOP_PERIOD_TIME * desiredChassisSpeeds.vy - ), - Constants.Universal.LOOP_PERIOD_TIME * desiredChassisSpeeds.omega - ) - - val twistToNextPose: Twist2d = velocityTransform.log() - - desiredChassisSpeeds = - ChassisSpeeds( - (twistToNextPose.dx / Constants.Universal.LOOP_PERIOD_TIME), - (twistToNextPose.dy / Constants.Universal.LOOP_PERIOD_TIME), - (twistToNextPose.dtheta / Constants.Universal.LOOP_PERIOD_TIME) - ) - } - - swerveModuleStates = - swerveDriveKinematics.toSwerveModuleStates(desiredChassisSpeeds.chassisSpeedsWPILIB) - - SwerveDriveKinematics.desaturateWheelSpeeds( - swerveModuleStates, DrivetrainConstants.DRIVE_SETPOINT_MAX.inMetersPerSecond + } + + /** + * Sets the drivetrain to the specified angular and X & Y velocities based on the current angular + * and linear acceleration. Calculates both angular and linear velocities and acceleration and + * calls setPositionClosedLoop for each SwerveModule object. + * + * @param angularVelocity The angular velocity of a specified drive + * @param driveVector.first The linear velocity on the X axis + * @param driveVector.second The linear velocity on the Y axis + * @param angularAcceleration The angular acceleration of a specified drive + * @param driveAcceleration.first The linear acceleration on the X axis + * @param driveAcceleration.second The linear acceleration on the Y axis + * @param fieldOriented Defines whether module states are calculated relative to field + */ + fun setClosedLoop( + angularVelocity: AngularVelocity, + driveVector: Pair, + angularAcceleration: AngularAcceleration = 0.0.radians.perSecond.perSecond, + driveAcceleration: Pair = + Pair(0.0.meters.perSecond.perSecond, 0.0.meters.perSecond.perSecond), + fieldOriented: Boolean = true, + ) { + val velSwerveModuleStates: Array? + val accelSwerveModuleStates: Array? + + if (fieldOriented) { + // Getting velocity and acceleration states from the drive & angular velocity vectors and + // drive & angular acceleration vectors (respectively) + // This is with respect to the field, meaning all velocity and acceleration vectors are + // adjusted to be relative to the field instead of relative to the robot. + velSwerveModuleStates = + swerveDriveKinematics.toSwerveModuleStates( + ChassisSpeeds.fromFieldRelativeSpeeds( + driveVector.first, driveVector.second, angularVelocity, odometryPose.rotation + ) + .chassisSpeedsWPILIB ) - setPointStates = swerveModuleStates.toMutableList() - - for (moduleIndex in 0 until DrivetrainConstants.WHEEL_COUNT) { - swerveModules[moduleIndex].setPositionOpenLoop(swerveModuleStates[moduleIndex]) - } - } - - /** - * Sets the drivetrain to the specified angular and X & Y velocities based on the current angular - * and linear acceleration. Calculates both angular and linear velocities and acceleration and - * calls setPositionClosedLoop for each SwerveModule object. - * - * @param angularVelocity The angular velocity of a specified drive - * @param driveVector.first The linear velocity on the X axis - * @param driveVector.second The linear velocity on the Y axis - * @param angularAcceleration The angular acceleration of a specified drive - * @param driveAcceleration.first The linear acceleration on the X axis - * @param driveAcceleration.second The linear acceleration on the Y axis - * @param fieldOriented Defines whether module states are calculated relative to field - */ - fun setClosedLoop( - angularVelocity: AngularVelocity, - driveVector: Pair, - angularAcceleration: AngularAcceleration = 0.0.radians.perSecond.perSecond, - driveAcceleration: Pair = - Pair(0.0.meters.perSecond.perSecond, 0.0.meters.perSecond.perSecond), - fieldOriented: Boolean = true, - ) { - val velSwerveModuleStates: Array? - val accelSwerveModuleStates: Array? - - if (fieldOriented) { - // Getting velocity and acceleration states from the drive & angular velocity vectors and - // drive & angular acceleration vectors (respectively) - // This is with respect to the field, meaning all velocity and acceleration vectors are - // adjusted to be relative to the field instead of relative to the robot. - velSwerveModuleStates = - swerveDriveKinematics.toSwerveModuleStates( - ChassisSpeeds.fromFieldRelativeSpeeds( - driveVector.first, driveVector.second, angularVelocity, odometryPose.rotation - ) - .chassisSpeedsWPILIB - ) - - // Although this isn't perfect, calculating acceleration states using the same math as - // velocity can get us "good enough" accel states to minimize skew - accelSwerveModuleStates = - swerveDriveKinematics.toSwerveModuleStates( - ChassisAccels.fromFieldRelativeAccels( - driveAcceleration.first, - driveAcceleration.second, - angularAcceleration, - odometryPose.rotation - ) - .chassisAccelsWPILIB - ) - } else { - // Getting velocity and acceleration states from the drive & angular velocity vectors and - // drive & angular acceleration vectors (respectively) - velSwerveModuleStates = - swerveDriveKinematics.toSwerveModuleStates( - ChassisSpeeds(driveVector.first, driveVector.second, angularVelocity) - .chassisSpeedsWPILIB - ) - accelSwerveModuleStates = - swerveDriveKinematics.toSwerveModuleStates( - ChassisAccels(driveAcceleration.first, driveAcceleration.second, angularAcceleration) - .chassisAccelsWPILIB - ) - } - - SwerveDriveKinematics.desaturateWheelSpeeds( - velSwerveModuleStates, DrivetrainConstants.MAX_AUTO_VEL.inMetersPerSecond + // Although this isn't perfect, calculating acceleration states using the same math as + // velocity can get us "good enough" accel states to minimize skew + accelSwerveModuleStates = + swerveDriveKinematics.toSwerveModuleStates( + ChassisAccels.fromFieldRelativeAccels( + driveAcceleration.first, + driveAcceleration.second, + angularAcceleration, + odometryPose.rotation + ) + .chassisAccelsWPILIB + ) + } else { + // Getting velocity and acceleration states from the drive & angular velocity vectors and + // drive & angular acceleration vectors (respectively) + velSwerveModuleStates = + swerveDriveKinematics.toSwerveModuleStates( + ChassisSpeeds(driveVector.first, driveVector.second, angularVelocity) + .chassisSpeedsWPILIB + ) + accelSwerveModuleStates = + swerveDriveKinematics.toSwerveModuleStates( + ChassisAccels(driveAcceleration.first, driveAcceleration.second, angularAcceleration) + .chassisAccelsWPILIB ) - - setPointStates = velSwerveModuleStates.toMutableList() - - // Once we have all of our states obtained for both velocity and acceleration, apply these - // states to each swerve module - for (moduleIndex in 0 until DrivetrainConstants.WHEEL_COUNT) { - swerveModules[moduleIndex].setPositionClosedLoop( - velSwerveModuleStates[moduleIndex], accelSwerveModuleStates[moduleIndex] - ) - } } - fun setClosedLoop( - chassisSpeeds: edu.wpi.first.math.kinematics.ChassisSpeeds, - chassisAccels: edu.wpi.first.math.kinematics.ChassisSpeeds = - edu.wpi.first.math.kinematics.ChassisSpeeds(0.0, 0.0, 0.0) - ) { - var chassisSpeeds = chassisSpeeds - - if (DrivetrainConstants.MINIMIZE_SKEW) { - val velocityTransform = - Transform2d( - Translation2d( - Constants.Universal.LOOP_PERIOD_TIME * - chassisSpeeds.vxMetersPerSecond.meters.perSecond, - Constants.Universal.LOOP_PERIOD_TIME * - chassisSpeeds.vyMetersPerSecond.meters.perSecond - ), - Constants.Universal.LOOP_PERIOD_TIME * - chassisSpeeds.omegaRadiansPerSecond.radians.perSecond - ) - - val twistToNextPose: Twist2d = velocityTransform.log() - - chassisSpeeds = - ChassisSpeeds( - (twistToNextPose.dx / Constants.Universal.LOOP_PERIOD_TIME), - (twistToNextPose.dy / Constants.Universal.LOOP_PERIOD_TIME), - (twistToNextPose.dtheta / Constants.Universal.LOOP_PERIOD_TIME) - ) - .chassisSpeedsWPILIB - } - - val velSwerveModuleStates: Array = - swerveDriveKinematics.toSwerveModuleStates(chassisSpeeds) - val accelSwerveModuleStates: Array = - swerveDriveKinematics.toSwerveModuleStates(chassisAccels) + SwerveDriveKinematics.desaturateWheelSpeeds( + velSwerveModuleStates, DrivetrainConstants.MAX_AUTO_VEL.inMetersPerSecond + ) - SwerveDriveKinematics.desaturateWheelSpeeds( - velSwerveModuleStates, DrivetrainConstants.MAX_AUTO_VEL.inMetersPerSecond - ) - - setPointStates = velSwerveModuleStates.toMutableList() + setPointStates = velSwerveModuleStates.toMutableList() - // Once we have all of our states obtained for both velocity and acceleration, apply these - // states to each swerve module - for (moduleIndex in 0 until DrivetrainConstants.WHEEL_COUNT) { - swerveModules[moduleIndex].setPositionClosedLoop( - velSwerveModuleStates[moduleIndex], accelSwerveModuleStates[moduleIndex] - ) - } + // Once we have all of our states obtained for both velocity and acceleration, apply these + // states to each swerve module + for (moduleIndex in 0 until DrivetrainConstants.WHEEL_COUNT) { + swerveModules[moduleIndex].setPositionClosedLoop( + velSwerveModuleStates[moduleIndex], accelSwerveModuleStates[moduleIndex] + ) } + } + + fun setClosedLoop( + chassisSpeeds: edu.wpi.first.math.kinematics.ChassisSpeeds, + chassisAccels: edu.wpi.first.math.kinematics.ChassisSpeeds = + edu.wpi.first.math.kinematics.ChassisSpeeds(0.0, 0.0, 0.0) + ) { + var chassisSpeeds = chassisSpeeds + + if (DrivetrainConstants.MINIMIZE_SKEW) { + val velocityTransform = + Transform2d( + Translation2d( + Constants.Universal.LOOP_PERIOD_TIME * + chassisSpeeds.vxMetersPerSecond.meters.perSecond, + Constants.Universal.LOOP_PERIOD_TIME * + chassisSpeeds.vyMetersPerSecond.meters.perSecond + ), + Constants.Universal.LOOP_PERIOD_TIME * + chassisSpeeds.omegaRadiansPerSecond.radians.perSecond + ) - fun resetModuleZero() { - swerveModules.forEach { it.resetModuleZero() } - } + val twistToNextPose: Twist2d = velocityTransform.log() - /** Zeros all the sensors on the drivetrain. */ - fun zeroSensors() { - zeroGyroPitch(0.0.degrees) - zeroGyroRoll() - zeroSteering() - zeroDrive() + chassisSpeeds = + ChassisSpeeds( + (twistToNextPose.dx / Constants.Universal.LOOP_PERIOD_TIME), + (twistToNextPose.dy / Constants.Universal.LOOP_PERIOD_TIME), + (twistToNextPose.dtheta / Constants.Universal.LOOP_PERIOD_TIME) + ) + .chassisSpeedsWPILIB } - /** - * Sets the gyroOffset in such a way that when added to the gyro angle it gives back toAngle. - * - * @param toAngle Zeros the gyro to the value - */ - fun zeroGyroYaw(toAngle: Angle = 0.degrees) { - gyroIO.zeroGyroYaw(toAngle) - - swerveDrivePoseEstimator.resetPose(Pose2d(odometryPose.x, odometryPose.y, gyroInputs.gyroYaw)) - - if (RobotBase.isSimulation()) { - swerveDriveOdometry.resetPosition( - toAngle.inRotation2ds, - swerveModules.map { it.modulePosition }.toTypedArray(), - undriftedPose.pose2d - ) - } + val velSwerveModuleStates: Array = + swerveDriveKinematics.toSwerveModuleStates(chassisSpeeds) + val accelSwerveModuleStates: Array = + swerveDriveKinematics.toSwerveModuleStates(chassisAccels) - if (!(gyroInputs.gyroConnected)) { - gyroYawOffset = toAngle - rawGyroAngle - } - } + SwerveDriveKinematics.desaturateWheelSpeeds( + velSwerveModuleStates, DrivetrainConstants.MAX_AUTO_VEL.inMetersPerSecond + ) - fun zeroGyroPitch(toAngle: Angle = 0.0.degrees) { - gyroIO.zeroGyroPitch(toAngle) - } + setPointStates = velSwerveModuleStates.toMutableList() - fun zeroGyroRoll(toAngle: Angle = 0.0.degrees) { - gyroIO.zeroGyroRoll(toAngle) + // Once we have all of our states obtained for both velocity and acceleration, apply these + // states to each swerve module + for (moduleIndex in 0 until DrivetrainConstants.WHEEL_COUNT) { + swerveModules[moduleIndex].setPositionClosedLoop( + velSwerveModuleStates[moduleIndex], accelSwerveModuleStates[moduleIndex] + ) } - - /** Zeros the steering motors for each swerve module. */ - fun zeroSteering() { - swerveModules.forEach { it.zeroSteering() } + } + + fun resetModuleZero() { + swerveModules.forEach { it.resetModuleZero() } + } + + /** Zeros all the sensors on the drivetrain. */ + fun zeroSensors() { + zeroGyroPitch(0.0.degrees) + zeroGyroRoll() + zeroSteering() + zeroDrive() + } + + /** + * Sets the gyroOffset in such a way that when added to the gyro angle it gives back toAngle. + * + * @param toAngle Zeros the gyro to the value + */ + fun zeroGyroYaw(toAngle: Angle = 0.degrees) { + gyroIO.zeroGyroYaw(toAngle) + + swerveDrivePoseEstimator.resetPose(Pose2d(odometryPose.x, odometryPose.y, gyroInputs.gyroYaw)) + + if (RobotBase.isSimulation()) { + swerveDriveOdometry.resetPosition( + toAngle.inRotation2ds, + swerveModules.map { it.modulePosition }.toTypedArray(), + undriftedPose.pose2d + ) } - /** Zeros the drive motors for each swerve module. */ - private fun zeroDrive() { - swerveModules.forEach { it.zeroDrive() } + if (!(gyroInputs.gyroConnected)) { + gyroYawOffset = toAngle - rawGyroAngle } - - fun addVisionData(visionData: List) { - swerveDrivePoseEstimator.addVisionData(visionData) + } + + fun zeroGyroPitch(toAngle: Angle = 0.0.degrees) { + gyroIO.zeroGyroPitch(toAngle) + } + + fun zeroGyroRoll(toAngle: Angle = 0.0.degrees) { + gyroIO.zeroGyroRoll(toAngle) + } + + /** Zeros the steering motors for each swerve module. */ + fun zeroSteering() { + swerveModules.forEach { it.zeroSteering() } + } + + /** Zeros the drive motors for each swerve module. */ + private fun zeroDrive() { + swerveModules.forEach { it.zeroDrive() } + } + + fun addVisionData(visionData: List) { + swerveDrivePoseEstimator.addVisionData(visionData) + } + + companion object { + enum class DrivetrainState { + UNINITIALIZED, + IDLE, + ZEROING_SENSORS, + OPEN_LOOP, + CLOSED_LOOP; + + inline fun equivalentToRequest(request: DrivetrainRequest): Boolean { + return ( + (request is DrivetrainRequest.ZeroSensors && this == ZEROING_SENSORS) || + (request is DrivetrainRequest.OpenLoop && this == OPEN_LOOP) || + (request is DrivetrainRequest.ClosedLoop && this == CLOSED_LOOP) || + (request is DrivetrainRequest.Idle && this == IDLE) + ) + } } - companion object { - enum class DrivetrainState { - UNINITIALIZED, - IDLE, - ZEROING_SENSORS, - OPEN_LOOP, - CLOSED_LOOP; - - inline fun equivalentToRequest(request: DrivetrainRequest): Boolean { - return ( - (request is DrivetrainRequest.ZeroSensors && this == ZEROING_SENSORS) || - (request is DrivetrainRequest.OpenLoop && this == OPEN_LOOP) || - (request is DrivetrainRequest.ClosedLoop && this == CLOSED_LOOP) || - (request is DrivetrainRequest.Idle && this == IDLE) - ) - } - } - - inline fun fromRequestToState(request: DrivetrainRequest): DrivetrainState { - return when (request) { - is DrivetrainRequest.OpenLoop -> DrivetrainState.OPEN_LOOP - is DrivetrainRequest.ClosedLoop -> DrivetrainState.CLOSED_LOOP - is DrivetrainRequest.ZeroSensors -> DrivetrainState.ZEROING_SENSORS - is DrivetrainRequest.Idle -> DrivetrainState.IDLE - } - } + inline fun fromRequestToState(request: DrivetrainRequest): DrivetrainState { + return when (request) { + is DrivetrainRequest.OpenLoop -> DrivetrainState.OPEN_LOOP + is DrivetrainRequest.ClosedLoop -> DrivetrainState.CLOSED_LOOP + is DrivetrainRequest.ZeroSensors -> DrivetrainState.ZEROING_SENSORS + is DrivetrainRequest.Idle -> DrivetrainState.IDLE + } } -} \ No newline at end of file + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/DrivetrainIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/DrivetrainIO.kt index 8ffd9987..419b53e4 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/DrivetrainIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/DrivetrainIO.kt @@ -4,24 +4,24 @@ import com.team4099.robot2023.subsystems.drivetrain.swervemodule.SwerveModule import com.team4099.robot2023.subsystems.drivetrain.swervemodule.SwerveModuleIO interface DrivetrainIO { - fun getSwerveModules(): List { - return listOf( - SwerveModule( - object : SwerveModuleIO { - override val label = "Front Left Wheel" - }), - SwerveModule( - object : SwerveModuleIO { - override val label = "Front Right Wheel" - }), - SwerveModule( - object : SwerveModuleIO { - override val label = "Back Left Wheel" - }), - SwerveModule( - object : SwerveModuleIO { - override val label = "Back Right Wheel" - }) - ) - } -} \ No newline at end of file + fun getSwerveModules(): List { + return listOf( + SwerveModule( + object : SwerveModuleIO { + override val label = "Front Left Wheel" + }), + SwerveModule( + object : SwerveModuleIO { + override val label = "Front Right Wheel" + }), + SwerveModule( + object : SwerveModuleIO { + override val label = "Back Left Wheel" + }), + SwerveModule( + object : SwerveModuleIO { + override val label = "Back Right Wheel" + }) + ) + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/DrivetrainIOSim.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/DrivetrainIOSim.kt index 25e40a71..b77281b3 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/DrivetrainIOSim.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/DrivetrainIOSim.kt @@ -4,12 +4,12 @@ import com.team4099.robot2023.subsystems.drivetrain.swervemodule.SwerveModule import com.team4099.robot2023.subsystems.drivetrain.swervemodule.SwerveModuleIOSim object DrivetrainIOSim : DrivetrainIO { - override fun getSwerveModules(): List { - return listOf( - SwerveModule(SwerveModuleIOSim("Front Left Wheel")), - SwerveModule(SwerveModuleIOSim("Front Right Wheel")), - SwerveModule(SwerveModuleIOSim("Back Left Wheel")), - SwerveModule(SwerveModuleIOSim("Back Right Wheel")) - ) - } -} \ No newline at end of file + override fun getSwerveModules(): List { + return listOf( + SwerveModule(SwerveModuleIOSim("Front Left Wheel")), + SwerveModule(SwerveModuleIOSim("Front Right Wheel")), + SwerveModule(SwerveModuleIOSim("Back Left Wheel")), + SwerveModule(SwerveModuleIOSim("Back Right Wheel")) + ) + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/gyro/GyroIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/gyro/GyroIO.kt index 481e3875..89cbd4d9 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/gyro/GyroIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/gyro/GyroIO.kt @@ -10,52 +10,52 @@ import org.team4099.lib.units.inDegreesPerSecond import org.team4099.lib.units.perSecond interface GyroIO { - class GyroIOInputs : LoggableInputs { - var rawGyroYaw = 0.0.radians - var gyroYaw = 0.0.radians - var gyroPitch = -3.degrees - var gyroRoll = 0.0.radians - var gyroYawRate = 0.0.radians.perSecond - var gyroPitchRate = 0.0.radians.perSecond - var gyroRollRate = 0.0.radians.perSecond - - var odometryYawPositions = arrayOf() - - var gyroConnected = false - - override fun toLog(table: LogTable?) { - table?.put("rawGyroYawDegrees", rawGyroYaw.inDegrees) - table?.put("gyroYawAngleDegrees", gyroYaw.inDegrees) - table?.put("gyroPitchAngleDegrees", gyroPitch.inDegrees) - table?.put("gyroRollAngleDegrees", gyroRoll.inDegrees) - table?.put("gyroYawRateDegreesPerSecond", gyroYawRate.inDegreesPerSecond) - table?.put("gyroPitchRateDegreesPerSecond", gyroPitchRate.inDegreesPerSecond) - table?.put("gyroRollRateDegreesPerSecond", gyroRollRate.inDegreesPerSecond) - table?.put("gyroConnected", gyroConnected) - } - - override fun fromLog(table: LogTable?) { - table?.get("rawGyroYawDegrees", rawGyroYaw.inDegrees)?.let { rawGyroYaw = it.degrees } - table?.get("gyroYawAngleDegrees", gyroYaw.inDegrees)?.let { gyroYaw = it.degrees } - table?.get("gyroPitchDegrees", gyroPitch.inDegrees)?.let { gyroPitch = it.degrees } - table?.get("gyroRollDegrees", gyroRoll.inDegrees)?.let { gyroRoll = it.degrees } - table?.get("gyroYawRateDegreesPerSecond", gyroYawRate.inDegreesPerSecond)?.let { - gyroYawRate = it.degrees.perSecond - } - table?.get("gyroPitchRateDegreesPerSecond", gyroPitchRate.inDegreesPerSecond)?.let { - gyroPitchRate = it.degrees.perSecond - } - table?.get("gyroRollRateDegreesPerSecond", gyroRollRate.inDegreesPerSecond)?.let { - gyroRollRate = it.degrees.perSecond - } - table?.getBoolean("gyroConnected", gyroConnected)?.let { gyroConnected = it } - } + class GyroIOInputs : LoggableInputs { + var rawGyroYaw = 0.0.radians + var gyroYaw = 0.0.radians + var gyroPitch = -3.degrees + var gyroRoll = 0.0.radians + var gyroYawRate = 0.0.radians.perSecond + var gyroPitchRate = 0.0.radians.perSecond + var gyroRollRate = 0.0.radians.perSecond + + var odometryYawPositions = arrayOf() + + var gyroConnected = false + + override fun toLog(table: LogTable?) { + table?.put("rawGyroYawDegrees", rawGyroYaw.inDegrees) + table?.put("gyroYawAngleDegrees", gyroYaw.inDegrees) + table?.put("gyroPitchAngleDegrees", gyroPitch.inDegrees) + table?.put("gyroRollAngleDegrees", gyroRoll.inDegrees) + table?.put("gyroYawRateDegreesPerSecond", gyroYawRate.inDegreesPerSecond) + table?.put("gyroPitchRateDegreesPerSecond", gyroPitchRate.inDegreesPerSecond) + table?.put("gyroRollRateDegreesPerSecond", gyroRollRate.inDegreesPerSecond) + table?.put("gyroConnected", gyroConnected) } - fun updateInputs(inputs: GyroIOInputs) {} - fun zeroGyroYaw(toAngle: Angle) {} + override fun fromLog(table: LogTable?) { + table?.get("rawGyroYawDegrees", rawGyroYaw.inDegrees)?.let { rawGyroYaw = it.degrees } + table?.get("gyroYawAngleDegrees", gyroYaw.inDegrees)?.let { gyroYaw = it.degrees } + table?.get("gyroPitchDegrees", gyroPitch.inDegrees)?.let { gyroPitch = it.degrees } + table?.get("gyroRollDegrees", gyroRoll.inDegrees)?.let { gyroRoll = it.degrees } + table?.get("gyroYawRateDegreesPerSecond", gyroYawRate.inDegreesPerSecond)?.let { + gyroYawRate = it.degrees.perSecond + } + table?.get("gyroPitchRateDegreesPerSecond", gyroPitchRate.inDegreesPerSecond)?.let { + gyroPitchRate = it.degrees.perSecond + } + table?.get("gyroRollRateDegreesPerSecond", gyroRollRate.inDegreesPerSecond)?.let { + gyroRollRate = it.degrees.perSecond + } + table?.getBoolean("gyroConnected", gyroConnected)?.let { gyroConnected = it } + } + } + fun updateInputs(inputs: GyroIOInputs) {} + + fun zeroGyroYaw(toAngle: Angle) {} - fun zeroGyroPitch(toAngle: Angle) {} + fun zeroGyroPitch(toAngle: Angle) {} - fun zeroGyroRoll(toAngle: Angle) {} -} \ No newline at end of file + fun zeroGyroRoll(toAngle: Angle) {} +} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/gyro/GyroIONavx.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/gyro/GyroIONavx.kt index c08c5395..06fd482b 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/gyro/GyroIONavx.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/gyro/GyroIONavx.kt @@ -11,77 +11,77 @@ import org.team4099.lib.units.perSecond import kotlin.math.IEEErem object GyroIONavx : GyroIO { - private val gyro = AHRS(SerialPort.Port.kMXP) + private val gyro = AHRS(SerialPort.Port.kMXP) - init { - gyro.zeroYaw() - } + init { + gyro.zeroYaw() + } - var gyroYawOffset: Angle = 0.0.degrees - var gyroPitchOffset: Angle = 0.0.degrees - var gyroRollOffset: Angle = 0.0.degrees + var gyroYawOffset: Angle = 0.0.degrees + var gyroPitchOffset: Angle = 0.0.degrees + var gyroRollOffset: Angle = 0.0.degrees - /** The current angle of the drivetrain. */ - val gyroYaw: Angle - get() { - if (gyro.isConnected) { - var rawYaw = gyro.angle + gyroYawOffset.inDegrees - rawYaw += DrivetrainConstants.GYRO_RATE_COEFFICIENT * gyro.rate - return rawYaw.IEEErem(360.0).degrees - } else { - return (-1.337).degrees - } - } + /** The current angle of the drivetrain. */ + val gyroYaw: Angle + get() { + if (gyro.isConnected) { + var rawYaw = gyro.angle + gyroYawOffset.inDegrees + rawYaw += DrivetrainConstants.GYRO_RATE_COEFFICIENT * gyro.rate + return rawYaw.IEEErem(360.0).degrees + } else { + return (-1.337).degrees + } + } - val gyroPitch: Angle - get() { - if (gyro.isConnected) { - val rawPitch = gyro.pitch + gyroPitchOffset.inDegrees - return rawPitch.IEEErem(360.0).degrees - } else { - return -1.337.degrees - } - } + val gyroPitch: Angle + get() { + if (gyro.isConnected) { + val rawPitch = gyro.pitch + gyroPitchOffset.inDegrees + return rawPitch.IEEErem(360.0).degrees + } else { + return -1.337.degrees + } + } - val gyroRoll: Angle - get() { - if (gyro.isConnected) { - val rawRoll = gyro.roll + gyroRollOffset.inDegrees - return rawRoll.IEEErem(360.0).degrees - } else { - return -1.337.degrees - } - } + val gyroRoll: Angle + get() { + if (gyro.isConnected) { + val rawRoll = gyro.roll + gyroRollOffset.inDegrees + return rawRoll.IEEErem(360.0).degrees + } else { + return -1.337.degrees + } + } - val gyroYawRate: AngularVelocity - get() { - if (gyro.isConnected) { - return gyro.rate.degrees.perSecond - } else { - return -1.337.degrees.perSecond - } - } + val gyroYawRate: AngularVelocity + get() { + if (gyro.isConnected) { + return gyro.rate.degrees.perSecond + } else { + return -1.337.degrees.perSecond + } + } - override fun updateInputs(inputs: GyroIO.GyroIOInputs) { - inputs.rawGyroYaw = gyro.angle.degrees - inputs.gyroYaw = gyro.angle.degrees - inputs.gyroYaw = gyroYaw - inputs.gyroYawRate = gyroYawRate - inputs.gyroPitch = gyroPitch - inputs.gyroRoll = gyroRoll + override fun updateInputs(inputs: GyroIO.GyroIOInputs) { + inputs.rawGyroYaw = gyro.angle.degrees + inputs.gyroYaw = gyro.angle.degrees + inputs.gyroYaw = gyroYaw + inputs.gyroYawRate = gyroYawRate + inputs.gyroPitch = gyroPitch + inputs.gyroRoll = gyroRoll - inputs.gyroConnected = gyro.isConnected - } + inputs.gyroConnected = gyro.isConnected + } - override fun zeroGyroYaw(toAngle: Angle) { - gyroYawOffset = (toAngle.inDegrees - gyro.angle).degrees - } + override fun zeroGyroYaw(toAngle: Angle) { + gyroYawOffset = (toAngle.inDegrees - gyro.angle).degrees + } - override fun zeroGyroPitch(toAngle: Angle) { - gyroPitchOffset = (toAngle.inDegrees - gyro.pitch).degrees - } + override fun zeroGyroPitch(toAngle: Angle) { + gyroPitchOffset = (toAngle.inDegrees - gyro.pitch).degrees + } - override fun zeroGyroRoll(toAngle: Angle) { - gyroRollOffset = (toAngle.inDegrees - gyro.roll).degrees - } -} \ No newline at end of file + override fun zeroGyroRoll(toAngle: Angle) { + gyroRollOffset = (toAngle.inDegrees - gyro.roll).degrees + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/servemodule/SwerveModule.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/servemodule/SwerveModule.kt index 77060f32..f43e426f 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/servemodule/SwerveModule.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/servemodule/SwerveModule.kt @@ -12,265 +12,282 @@ import org.team4099.lib.units.base.feet import org.team4099.lib.units.base.inMeters import org.team4099.lib.units.base.meters import org.team4099.lib.units.base.seconds -import org.team4099.lib.units.derived.* +import org.team4099.lib.units.derived.Angle +import org.team4099.lib.units.derived.angle +import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.inRadians +import org.team4099.lib.units.derived.inRotation2ds +import org.team4099.lib.units.derived.inVoltsPerDegree +import org.team4099.lib.units.derived.inVoltsPerDegreePerSecond +import org.team4099.lib.units.derived.inVoltsPerDegreeSeconds +import org.team4099.lib.units.derived.inVoltsPerMeters +import org.team4099.lib.units.derived.inVoltsPerMetersPerSecond +import org.team4099.lib.units.derived.inVoltsPerMetersPerSecondPerSecond +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.perMeterPerSecond +import org.team4099.lib.units.derived.perMeterPerSecondPerSecond +import org.team4099.lib.units.derived.radians +import org.team4099.lib.units.derived.volts import org.team4099.lib.units.perSecond import kotlin.math.IEEErem import kotlin.math.withSign class SwerveModule(val io: SwerveModuleIO) { - val inputs = SwerveModuleIO.SwerveModuleIOInputs() - - var modulePosition = SwerveModulePosition() - - private var speedSetPoint: LinearVelocity = 0.feet.perSecond - private var accelerationSetPoint: LinearAcceleration = 0.feet.perSecond.perSecond - - private var steeringSetPoint: Angle = 0.degrees - - private var shouldInvert = false - - private val steeringkP = - LoggedTunableValue( - "Drivetrain/moduleSteeringkP", Pair({ it.inVoltsPerDegree }, { it.volts.perDegree }) - ) - private val steeringkI = - LoggedTunableValue( - "Drivetrain/moduleSteeringkI", - Pair({ it.inVoltsPerDegreeSeconds }, { it.volts.perDegreeSeconds }) - ) - private val steeringkD = - LoggedTunableValue( - "Drivetrain/moduleSteeringkD", - Pair({ it.inVoltsPerDegreePerSecond }, { it.volts.perDegreePerSecond }) - ) - - private val steeringMaxVel = - LoggedTunableValue( - "Drivetrain/moduleSteeringMaxVelRadPerSec", DrivetrainConstants.STEERING_VEL_MAX - ) - private val steeringMaxAccel = - LoggedTunableValue( - "Drivetrain/moduleSteeringMaxAccelRadPerSecSq", DrivetrainConstants.STEERING_ACCEL_MAX - ) - - private val drivekP = - LoggedTunableValue( - "Drivetrain/moduleDrivekP", - Pair({ it.inVoltsPerMetersPerSecond }, { it.volts.perMeterPerSecond }) - ) - - private val drivekI = - LoggedTunableValue( - "Drivetrain/moduleDrivekI", - Pair({ it.inVoltsPerMeters }, { it.volts / (1.meters.perSecond * 1.seconds) }) - ) - - private val drivekD = - LoggedTunableValue( - "Drivetrain/moduleDrivekD", - Pair({ it.inVoltsPerMetersPerSecondPerSecond }, { it.volts.perMeterPerSecondPerSecond }) - ) - - init { - if (isReal()) { - steeringkP.initDefault(DrivetrainConstants.PID.STEERING_KP) - steeringkI.initDefault(DrivetrainConstants.PID.STEERING_KI) - steeringkD.initDefault(DrivetrainConstants.PID.STEERING_KD) - - drivekP.initDefault(DrivetrainConstants.PID.DRIVE_KP) - drivekI.initDefault(DrivetrainConstants.PID.DRIVE_KI) - drivekD.initDefault(DrivetrainConstants.PID.DRIVE_KD) - } else { - steeringkP.initDefault(DrivetrainConstants.PID.SIM_STEERING_KP) - steeringkI.initDefault(DrivetrainConstants.PID.SIM_STEERING_KI) - steeringkD.initDefault(DrivetrainConstants.PID.SIM_STEERING_KD) - - drivekP.initDefault(DrivetrainConstants.PID.SIM_DRIVE_KP) - drivekI.initDefault(DrivetrainConstants.PID.SIM_DRIVE_KI) - drivekD.initDefault(DrivetrainConstants.PID.SIM_DRIVE_KD) - } + val inputs = SwerveModuleIO.SwerveModuleIOInputs() + + var modulePosition = SwerveModulePosition() + + private var speedSetPoint: LinearVelocity = 0.feet.perSecond + private var accelerationSetPoint: LinearAcceleration = 0.feet.perSecond.perSecond + + private var steeringSetPoint: Angle = 0.degrees + + private var shouldInvert = false + + private val steeringkP = + LoggedTunableValue( + "Drivetrain/moduleSteeringkP", Pair({ it.inVoltsPerDegree }, { it.volts.perDegree }) + ) + private val steeringkI = + LoggedTunableValue( + "Drivetrain/moduleSteeringkI", + Pair({ it.inVoltsPerDegreeSeconds }, { it.volts.perDegreeSeconds }) + ) + private val steeringkD = + LoggedTunableValue( + "Drivetrain/moduleSteeringkD", + Pair({ it.inVoltsPerDegreePerSecond }, { it.volts.perDegreePerSecond }) + ) + + private val steeringMaxVel = + LoggedTunableValue( + "Drivetrain/moduleSteeringMaxVelRadPerSec", DrivetrainConstants.STEERING_VEL_MAX + ) + private val steeringMaxAccel = + LoggedTunableValue( + "Drivetrain/moduleSteeringMaxAccelRadPerSecSq", DrivetrainConstants.STEERING_ACCEL_MAX + ) + + private val drivekP = + LoggedTunableValue( + "Drivetrain/moduleDrivekP", + Pair({ it.inVoltsPerMetersPerSecond }, { it.volts.perMeterPerSecond }) + ) + + private val drivekI = + LoggedTunableValue( + "Drivetrain/moduleDrivekI", + Pair({ it.inVoltsPerMeters }, { it.volts / (1.meters.perSecond * 1.seconds) }) + ) + + private val drivekD = + LoggedTunableValue( + "Drivetrain/moduleDrivekD", + Pair({ it.inVoltsPerMetersPerSecondPerSecond }, { it.volts.perMeterPerSecondPerSecond }) + ) + + init { + if (isReal()) { + steeringkP.initDefault(DrivetrainConstants.PID.STEERING_KP) + steeringkI.initDefault(DrivetrainConstants.PID.STEERING_KI) + steeringkD.initDefault(DrivetrainConstants.PID.STEERING_KD) + + drivekP.initDefault(DrivetrainConstants.PID.DRIVE_KP) + drivekI.initDefault(DrivetrainConstants.PID.DRIVE_KI) + drivekD.initDefault(DrivetrainConstants.PID.DRIVE_KD) + } else { + steeringkP.initDefault(DrivetrainConstants.PID.SIM_STEERING_KP) + steeringkI.initDefault(DrivetrainConstants.PID.SIM_STEERING_KI) + steeringkD.initDefault(DrivetrainConstants.PID.SIM_STEERING_KD) + + drivekP.initDefault(DrivetrainConstants.PID.SIM_DRIVE_KP) + drivekI.initDefault(DrivetrainConstants.PID.SIM_DRIVE_KI) + drivekD.initDefault(DrivetrainConstants.PID.SIM_DRIVE_KD) } + } - fun periodic() { - io.updateInputs(inputs) - - // Updating SwerveModulePosition every loop cycle - modulePosition.distanceMeters = inputs.drivePosition.inMeters - modulePosition.angle = inputs.steeringPosition.inRotation2ds - - if (steeringkP.hasChanged() || steeringkI.hasChanged() || steeringkD.hasChanged()) { - io.configureSteeringPID(steeringkP.get(), steeringkI.get(), steeringkD.get()) - } - - if (steeringMaxVel.hasChanged() || steeringMaxAccel.hasChanged()) { - io.configureSteeringMotionMagic(steeringMaxVel.get(), steeringMaxAccel.get()) - } - - if (drivekP.hasChanged() || drivekI.hasChanged() || drivekD.hasChanged()) { - io.configureDrivePID(drivekP.get(), drivekI.get(), drivekD.get()) - } - - Logger.processInputs(io.label, inputs) - // Logger.getInstance() - // .recordOutput( - // "${io.label}/driveSpeedSetpointMetersPerSecond", - // if (!shouldInvert) speedSetPoint.inMetersPerSecond - // else -speedSetPoint.inMetersPerSecond - // ) - // Logger.getInstance() - // .recordOutput( - // "${io.label}/driveAccelSetpointMetersPerSecondSq", - // accelerationSetPoint.inMetersPerSecondPerSecond - // ) - // Logger.getInstance() - // .recordOutput("${io.label}/steeringSetpointDegrees", steeringSetPoint.inDegrees) - // Logger.getInstance() - // .recordOutput( - // "${io.label}/steeringValueDegreesWithMod", - // inputs.steeringPosition.inDegrees.IEEErem(360.0) - // ) - } + fun periodic() { + io.updateInputs(inputs) - /** - * Sets the swerve module to the specified angular and X & Y velocities using feed forward. - * - * @param steering The angular position desired for the swerve module to be set to - * @param speed The speed desired for the swerve module to reach - * @param acceleration The linear acceleration used to calculate how to reach the desired speed - */ - fun set( - steering: Angle, - speed: LinearVelocity, - acceleration: LinearAcceleration = 0.0.meters.perSecond.perSecond, - optimize: Boolean = true - ) { - if (speed == 0.feet.perSecond) { - io.setOpenLoop(steeringSetPoint, 0.0.meters.perSecond) - return - } - var steeringDifference = - (steering - inputs.steeringPosition).inRadians.IEEErem(2 * Math.PI).radians - - shouldInvert = steeringDifference.absoluteValue > (Math.PI / 2).radians && optimize - if (shouldInvert) { - steeringDifference -= Math.PI.withSign(steeringDifference.inRadians).radians - } - - speedSetPoint = - if (shouldInvert) { - speed * -1 - } else { - speed - } - accelerationSetPoint = - if (shouldInvert) { - acceleration * -1 - } else { - acceleration - } - steeringSetPoint = inputs.steeringPosition + steeringDifference - - // io.setClosedLoop(steeringSetPoint, speedSetPoint, accelerationSetPoint) - io.setClosedLoop(steeringSetPoint, speedSetPoint, accelerationSetPoint) - } + // Updating SwerveModulePosition every loop cycle + modulePosition.distanceMeters = inputs.drivePosition.inMeters + modulePosition.angle = inputs.steeringPosition.inRotation2ds - fun setOpenLoop(steering: Angle, speed: LinearVelocity, optimize: Boolean = true) { - var steeringDifference = - (steering - inputs.steeringPosition).inRadians.IEEErem(2 * Math.PI).radians - - shouldInvert = steeringDifference.absoluteValue > (Math.PI / 2).radians && optimize - if (shouldInvert) { - steeringDifference -= Math.PI.withSign(steeringDifference.inRadians).radians - } - - val outputSpeed = - if (shouldInvert) { - speed * -1 - } else { - speed - } - steeringSetPoint = inputs.steeringPosition + steeringDifference - io.setOpenLoop(steeringSetPoint, outputSpeed) + if (steeringkP.hasChanged() || steeringkI.hasChanged() || steeringkD.hasChanged()) { + io.configureSteeringPID(steeringkP.get(), steeringkI.get(), steeringkD.get()) } - /** - * Sets the swerve module to the specified angular and X & Y velocities using open loop control. - * - * @param desiredState The desired SwerveModuleState. Contains desired angle as well as X and Y - * velocities - */ - fun setPositionOpenLoop(desiredState: SwerveModuleState, optimize: Boolean = true) { - if (optimize) { - val optimizedState = - SwerveModuleState.optimize(desiredState, inputs.steeringPosition.inRotation2ds) - io.setOpenLoop( - optimizedState.angle.angle, - optimizedState - .speedMetersPerSecond - .meters - .perSecond // consider desaturating wheel speeds here if it doesn't work - // from drivetrain - ) - Logger.recordOutput("${io.label}/steeringSetpointOptimized", optimizedState.angle.degrees) - } else { - io.setOpenLoop(desiredState.angle.angle, desiredState.speedMetersPerSecond.meters.perSecond) - Logger.recordOutput("${io.label}/steeringSetpointNonOptimized", desiredState.angle.degrees) - } + if (steeringMaxVel.hasChanged() || steeringMaxAccel.hasChanged()) { + io.configureSteeringMotionMagic(steeringMaxVel.get(), steeringMaxAccel.get()) } - /** - * Sets the swerve module to the specified angular and X & Y velocities using feed forward. - * - * @param desiredVelState The desired SwerveModuleState. Contains desired angle as well as X and Y - * velocities - * @param desiredAccelState The desired SwerveModuleState that contains desired acceleration - * vectors. - * @param optimize Whether velocity and acceleration vectors should be optimized (if possible) - */ - fun setPositionClosedLoop( - desiredVelState: SwerveModuleState, - desiredAccelState: SwerveModuleState, - optimize: Boolean = true - ) { - if (optimize) { - val optimizedVelState = - SwerveModuleState.optimize(desiredVelState, inputs.steeringPosition.inRotation2ds) - val optimizedAccelState = - SwerveModuleState.optimize(desiredAccelState, inputs.steeringPosition.inRotation2ds) - io.setClosedLoop( - optimizedVelState.angle.angle, - optimizedVelState.speedMetersPerSecond.meters.perSecond, - optimizedAccelState.speedMetersPerSecond.meters.perSecond.perSecond - ) - } else { - io.setClosedLoop( - desiredVelState.angle.angle, - desiredVelState.speedMetersPerSecond.meters.perSecond, - desiredAccelState.speedMetersPerSecond.meters.perSecond.perSecond - ) - } + if (drivekP.hasChanged() || drivekI.hasChanged() || drivekD.hasChanged()) { + io.configureDrivePID(drivekP.get(), drivekI.get(), drivekD.get()) } - /** Creates event of the current potentiometer value as needs to be manually readjusted. */ - fun resetModuleZero() { - io.resetModuleZero() + Logger.processInputs(io.label, inputs) + // Logger.getInstance() + // .recordOutput( + // "${io.label}/driveSpeedSetpointMetersPerSecond", + // if (!shouldInvert) speedSetPoint.inMetersPerSecond + // else -speedSetPoint.inMetersPerSecond + // ) + // Logger.getInstance() + // .recordOutput( + // "${io.label}/driveAccelSetpointMetersPerSecondSq", + // accelerationSetPoint.inMetersPerSecondPerSecond + // ) + // Logger.getInstance() + // .recordOutput("${io.label}/steeringSetpointDegrees", steeringSetPoint.inDegrees) + // Logger.getInstance() + // .recordOutput( + // "${io.label}/steeringValueDegreesWithMod", + // inputs.steeringPosition.inDegrees.IEEErem(360.0) + // ) + } + + /** + * Sets the swerve module to the specified angular and X & Y velocities using feed forward. + * + * @param steering The angular position desired for the swerve module to be set to + * @param speed The speed desired for the swerve module to reach + * @param acceleration The linear acceleration used to calculate how to reach the desired speed + */ + fun set( + steering: Angle, + speed: LinearVelocity, + acceleration: LinearAcceleration = 0.0.meters.perSecond.perSecond, + optimize: Boolean = true + ) { + if (speed == 0.feet.perSecond) { + io.setOpenLoop(steeringSetPoint, 0.0.meters.perSecond) + return } + var steeringDifference = + (steering - inputs.steeringPosition).inRadians.IEEErem(2 * Math.PI).radians - /** Zeros the steering motor */ - fun zeroSteering() { - io.zeroSteering() + shouldInvert = steeringDifference.absoluteValue > (Math.PI / 2).radians && optimize + if (shouldInvert) { + steeringDifference -= Math.PI.withSign(steeringDifference.inRadians).radians } - /** Zeros the drive motor */ - fun zeroDrive() { - io.zeroDrive() + speedSetPoint = + if (shouldInvert) { + speed * -1 + } else { + speed + } + accelerationSetPoint = + if (shouldInvert) { + acceleration * -1 + } else { + acceleration + } + steeringSetPoint = inputs.steeringPosition + steeringDifference + + // io.setClosedLoop(steeringSetPoint, speedSetPoint, accelerationSetPoint) + io.setClosedLoop(steeringSetPoint, speedSetPoint, accelerationSetPoint) + } + + fun setOpenLoop(steering: Angle, speed: LinearVelocity, optimize: Boolean = true) { + var steeringDifference = + (steering - inputs.steeringPosition).inRadians.IEEErem(2 * Math.PI).radians + + shouldInvert = steeringDifference.absoluteValue > (Math.PI / 2).radians && optimize + if (shouldInvert) { + steeringDifference -= Math.PI.withSign(steeringDifference.inRadians).radians } - fun setDriveBrakeMode(brake: Boolean) { - io.setDriveBrakeMode(brake) + val outputSpeed = + if (shouldInvert) { + speed * -1 + } else { + speed + } + steeringSetPoint = inputs.steeringPosition + steeringDifference + io.setOpenLoop(steeringSetPoint, outputSpeed) + } + + /** + * Sets the swerve module to the specified angular and X & Y velocities using open loop control. + * + * @param desiredState The desired SwerveModuleState. Contains desired angle as well as X and Y + * velocities + */ + fun setPositionOpenLoop(desiredState: SwerveModuleState, optimize: Boolean = true) { + if (optimize) { + val optimizedState = + SwerveModuleState.optimize(desiredState, inputs.steeringPosition.inRotation2ds) + io.setOpenLoop( + optimizedState.angle.angle, + optimizedState + .speedMetersPerSecond + .meters + .perSecond // consider desaturating wheel speeds here if it doesn't work + // from drivetrain + ) + Logger.recordOutput("${io.label}/steeringSetpointOptimized", optimizedState.angle.degrees) + } else { + io.setOpenLoop(desiredState.angle.angle, desiredState.speedMetersPerSecond.meters.perSecond) + Logger.recordOutput("${io.label}/steeringSetpointNonOptimized", desiredState.angle.degrees) } - - fun setSteeringBrakeMode(brake: Boolean) { - io.setSteeringBrakeMode(brake) + } + + /** + * Sets the swerve module to the specified angular and X & Y velocities using feed forward. + * + * @param desiredVelState The desired SwerveModuleState. Contains desired angle as well as X and Y + * velocities + * @param desiredAccelState The desired SwerveModuleState that contains desired acceleration + * vectors. + * @param optimize Whether velocity and acceleration vectors should be optimized (if possible) + */ + fun setPositionClosedLoop( + desiredVelState: SwerveModuleState, + desiredAccelState: SwerveModuleState, + optimize: Boolean = true + ) { + if (optimize) { + val optimizedVelState = + SwerveModuleState.optimize(desiredVelState, inputs.steeringPosition.inRotation2ds) + val optimizedAccelState = + SwerveModuleState.optimize(desiredAccelState, inputs.steeringPosition.inRotation2ds) + io.setClosedLoop( + optimizedVelState.angle.angle, + optimizedVelState.speedMetersPerSecond.meters.perSecond, + optimizedAccelState.speedMetersPerSecond.meters.perSecond.perSecond + ) + } else { + io.setClosedLoop( + desiredVelState.angle.angle, + desiredVelState.speedMetersPerSecond.meters.perSecond, + desiredAccelState.speedMetersPerSecond.meters.perSecond.perSecond + ) } -} \ No newline at end of file + } + + /** Creates event of the current potentiometer value as needs to be manually readjusted. */ + fun resetModuleZero() { + io.resetModuleZero() + } + + /** Zeros the steering motor */ + fun zeroSteering() { + io.zeroSteering() + } + + /** Zeros the drive motor */ + fun zeroDrive() { + io.zeroDrive() + } + + fun setDriveBrakeMode(brake: Boolean) { + io.setDriveBrakeMode(brake) + } + + fun setSteeringBrakeMode(brake: Boolean) { + io.setSteeringBrakeMode(brake) + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/servemodule/SwerveModuleIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/servemodule/SwerveModuleIO.kt index c6c01a06..bbc82e41 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/servemodule/SwerveModuleIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/servemodule/SwerveModuleIO.kt @@ -28,117 +28,118 @@ import org.team4099.lib.units.derived.volts import org.team4099.lib.units.inMetersPerSecond import org.team4099.lib.units.inRadiansPerSecond import org.team4099.lib.units.perSecond + interface SwerveModuleIO { - class SwerveModuleIOInputs : LoggableInputs { - var driveAppliedVoltage = 0.0.volts - var steeringAppliedVoltage = 0.0.volts - - var driveStatorCurrent = 0.0.amps - var driveSupplyCurrent = 0.0.amps - var steeringStatorCurrent = 0.0.amps - var steeringSupplyCurrent = 0.0.amps - - var drivePosition = 0.0.meters - var steeringPosition = 0.0.degrees - - var driveVelocity = 0.0.meters.perSecond - var steeringVelocity = 0.0.degrees.perSecond - - var driveTemp = 0.0.celsius - var steeringTemp = 0.0.celsius - - var potentiometerOutputRaw = 0.0 - var potentiometerOutputRadians = 0.0.radians - - var drift = 0.0.meters - - override fun toLog(table: LogTable?) { - table?.put("driveAppliedVoltage", driveAppliedVoltage.inVolts) - table?.put("steeringAppliedVoltage", steeringAppliedVoltage.inVolts) - table?.put("driveStatorCurrentAmps", driveStatorCurrent.inAmperes) - table?.put("driveSupplyCurrentAmps", driveSupplyCurrent.inAmperes) - table?.put("steeringStatorCurrentAmps", steeringStatorCurrent.inAmperes) - table?.put("steeringSupplyCurrentAmps", steeringSupplyCurrent.inAmperes) - table?.put("drivePositionMeters", drivePosition.inMeters) - table?.put("steeringPositionRadians", steeringPosition.inRadians) - table?.put("driveVelocityMetersPerSecond", driveVelocity.inMetersPerSecond) - table?.put("steeringVelocityRadiansPerSecond", steeringVelocity.inRadiansPerSecond) - table?.put("driveTempCelcius", driveTemp.inCelsius) - table?.put("steeringTempCelcius", steeringTemp.inCelsius) - table?.put("potentiometerOutputRaw", potentiometerOutputRaw) - table?.put("potentiometerOutputRadians", potentiometerOutputRadians.inRadians) - table?.put("driftPositionMeters", drift.inMeters) - } - - override fun fromLog(table: LogTable?) { - table?.getDouble("driveAppliedVoltage", driveAppliedVoltage.inVolts)?.let { - driveAppliedVoltage = it.volts - } - table?.getDouble("steeringAppliedVoltage", steeringAppliedVoltage.inVolts)?.let { - steeringAppliedVoltage = it.volts - } - table?.getDouble("driveStatorCurrentAmps", driveStatorCurrent.inAmperes)?.let { - driveStatorCurrent = it.amps - } - table?.getDouble("driveSupplyCurrentAmps", driveSupplyCurrent.inAmperes)?.let { - driveSupplyCurrent = it.amps - } - table?.getDouble("steeringStatorCurrentAmps", steeringStatorCurrent.inAmperes)?.let { - steeringStatorCurrent = it.amps - } - table?.getDouble("steeringSupplyCurrentAmps", steeringSupplyCurrent.inAmperes)?.let { - steeringSupplyCurrent = it.amps - } - table?.getDouble("drivePositionMeters", drivePosition.inMeters)?.let { - drivePosition = it.meters - } - table?.getDouble("steeringPositionRadians", steeringPosition.inRadians)?.let { - steeringPosition = it.radians - } - table?.getDouble("driveVelocityMetersPerSecond", driveVelocity.inMetersPerSecond)?.let { - driveVelocity = it.meters.perSecond - } - table?.getDouble("steeringVelocityRadiansPerSecond", steeringVelocity.inRadiansPerSecond) - ?.let { steeringVelocity = it.radians.perSecond } - table?.getDouble("driveTempCelcius", driveTemp.inCelsius)?.let { driveTemp = it.celsius } - table?.getDouble("steeringTempCelcius", steeringTemp.inCelsius)?.let { - steeringTemp = it.celsius - } - table?.getDouble("potentiometerOutputRaw", potentiometerOutputRaw)?.let { - potentiometerOutputRaw = it - } - table?.getDouble("potentiometerOutputRaw", potentiometerOutputRadians.inRadians)?.let { - potentiometerOutputRadians = it.radians - } - table?.getDouble("driftPositionMeters", drift.inMeters)?.let { drift = it.meters } - } + class SwerveModuleIOInputs : LoggableInputs { + var driveAppliedVoltage = 0.0.volts + var steeringAppliedVoltage = 0.0.volts + + var driveStatorCurrent = 0.0.amps + var driveSupplyCurrent = 0.0.amps + var steeringStatorCurrent = 0.0.amps + var steeringSupplyCurrent = 0.0.amps + + var drivePosition = 0.0.meters + var steeringPosition = 0.0.degrees + + var driveVelocity = 0.0.meters.perSecond + var steeringVelocity = 0.0.degrees.perSecond + + var driveTemp = 0.0.celsius + var steeringTemp = 0.0.celsius + + var potentiometerOutputRaw = 0.0 + var potentiometerOutputRadians = 0.0.radians + + var drift = 0.0.meters + + override fun toLog(table: LogTable?) { + table?.put("driveAppliedVoltage", driveAppliedVoltage.inVolts) + table?.put("steeringAppliedVoltage", steeringAppliedVoltage.inVolts) + table?.put("driveStatorCurrentAmps", driveStatorCurrent.inAmperes) + table?.put("driveSupplyCurrentAmps", driveSupplyCurrent.inAmperes) + table?.put("steeringStatorCurrentAmps", steeringStatorCurrent.inAmperes) + table?.put("steeringSupplyCurrentAmps", steeringSupplyCurrent.inAmperes) + table?.put("drivePositionMeters", drivePosition.inMeters) + table?.put("steeringPositionRadians", steeringPosition.inRadians) + table?.put("driveVelocityMetersPerSecond", driveVelocity.inMetersPerSecond) + table?.put("steeringVelocityRadiansPerSecond", steeringVelocity.inRadiansPerSecond) + table?.put("driveTempCelcius", driveTemp.inCelsius) + table?.put("steeringTempCelcius", steeringTemp.inCelsius) + table?.put("potentiometerOutputRaw", potentiometerOutputRaw) + table?.put("potentiometerOutputRadians", potentiometerOutputRadians.inRadians) + table?.put("driftPositionMeters", drift.inMeters) + } + + override fun fromLog(table: LogTable?) { + table?.getDouble("driveAppliedVoltage", driveAppliedVoltage.inVolts)?.let { + driveAppliedVoltage = it.volts + } + table?.getDouble("steeringAppliedVoltage", steeringAppliedVoltage.inVolts)?.let { + steeringAppliedVoltage = it.volts + } + table?.getDouble("driveStatorCurrentAmps", driveStatorCurrent.inAmperes)?.let { + driveStatorCurrent = it.amps + } + table?.getDouble("driveSupplyCurrentAmps", driveSupplyCurrent.inAmperes)?.let { + driveSupplyCurrent = it.amps + } + table?.getDouble("steeringStatorCurrentAmps", steeringStatorCurrent.inAmperes)?.let { + steeringStatorCurrent = it.amps + } + table?.getDouble("steeringSupplyCurrentAmps", steeringSupplyCurrent.inAmperes)?.let { + steeringSupplyCurrent = it.amps + } + table?.getDouble("drivePositionMeters", drivePosition.inMeters)?.let { + drivePosition = it.meters + } + table?.getDouble("steeringPositionRadians", steeringPosition.inRadians)?.let { + steeringPosition = it.radians + } + table?.getDouble("driveVelocityMetersPerSecond", driveVelocity.inMetersPerSecond)?.let { + driveVelocity = it.meters.perSecond + } + table?.getDouble("steeringVelocityRadiansPerSecond", steeringVelocity.inRadiansPerSecond) + ?.let { steeringVelocity = it.radians.perSecond } + table?.getDouble("driveTempCelcius", driveTemp.inCelsius)?.let { driveTemp = it.celsius } + table?.getDouble("steeringTempCelcius", steeringTemp.inCelsius)?.let { + steeringTemp = it.celsius + } + table?.getDouble("potentiometerOutputRaw", potentiometerOutputRaw)?.let { + potentiometerOutputRaw = it + } + table?.getDouble("potentiometerOutputRaw", potentiometerOutputRadians.inRadians)?.let { + potentiometerOutputRadians = it.radians + } + table?.getDouble("driftPositionMeters", drift.inMeters)?.let { drift = it.meters } } + } - val label: String + val label: String - fun updateInputs(inputs: SwerveModuleIOInputs) {} + fun updateInputs(inputs: SwerveModuleIOInputs) {} - fun setSteeringSetpoint(angle: Angle) {} - fun setClosedLoop(steering: Angle, speed: LinearVelocity, acceleration: LinearAcceleration) {} - fun setOpenLoop(steering: Angle, speed: LinearVelocity) {} + fun setSteeringSetpoint(angle: Angle) {} + fun setClosedLoop(steering: Angle, speed: LinearVelocity, acceleration: LinearAcceleration) {} + fun setOpenLoop(steering: Angle, speed: LinearVelocity) {} - fun resetModuleZero() {} - fun zeroSteering() {} - fun zeroDrive() {} + fun resetModuleZero() {} + fun zeroSteering() {} + fun zeroDrive() {} - fun setDriveBrakeMode(brake: Boolean) {} + fun setDriveBrakeMode(brake: Boolean) {} - fun setSteeringBrakeMode(brake: Boolean) {} + fun setSteeringBrakeMode(brake: Boolean) {} - fun configureDrivePID( - kP: ProportionalGain, Volt>, - kI: IntegralGain, Volt>, - kD: DerivativeGain, Volt> - ) {} - fun configureSteeringPID( - kP: ProportionalGain, - kI: IntegralGain, - kD: DerivativeGain - ) {} - fun configureSteeringMotionMagic(maxVel: AngularVelocity, maxAccel: AngularAcceleration) {} -} \ No newline at end of file + fun configureDrivePID( + kP: ProportionalGain, Volt>, + kI: IntegralGain, Volt>, + kD: DerivativeGain, Volt> + ) {} + fun configureSteeringPID( + kP: ProportionalGain, + kI: IntegralGain, + kD: DerivativeGain + ) {} + fun configureSteeringMotionMagic(maxVel: AngularVelocity, maxAccel: AngularAcceleration) {} +} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/servemodule/SwerveModuleIOSim.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/servemodule/SwerveModuleIOSim.kt index 92ce36db..5a219453 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/servemodule/SwerveModuleIOSim.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/servemodule/SwerveModuleIOSim.kt @@ -13,7 +13,6 @@ import edu.wpi.first.wpilibj.simulation.RoboRioSim import org.littletonrobotics.junction.Logger import org.team4099.lib.controller.PIDController import org.team4099.lib.controller.SimpleMotorFeedforward -import kotlin.random.Random import org.team4099.lib.units.AngularAcceleration import org.team4099.lib.units.AngularVelocity import org.team4099.lib.units.LinearAcceleration @@ -44,231 +43,232 @@ 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 kotlin.random.Random class SwerveModuleIOSim(override val label: String) : SwerveModuleIO { - // Use inverses of gear ratios because our standard is <1 is reduction - val driveMotorSim: FlywheelSim = - FlywheelSim( - DCMotor.getNEO(1), - 1 / DrivetrainConstants.DRIVE_SENSOR_GEAR_RATIO, - DrivetrainConstants.DRIVE_WHEEL_INERTIA.inKilogramsMeterSquared - ) - - val steerMotorSim = - FlywheelSim( - DCMotor.getNEO(1), - 1 / DrivetrainConstants.STEERING_SENSOR_GEAR_RATIO, - DrivetrainConstants.STEERING_WHEEL_INERTIA.inKilogramsMeterSquared - ) - - init { - MotorChecker.add( - "Drivetrain", - "Drive", - MotorCollection( - mutableListOf(SimulatedMotor(driveMotorSim, "$label Drive Motor")), - 65.amps, - 90.celsius, - 45.amps, - 100.celsius - ) - ) - - MotorChecker.add( - "Drivetrain", - "Steering", - MotorCollection( - mutableListOf(SimulatedMotor(steerMotorSim, "$label Steering Motor")), - 65.amps, - 90.celsius, - 45.amps, - 100.celsius - ) - ) + // Use inverses of gear ratios because our standard is <1 is reduction + val driveMotorSim: FlywheelSim = + FlywheelSim( + DCMotor.getNEO(1), + 1 / DrivetrainConstants.DRIVE_SENSOR_GEAR_RATIO, + DrivetrainConstants.DRIVE_WHEEL_INERTIA.inKilogramsMeterSquared + ) + + val steerMotorSim = + FlywheelSim( + DCMotor.getNEO(1), + 1 / DrivetrainConstants.STEERING_SENSOR_GEAR_RATIO, + DrivetrainConstants.STEERING_WHEEL_INERTIA.inKilogramsMeterSquared + ) + + init { + MotorChecker.add( + "Drivetrain", + "Drive", + MotorCollection( + mutableListOf(SimulatedMotor(driveMotorSim, "$label Drive Motor")), + 65.amps, + 90.celsius, + 45.amps, + 100.celsius + ) + ) + + MotorChecker.add( + "Drivetrain", + "Steering", + MotorCollection( + mutableListOf(SimulatedMotor(steerMotorSim, "$label Steering Motor")), + 65.amps, + 90.celsius, + 45.amps, + 100.celsius + ) + ) + } + + var turnRelativePosition = 0.0.radians + var turnAbsolutePosition = + (Math.random() * 2.0 * Math.PI).radians // getting a random value that we zero to + var driveVelocity = 0.0.meters.perSecond + var drift: Length = 0.0.meters + + private val driveFeedback = + PIDController( + DrivetrainConstants.PID.SIM_DRIVE_KP, + DrivetrainConstants.PID.SIM_DRIVE_KI, + DrivetrainConstants.PID.SIM_DRIVE_KD, + Constants.Universal.LOOP_PERIOD_TIME + ) + private val driveFeedForward = + SimpleMotorFeedforward( + DrivetrainConstants.PID.SIM_DRIVE_KS, DrivetrainConstants.PID.SIM_DRIVE_KV + ) + + private val steeringFeedback = + PIDController( + DrivetrainConstants.PID.SIM_STEERING_KP, + DrivetrainConstants.PID.SIM_STEERING_KI, + DrivetrainConstants.PID.SIM_STEERING_KD, + Constants.Universal.LOOP_PERIOD_TIME + ) + + init { + steeringFeedback.enableContinuousInput(-Math.PI.radians, Math.PI.radians) + steeringFeedback.errorTolerance = DrivetrainConstants.ALLOWED_STEERING_ANGLE_ERROR + } + + override fun updateInputs(inputs: SwerveModuleIO.SwerveModuleIOInputs) { + driveMotorSim.update(Constants.Universal.LOOP_PERIOD_TIME.inSeconds) + steerMotorSim.update(Constants.Universal.LOOP_PERIOD_TIME.inSeconds) + val angleDifference: Angle = + (steerMotorSim.angularVelocityRadPerSec * Constants.Universal.LOOP_PERIOD_TIME.inSeconds) + .radians + turnAbsolutePosition += angleDifference + turnRelativePosition += angleDifference + + // constrains it to 2pi radians + while (turnAbsolutePosition < 0.radians) { + turnAbsolutePosition += (2.0 * Math.PI).radians } - - var turnRelativePosition = 0.0.radians - var turnAbsolutePosition = - (Math.random() * 2.0 * Math.PI).radians // getting a random value that we zero to - var driveVelocity = 0.0.meters.perSecond - var drift: Length = 0.0.meters - - private val driveFeedback = - PIDController( - DrivetrainConstants.PID.SIM_DRIVE_KP, - DrivetrainConstants.PID.SIM_DRIVE_KI, - DrivetrainConstants.PID.SIM_DRIVE_KD, - Constants.Universal.LOOP_PERIOD_TIME - ) - private val driveFeedForward = - SimpleMotorFeedforward( - DrivetrainConstants.PID.SIM_DRIVE_KS, DrivetrainConstants.PID.SIM_DRIVE_KV - ) - - private val steeringFeedback = - PIDController( - DrivetrainConstants.PID.SIM_STEERING_KP, - DrivetrainConstants.PID.SIM_STEERING_KI, - DrivetrainConstants.PID.SIM_STEERING_KD, - Constants.Universal.LOOP_PERIOD_TIME - ) - - init { - steeringFeedback.enableContinuousInput(-Math.PI.radians, Math.PI.radians) - steeringFeedback.errorTolerance = DrivetrainConstants.ALLOWED_STEERING_ANGLE_ERROR - } - - override fun updateInputs(inputs: SwerveModuleIO.SwerveModuleIOInputs) { - driveMotorSim.update(Constants.Universal.LOOP_PERIOD_TIME.inSeconds) - steerMotorSim.update(Constants.Universal.LOOP_PERIOD_TIME.inSeconds) - val angleDifference: Angle = - (steerMotorSim.angularVelocityRadPerSec * Constants.Universal.LOOP_PERIOD_TIME.inSeconds) - .radians - turnAbsolutePosition += angleDifference - turnRelativePosition += angleDifference - - // constrains it to 2pi radians - while (turnAbsolutePosition < 0.radians) { - turnAbsolutePosition += (2.0 * Math.PI).radians - } - while (turnAbsolutePosition > (2.0 * Math.PI).radians) { - turnAbsolutePosition -= (2.0 * Math.PI).radians - } - - // s = r * theta -> d/2 * rad/s = m/s - driveVelocity = - (DrivetrainConstants.WHEEL_DIAMETER / 2 * driveMotorSim.angularVelocityRadPerSec).perSecond - - // simming drift - var loopCycleDrift = 0.0.meters - if (Constants.Tuning.SIMULATE_DRIFT && driveVelocity > 2.0.meters.perSecond) { - loopCycleDrift = - (Random.nextDouble() * Constants.Tuning.DRIFT_CONSTANT) - .meters // 0.0005 is just a nice number that ended up working out for drift - } - drift += loopCycleDrift - - // pi * d * rotations = distance travelled - inputs.drivePosition = - inputs.drivePosition + - DrivetrainConstants.WHEEL_DIAMETER * - Math.PI * - ( - driveMotorSim.angularVelocityRadPerSec * - Constants.Universal.LOOP_PERIOD_TIME.inSeconds - ) - .radians - .inRotations + - loopCycleDrift // adding a random amount of drift - inputs.steeringPosition = turnAbsolutePosition - inputs.drift = drift - - inputs.driveVelocity = driveVelocity - inputs.steeringVelocity = steerMotorSim.angularVelocityRadPerSec.radians.perSecond - - inputs.driveAppliedVoltage = (-1337).volts - inputs.driveSupplyCurrent = driveMotorSim.currentDrawAmps.amps - inputs.driveStatorCurrent = - (-1337).amps // no way to get applied voltage to motor so can't actually calculate stator - // current - - inputs.driveTemp = (-1337).celsius - inputs.steeringTemp = (-1337).celsius - - inputs.steeringAppliedVoltage = (-1337).volts - inputs.steeringSupplyCurrent = steerMotorSim.currentDrawAmps.amps - inputs.steeringStatorCurrent = - (-1337).amps // no way to get applied voltage to motor so can't actually calculate stator - // current - - inputs.potentiometerOutputRadians = turnAbsolutePosition - inputs.potentiometerOutputRaw = turnAbsolutePosition.inRadians - - // Setting a more accurate simulated voltage under load - RoboRioSim.setVInVoltage( - BatterySim.calculateDefaultBatteryLoadedVoltage( - inputs.driveSupplyCurrent.inAmperes + inputs.steeringSupplyCurrent.inAmperes - ) - ) + while (turnAbsolutePosition > (2.0 * Math.PI).radians) { + turnAbsolutePosition -= (2.0 * Math.PI).radians } - // helper functions to clamp all inputs and set sim motor voltages properly - private fun setDriveVoltage(volts: ElectricalPotential) { - val driveAppliedVolts = clamp(volts, -12.0.volts, 12.0.volts) - driveMotorSim.setInputVoltage(driveAppliedVolts.inVolts) - } + // s = r * theta -> d/2 * rad/s = m/s + driveVelocity = + (DrivetrainConstants.WHEEL_DIAMETER / 2 * driveMotorSim.angularVelocityRadPerSec).perSecond - private fun setSteeringVoltage(volts: ElectricalPotential) { - val turnAppliedVolts = clamp(volts, -12.0.volts, 12.0.volts) - steerMotorSim.setInputVoltage(turnAppliedVolts.inVolts) + // simming drift + var loopCycleDrift = 0.0.meters + if (Constants.Tuning.SIMULATE_DRIFT && driveVelocity > 2.0.meters.perSecond) { + loopCycleDrift = + (Random.nextDouble() * Constants.Tuning.DRIFT_CONSTANT) + .meters // 0.0005 is just a nice number that ended up working out for drift } - - override fun setSteeringSetpoint(angle: Angle) { - val feedback = steeringFeedback.calculate(turnAbsolutePosition, angle) - Logger.recordOutput("Drivetrain/PID/steeringFeedback", feedback.inVolts) - Logger.recordOutput("Drivetrain/PID/kP", steeringFeedback.proportionalGain.inVoltsPerDegree) - Logger.recordOutput("Drivetrain/PID/kI", steeringFeedback.integralGain.inVoltsPerDegreeSeconds) - Logger.recordOutput( - "Drivetrain/PID/kD", steeringFeedback.derivativeGain.inVoltsPerDegreePerSecond + drift += loopCycleDrift + + // pi * d * rotations = distance travelled + inputs.drivePosition = + inputs.drivePosition + + DrivetrainConstants.WHEEL_DIAMETER * + Math.PI * + ( + driveMotorSim.angularVelocityRadPerSec * + Constants.Universal.LOOP_PERIOD_TIME.inSeconds ) - setSteeringVoltage(feedback) - } - - override fun setClosedLoop( - steering: Angle, - speed: LinearVelocity, - acceleration: LinearAcceleration - ) { - Logger.recordOutput("$label/desiredDriveSpeedMPS", speed.inMetersPerSecond) - val feedforward = driveFeedForward.calculate(speed, acceleration) - setDriveVoltage(feedforward + driveFeedback.calculate(driveVelocity, speed)) - - setSteeringSetpoint(steering) - } - - override fun setOpenLoop(steering: Angle, speed: LinearVelocity) { - setDriveVoltage( - RoboRioSim.getVInVoltage().volts * (speed / DrivetrainConstants.DRIVE_SETPOINT_MAX) - ) - setSteeringSetpoint(steering) - } - - override fun resetModuleZero() { - println("Resetting your module's 0 doesn't do anything meaningful in sim :(") - } - - override fun zeroDrive() { - println("Zero drive do anything meaningful in sim") - } - - override fun zeroSteering() { - turnAbsolutePosition = 0.0.radians - } - - override fun configureDrivePID( - kP: ProportionalGain, Volt>, - kI: IntegralGain, Volt>, - kD: DerivativeGain, Volt> - ) { - driveFeedback.setPID(kP, kI, kD) - } - - override fun configureSteeringPID( - kP: ProportionalGain, - kI: IntegralGain, - kD: DerivativeGain - ) { - steeringFeedback.setPID(kP, kI, kD) - } - - override fun setDriveBrakeMode(brake: Boolean) { - println("Can't set brake mode in simulation") - } - - override fun configureSteeringMotionMagic( - maxVel: AngularVelocity, - maxAccel: AngularAcceleration - ) { - println("Can't configure motion magic in simulation") - } -} \ No newline at end of file + .radians + .inRotations + + loopCycleDrift // adding a random amount of drift + inputs.steeringPosition = turnAbsolutePosition + inputs.drift = drift + + inputs.driveVelocity = driveVelocity + inputs.steeringVelocity = steerMotorSim.angularVelocityRadPerSec.radians.perSecond + + inputs.driveAppliedVoltage = (-1337).volts + inputs.driveSupplyCurrent = driveMotorSim.currentDrawAmps.amps + inputs.driveStatorCurrent = + (-1337).amps // no way to get applied voltage to motor so can't actually calculate stator + // current + + inputs.driveTemp = (-1337).celsius + inputs.steeringTemp = (-1337).celsius + + inputs.steeringAppliedVoltage = (-1337).volts + inputs.steeringSupplyCurrent = steerMotorSim.currentDrawAmps.amps + inputs.steeringStatorCurrent = + (-1337).amps // no way to get applied voltage to motor so can't actually calculate stator + // current + + inputs.potentiometerOutputRadians = turnAbsolutePosition + inputs.potentiometerOutputRaw = turnAbsolutePosition.inRadians + + // Setting a more accurate simulated voltage under load + RoboRioSim.setVInVoltage( + BatterySim.calculateDefaultBatteryLoadedVoltage( + inputs.driveSupplyCurrent.inAmperes + inputs.steeringSupplyCurrent.inAmperes + ) + ) + } + + // helper functions to clamp all inputs and set sim motor voltages properly + private fun setDriveVoltage(volts: ElectricalPotential) { + val driveAppliedVolts = clamp(volts, -12.0.volts, 12.0.volts) + driveMotorSim.setInputVoltage(driveAppliedVolts.inVolts) + } + + private fun setSteeringVoltage(volts: ElectricalPotential) { + val turnAppliedVolts = clamp(volts, -12.0.volts, 12.0.volts) + steerMotorSim.setInputVoltage(turnAppliedVolts.inVolts) + } + + override fun setSteeringSetpoint(angle: Angle) { + val feedback = steeringFeedback.calculate(turnAbsolutePosition, angle) + Logger.recordOutput("Drivetrain/PID/steeringFeedback", feedback.inVolts) + Logger.recordOutput("Drivetrain/PID/kP", steeringFeedback.proportionalGain.inVoltsPerDegree) + Logger.recordOutput("Drivetrain/PID/kI", steeringFeedback.integralGain.inVoltsPerDegreeSeconds) + Logger.recordOutput( + "Drivetrain/PID/kD", steeringFeedback.derivativeGain.inVoltsPerDegreePerSecond + ) + setSteeringVoltage(feedback) + } + + override fun setClosedLoop( + steering: Angle, + speed: LinearVelocity, + acceleration: LinearAcceleration + ) { + Logger.recordOutput("$label/desiredDriveSpeedMPS", speed.inMetersPerSecond) + val feedforward = driveFeedForward.calculate(speed, acceleration) + setDriveVoltage(feedforward + driveFeedback.calculate(driveVelocity, speed)) + + setSteeringSetpoint(steering) + } + + override fun setOpenLoop(steering: Angle, speed: LinearVelocity) { + setDriveVoltage( + RoboRioSim.getVInVoltage().volts * (speed / DrivetrainConstants.DRIVE_SETPOINT_MAX) + ) + setSteeringSetpoint(steering) + } + + override fun resetModuleZero() { + println("Resetting your module's 0 doesn't do anything meaningful in sim :(") + } + + override fun zeroDrive() { + println("Zero drive do anything meaningful in sim") + } + + override fun zeroSteering() { + turnAbsolutePosition = 0.0.radians + } + + override fun configureDrivePID( + kP: ProportionalGain, Volt>, + kI: IntegralGain, Volt>, + kD: DerivativeGain, Volt> + ) { + driveFeedback.setPID(kP, kI, kD) + } + + override fun configureSteeringPID( + kP: ProportionalGain, + kI: IntegralGain, + kD: DerivativeGain + ) { + steeringFeedback.setPID(kP, kI, kD) + } + + override fun setDriveBrakeMode(brake: Boolean) { + println("Can't set brake mode in simulation") + } + + override fun configureSteeringMotionMagic( + maxVel: AngularVelocity, + maxAccel: AngularAcceleration + ) { + println("Can't configure motion magic in simulation") + } +} 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 b53531ee..e3e10729 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt @@ -5,352 +5,379 @@ import com.team4099.lib.logging.LoggedTunableNumber import com.team4099.lib.logging.LoggedTunableValue import com.team4099.robot2023.config.constants.Constants import com.team4099.robot2023.config.constants.ElevatorConstants -import edu.wpi.first.units.Voltage import edu.wpi.first.wpilibj.RobotBase import edu.wpi.first.wpilibj2.command.Command -import edu.wpi.first.wpilibj2.command.Commands -import edu.wpi.first.wpilibj2.command.Commands.run import edu.wpi.first.wpilibj2.command.Commands.runOnce -import com.team4099.robot2023.subsystems.superstructure.Request.ElevatorRequest as ElevatorRequest +import org.littletonrobotics.junction.Logger import org.team4099.lib.controller.ElevatorFeedforward import org.team4099.lib.controller.TrapezoidProfile import org.team4099.lib.units.base.Meter -import org.team4099.lib.units.base.inches -import org.team4099.lib.units.derived.* -import org.team4099.lib.units.perSecond import org.team4099.lib.units.base.inInches -import org.littletonrobotics.junction.Logger import org.team4099.lib.units.base.inSeconds +import org.team4099.lib.units.base.inches +import org.team4099.lib.units.derived.ElectricalPotential +import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.inDegrees +import org.team4099.lib.units.derived.inVolts +import org.team4099.lib.units.derived.inVoltsPerInch +import org.team4099.lib.units.derived.inVoltsPerInchPerSecond +import org.team4099.lib.units.derived.inVoltsPerInchSeconds +import org.team4099.lib.units.derived.perInch +import org.team4099.lib.units.derived.perInchSeconds +import org.team4099.lib.units.derived.volts import org.team4099.lib.units.inInchesPerSecond -import kotlin.time.Duration.Companion.seconds +import org.team4099.lib.units.perSecond +import com.team4099.robot2023.subsystems.superstructure.Request.ElevatorRequest as ElevatorRequest class Elevator(val io: ElevatorIO) { - val inputs = ElevatorIO.ElevatorInputs() - private var elevatorFeedforward : ElevatorFeedforward = - ElevatorFeedforward( - ElevatorConstants.ELEVATOR_KS, - ElevatorConstants.ELEVATOR_KG, - ElevatorConstants.ELEVATOR_KV, - ElevatorConstants.ELEVATOR_KA - ) - - private val kP = LoggedTunableValue( - "Elevator/kP", - Pair({ it.inVoltsPerInch }, { it.volts.perInch }) + val inputs = ElevatorIO.ElevatorInputs() + private var elevatorFeedforward: ElevatorFeedforward = + ElevatorFeedforward( + ElevatorConstants.ELEVATOR_KS, + ElevatorConstants.ELEVATOR_KG, + ElevatorConstants.ELEVATOR_KV, + ElevatorConstants.ELEVATOR_KA ) - private val kI = LoggedTunableValue( - "Elevator/kI", - Pair({ it.inVoltsPerInchSeconds }, { it.volts.perInchSeconds }) + + private val kP = + LoggedTunableValue("Elevator/kP", Pair({ it.inVoltsPerInch }, { it.volts.perInch })) + private val kI = + LoggedTunableValue( + "Elevator/kI", Pair({ it.inVoltsPerInchSeconds }, { it.volts.perInchSeconds }) ) - private val kD = LoggedTunableValue( - "Elevator/kD", - Pair({ it.inVoltsPerInchPerSecond }, { it.volts / 1.0.inches.perSecond }) + private val kD = + LoggedTunableValue( + "Elevator/kD", Pair({ it.inVoltsPerInchPerSecond }, { it.volts / 1.0.inches.perSecond }) ) - object TunableElevatorHeights { - val enableElevator = - LoggedTunableNumber("Elevator/enableMovementElevator", ElevatorConstants.ENABLE_ELEVATOR) - - val minPosition = - LoggedTunableValue( - "Elevator/minPosition", - ElevatorConstants.ELEVATOR_IDLE_HEIGHT, - Pair({ it.inInches }, { it.inches }) - ) - val maxPosition = - LoggedTunableValue( - "Elevator/maxPosition", - ElevatorConstants.ELEVATOR_SOFT_LIMIT_EXTENSION, - Pair({ it.inInches }, { it.inches }) - ) - - //TODO: change voltages - val openLoopExtendVoltage = - LoggedTunableValue( - "Elevator/openLoopExtendVoltage", - 8.volts, - Pair({ it.inVolts }, { it.volts }) - ) - val openLoopRetractVoltage = - LoggedTunableValue( - "Elevator/openLoopRetractVoltage", - -12.0.volts, - Pair({ it.inVolts }, { it.volts }) - ) - - val shootSpeakerPosition = LoggedTunableValue( - "Elevator/shootSpeakerPosition", - ElevatorConstants.SHOOT_SPEAKER_POSITION - ) - val shootAmpPosition = LoggedTunableValue( - "Elevator/shootAmpPosition", - ElevatorConstants.SHOOT_AMP_POSITION - ) - val sourceNoteOffset = LoggedTunableValue( - "Elevator/sourceNoteOffset", - ElevatorConstants.SOURCE_NOTE_OFFSET - ) - - val xPos = LoggedTunableValue("Elevator/xPos", 0.0.inches) - val yPos = LoggedTunableValue("Elevator/yPos", 0.0.inches) - val zPos = LoggedTunableValue("Elevator/zPos", 0.0.inches) - val thetaPos = LoggedTunableValue("Elevator/thetaPos", 0.0.degrees) - val xPos1 = LoggedTunableValue("Elevator/xPos1", 0.0.inches) - val yPos1 = LoggedTunableValue("Elevator/yPos1", 0.0.inches) - val zPos1 = LoggedTunableValue("Elevator/zPos1", 0.0.inches) - val thetaPos1 = LoggedTunableValue("Elevator/thetaPos1", - ElevatorConstants.ELEVATOR_THETA_POS, - Pair({ it.inDegrees }, { it.degrees }) - ) - } - - val forwardLimitReached: Boolean - get() = inputs.elevatorPosition >= ElevatorConstants.ELEVATOR_SOFT_LIMIT_EXTENSION - val reverseLimitReached: Boolean - get() = inputs.elevatorPosition <= ElevatorConstants.ELEVATOR_SOFT_LIMIT_RETRACTION - - val forwardOpenLoopLimitReached: Boolean - get() = inputs.elevatorPosition >= ElevatorConstants.ELEVATOR_OPEN_LOOP_SOFT_LIMIT_EXTENSION - val reverseOpenLoopLimitReached: Boolean - get() = inputs.elevatorPosition <= ElevatorConstants.ELEVATOR_OPEN_LOOP_SOFT_LIMIT_RETRACTION - - var isHomed = false - - var currentState: ElevatorState = ElevatorState.UNINITIALIZED - var currentRequest: ElevatorRequest = ElevatorRequest.OpenLoop(0.0.volts) - set(value) { - when(value) { - is ElevatorRequest.OpenLoop -> elevatorVoltageTarget = value.voltage - is ElevatorRequest.TargetingPosition -> { - elevatorPositionTarget = value.position - } - else -> {} - } - field = value + object TunableElevatorHeights { + val enableElevator = + LoggedTunableNumber("Elevator/enableMovementElevator", ElevatorConstants.ENABLE_ELEVATOR) + + val minPosition = + LoggedTunableValue( + "Elevator/minPosition", + ElevatorConstants.ELEVATOR_IDLE_HEIGHT, + Pair({ it.inInches }, { it.inches }) + ) + val maxPosition = + LoggedTunableValue( + "Elevator/maxPosition", + ElevatorConstants.ELEVATOR_SOFT_LIMIT_EXTENSION, + Pair({ it.inInches }, { it.inches }) + ) + + // TODO: change voltages + val openLoopExtendVoltage = + LoggedTunableValue( + "Elevator/openLoopExtendVoltage", 8.volts, Pair({ it.inVolts }, { it.volts }) + ) + val openLoopRetractVoltage = + LoggedTunableValue( + "Elevator/openLoopRetractVoltage", -12.0.volts, Pair({ it.inVolts }, { it.volts }) + ) + + val shootSpeakerPosition = + LoggedTunableValue( + "Elevator/shootSpeakerPosition", ElevatorConstants.SHOOT_SPEAKER_POSITION + ) + val shootAmpPosition = + LoggedTunableValue("Elevator/shootAmpPosition", ElevatorConstants.SHOOT_AMP_POSITION) + val sourceNoteOffset = + LoggedTunableValue("Elevator/sourceNoteOffset", ElevatorConstants.SOURCE_NOTE_OFFSET) + + val xPos = LoggedTunableValue("Elevator/xPos", 0.0.inches) + val yPos = LoggedTunableValue("Elevator/yPos", 0.0.inches) + val zPos = LoggedTunableValue("Elevator/zPos", 0.0.inches) + val thetaPos = LoggedTunableValue("Elevator/thetaPos", 0.0.degrees) + val xPos1 = LoggedTunableValue("Elevator/xPos1", 0.0.inches) + val yPos1 = LoggedTunableValue("Elevator/yPos1", 0.0.inches) + val zPos1 = LoggedTunableValue("Elevator/zPos1", 0.0.inches) + val thetaPos1 = + LoggedTunableValue( + "Elevator/thetaPos1", + ElevatorConstants.ELEVATOR_THETA_POS, + Pair({ it.inDegrees }, { it.degrees }) + ) + } + + val forwardLimitReached: Boolean + get() = inputs.elevatorPosition >= ElevatorConstants.ELEVATOR_SOFT_LIMIT_EXTENSION + val reverseLimitReached: Boolean + get() = inputs.elevatorPosition <= ElevatorConstants.ELEVATOR_SOFT_LIMIT_RETRACTION + + val forwardOpenLoopLimitReached: Boolean + get() = inputs.elevatorPosition >= ElevatorConstants.ELEVATOR_OPEN_LOOP_SOFT_LIMIT_EXTENSION + val reverseOpenLoopLimitReached: Boolean + get() = inputs.elevatorPosition <= ElevatorConstants.ELEVATOR_OPEN_LOOP_SOFT_LIMIT_RETRACTION + + var isHomed = false + + var currentState: ElevatorState = ElevatorState.UNINITIALIZED + var currentRequest: ElevatorRequest = ElevatorRequest.OpenLoop(0.0.volts) + set(value) { + when (value) { + is ElevatorRequest.OpenLoop -> elevatorVoltageTarget = value.voltage + is ElevatorRequest.TargetingPosition -> { + elevatorPositionTarget = value.position } + else -> {} + } + field = value + } - var elevatorPositionTarget = 0.0.inches - private set - var elevatorVelocityTarget = 0.0.inches.perSecond - private set - var elevatorVoltageTarget = 0.0.volts - private set - - private var lastRequestedPosition = -9999.inches - private var lastRequestedVelocity = -9999.inches.perSecond - private var lastRequestedVoltage = -9999.volts - - private var timeProfileGeneratedAt = Clock.fpgaTime - - private var lastHomingStatorCurrentTripTime = Clock.fpgaTime - - // Trapezoidal Profile Constraints - private var elevatorConstraints: TrapezoidProfile.Constraints = - TrapezoidProfile.Constraints(ElevatorConstants.MAX_VELOCITY, ElevatorConstants.MAX_ACCELERATION) - private var elevatorSetpoint: TrapezoidProfile.State = - TrapezoidProfile.State(inputs.elevatorPosition, inputs.elevatorVelocity) - private var elevatorProfile = - TrapezoidProfile( - elevatorConstraints, - TrapezoidProfile.State(-9999.inches, -9999.inches.perSecond), - TrapezoidProfile.State(-9999.inches, -9999.inches.perSecond) - ) - - val isAtTargetedPosition: Boolean - get() = - (currentRequest is ElevatorRequest.TargetingPosition && - elevatorProfile.isFinished(Clock.fpgaTime - timeProfileGeneratedAt) && - (inputs.elevatorPosition - elevatorPositionTarget).absoluteValue <= - ElevatorConstants.ELEVATOR_TOLERANCE) || - (TunableElevatorHeights.enableElevator.get() != 1.0) - - val canContinueSafely: Boolean - get() = - currentRequest is ElevatorRequest.TargetingPosition && ( - ((inputs.elevatorPosition - elevatorPositionTarget).absoluteValue <= 5.inches) || - elevatorProfile.isFinished(Clock.fpgaTime - timeProfileGeneratedAt) - ) && lastRequestedPosition == elevatorPositionTarget - - - init { - TunableElevatorHeights + var elevatorPositionTarget = 0.0.inches + private set + var elevatorVelocityTarget = 0.0.inches.perSecond + private set + var elevatorVoltageTarget = 0.0.volts + private set - // Initializes PID constants and changes FF depending on if sim or real - if(RobotBase.isReal()) { - isHomed = false + private var lastRequestedPosition = -9999.inches + private var lastRequestedVelocity = -9999.inches.perSecond + private var lastRequestedVoltage = -9999.volts - kP.initDefault(ElevatorConstants.REAL_KP) - kI.initDefault(ElevatorConstants.REAL_KI) - kD.initDefault(ElevatorConstants.REAL_KD) - } else { - isHomed = true + private var timeProfileGeneratedAt = Clock.fpgaTime - kP.initDefault(ElevatorConstants.SIM_KP) - kI.initDefault(ElevatorConstants.SIM_KI) - kD.initDefault(ElevatorConstants.SIM_KD) - } + private var lastHomingStatorCurrentTripTime = Clock.fpgaTime - elevatorFeedforward = ElevatorFeedforward( - ElevatorConstants.ELEVATOR_KS, - ElevatorConstants.ELEVATOR_KG, - ElevatorConstants.ELEVATOR_KV, - ElevatorConstants.ELEVATOR_KA - ) + // Trapezoidal Profile Constraints + private var elevatorConstraints: TrapezoidProfile.Constraints = + TrapezoidProfile.Constraints( + ElevatorConstants.MAX_VELOCITY, ElevatorConstants.MAX_ACCELERATION + ) + private var elevatorSetpoint: TrapezoidProfile.State = + TrapezoidProfile.State(inputs.elevatorPosition, inputs.elevatorVelocity) + private var elevatorProfile = + TrapezoidProfile( + elevatorConstraints, + TrapezoidProfile.State(-9999.inches, -9999.inches.perSecond), + TrapezoidProfile.State(-9999.inches, -9999.inches.perSecond) + ) - io.configPID(kP.get(), kI.get(), kD.get()) + val isAtTargetedPosition: Boolean + get() = + ( + currentRequest is ElevatorRequest.TargetingPosition && + elevatorProfile.isFinished(Clock.fpgaTime - timeProfileGeneratedAt) && + (inputs.elevatorPosition - elevatorPositionTarget).absoluteValue <= + ElevatorConstants.ELEVATOR_TOLERANCE + ) || + (TunableElevatorHeights.enableElevator.get() != 1.0) + + val canContinueSafely: Boolean + get() = + currentRequest is ElevatorRequest.TargetingPosition && + ( + ((inputs.elevatorPosition - elevatorPositionTarget).absoluteValue <= 5.inches) || + elevatorProfile.isFinished(Clock.fpgaTime - timeProfileGeneratedAt) + ) && + lastRequestedPosition == elevatorPositionTarget + + init { + TunableElevatorHeights + + // Initializes PID constants and changes FF depending on if sim or real + if (RobotBase.isReal()) { + isHomed = false + + kP.initDefault(ElevatorConstants.REAL_KP) + kI.initDefault(ElevatorConstants.REAL_KI) + kD.initDefault(ElevatorConstants.REAL_KD) + } else { + isHomed = true + + kP.initDefault(ElevatorConstants.SIM_KP) + kI.initDefault(ElevatorConstants.SIM_KI) + kD.initDefault(ElevatorConstants.SIM_KD) } - fun periodic() { - io.updateInputs(inputs) - if ((kP.hasChanged()) || (kI.hasChanged()) || (kD.hasChanged())) { - io.configPID(kP.get(), kI.get(), kD.get()) + elevatorFeedforward = + ElevatorFeedforward( + ElevatorConstants.ELEVATOR_KS, + ElevatorConstants.ELEVATOR_KG, + ElevatorConstants.ELEVATOR_KV, + ElevatorConstants.ELEVATOR_KA + ) + + io.configPID(kP.get(), kI.get(), kD.get()) + } + + fun periodic() { + io.updateInputs(inputs) + if ((kP.hasChanged()) || (kI.hasChanged()) || (kD.hasChanged())) { + io.configPID(kP.get(), kI.get(), kD.get()) + } + Logger.processInputs("Elevator", inputs) + Logger.recordOutput("Elevator/currentState", currentState.name) + Logger.recordOutput("Elevator/currentRequest", currentRequest.javaClass.simpleName) + Logger.recordOutput( + "Elevator/elevatorHeight", + inputs.elevatorPosition - ElevatorConstants.ELEVATOR_GROUND_OFFSET + ) + if (Constants.Tuning.DEBUGING_MODE) { + Logger.recordOutput("Elevator/isHomed", isHomed) + Logger.recordOutput("Elevator/canContinueSafely", canContinueSafely) + + Logger.recordOutput("Elevator/isAtTargetPosition", isAtTargetedPosition) + Logger.recordOutput("Elevator/lastGeneratedAt", timeProfileGeneratedAt.inSeconds) + + Logger.recordOutput("Elevator/elevatorPositionTarget", elevatorPositionTarget.inInches) + Logger.recordOutput( + "Elevator/elevatorVelocityTarget", elevatorVelocityTarget.inInchesPerSecond + ) + Logger.recordOutput("Elevator/elevatorVoltageTarget", elevatorVoltageTarget.inVolts) + + Logger.recordOutput("Elevator/lastElevatorPositionTarget", lastRequestedPosition.inInches) + Logger.recordOutput( + "Elevator/lastElevatorVelocityTarget", lastRequestedVelocity.inInchesPerSecond + ) + Logger.recordOutput("Elevator/lastElevatorVoltageTarget", lastRequestedVoltage.inVolts) + + Logger.recordOutput("Elevator/forwardLimitReached", forwardLimitReached) + Logger.recordOutput("Elevator/reverseLimitReached", reverseLimitReached) + } + var nextState = currentState + when (currentState) { + ElevatorState.UNINITIALIZED -> { + nextState = fromElevatorRequestToState(currentRequest) + } + ElevatorState.OPEN_LOOP -> { + setOutputVoltage(elevatorVoltageTarget) + nextState = fromElevatorRequestToState(currentRequest) + } + ElevatorState.TARGETING_POSITION -> { + if ((elevatorPositionTarget != lastRequestedPosition) || + (elevatorVelocityTarget != lastRequestedVelocity) + ) { + val preProfileGenerate = Clock.realTimestamp + elevatorProfile = + TrapezoidProfile( + elevatorConstraints, + TrapezoidProfile.State(elevatorPositionTarget, elevatorVelocityTarget), + TrapezoidProfile.State(inputs.elevatorPosition, inputs.elevatorVelocity) + ) + val postProfileGenerate = Clock.realTimestamp + Logger.recordOutput( + "/Elevator/profileGenerationMS", + postProfileGenerate.inSeconds - preProfileGenerate.inSeconds + ) + Logger.recordOutput("Elevator/initialPosition", elevatorProfile.initial.position.inInches) + Logger.recordOutput( + "Elevator/initialVelocity", elevatorProfile.initial.velocity.inInchesPerSecond + ) + timeProfileGeneratedAt = Clock.fpgaTime + lastRequestedPosition = elevatorPositionTarget + lastRequestedVelocity = elevatorVelocityTarget } - Logger.processInputs("Elevator", inputs) - Logger.recordOutput("Elevator/currentState", currentState.name) - Logger.recordOutput("Elevator/currentRequest", currentRequest.javaClass.simpleName) - Logger.recordOutput("Elevator/elevatorHeight", inputs.elevatorPosition - ElevatorConstants.ELEVATOR_GROUND_OFFSET) - if (Constants.Tuning.DEBUGING_MODE) { - Logger.recordOutput("Elevator/isHomed", isHomed) - Logger.recordOutput("Elevator/canContinueSafely", canContinueSafely) - - Logger.recordOutput("Elevator/isAtTargetPosition", isAtTargetedPosition) - Logger.recordOutput("Elevator/lastGeneratedAt", timeProfileGeneratedAt.inSeconds) - - Logger.recordOutput("Elevator/elevatorPositionTarget", elevatorPositionTarget.inInches) - Logger.recordOutput("Elevator/elevatorVelocityTarget", elevatorVelocityTarget.inInchesPerSecond) - Logger.recordOutput("Elevator/elevatorVoltageTarget", elevatorVoltageTarget.inVolts) - - Logger.recordOutput("Elevator/lastElevatorPositionTarget", lastRequestedPosition.inInches) - Logger.recordOutput( - "Elevator/lastElevatorVelocityTarget", lastRequestedVelocity.inInchesPerSecond - ) - Logger.recordOutput("Elevator/lastElevatorVoltageTarget", lastRequestedVoltage.inVolts) - - Logger.recordOutput("Elevator/forwardLimitReached", forwardLimitReached) - Logger.recordOutput("Elevator/reverseLimitReached", reverseLimitReached) + val timeElapsed = Clock.fpgaTime - timeProfileGeneratedAt + val profilePosition = elevatorProfile.calculate(timeElapsed) + setPosition(profilePosition) + Logger.recordOutput( + "Elevator/completedMotionProfile", elevatorProfile.isFinished(timeElapsed) + ) + Logger.recordOutput( + "Elevator/profileTargetVelocity", profilePosition.velocity.inInchesPerSecond + ) + Logger.recordOutput("Elevator/profileTargetPosition", profilePosition.position.inInches) + nextState = fromElevatorRequestToState(currentRequest) + if (!(currentState.equivalentToRequest(currentRequest))) { + lastRequestedVelocity = -999.inches.perSecond + lastRequestedPosition = -999.inches } - var nextState = currentState - when (currentState) { - ElevatorState.UNINITIALIZED -> { - nextState = fromElevatorRequestToState(currentRequest) - } - ElevatorState.OPEN_LOOP -> { - setOutputVoltage(elevatorVoltageTarget) - nextState = fromElevatorRequestToState(currentRequest) - } - ElevatorState.TARGETING_POSITION -> { - if ((elevatorPositionTarget != lastRequestedPosition) || (elevatorVelocityTarget != lastRequestedVelocity)) { - val preProfileGenerate = Clock.realTimestamp - elevatorProfile = TrapezoidProfile(elevatorConstraints, TrapezoidProfile.State(elevatorPositionTarget, elevatorVelocityTarget), TrapezoidProfile.State(inputs.elevatorPosition, inputs.elevatorVelocity)) - val postProfileGenerate = Clock.realTimestamp - Logger.recordOutput("/Elevator/profileGenerationMS", postProfileGenerate.inSeconds - preProfileGenerate.inSeconds) - Logger.recordOutput("Elevator/initialPosition", elevatorProfile.initial.position.inInches) - Logger.recordOutput("Elevator/initialVelocity", elevatorProfile.initial.velocity.inInchesPerSecond) - timeProfileGeneratedAt = Clock.fpgaTime - lastRequestedPosition = elevatorPositionTarget - lastRequestedVelocity = elevatorVelocityTarget - } - val timeElapsed = Clock.fpgaTime - timeProfileGeneratedAt - val profilePosition = elevatorProfile.calculate(timeElapsed) - setPosition(profilePosition) - Logger.recordOutput("Elevator/completedMotionProfile", elevatorProfile.isFinished(timeElapsed)) - Logger.recordOutput("Elevator/profileTargetVelocity", profilePosition.velocity.inInchesPerSecond) - Logger.recordOutput("Elevator/profileTargetPosition", profilePosition.position.inInches) - nextState = fromElevatorRequestToState(currentRequest) - if (! (currentState.equivalentToRequest(currentRequest))) { - lastRequestedVelocity = -999.inches.perSecond - lastRequestedPosition = -999.inches - } - } - ElevatorState.HOME -> { - if (inputs.leaderStatorCurrent < ElevatorConstants.HOMING_STATOR_CURRENT) { - lastHomingStatorCurrentTripTime = Clock.fpgaTime - } - if (! inputs.isSimulating && (! isHomed && inputs.leaderStatorCurrent < ElevatorConstants.HOMING_STATOR_CURRENT && Clock.fpgaTime - lastHomingStatorCurrentTripTime < ElevatorConstants.HOMING_STALL_TIME_THRESHOLD)) { - setHomeVoltage(ElevatorConstants.HOMING_APPLIED_VOLTAGE) - } - else { - zeroEncoder() - isHomed = true - } - if (isHomed) { - nextState = fromElevatorRequestToState(currentRequest) - } - } + } + ElevatorState.HOME -> { + if (inputs.leaderStatorCurrent < ElevatorConstants.HOMING_STATOR_CURRENT) { + lastHomingStatorCurrentTripTime = Clock.fpgaTime } - currentState = nextState - } - - fun setOutputVoltage(voltage: ElectricalPotential) { - if ((forwardLimitReached) && (voltage > 0.volts) || (reverseLimitReached) && (voltage < 0.volts)) { - io.setOutputVoltage(0.volts) + if (!inputs.isSimulating && + ( + !isHomed && + inputs.leaderStatorCurrent < ElevatorConstants.HOMING_STATOR_CURRENT && + Clock.fpgaTime - lastHomingStatorCurrentTripTime < + ElevatorConstants.HOMING_STALL_TIME_THRESHOLD + ) + ) { + setHomeVoltage(ElevatorConstants.HOMING_APPLIED_VOLTAGE) + } else { + zeroEncoder() + isHomed = true } - else { - io.setOutputVoltage(voltage) + if (isHomed) { + nextState = fromElevatorRequestToState(currentRequest) } + } } - - fun setHomeVoltage(voltage: ElectricalPotential) { - io.setOutputVoltage(voltage) + currentState = nextState + } + + fun setOutputVoltage(voltage: ElectricalPotential) { + if ((forwardLimitReached) && (voltage > 0.volts) || + (reverseLimitReached) && (voltage < 0.volts) + ) { + io.setOutputVoltage(0.volts) + } else { + io.setOutputVoltage(voltage) } - - fun zeroEncoder() { - io.zeroEncoder() + } + + fun setHomeVoltage(voltage: ElectricalPotential) { + io.setOutputVoltage(voltage) + } + + fun zeroEncoder() { + io.zeroEncoder() + } + + fun setPosition(setpoint: TrapezoidProfile.State) { + val elevatorAcceleration = + (setpoint.velocity - elevatorSetpoint.velocity) / Constants.Universal.LOOP_PERIOD_TIME + elevatorSetpoint = setpoint + val feedForward = elevatorFeedforward.calculate(setpoint.velocity, elevatorAcceleration) + if ((forwardLimitReached) && (setpoint.position > inputs.elevatorPosition) || + (reverseLimitReached) && (setpoint.position < inputs.elevatorPosition) + ) { + io.setOutputVoltage(0.volts) + } else { + io.setPosition(setpoint.position, feedForward) } - - fun setPosition(setpoint : TrapezoidProfile.State < Meter >) { - val elevatorAcceleration = (setpoint.velocity - elevatorSetpoint.velocity) / Constants.Universal.LOOP_PERIOD_TIME - elevatorSetpoint = setpoint - val feedForward = elevatorFeedforward.calculate(setpoint.velocity, elevatorAcceleration) - if ((forwardLimitReached) && (setpoint.position > inputs.elevatorPosition) || (reverseLimitReached) && (setpoint.position < inputs.elevatorPosition)) { - io.setOutputVoltage(0.volts) - } - else { - io.setPosition(setpoint.position, feedForward) - } + } + + companion object { + enum class ElevatorState { + UNINITIALIZED, + TARGETING_POSITION, + OPEN_LOOP, + HOME; + inline fun equivalentToRequest(request: ElevatorRequest): Boolean { + return (request is ElevatorRequest.Home && this == HOME) || + (request is ElevatorRequest.OpenLoop && this == OPEN_LOOP) || + (request is ElevatorRequest.TargetingPosition && this == TARGETING_POSITION) + } } - companion object { - enum class ElevatorState { - UNINITIALIZED, - TARGETING_POSITION, - OPEN_LOOP, - HOME; - inline fun equivalentToRequest(request : ElevatorRequest) : Boolean { - return (request is ElevatorRequest.Home && this == HOME) || (request is ElevatorRequest.OpenLoop && this == OPEN_LOOP) || (request is ElevatorRequest.TargetingPosition && this == TARGETING_POSITION) - } - } - - inline fun fromElevatorRequestToState(request : ElevatorRequest) : ElevatorState { - return when(request) { - is ElevatorRequest.Home -> ElevatorState.HOME - is ElevatorRequest.OpenLoop -> ElevatorState.OPEN_LOOP - is ElevatorRequest.TargetingPosition -> ElevatorState.TARGETING_POSITION - } - } + inline fun fromElevatorRequestToState(request: ElevatorRequest): ElevatorState { + return when (request) { + is ElevatorRequest.Home -> ElevatorState.HOME + is ElevatorRequest.OpenLoop -> ElevatorState.OPEN_LOOP + is ElevatorRequest.TargetingPosition -> ElevatorState.TARGETING_POSITION + } } + } - fun testElevatorOpenLoopRetractCommand(): Command { - return runOnce({ - currentRequest = ElevatorRequest.OpenLoop(-10.volts) - }) - } + fun testElevatorOpenLoopRetractCommand(): Command { + return runOnce({ currentRequest = ElevatorRequest.OpenLoop(-10.volts) }) + } - fun testElevatorOpenLoopExtendCommand(): Command { - return runOnce({ - currentRequest = ElevatorRequest.OpenLoop(10.volts) - }) - } + fun testElevatorOpenLoopExtendCommand(): Command { + return runOnce({ currentRequest = ElevatorRequest.OpenLoop(10.volts) }) + } - fun elevatorClosedLoopRetractCommand(): Command { - return runOnce({ - currentRequest = ElevatorRequest.TargetingPosition(12.inches) - }) - } + fun elevatorClosedLoopRetractCommand(): Command { + return runOnce({ currentRequest = ElevatorRequest.TargetingPosition(12.inches) }) + } - fun testElevatorClosedLoopExtendCommand(): Command { - return runOnce({ - currentRequest = ElevatorRequest.TargetingPosition(4.inches) - }) - } -} \ No newline at end of file + fun testElevatorClosedLoopExtendCommand(): Command { + return runOnce({ currentRequest = ElevatorRequest.TargetingPosition(4.inches) }) + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIO.kt index d5c27eb3..e01bd0a4 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIO.kt @@ -21,93 +21,89 @@ import org.team4099.lib.units.inInchesPerSecond import org.team4099.lib.units.perSecond interface ElevatorIO { - class ElevatorInputs : LoggableInputs { - var elevatorPosition = 0.0.inches - var elevatorVelocity = 0.0.inches.perSecond + class ElevatorInputs : LoggableInputs { + var elevatorPosition = 0.0.inches + var elevatorVelocity = 0.0.inches.perSecond - var leaderAppliedVoltage = 0.0.volts - var followerAppliedVoltage = 0.0.volts + var leaderAppliedVoltage = 0.0.volts + var followerAppliedVoltage = 0.0.volts - var leaderSupplyCurrent = 0.0.amps - var leaderStatorCurrent = 0.0.amps + var leaderSupplyCurrent = 0.0.amps + var leaderStatorCurrent = 0.0.amps - var followerSupplyCurrent = 0.0.amps - var followerStatorCurrent = 0.0.amps + var followerSupplyCurrent = 0.0.amps + var followerStatorCurrent = 0.0.amps - var leaderTempCelcius = 0.0.celsius - var followerTempCelcius = 0.0.celsius + var leaderTempCelcius = 0.0.celsius + var followerTempCelcius = 0.0.celsius - var leaderRawPosition = 0.0 - var followerRawPosition = 0.0 + var leaderRawPosition = 0.0 + var followerRawPosition = 0.0 - var isSimulating = false + var isSimulating = false - override fun toLog(table: LogTable) { - table?.put("elevatorPositionInches", elevatorPosition.inInches) - table?.put("elevatorVelocityInchesPerSec", elevatorVelocity.inInchesPerSecond) - table?.put("elevatorLeaderAppliedVolts", leaderAppliedVoltage.inVolts) - table?.put("elevatorFollowerAppliedVolts", followerAppliedVoltage.inVolts) - table?.put("elevatorLeaderStatorCurrentAmps", leaderStatorCurrent.inAmperes) - table?.put("elevatorFollowerStatorCurrentAmps", followerStatorCurrent.inAmperes) - table?.put("elevatorLeaderSupplyCurrentAmps", leaderSupplyCurrent.inAmperes) - table?.put("elevatorFollowerSupplyCurrentAmps", followerSupplyCurrent.inAmperes) - table?.put("elevatorLeaderTempCelsius", leaderTempCelcius.inCelsius) - table?.put("elevatorFollowerTempCelsius", followerTempCelcius.inCelsius) - table?.put("elevatorLeaderRawPosition", leaderRawPosition) - table?.put("elevatorFollowRawPosition", followerRawPosition) - } + override fun toLog(table: LogTable) { + table?.put("elevatorPositionInches", elevatorPosition.inInches) + table?.put("elevatorVelocityInchesPerSec", elevatorVelocity.inInchesPerSecond) + table?.put("elevatorLeaderAppliedVolts", leaderAppliedVoltage.inVolts) + table?.put("elevatorFollowerAppliedVolts", followerAppliedVoltage.inVolts) + table?.put("elevatorLeaderStatorCurrentAmps", leaderStatorCurrent.inAmperes) + table?.put("elevatorFollowerStatorCurrentAmps", followerStatorCurrent.inAmperes) + table?.put("elevatorLeaderSupplyCurrentAmps", leaderSupplyCurrent.inAmperes) + table?.put("elevatorFollowerSupplyCurrentAmps", followerSupplyCurrent.inAmperes) + table?.put("elevatorLeaderTempCelsius", leaderTempCelcius.inCelsius) + table?.put("elevatorFollowerTempCelsius", followerTempCelcius.inCelsius) + table?.put("elevatorLeaderRawPosition", leaderRawPosition) + table?.put("elevatorFollowRawPosition", followerRawPosition) + } - override fun fromLog(table: LogTable?) { - table?.get("elevatorPositionInches", elevatorPosition.inInches)?.let { - elevatorPosition = it.inches - } - table?.get("elevatorVelocityInchesPerSec", elevatorVelocity.inInchesPerSecond)?.let { - elevatorVelocity = it.inches.perSecond - } - table?.get("elevatorLeaderAppliedVolts", leaderAppliedVoltage.inVolts)?.let { - leaderAppliedVoltage = it.volts - } - table?.get("elevatorFollowerAppliedVolts", followerAppliedVoltage.inVolts)?.let { - followerAppliedVoltage = it.volts - } - table?.get("elevatorLeaderStatorCurrentAmps", leaderStatorCurrent.inAmperes)?.let { - leaderStatorCurrent = it.amps - } - table?.get("elevatorLeaderSupplyCurrentAmps", leaderSupplyCurrent.inAmperes)?.let { - leaderSupplyCurrent = it.amps - } - table?.get("elevatorLeaderTempCelcius", leaderTempCelcius.inCelsius)?.let { - leaderTempCelcius = it.celsius - } + override fun fromLog(table: LogTable?) { + table?.get("elevatorPositionInches", elevatorPosition.inInches)?.let { + elevatorPosition = it.inches + } + table?.get("elevatorVelocityInchesPerSec", elevatorVelocity.inInchesPerSecond)?.let { + elevatorVelocity = it.inches.perSecond + } + table?.get("elevatorLeaderAppliedVolts", leaderAppliedVoltage.inVolts)?.let { + leaderAppliedVoltage = it.volts + } + table?.get("elevatorFollowerAppliedVolts", followerAppliedVoltage.inVolts)?.let { + followerAppliedVoltage = it.volts + } + table?.get("elevatorLeaderStatorCurrentAmps", leaderStatorCurrent.inAmperes)?.let { + leaderStatorCurrent = it.amps + } + table?.get("elevatorLeaderSupplyCurrentAmps", leaderSupplyCurrent.inAmperes)?.let { + leaderSupplyCurrent = it.amps + } + table?.get("elevatorLeaderTempCelcius", leaderTempCelcius.inCelsius)?.let { + leaderTempCelcius = it.celsius + } - table?.get("elevatorFollowerAppliedVolts", followerAppliedVoltage.inVolts)?.let { - followerAppliedVoltage = it.volts - } - table?.get("elevatorFollowerStatorCurrentAmps", followerStatorCurrent.inAmperes)?.let { - followerStatorCurrent = it.amps - } - table?.get("elevatorFollowerSupplyCurrentAmps", followerSupplyCurrent.inAmperes)?.let { - followerSupplyCurrent = it.amps - } - table?.get("elevatorFollowerTempCelcius", followerTempCelcius.inCelsius)?.let { - followerTempCelcius = it.celsius - } - table?.get("elevatorLeaderRawPosition", leaderRawPosition)?.let { - leaderRawPosition = it - } - table?.get("elevatorFollowerRawPosition", leaderRawPosition)?.let { - followerRawPosition = it - } - } + table?.get("elevatorFollowerAppliedVolts", followerAppliedVoltage.inVolts)?.let { + followerAppliedVoltage = it.volts + } + table?.get("elevatorFollowerStatorCurrentAmps", followerStatorCurrent.inAmperes)?.let { + followerStatorCurrent = it.amps + } + table?.get("elevatorFollowerSupplyCurrentAmps", followerSupplyCurrent.inAmperes)?.let { + followerSupplyCurrent = it.amps + } + table?.get("elevatorFollowerTempCelcius", followerTempCelcius.inCelsius)?.let { + followerTempCelcius = it.celsius + } + table?.get("elevatorLeaderRawPosition", leaderRawPosition)?.let { leaderRawPosition = it } + table?.get("elevatorFollowerRawPosition", leaderRawPosition)?.let { followerRawPosition = it } } + } - fun updateInputs(inputs: ElevatorInputs) {} - fun setOutputVoltage(voltage: ElectricalPotential) {} - fun setPosition(position: Length, feedForward: ElectricalPotential) {} - fun zeroEncoder() {} - fun configPID( - kP: ProportionalGain, - kI: IntegralGain, - kD: DerivativeGain - ) {} + fun updateInputs(inputs: ElevatorInputs) {} + fun setOutputVoltage(voltage: ElectricalPotential) {} + fun setPosition(position: Length, feedForward: ElectricalPotential) {} + fun zeroEncoder() {} + fun configPID( + kP: ProportionalGain, + kI: IntegralGain, + kD: DerivativeGain + ) {} } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOKraken.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOKraken.kt index 97b6ad8b..fd3907d1 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOKraken.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOKraken.kt @@ -6,113 +6,176 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration import com.ctre.phoenix6.controls.PositionDutyCycle import com.ctre.phoenix6.hardware.TalonFX import com.ctre.phoenix6.signals.NeutralModeValue +import com.team4099.lib.phoenix6.PositionVoltage import com.team4099.robot2023.config.constants.Constants import com.team4099.robot2023.config.constants.ElevatorConstants import com.team4099.robot2023.subsystems.falconspin.Falcon500 import com.team4099.robot2023.subsystems.falconspin.MotorChecker import com.team4099.robot2023.subsystems.falconspin.MotorCollection -import edu.wpi.first.math.controller.PIDController -import org.team4099.lib.units.Velocity -import org.team4099.lib.units.base.* +import org.team4099.lib.units.base.Length +import org.team4099.lib.units.base.Meter +import org.team4099.lib.units.base.amps +import org.team4099.lib.units.base.celsius +import org.team4099.lib.units.base.inAmperes +import org.team4099.lib.units.base.inSeconds import org.team4099.lib.units.ctreLinearMechanismSensor -import org.team4099.lib.units.derived.* - -object ElevatorIOKraken: ElevatorIO { - private val elevatorLeaderKraken: TalonFX = TalonFX(Constants.Elevator.LEADER_MOTOR_ID) - private val elevatorFollowerKraken: TalonFX = TalonFX(Constants.Elevator.FOLLOWER_MOTOR_ID) - private val leaderSensor - - - - = ctreLinearMechanismSensor(elevatorLeaderKraken, ElevatorConstants.SENSOR_CPR, ElevatorConstants.GEAR_RATIO, ElevatorConstants.SPOOL_DIAMETER, ElevatorConstants.VOLTAGE_COMPENSATION) - private val followerSensor = ctreLinearMechanismSensor(elevatorLeaderKraken, ElevatorConstants.SENSOR_CPR, ElevatorConstants.GEAR_RATIO, ElevatorConstants.SPOOL_DIAMETER, ElevatorConstants.VOLTAGE_COMPENSATION) - private val elevatorLeaderConfiguration: TalonFXConfiguration = TalonFXConfiguration() - private val elevatorFollowerConfiguration: TalonFXConfiguration = TalonFXConfiguration() - - var elevatorLeaderStatorCurrentSignal: StatusSignal - var elevatorLeaderSupplyCurrentSignal: StatusSignal - var elevatorLeadertempSignal: StatusSignal - var elevatorLeaderDutyCycle: StatusSignal - var elevatorFollowerStatorCurrentSignal: StatusSignal - var elevatorFollowerSupplyCurrentSignal: StatusSignal - var elevatorFollowertempSignal: StatusSignal - var elevatorFollowerDutyCycle: StatusSignal - - init { - elevatorLeaderKraken.clearStickyFaults() - elevatorFollowerKraken.clearStickyFaults() - elevatorLeaderConfiguration.Slot0.kP = leaderSensor.proportionalVelocityGainToRawUnits(ElevatorConstants.LEADER_KP) - elevatorLeaderConfiguration.Slot0.kI = leaderSensor.integralVelocityGainToRawUnits(ElevatorConstants.LEADER_KI) - elevatorLeaderConfiguration.Slot0.kD = leaderSensor.derivativeVelocityGainToRawUnits(ElevatorConstants.LEADER_KD) - - elevatorFollowerConfiguration.Slot0.kP = followerSensor.proportionalVelocityGainToRawUnits(ElevatorConstants.FOLLOWER_KP) - elevatorFollowerConfiguration.Slot0.kI = followerSensor.integralVelocityGainToRawUnits(ElevatorConstants.FOLLOWER_KI) - elevatorFollowerConfiguration.Slot0.kD = followerSensor.derivativeVelocityGainToRawUnits(ElevatorConstants.FOLLOWER_KD) - - elevatorLeaderConfiguration.CurrentLimits.SupplyCurrentLimit = ElevatorConstants.LEADER_SUPPLY_CURRENT_LIMIT.inAmperes - elevatorLeaderConfiguration.CurrentLimits.SupplyCurrentThreshold = ElevatorConstants.LEADER_THRESHOLD_CURRENT_LIMIT.inAmperes - elevatorLeaderConfiguration.CurrentLimits.SupplyTimeThreshold = ElevatorConstants.LEADER_SUPPLY_TIME_THRESHOLD.inSeconds - elevatorLeaderConfiguration.CurrentLimits.SupplyCurrentLimitEnable = true - elevatorLeaderConfiguration.CurrentLimits.StatorCurrentLimit = ElevatorConstants.LEADER_STATOR_CURRENT_LIMIT.inAmperes - elevatorLeaderConfiguration.CurrentLimits.StatorCurrentLimitEnable = false - - elevatorFollowerConfiguration.CurrentLimits.SupplyCurrentLimit = ElevatorConstants.FOLLOWER_SUPPLY_CURRENT_LIMIT.inAmperes - elevatorFollowerConfiguration.CurrentLimits.SupplyCurrentThreshold = ElevatorConstants.FOLLOWER_THRESHOLD_CURRENT_LIMIT.inAmperes - elevatorFollowerConfiguration.CurrentLimits.SupplyTimeThreshold = ElevatorConstants.FOLLOWER_SUPPLY_TIME_THRESHOLD.inSeconds - elevatorFollowerConfiguration.CurrentLimits.SupplyCurrentLimitEnable = true - elevatorFollowerConfiguration.CurrentLimits.StatorCurrentLimit = ElevatorConstants.FOLLOWER_STATOR_CURRENT_LIMIT.inAmperes - elevatorFollowerConfiguration.CurrentLimits.StatorCurrentLimitEnable = false - - elevatorLeaderConfiguration.MotorOutput.NeutralMode = NeutralModeValue.Brake - elevatorFollowerConfiguration.MotorOutput.NeutralMode = NeutralModeValue.Brake - elevatorLeaderKraken.configurator.apply(elevatorLeaderConfiguration) - elevatorFollowerKraken.configurator.apply(elevatorFollowerConfiguration) - elevatorLeaderKraken.inverted = true - elevatorFollowerKraken.inverted = false - MotorChecker.add("Elevator", "Extension", MotorCollection(mutableListOf( - Falcon500(elevatorLeaderKraken, "Leader Extension Motor"), (Falcon500(elevatorFollowerKraken, "Follower Extension Motor"))), ElevatorConstants.LEADER_STATOR_CURRENT_LIMIT, 30.celsius, ElevatorConstants.LEADER_STATOR_CURRENT_LIMIT-30.amps, 110.celsius)) - elevatorLeaderStatorCurrentSignal = elevatorLeaderKraken.statorCurrent - elevatorLeaderSupplyCurrentSignal = elevatorLeaderKraken.supplyCurrent - elevatorLeadertempSignal = elevatorLeaderKraken.deviceTemp - elevatorLeaderDutyCycle = elevatorLeaderKraken.dutyCycle - elevatorFollowerStatorCurrentSignal = elevatorFollowerKraken.statorCurrent - elevatorFollowerSupplyCurrentSignal = elevatorFollowerKraken.supplyCurrent - elevatorFollowertempSignal = elevatorFollowerKraken.deviceTemp - elevatorFollowerDutyCycle = elevatorFollowerKraken.dutyCycle - } - - override fun configPID(kP: ProportionalGain, kI: IntegralGain, kD: DerivativeGain) { - val pidController = Slot0Configs() - pidController.kP = leaderSensor.proportionalPositionGainToRawUnits(kP) - pidController.kI = leaderSensor.integralPositionGainToRawUnits(kI) - pidController.kD = leaderSensor.derivativePositionGainToRawUnits(kD) - } - - override fun updateInputs(inputs: ElevatorIO.ElevatorInputs) { - inputs.elevatorPosition = leaderSensor.position - inputs.elevatorVelocity = leaderSensor.velocity - inputs.leaderAppliedVoltage = elevatorLeaderDutyCycle.value.volts - inputs.followerAppliedVoltage = elevatorFollowerDutyCycle.value.volts - inputs.leaderSupplyCurrent = elevatorLeaderSupplyCurrentSignal.value.amps - inputs.leaderStatorCurrent = elevatorLeaderStatorCurrentSignal.value.amps - inputs.followerSupplyCurrent = elevatorFollowerSupplyCurrentSignal.value.amps - inputs.followerStatorCurrent = elevatorFollowerStatorCurrentSignal.value.amps - inputs.leaderTempCelcius = elevatorLeadertempSignal.value.celsius - inputs.followerTempCelcius = elevatorFollowertempSignal.value.celsius - inputs.leaderRawPosition = leaderSensor.getRawPosition() - inputs.followerRawPosition = followerSensor.getRawPosition() - } - - override fun setOutputVoltage(voltage: ElectricalPotential) { - elevatorLeaderKraken.setVoltage(voltage.inVolts) - } - - override fun setPosition(position: Length, feedForward: ElectricalPotential) { - elevatorLeaderKraken.setControl(PositionDutyCycle(leaderSensor.positionToRawUnits(position), 0.0, true, feedForward.inVolts, 0, false, false, false)) - } - - override fun zeroEncoder() { - elevatorLeaderKraken.setPosition(0.0) - elevatorFollowerKraken.setPosition(0.0) - } -} \ No newline at end of file +import org.team4099.lib.units.derived.DerivativeGain +import org.team4099.lib.units.derived.ElectricalPotential +import org.team4099.lib.units.derived.IntegralGain +import org.team4099.lib.units.derived.ProportionalGain +import org.team4099.lib.units.derived.Volt +import org.team4099.lib.units.derived.inVolts +import org.team4099.lib.units.derived.volts + +object ElevatorIOKraken : ElevatorIO { + private val elevatorLeaderKraken: TalonFX = TalonFX(Constants.Elevator.LEADER_MOTOR_ID) + private val elevatorFollowerKraken: TalonFX = TalonFX(Constants.Elevator.FOLLOWER_MOTOR_ID) + private val leaderSensor = + ctreLinearMechanismSensor( + elevatorLeaderKraken, + ElevatorConstants.SENSOR_CPR, + ElevatorConstants.GEAR_RATIO, + ElevatorConstants.SPOOL_DIAMETER, + ElevatorConstants.VOLTAGE_COMPENSATION + ) + private val followerSensor = + ctreLinearMechanismSensor( + elevatorLeaderKraken, + ElevatorConstants.SENSOR_CPR, + ElevatorConstants.GEAR_RATIO, + ElevatorConstants.SPOOL_DIAMETER, + ElevatorConstants.VOLTAGE_COMPENSATION + ) + private val elevatorLeaderConfiguration: TalonFXConfiguration = TalonFXConfiguration() + private val elevatorFollowerConfiguration: TalonFXConfiguration = TalonFXConfiguration() + + var elevatorLeaderStatorCurrentSignal: StatusSignal + var elevatorLeaderSupplyCurrentSignal: StatusSignal + var elevatorLeadertempSignal: StatusSignal + var elevatorLeaderDutyCycle: StatusSignal + var elevatorFollowerStatorCurrentSignal: StatusSignal + var elevatorFollowerSupplyCurrentSignal: StatusSignal + var elevatorFollowertempSignal: StatusSignal + var elevatorFollowerDutyCycle: StatusSignal + + init { + elevatorLeaderKraken.clearStickyFaults() + elevatorFollowerKraken.clearStickyFaults() + elevatorLeaderConfiguration.Slot0.kP = + leaderSensor.proportionalPositionGainToRawUnits(ElevatorConstants.LEADER_KP) + elevatorLeaderConfiguration.Slot0.kI = + leaderSensor.integralPositionGainToRawUnits(ElevatorConstants.LEADER_KI) + elevatorLeaderConfiguration.Slot0.kD = + leaderSensor.derivativePositionGainToRawUnits(ElevatorConstants.LEADER_KD) + + elevatorFollowerConfiguration.Slot0.kP = + followerSensor.proportionalPositionGainToRawUnits(ElevatorConstants.FOLLOWER_KP) + elevatorFollowerConfiguration.Slot0.kI = + followerSensor.integralPositionGainToRawUnits(ElevatorConstants.FOLLOWER_KI) + elevatorFollowerConfiguration.Slot0.kD = + followerSensor.derivativePositionGainToRawUnits(ElevatorConstants.FOLLOWER_KD) + + elevatorLeaderConfiguration.CurrentLimits.SupplyCurrentLimit = + ElevatorConstants.LEADER_SUPPLY_CURRENT_LIMIT.inAmperes + elevatorLeaderConfiguration.CurrentLimits.SupplyCurrentThreshold = + ElevatorConstants.LEADER_THRESHOLD_CURRENT_LIMIT.inAmperes + elevatorLeaderConfiguration.CurrentLimits.SupplyTimeThreshold = + ElevatorConstants.LEADER_SUPPLY_TIME_THRESHOLD.inSeconds + elevatorLeaderConfiguration.CurrentLimits.SupplyCurrentLimitEnable = true + elevatorLeaderConfiguration.CurrentLimits.StatorCurrentLimit = + ElevatorConstants.LEADER_STATOR_CURRENT_LIMIT.inAmperes + elevatorLeaderConfiguration.CurrentLimits.StatorCurrentLimitEnable = false + + elevatorFollowerConfiguration.CurrentLimits.SupplyCurrentLimit = + ElevatorConstants.FOLLOWER_SUPPLY_CURRENT_LIMIT.inAmperes + elevatorFollowerConfiguration.CurrentLimits.SupplyCurrentThreshold = + ElevatorConstants.FOLLOWER_THRESHOLD_CURRENT_LIMIT.inAmperes + elevatorFollowerConfiguration.CurrentLimits.SupplyTimeThreshold = + ElevatorConstants.FOLLOWER_SUPPLY_TIME_THRESHOLD.inSeconds + elevatorFollowerConfiguration.CurrentLimits.SupplyCurrentLimitEnable = true + elevatorFollowerConfiguration.CurrentLimits.StatorCurrentLimit = + ElevatorConstants.FOLLOWER_STATOR_CURRENT_LIMIT.inAmperes + elevatorFollowerConfiguration.CurrentLimits.StatorCurrentLimitEnable = false + + elevatorLeaderConfiguration.MotorOutput.NeutralMode = NeutralModeValue.Brake + elevatorFollowerConfiguration.MotorOutput.NeutralMode = NeutralModeValue.Brake + elevatorLeaderKraken.configurator.apply(elevatorLeaderConfiguration) + elevatorFollowerKraken.configurator.apply(elevatorFollowerConfiguration) + elevatorLeaderKraken.inverted = true + elevatorFollowerKraken.inverted = false + MotorChecker.add( + "Elevator", + "Extension", + MotorCollection( + mutableListOf( + Falcon500(elevatorLeaderKraken, "Leader Extension Motor"), + (Falcon500(elevatorFollowerKraken, "Follower Extension Motor")) + ), + ElevatorConstants.LEADER_STATOR_CURRENT_LIMIT, + 30.celsius, + ElevatorConstants.LEADER_STATOR_CURRENT_LIMIT - 30.amps, + 110.celsius + ) + ) + elevatorLeaderStatorCurrentSignal = elevatorLeaderKraken.statorCurrent + elevatorLeaderSupplyCurrentSignal = elevatorLeaderKraken.supplyCurrent + elevatorLeadertempSignal = elevatorLeaderKraken.deviceTemp + elevatorLeaderDutyCycle = elevatorLeaderKraken.dutyCycle + elevatorFollowerStatorCurrentSignal = elevatorFollowerKraken.statorCurrent + elevatorFollowerSupplyCurrentSignal = elevatorFollowerKraken.supplyCurrent + elevatorFollowertempSignal = elevatorFollowerKraken.deviceTemp + elevatorFollowerDutyCycle = elevatorFollowerKraken.dutyCycle + } + + override fun configPID( + kP: ProportionalGain, + kI: IntegralGain, + kD: DerivativeGain + ) { + val pidConfiguration = Slot0Configs() + pidConfiguration.kP = leaderSensor.proportionalPositionGainToRawUnits(kP) + pidConfiguration.kI = leaderSensor.integralPositionGainToRawUnits(kI) + pidConfiguration.kD = leaderSensor.derivativePositionGainToRawUnits(kD) + elevatorLeaderKraken.configurator.apply(pidConfiguration) + elevatorFollowerKraken.configurator.apply(pidConfiguration) + } + + override fun updateInputs(inputs: ElevatorIO.ElevatorInputs) { + inputs.elevatorPosition = leaderSensor.position + inputs.elevatorVelocity = leaderSensor.velocity + inputs.leaderAppliedVoltage = elevatorLeaderDutyCycle.value.volts + inputs.followerAppliedVoltage = elevatorFollowerDutyCycle.value.volts + inputs.leaderSupplyCurrent = elevatorLeaderSupplyCurrentSignal.value.amps + inputs.leaderStatorCurrent = elevatorLeaderStatorCurrentSignal.value.amps + inputs.followerSupplyCurrent = elevatorFollowerSupplyCurrentSignal.value.amps + inputs.followerStatorCurrent = elevatorFollowerStatorCurrentSignal.value.amps + inputs.leaderTempCelcius = elevatorLeadertempSignal.value.celsius + inputs.followerTempCelcius = elevatorFollowertempSignal.value.celsius + inputs.leaderRawPosition = leaderSensor.getRawPosition() + inputs.followerRawPosition = followerSensor.getRawPosition() + } + + override fun setOutputVoltage(voltage: ElectricalPotential) { + elevatorLeaderKraken.setVoltage(voltage.inVolts) + } + + override fun setPosition(position: Length, feedForward: ElectricalPotential) { + elevatorLeaderKraken.setControl( + PositionDutyCycle( + leaderSensor.positionToRawUnits(position), + 0.0, + true, + feedForward.inVolts, + 0, + false, + false, + false + ) + ) + } + + override fun zeroEncoder() { + elevatorLeaderKraken.setPosition(0.0) + elevatorFollowerKraken.setPosition(0.0) + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIONEO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIONEO.kt index 72cded85..7aa8c0c5 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIONEO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIONEO.kt @@ -28,161 +28,173 @@ import kotlin.math.absoluteValue object ElevatorIONEO : ElevatorIO { - private val leaderSparkMax = - CANSparkMax(Constants.Elevator.LEADER_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless) - - private val leaderSensor = - sparkMaxLinearMechanismSensor( - leaderSparkMax, - ElevatorConstants.GEAR_RATIO, - ElevatorConstants.SPOOL_DIAMETER, - ElevatorConstants.VOLTAGE_COMPENSATION - ) - - private val followerSparkMax = - CANSparkMax(Constants.Elevator.FOLLOWER_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless) - - private val leaderPIDController: SparkMaxPIDController = leaderSparkMax.pidController - - init { - - // reseting motor - leaderSparkMax.restoreFactoryDefaults() - followerSparkMax.restoreFactoryDefaults() - - leaderSparkMax.clearFaults() - followerSparkMax.clearFaults() + private val leaderSparkMax = + CANSparkMax(Constants.Elevator.LEADER_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless) - // basic settings - leaderSparkMax.enableVoltageCompensation(ElevatorConstants.VOLTAGE_COMPENSATION.inVolts) - followerSparkMax.enableVoltageCompensation(ElevatorConstants.VOLTAGE_COMPENSATION.inVolts) + private val leaderSensor = + sparkMaxLinearMechanismSensor( + leaderSparkMax, + ElevatorConstants.GEAR_RATIO, + ElevatorConstants.SPOOL_DIAMETER, + ElevatorConstants.VOLTAGE_COMPENSATION + ) - leaderSparkMax.inverted = true + private val followerSparkMax = + CANSparkMax(Constants.Elevator.FOLLOWER_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless) - leaderSparkMax.setSmartCurrentLimit(ElevatorConstants.LEADER_STATOR_CURRENT_LIMIT.inAmperes.toInt()) - followerSparkMax.setSmartCurrentLimit(ElevatorConstants.FOLLOWER_STATOR_CURRENT_LIMIT.inAmperes.toInt()) + private val followerSensor = + sparkMaxLinearMechanismSensor( + followerSparkMax, + ElevatorConstants.GEAR_RATIO, + ElevatorConstants.SPOOL_DIAMETER, + ElevatorConstants.VOLTAGE_COMPENSATION + ) - leaderSparkMax.openLoopRampRate = ElevatorConstants.RAMP_RATE.inPercentOutputPerSecond - followerSparkMax.openLoopRampRate = ElevatorConstants.RAMP_RATE.inPercentOutputPerSecond - - leaderSparkMax.idleMode = CANSparkMax.IdleMode.kBrake - followerSparkMax.idleMode = CANSparkMax.IdleMode.kBrake - - // makes follower motor output exact same power as leader - followerSparkMax.follow(leaderSparkMax, ElevatorConstants.FOLLOWER_INVERTED) - - leaderPIDController.ff = 0.0 - - leaderSparkMax.burnFlash() - followerSparkMax.burnFlash() - - MotorChecker.add( - "Elevator", - "Extension", - MotorCollection( - mutableListOf( - Neo(leaderSparkMax, "Leader Extension Motor"), - Neo(followerSparkMax, "Follower Extension Motor") - ), - ElevatorConstants.LEADER_STATOR_CURRENT_LIMIT, - 30.celsius, - ElevatorConstants.LEADER_STATOR_CURRENT_LIMIT - 30.amps, - 90.celsius - ), - ) - } - - override fun updateInputs(inputs: ElevatorIO.ElevatorInputs) { - inputs.elevatorPosition = leaderSensor.position - - inputs.elevatorVelocity = leaderSensor.velocity - - // voltage in * percent out - inputs.leaderAppliedVoltage = leaderSparkMax.busVoltage.volts * leaderSparkMax.appliedOutput - - inputs.leaderStatorCurrent = leaderSparkMax.outputCurrent.amps - - // BatteryVoltage * SupplyCurrent = percentOutput * BatteryVoltage * StatorCurrent - // AppliedVoltage = percentOutput * BatteryVoltage - // SupplyCurrent = (percentOutput * BatteryVoltage / BatteryVoltage) * StatorCurrent = - // percentOutput * statorCurrent - - inputs.leaderSupplyCurrent = - inputs.leaderStatorCurrent * leaderSparkMax.appliedOutput.absoluteValue - - inputs.leaderTempCelcius = leaderSparkMax.motorTemperature.celsius - - // voltage in * percent out - inputs.followerAppliedVoltage = leaderSparkMax.busVoltage.volts * followerSparkMax.appliedOutput - - inputs.followerStatorCurrent = followerSparkMax.outputCurrent.amps - - inputs.followerSupplyCurrent = - inputs.followerStatorCurrent * followerSparkMax.appliedOutput.absoluteValue - - inputs.followerTempCelcius = followerSparkMax.motorTemperature.celsius - - inputs.leaderRawPosition = leaderSparkMax.encoder.position - - inputs.followerRawPosition = followerSparkMax.encoder.position - - Logger.recordOutput("Elevator/leaderRawRotations", leaderSparkMax.encoder.position) - } - - /** - * Sets the voltage of the elevator motors but also checks to make sure elevator doesn't exceed - * limit - * - * @param voltage the voltage to set the motor to - */ - override fun setOutputVoltage(voltage: ElectricalPotential) { - // divide by 2 cause full power elevator is scary - leaderSparkMax.setVoltage(voltage.inVolts) - } - - /** - * Sets the voltage of the elevator motors but also checks to make sure elevator doesn't exceed - * limit - * - * @param position the target position the PID controller will use - * @param feedforward change in voltage to account for external forces on the system - */ - override fun setPosition(position: Length, feedforward: ElectricalPotential) { - leaderPIDController.setReference( - leaderSensor.positionToRawUnits( - clamp( - position, - ElevatorConstants.ELEVATOR_SOFT_LIMIT_RETRACTION, - ElevatorConstants.ELEVATOR_SOFT_LIMIT_EXTENSION - ) - ), - CANSparkMax.ControlType.kPosition, - 0, - feedforward.inVolts, + private val leaderPIDController: SparkMaxPIDController = leaderSparkMax.pidController + private val followerPIDController: SparkMaxPIDController = followerSparkMax.pidController + init { + + // reseting motor + leaderSparkMax.restoreFactoryDefaults() + followerSparkMax.restoreFactoryDefaults() + + leaderSparkMax.clearFaults() + followerSparkMax.clearFaults() + + // basic settings + leaderSparkMax.enableVoltageCompensation(ElevatorConstants.VOLTAGE_COMPENSATION.inVolts) + followerSparkMax.enableVoltageCompensation(ElevatorConstants.VOLTAGE_COMPENSATION.inVolts) + + leaderSparkMax.inverted = true + + leaderSparkMax.setSmartCurrentLimit( + ElevatorConstants.LEADER_STATOR_CURRENT_LIMIT.inAmperes.toInt() + ) + followerSparkMax.setSmartCurrentLimit( + ElevatorConstants.FOLLOWER_STATOR_CURRENT_LIMIT.inAmperes.toInt() + ) + + leaderSparkMax.idleMode = CANSparkMax.IdleMode.kBrake + followerSparkMax.idleMode = CANSparkMax.IdleMode.kBrake + + // makes follower motor output exact same power as leader + followerSparkMax.follow(leaderSparkMax, ElevatorConstants.FOLLOWER_INVERTED) + + leaderPIDController.ff = 0.0 + + leaderSparkMax.burnFlash() + followerSparkMax.burnFlash() + + MotorChecker.add( + "Elevator", + "Extension", + MotorCollection( + mutableListOf( + Neo(leaderSparkMax, "Leader Extension Motor"), + Neo(followerSparkMax, "Follower Extension Motor") + ), + ElevatorConstants.LEADER_STATOR_CURRENT_LIMIT, + 30.celsius, + ElevatorConstants.LEADER_STATOR_CURRENT_LIMIT - 30.amps, + 90.celsius + ), + ) + } + + override fun updateInputs(inputs: ElevatorIO.ElevatorInputs) { + inputs.elevatorPosition = leaderSensor.position + + inputs.elevatorVelocity = leaderSensor.velocity + + // voltage in * percent out + inputs.leaderAppliedVoltage = leaderSparkMax.busVoltage.volts * leaderSparkMax.appliedOutput + + inputs.leaderStatorCurrent = leaderSparkMax.outputCurrent.amps + + // BatteryVoltage * SupplyCurrent = percentOutput * BatteryVoltage * StatorCurrent + // AppliedVoltage = percentOutput * BatteryVoltage + // SupplyCurrent = (percentOutput * BatteryVoltage / BatteryVoltage) * StatorCurrent = + // percentOutput * statorCurrent + + inputs.leaderSupplyCurrent = + inputs.leaderStatorCurrent * leaderSparkMax.appliedOutput.absoluteValue + + inputs.leaderTempCelcius = leaderSparkMax.motorTemperature.celsius + + // voltage in * percent out + inputs.followerAppliedVoltage = leaderSparkMax.busVoltage.volts * followerSparkMax.appliedOutput + + inputs.followerStatorCurrent = followerSparkMax.outputCurrent.amps + + inputs.followerSupplyCurrent = + inputs.followerStatorCurrent * followerSparkMax.appliedOutput.absoluteValue + + inputs.followerTempCelcius = followerSparkMax.motorTemperature.celsius + + inputs.leaderRawPosition = leaderSparkMax.encoder.position + + inputs.followerRawPosition = followerSparkMax.encoder.position + + Logger.recordOutput("Elevator/leaderRawRotations", leaderSparkMax.encoder.position) + } + + /** + * Sets the voltage of the elevator motors but also checks to make sure elevator doesn't exceed + * limit + * + * @param voltage the voltage to set the motor to + */ + override fun setOutputVoltage(voltage: ElectricalPotential) { + // divide by 2 cause full power elevator is scary + leaderSparkMax.setVoltage(voltage.inVolts) + } + + /** + * Sets the voltage of the elevator motors but also checks to make sure elevator doesn't exceed + * limit + * + * @param position the target position the PID controller will use + * @param feedforward change in voltage to account for external forces on the system + */ + override fun setPosition(position: Length, feedforward: ElectricalPotential) { + leaderPIDController.setReference( + leaderSensor.positionToRawUnits( + clamp( + position, + ElevatorConstants.ELEVATOR_SOFT_LIMIT_RETRACTION, + ElevatorConstants.ELEVATOR_SOFT_LIMIT_EXTENSION ) - } - - /** set the current encoder position to be the encoders zero value */ - override fun zeroEncoder() { - leaderSparkMax.encoder.position = 0.0 - followerSparkMax.encoder.position = 0.0 - } - - /** - * updates the PID controller values using the sensor measurement for proportional intregral and - * derivative gain multiplied by the 3 PID constants - * - * @param kP a constant which will be used to scale the proportion gain - * @param kI a constant which will be used to scale the integral gain - * @param kD a constant which will be used to scale the derivative gain - */ - override fun configPID( - kP: ProportionalGain, - kI: IntegralGain, - kD: DerivativeGain - ) { - leaderPIDController.p = leaderSensor.proportionalPositionGainToRawUnits(kP) - leaderPIDController.i = leaderSensor.integralPositionGainToRawUnits(kI) - leaderPIDController.d = leaderSensor.derivativePositionGainToRawUnits(kD) - } -} \ No newline at end of file + ), + CANSparkMax.ControlType.kPosition, + 0, + feedforward.inVolts, + ) + } + + /** set the current encoder position to be the encoders zero value */ + override fun zeroEncoder() { + leaderSparkMax.encoder.position = 0.0 + followerSparkMax.encoder.position = 0.0 + } + + /** + * updates the PID controller values using the sensor measurement for proportional intregral and + * derivative gain multiplied by the 3 PID constants + * + * @param kP a constant which will be used to scale the proportion gain + * @param kI a constant which will be used to scale the integral gain + * @param kD a constant which will be used to scale the derivative gain + */ + override fun configPID( + kP: ProportionalGain, + kI: IntegralGain, + kD: DerivativeGain + ) { + leaderPIDController.p = leaderSensor.proportionalPositionGainToRawUnits(kP) + leaderPIDController.i = leaderSensor.integralPositionGainToRawUnits(kI) + leaderPIDController.d = leaderSensor.derivativePositionGainToRawUnits(kD) + followerPIDController.p = followerSensor.proportionalPositionGainToRawUnits(kP) + followerPIDController.i = followerSensor.integralPositionGainToRawUnits(kI) + followerPIDController.d = followerSensor.derivativePositionGainToRawUnits(kD) + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOSim.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOSim.kt index 0acf1792..915c87ac 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOSim.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOSim.kt @@ -11,7 +11,14 @@ import edu.wpi.first.wpilibj.simulation.BatterySim import edu.wpi.first.wpilibj.simulation.ElevatorSim import edu.wpi.first.wpilibj.simulation.RoboRioSim import org.team4099.lib.controller.PIDController -import org.team4099.lib.units.base.* +import org.team4099.lib.units.base.Length +import org.team4099.lib.units.base.Meter +import org.team4099.lib.units.base.amps +import org.team4099.lib.units.base.celsius +import org.team4099.lib.units.base.inKilograms +import org.team4099.lib.units.base.inMeters +import org.team4099.lib.units.base.inSeconds +import org.team4099.lib.units.base.meters import org.team4099.lib.units.derived.DerivativeGain import org.team4099.lib.units.derived.ElectricalPotential import org.team4099.lib.units.derived.IntegralGain @@ -23,124 +30,125 @@ import org.team4099.lib.units.perSecond object ElevatorIOSim : ElevatorIO { - val elevatorSim: ElevatorSim = ElevatorSim( - DCMotor.getNEO(2), - ElevatorConstants.GEAR_RATIO, - ElevatorConstants.CARRIAGE_MASS.inKilograms, - ElevatorConstants.SPOOL_DIAMETER.inMeters/2, - ElevatorConstants.ELEVATOR_MAX_RETRACTION.inMeters, - ElevatorConstants.ELEVATOR_MAX_EXTENSION.inMeters, - true, - 0.0 - ) - - init { - MotorChecker.add( - "Elevator", - "Extension", - MotorCollection( - mutableListOf( - SimulatedMotor( - elevatorSim, - "Extension Motor", - ) - ), - baseCurrentLimit = 60.amps, - firstStageTemperatureLimit = 10.celsius, - firstStageCurrentLimit = 45.amps, - motorShutDownThreshold = 20.celsius - ) - ) - } - - private val elevatorController = - PIDController(ElevatorConstants.SIM_KP, ElevatorConstants.SIM_KI, ElevatorConstants.SIM_KD) - - private var lastAppliedVoltage = 0.0.volts - - override fun updateInputs(inputs: ElevatorIO.ElevatorInputs) { - elevatorSim.update(Constants.Universal.LOOP_PERIOD_TIME.inSeconds) - - inputs.elevatorPosition = elevatorSim.positionMeters.meters - inputs.elevatorVelocity = elevatorSim.velocityMetersPerSecond.meters.perSecond - - inputs.leaderTempCelcius = 0.0.celsius - inputs.leaderStatorCurrent = 0.0.amps - inputs.leaderSupplyCurrent = elevatorSim.currentDrawAmps.amps / 2 - inputs.leaderAppliedVoltage = lastAppliedVoltage - - inputs.followerTempCelcius = 0.0.celsius - inputs.followerStatorCurrent = 0.0.amps - inputs.followerSupplyCurrent = elevatorSim.currentDrawAmps.amps / 2 - inputs.followerAppliedVoltage = lastAppliedVoltage - - inputs.leaderRawPosition = 0.0 - inputs.followerRawPosition = 0.0 - - inputs.isSimulating = true - - RoboRioSim.setVInVoltage( - BatterySim.calculateDefaultBatteryLoadedVoltage(elevatorSim.currentDrawAmps) - ) - } - - /** - * Sets the voltage of the elevator motors but also checks to make sure elevator doesn't exceed - * limit - * - * @param voltage the voltage to set the motor to - */ - override fun setOutputVoltage(voltage: ElectricalPotential) { - val clampedVoltage = - clamp( - voltage, - -ElevatorConstants.VOLTAGE_COMPENSATION, - ElevatorConstants.VOLTAGE_COMPENSATION - ) - - lastAppliedVoltage = clampedVoltage - - elevatorSim.setInputVoltage(clampedVoltage.inVolts) - } - - /** - * Sets the voltage of the elevator motors but also checks to make sure elevator doesn't exceed - * limit - * - * @param position the target position the PID controller will use - * @param feedforward change in voltage to account for external forces on the system - */ - override fun setPosition(position: Length, feedforward: ElectricalPotential) { - val ff = - clamp( - feedforward, - -ElevatorConstants.VOLTAGE_COMPENSATION, - ElevatorConstants.VOLTAGE_COMPENSATION - ) - val feedback = elevatorController.calculate(elevatorSim.positionMeters.meters, position) - - lastAppliedVoltage = ff + feedback - elevatorSim.setInputVoltage((ff + feedback).inVolts) - } - - /** set the current encoder position to be the encoders zero value */ - override fun zeroEncoder() { - println("don't work right now") - } - - /** - * updates the PID controller values using the sensor measurement for proportional intregral and - * derivative gain multiplied by the 3 PID constants - * - * @param kP a constant which will be used to scale the proportion gain - * @param kI a constant which will be used to scale the integral gain - * @param kD a constant which will be used to scale the derivative gain - */ - override fun configPID( - kP: ProportionalGain, - kI: IntegralGain, - kD: DerivativeGain - ) { - elevatorController.setPID(kP, kI, kD) - } -} \ No newline at end of file + val elevatorSim: ElevatorSim = + ElevatorSim( + DCMotor.getNEO(2), + ElevatorConstants.GEAR_RATIO, + ElevatorConstants.CARRIAGE_MASS.inKilograms, + ElevatorConstants.SPOOL_DIAMETER.inMeters / 2, + ElevatorConstants.ELEVATOR_MAX_RETRACTION.inMeters, + ElevatorConstants.ELEVATOR_MAX_EXTENSION.inMeters, + true, + 0.0 + ) + + init { + MotorChecker.add( + "Elevator", + "Extension", + MotorCollection( + mutableListOf( + SimulatedMotor( + elevatorSim, + "Extension Motor", + ) + ), + baseCurrentLimit = 60.amps, + firstStageTemperatureLimit = 10.celsius, + firstStageCurrentLimit = 45.amps, + motorShutDownThreshold = 20.celsius + ) + ) + } + + private val elevatorController = + PIDController(ElevatorConstants.SIM_KP, ElevatorConstants.SIM_KI, ElevatorConstants.SIM_KD) + + private var lastAppliedVoltage = 0.0.volts + + override fun updateInputs(inputs: ElevatorIO.ElevatorInputs) { + elevatorSim.update(Constants.Universal.LOOP_PERIOD_TIME.inSeconds) + + inputs.elevatorPosition = elevatorSim.positionMeters.meters + inputs.elevatorVelocity = elevatorSim.velocityMetersPerSecond.meters.perSecond + + inputs.leaderTempCelcius = 0.0.celsius + inputs.leaderStatorCurrent = 0.0.amps + inputs.leaderSupplyCurrent = elevatorSim.currentDrawAmps.amps / 2 + inputs.leaderAppliedVoltage = lastAppliedVoltage + + inputs.followerTempCelcius = 0.0.celsius + inputs.followerStatorCurrent = 0.0.amps + inputs.followerSupplyCurrent = elevatorSim.currentDrawAmps.amps / 2 + inputs.followerAppliedVoltage = lastAppliedVoltage + + inputs.leaderRawPosition = 0.0 + inputs.followerRawPosition = 0.0 + + inputs.isSimulating = true + + RoboRioSim.setVInVoltage( + BatterySim.calculateDefaultBatteryLoadedVoltage(elevatorSim.currentDrawAmps) + ) + } + + /** + * Sets the voltage of the elevator motors but also checks to make sure elevator doesn't exceed + * limit + * + * @param voltage the voltage to set the motor to + */ + override fun setOutputVoltage(voltage: ElectricalPotential) { + val clampedVoltage = + clamp( + voltage, + -ElevatorConstants.VOLTAGE_COMPENSATION, + ElevatorConstants.VOLTAGE_COMPENSATION + ) + + lastAppliedVoltage = clampedVoltage + + elevatorSim.setInputVoltage(clampedVoltage.inVolts) + } + + /** + * Sets the voltage of the elevator motors but also checks to make sure elevator doesn't exceed + * limit + * + * @param position the target position the PID controller will use + * @param feedforward change in voltage to account for external forces on the system + */ + override fun setPosition(position: Length, feedforward: ElectricalPotential) { + val ff = + clamp( + feedforward, + -ElevatorConstants.VOLTAGE_COMPENSATION, + ElevatorConstants.VOLTAGE_COMPENSATION + ) + val feedback = elevatorController.calculate(elevatorSim.positionMeters.meters, position) + + lastAppliedVoltage = ff + feedback + elevatorSim.setInputVoltage((ff + feedback).inVolts) + } + + /** set the current encoder position to be the encoders zero value */ + override fun zeroEncoder() { + println("don't work right now") + } + + /** + * updates the PID controller values using the sensor measurement for proportional intregral and + * derivative gain multiplied by the 3 PID constants + * + * @param kP a constant which will be used to scale the proportion gain + * @param kI a constant which will be used to scale the integral gain + * @param kD a constant which will be used to scale the derivative gain + */ + override fun configPID( + kP: ProportionalGain, + kI: IntegralGain, + kD: DerivativeGain + ) { + elevatorController.setPID(kP, kI, kD) + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt index a1e0e8c6..6ae241f6 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt @@ -1,24 +1,15 @@ package com.team4099.robot2023.subsystems.superstructure -import com.team4099.robot2023.config.constants.GamePiece -import com.team4099.robot2023.config.constants.NodeTier import edu.wpi.first.math.kinematics.ChassisSpeeds import org.team4099.lib.units.AngularVelocity import org.team4099.lib.units.LinearVelocity import org.team4099.lib.units.base.Length -import org.team4099.lib.units.base.inches import org.team4099.lib.units.derived.ElectricalPotential -import org.team4099.lib.units.perSecond - sealed interface Request { - - sealed interface ElevatorRequest : Request { - class TargetingPosition( - val position: Length - ) : ElevatorRequest + class TargetingPosition(val position: Length) : ElevatorRequest class OpenLoop(val voltage: ElectricalPotential) : ElevatorRequest class Home() : ElevatorRequest }