diff --git a/.gitignore b/.gitignore index 8842e0b4..8dcbce64 100644 --- a/.gitignore +++ b/.gitignore @@ -172,3 +172,4 @@ out/ # Simulation GUI and other tools window save file *-window.json + diff --git a/build.gradle b/build.gradle index ba2238c0..17663ff7 100644 --- a/build.gradle +++ b/build.gradle @@ -83,7 +83,7 @@ dependencies { simulationRelease wpi.sim.enableRelease() implementation 'org.jetbrains.kotlin:kotlin-test-junit5' - implementation 'com.github.team4099:FalconUtils:1.1.28' + implementation 'com.github.team4099:FalconUtils:1.1.29' implementation 'org.apache.commons:commons-collections4:4.0' implementation 'com.google.code.gson:gson:2.10.1' implementation "io.javalin:javalin:5.3.2" diff --git a/simgui-ds.json b/simgui-ds.json index 73cc713c..69b1a3cb 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -88,5 +88,10 @@ "buttonCount": 0, "povCount": 0 } + ], + "robotJoysticks": [ + { + "guid": "Keyboard0" + } ] } diff --git a/src/main/kotlin/com/team4099/lib/phoenix6/PositionVoltage.kt b/src/main/kotlin/com/team4099/lib/phoenix6/PositionVoltage.kt index 51816cb9..2efb09ec 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( - var position: Angle, // Assuming an AngularPosition type exists similar to AngularVelocity - var enableFOC: Boolean = true, - var feedforward: ElectricalPotential = 0.0.volts, - var slot: Int = 0, - var overrideBrakeDurNeutral: Boolean = false, - var limitForwardMotion: Boolean = false, - 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, + private 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 6efd986b..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(var velocity: AngularVelocity, - var acceleration: AngularAcceleration = 0.0.degrees.perSecond.perSecond, - var enableFOC:Boolean = true, - var feedforward: ElectricalPotential = 0.0.volts, - var slot:Int = 0, - var overrideBrakeDurNeutral: Boolean = false, - var limitForwardMotion: Boolean = false, - 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/BuildConstants.kt b/src/main/kotlin/com/team4099/robot2023/BuildConstants.kt index 39f5c23d..60a86e73 100644 --- a/src/main/kotlin/com/team4099/robot2023/BuildConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/BuildConstants.kt @@ -6,10 +6,10 @@ package com.team4099.robot2023 const val MAVEN_GROUP = "" const val MAVEN_NAME = "Crescendo-2024" const val VERSION = "unspecified" -const val GIT_REVISION = 28 -const val GIT_SHA = "eca16395eb62f3d6d367479ab32e985e6472149b" -const val GIT_DATE = "2024-01-16T14:49:50Z" -const val GIT_BRANCH = "targeting-command-with-drivetrain" -const val BUILD_DATE = "2024-01-16T15:27:50Z" -const val BUILD_UNIX_TIME = 1705436870465L +const val GIT_REVISION = 102 +const val GIT_SHA = "177b056f2e17e0175cc3fd37903303defea08ea0" +const val GIT_DATE = "2024-01-20T20:08:22Z" +const val GIT_BRANCH = "feeder" +const val BUILD_DATE = "2024-01-20T23:40:50Z" +const val BUILD_UNIX_TIME = 1705812050035L const val DIRTY = 1 diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt index c04891d5..6ebf4b96 100644 --- a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt +++ b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt @@ -9,34 +9,31 @@ import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain import com.team4099.robot2023.subsystems.drivetrain.drive.DrivetrainIO import com.team4099.robot2023.subsystems.drivetrain.drive.DrivetrainIOSim import com.team4099.robot2023.subsystems.drivetrain.gyro.GyroIO -import com.team4099.robot2023.subsystems.intake.Intake -import com.team4099.robot2023.subsystems.intake.IntakeIONEO -import com.team4099.robot2023.subsystems.intake.IntakeIOSim +import com.team4099.robot2023.subsystems.feeder.Feeder +import com.team4099.robot2023.subsystems.feeder.FeederIONeo +import com.team4099.robot2023.subsystems.feeder.FeederIOSim 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 - private val intake: Intake private val vision: Vision private val limelight: LimelightVision + private val feeder: Feeder init { if (RobotBase.isReal()) { // Real Hardware Implementations // drivetrain = Drivetrain(object: GyroIO {},object: DrivetrainIO {} drivetrain = Drivetrain(object : GyroIO {}, object : DrivetrainIO {}) - intake = Intake(IntakeIONEO) vision = Vision( // object: CameraIO {} @@ -48,10 +45,10 @@ object RobotContainer { // CameraIONorthstar("backward") ) limelight = LimelightVision(object : LimelightVisionIO {}) + feeder = Feeder(FeederIONeo) } else { // Simulation implementations drivetrain = Drivetrain(object : GyroIO {}, DrivetrainIOSim) - intake = Intake(IntakeIOSim) vision = Vision( CameraIONorthstar("northstar_1"), @@ -59,6 +56,7 @@ object RobotContainer { CameraIONorthstar("northstar_3"), ) limelight = LimelightVision(object : LimelightVisionIO {}) + feeder = Feeder(FeederIOSim) } vision.setDataInterfaces({ drivetrain.odometryPose }, { drivetrain.addVisionData(it) }) @@ -135,6 +133,9 @@ object RobotContainer { // Constants.Universal.Substation.SINGLE_SUBSTATION // ) // ) + + ControlBoard.feederShootTest.whileTrue(feeder.feederOpenLoopIntakeTestCommand()) + ControlBoard.feederIntakeTest.whileTrue(feeder.feederOpenLoopShootTestCommand()) } 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 9c6fea93..fb449709 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,7 @@ 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.subsystems.superstructure.Request.DrivetrainRequest import com.team4099.robot2023.util.AllianceFlipUtil import com.team4099.robot2023.util.Velocity2d import edu.wpi.first.math.kinematics.ChassisSpeeds @@ -22,9 +22,37 @@ 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 org.team4099.lib.units.* -import org.team4099.lib.units.base.* -import org.team4099.lib.units.derived.* +import org.team4099.lib.units.Velocity +import org.team4099.lib.units.base.Meter +import org.team4099.lib.units.base.inMeters +import org.team4099.lib.units.base.inSeconds +import org.team4099.lib.units.base.inches +import org.team4099.lib.units.base.meters +import org.team4099.lib.units.base.seconds +import org.team4099.lib.units.derived.Radian +import org.team4099.lib.units.derived.cos +import org.team4099.lib.units.derived.degrees +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.inMetersPerSecondPerMeter +import org.team4099.lib.units.derived.inMetersPerSecondPerMeterSeconds +import org.team4099.lib.units.derived.inMetersPerSecondPerMetersPerSecond +import org.team4099.lib.units.derived.inRadians +import org.team4099.lib.units.derived.metersPerSecondPerMetersPerSecond +import org.team4099.lib.units.derived.perDegree +import org.team4099.lib.units.derived.perDegreePerSecond +import org.team4099.lib.units.derived.perDegreeSeconds +import org.team4099.lib.units.derived.perMeter +import org.team4099.lib.units.derived.perMeterSeconds +import org.team4099.lib.units.derived.radians +import org.team4099.lib.units.derived.sin +import org.team4099.lib.units.inDegreesPerSecond +import org.team4099.lib.units.inMetersPerSecond +import org.team4099.lib.units.inMetersPerSecondPerSecond +import org.team4099.lib.units.inRadiansPerSecond +import org.team4099.lib.units.inRadiansPerSecondPerSecond +import org.team4099.lib.units.perSecond import java.util.function.Supplier import kotlin.math.PI @@ -216,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 ) @@ -281,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..bbfff2f6 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,24 @@ 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.Fraction +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 +100,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 +127,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 2a3446ac..76938ada 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt @@ -97,4 +97,8 @@ object ControlBoard { // for testing val setArmCommand = Trigger { technician.yButton } + + val feederIntakeTest = Trigger { driver.aButton } + val feederShootTest = Trigger { driver.bButton } } + diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt index 4f1f2add..75c5a2f6 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt @@ -115,9 +115,11 @@ object Constants { const val PIGEON_2_ID = 1 } - object Manipulator { - const val INTAKE_MOTOR_ID = 51 - const val ARM_MOTOR_ID = 52 + object Shooter { + const val FLYWHEEL_MOTOR_ID = 51 + // TODO find wrist motor id + const val SHOOTER_WRIST_MOTOR_ID = 999 + const val FEEDER_MOTOR_ID = 999 } object Alert { @@ -130,6 +132,8 @@ object Constants { const val REV_ENCODER_PORT = 7 } + object Feeder + object Led { const val LED_CANDLE_ID = 61 const val LED_BLINKEN_ID = 1 diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/FeederConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/FeederConstants.kt new file mode 100644 index 00000000..a4d0acdb --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/FeederConstants.kt @@ -0,0 +1,34 @@ +package com.team4099.robot2023.config.constants + +import org.team4099.lib.units.base.amps +import org.team4099.lib.units.base.grams +import org.team4099.lib.units.base.meters +import org.team4099.lib.units.base.seconds +import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.rotations +import org.team4099.lib.units.derived.volts +import org.team4099.lib.units.kilo +import org.team4099.lib.units.perMinute +import org.team4099.lib.units.perSecond + +object FeederConstants { + + val FEEDER_MOTOR_INVERTED = false + val FEEDER_IDLE_VOLTAGE = 0.0.volts + val VOLTAGE_COMPENSATION = 12.0.volts + val FEEDER_CURRENT_LIMIT = 40.0.amps + + val BEAM_BREAK_WAIT_TIME = 0.2.seconds + + val FEEDER_GEAR_RATIO = 24.0 / 12.0 + val FEEDER_INERTIA = 0.0017672509.kilo.grams * 1.0.meters.squared + + var NOTE_VELOCITY_THRESHOLD = 60.degrees.perSecond + + var WAIT_BEFORE_DETECT_VELOCITY_DROP = 1.seconds + + + val INTAKE_NOTE_VOLTAGE = 6.volts + val SHOOT_NOTE_VOLTAGE = 11.9.volts + +} diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/ShooterConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/ShooterConstants.kt new file mode 100644 index 00000000..450de710 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/ShooterConstants.kt @@ -0,0 +1,27 @@ +package com.team4099.robot2023.config.constants + +import org.team4099.lib.units.base.amps +import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.volts + +object ShooterConstants { + // val ROLLER_GEAR_RATIO = 0.0 + // val ROLLER_VOLTAGE_COMPENSATION = 0.0.volts + // val ROLLER_STATOR_CURRENT_LIMIT = 0.0.amps + + val WRIST_GEAR_RATIO = 0.0 + val WRIST_VOLTAGE_COMPENSATION = 0.0.volts + val WRIST_STATOR_CURRENT_LIMIT = 0.0.amps + + // val FEEDER_GEAR_RATIO = 0.0 + // val FEEDER_VOLTAGE_COMPENSATION = 0.0.volts + // val FEEDER_STATOR_CURRENT_LIMIT = 0.0.amps + + // val ROLLLER_INIT_VOLTAGE = 0.0.volts + // val FEEDER_INIT_VOLTAGE = 0.0.volts + val WRIST_INIT_VOLTAGE = 0.0.volts + + // val ROLLER_SHOOTING_VOLTAGE = 0.0.volts + val WRIST_SOFTLIMIT_UPWARDSTURN = 0.0.degrees + val WRIST_SOFTLIMIT_DOWNWARDSTURN = 0.0.degrees +} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/GroundIntake/GroundIntake.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/GroundIntake/GroundIntake.kt new file mode 100644 index 00000000..f74b8aa6 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/GroundIntake/GroundIntake.kt @@ -0,0 +1,3 @@ +package com.team4099.robot2023.subsystems.GroundIntake + +class GroundIntake diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt deleted file mode 100644 index c1ef2b6f..00000000 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt +++ /dev/null @@ -1,3 +0,0 @@ -package com.team4099.robot2023.subsystems.Shooter - -class Shooter 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..0eb3f017 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 f40da16e..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 @@ -2,121 +2,144 @@ package com.team4099.robot2023.subsystems.drivetrain.swervemodule import org.littletonrobotics.junction.LogTable import org.littletonrobotics.junction.inputs.LoggableInputs -import org.team4099.lib.units.* -import org.team4099.lib.units.base.* -import org.team4099.lib.units.derived.* +import org.team4099.lib.units.AngularAcceleration +import org.team4099.lib.units.AngularVelocity +import org.team4099.lib.units.LinearAcceleration +import org.team4099.lib.units.LinearVelocity +import org.team4099.lib.units.Velocity +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.inCelsius +import org.team4099.lib.units.base.inMeters +import org.team4099.lib.units.base.meters +import org.team4099.lib.units.derived.Angle +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.Radian +import org.team4099.lib.units.derived.Volt +import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.inRadians +import org.team4099.lib.units.derived.inVolts +import org.team4099.lib.units.derived.radians +import org.team4099.lib.units.derived.volts +import org.team4099.lib.units.inMetersPerSecond +import org.team4099.lib.units.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) } - val label: String + 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 - 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 49074abd..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,235 +13,262 @@ 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 org.team4099.lib.units.* -import org.team4099.lib.units.base.* -import org.team4099.lib.units.derived.* +import org.team4099.lib.units.AngularAcceleration +import org.team4099.lib.units.AngularVelocity +import org.team4099.lib.units.LinearAcceleration +import org.team4099.lib.units.LinearVelocity +import org.team4099.lib.units.Velocity +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.base.meters +import org.team4099.lib.units.derived.Angle +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.Radian +import org.team4099.lib.units.derived.Volt +import org.team4099.lib.units.derived.inKilogramsMeterSquared +import org.team4099.lib.units.derived.inRadians +import org.team4099.lib.units.derived.inRotations +import org.team4099.lib.units.derived.inVolts +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.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 - ) - ) - } - - 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 + // 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 } - - 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/feeder/Feeder.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt index f7209b2a..3fe709cb 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt @@ -1,5 +1,124 @@ package com.team4099.robot2023.subsystems.feeder +import com.team4099.lib.hal.Clock +import com.team4099.robot2023.config.constants.FeederConstants +import com.team4099.robot2023.subsystems.superstructure.Request +import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.SubsystemBase +import org.littletonrobotics.junction.Logger +import org.team4099.lib.units.base.amps +import org.team4099.lib.units.base.seconds +import org.team4099.lib.units.derived.ElectricalPotential +import org.team4099.lib.units.derived.volts -class Feeder(val io: FeederIO) : SubsystemBase() +class Feeder(val io: FeederIO) : SubsystemBase() { + val inputs = FeederIO.FeederIOInputs() + + var feederTargetVoltage: ElectricalPotential = 0.0.volts + var lastFeederRunTime = 0.0.seconds + + var currentState: FeederStates = FeederStates.UNINITIALIZED + var currentRequest: Request.FeederRequest = + Request.FeederRequest.OpenLoopIntake(FeederConstants.FEEDER_IDLE_VOLTAGE) + set(value) { + when (value) { + is Request.FeederRequest.OpenLoopIntake -> { + feederTargetVoltage = value.feederVoltage + } + is Request.FeederRequest.OpenLoopShoot -> { + feederTargetVoltage = value.feederVoltage + } + else -> {} + } + field = value + } + + val noteDetected: Boolean + get() { + return inputs.feederVelocity.absoluteValue <= FeederConstants.NOTE_VELOCITY_THRESHOLD && + inputs.feederStatorCurrent > 10.amps && + inputs.feederAppliedVoltage.sign < 0 && + (Clock.fpgaTime - lastFeederRunTime) >= + FeederConstants.WAIT_BEFORE_DETECT_VELOCITY_DROP || inputs.isSimulated + } + + var firstTripBeamBreakTime = Clock.fpgaTime + + var lastBeamState = false + val hasNote: Boolean + get() { + return inputs.beamBroken && + Clock.fpgaTime - firstTripBeamBreakTime > FeederConstants.BEAM_BREAK_WAIT_TIME + } + + override fun periodic() { + io.updateInputs(inputs) + + Logger.processInputs("Feeder", inputs) + + if (!lastBeamState && inputs.beamBroken) { + firstTripBeamBreakTime = Clock.fpgaTime + } + + lastBeamState = inputs.beamBroken + + var nextState = currentState + when (currentState) { + FeederStates.UNINITIALIZED -> { + nextState = FeederStates.OPEN_LOOP_INTAKE + } + FeederStates.OPEN_LOOP_INTAKE -> { + if (!hasNote) { + setFeederVoltage(feederTargetVoltage) + } + nextState = fromRequestToState(currentRequest) + } + FeederStates.OPEN_LOOP_SHOOT -> { + if (hasNote) { + setFeederVoltage(feederTargetVoltage) + } + nextState = fromRequestToState(currentRequest) + } + } + currentState = nextState + } + + fun setFeederVoltage(appliedVoltage: ElectricalPotential) { + io.setFeederVoltage(appliedVoltage) + } + + fun feederOpenLoopIntakeTestCommand(): Command { + return runOnce({ + currentRequest = Request.FeederRequest.OpenLoopIntake(FeederConstants.INTAKE_NOTE_VOLTAGE) + }) + } + + fun feederOpenLoopShootTestCommand(): Command { + return runOnce({ + currentRequest = Request.FeederRequest.OpenLoopShoot(FeederConstants.SHOOT_NOTE_VOLTAGE) + }) + } + + + companion object { + enum class FeederStates { + UNINITIALIZED, + OPEN_LOOP_INTAKE, + OPEN_LOOP_SHOOT; + + fun equivalentToRequest(request: Request.FeederRequest): Boolean { + return ( + (request is Request.FeederRequest.OpenLoopIntake && this == OPEN_LOOP_SHOOT) || + (request is Request.FeederRequest.OpenLoopIntake && this == OPEN_LOOP_INTAKE) + ) + } + } + + fun fromRequestToState(request: Request.FeederRequest): FeederStates { + return when (request) { + is Request.FeederRequest.OpenLoopIntake -> FeederStates.OPEN_LOOP_INTAKE + is Request.FeederRequest.OpenLoopShoot -> FeederStates.OPEN_LOOP_SHOOT + } + } + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt index 780fc386..00fafbbf 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt @@ -1,3 +1,66 @@ package com.team4099.robot2023.subsystems.feeder -interface FeederIO +import org.littletonrobotics.junction.LogTable +import org.littletonrobotics.junction.inputs.LoggableInputs +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.inCelsius +import org.team4099.lib.units.derived.ElectricalPotential +import org.team4099.lib.units.derived.inVolts +import org.team4099.lib.units.derived.radians +import org.team4099.lib.units.derived.rotations +import org.team4099.lib.units.derived.volts +import org.team4099.lib.units.inRadiansPerSecond +import org.team4099.lib.units.perMinute +import org.team4099.lib.units.perSecond + +interface FeederIO { + class FeederIOInputs : LoggableInputs { + var feederVelocity = 0.0.rotations.perMinute + var feederAppliedVoltage = 0.0.volts + var feederStatorCurrent = 0.0.amps + var feederSupplyCurrent = 0.0.amps + var feederTemp = 0.0.celsius + + var beamBroken = false + + var isSimulated = false + + override fun toLog(table: LogTable?) { + table?.put("feederVelocity", feederVelocity.inRadiansPerSecond) + table?.put("feederAppliedVoltage", feederAppliedVoltage.inVolts) + table?.put("feederStatorCurrent", feederStatorCurrent.inAmperes) + table?.put("feederSupplyCurrent", feederSupplyCurrent.inAmperes) + table?.put("feederTempCelcius", feederTemp.inCelsius) + table?.put("feederBeamBroken", beamBroken) + } + + override fun fromLog(table: LogTable?) { + table?.get("feederVelocity", feederVelocity.inRadiansPerSecond)?.let { + feederVelocity = it.radians.perSecond + } + + table?.get("feederAppliedVoltage", feederAppliedVoltage.inVolts)?.let { + feederAppliedVoltage = it.volts + } + + table?.get("feederStatorCurrent", feederStatorCurrent.inAmperes)?.let { + feederStatorCurrent = it.amps + } + + table?.get("feederSupplyCurrent", feederSupplyCurrent.inAmperes)?.let { + feederSupplyCurrent = it.amps + } + + table?.get("feederTempCelcius", feederTemp.inCelsius)?.let { feederTemp = it.celsius } + table?.get("feederBeamBroken", beamBroken)?.let { beamBroken = it } + } + } + + fun updateInputs(inputs: FeederIOInputs) {} + + fun setFeederVoltage(voltage: ElectricalPotential) {} + + fun setFeederBrakeMode(brake: Boolean) {} +} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIONeo.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIONeo.kt new file mode 100644 index 00000000..80c437a0 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIONeo.kt @@ -0,0 +1,94 @@ +package com.team4099.robot2023.subsystems.feeder + +import com.revrobotics.CANSparkMax +import com.revrobotics.CANSparkMaxLowLevel +import com.team4099.lib.math.clamp +import com.team4099.robot2023.config.constants.Constants +import com.team4099.robot2023.config.constants.FeederConstants +import com.team4099.robot2023.subsystems.falconspin.MotorChecker +import com.team4099.robot2023.subsystems.falconspin.MotorCollection +import com.team4099.robot2023.subsystems.falconspin.Neo +import org.littletonrobotics.junction.Logger +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.derived.ElectricalPotential +import org.team4099.lib.units.derived.inVolts +import org.team4099.lib.units.derived.volts +import org.team4099.lib.units.sparkMaxAngularMechanismSensor +import kotlin.math.absoluteValue + +object FeederIONeo : FeederIO { + private val feederSparkMax = + CANSparkMax(Constants.Intake.ROLLER_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless) + + private val feederSensor = + sparkMaxAngularMechanismSensor( + feederSparkMax, FeederConstants.FEEDER_GEAR_RATIO, FeederConstants.VOLTAGE_COMPENSATION + ) + + init { + feederSparkMax.restoreFactoryDefaults() + feederSparkMax.clearFaults() + + feederSparkMax.enableVoltageCompensation(FeederConstants.VOLTAGE_COMPENSATION.inVolts) + feederSparkMax.setSmartCurrentLimit(FeederConstants.FEEDER_CURRENT_LIMIT.inAmperes.toInt()) + feederSparkMax.inverted = FeederConstants.FEEDER_MOTOR_INVERTED + + feederSparkMax.idleMode = CANSparkMax.IdleMode.kCoast + + feederSparkMax.burnFlash() + + MotorChecker.add( + "Ground Intake", + "Roller", + MotorCollection( + mutableListOf(Neo(feederSparkMax, "Roller Motor")), + FeederConstants.FEEDER_CURRENT_LIMIT, + 70.celsius, + 30.amps, + 90.celsius + ), + ) + } + + override fun updateInputs(inputs: FeederIO.FeederIOInputs) { + inputs.feederVelocity = feederSensor.velocity + inputs.feederAppliedVoltage = feederSparkMax.busVoltage.volts * feederSparkMax.appliedOutput + inputs.feederStatorCurrent = feederSparkMax.outputCurrent.amps + + // BusVoltage * SupplyCurrent = AppliedVoltage * StatorCurrent + // AppliedVoltage = percentOutput * BusVoltage + // SupplyCurrent = (percentOutput * BusVoltage / BusVoltage) * StatorCurrent = + // percentOutput * statorCurrent + inputs.feederSupplyCurrent = + inputs.feederStatorCurrent * feederSparkMax.appliedOutput.absoluteValue + inputs.feederTemp = feederSparkMax.motorTemperature.celsius + + } + + /** + * Sets the roller motor voltage, ensures the voltage is limited to battery voltage compensation + * + * @param voltage the voltage to set the roller motor to + */ + override fun setFeederVoltage(voltage: ElectricalPotential) { + feederSparkMax.setVoltage( + clamp(voltage, -FeederConstants.VOLTAGE_COMPENSATION, FeederConstants.VOLTAGE_COMPENSATION) + .inVolts + ) + } + + /** + * Sets the roller motor brake mode + * + * @param brake if it brakes + */ + override fun setFeederBrakeMode(brake: Boolean) { + if (brake) { + feederSparkMax.idleMode = CANSparkMax.IdleMode.kBrake + } else { + feederSparkMax.idleMode = CANSparkMax.IdleMode.kCoast + } + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIOSim.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIOSim.kt new file mode 100644 index 00000000..b06a4d70 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIOSim.kt @@ -0,0 +1,45 @@ +package com.team4099.robot2023.subsystems.feeder + +import com.team4099.lib.math.clamp +import com.team4099.robot2023.config.constants.Constants +import com.team4099.robot2023.config.constants.FeederConstants +import edu.wpi.first.math.system.plant.DCMotor +import edu.wpi.first.wpilibj.simulation.FlywheelSim +import org.team4099.lib.units.base.amps +import org.team4099.lib.units.base.celsius +import org.team4099.lib.units.base.inSeconds +import org.team4099.lib.units.derived.ElectricalPotential +import org.team4099.lib.units.derived.inKilogramsMeterSquared +import org.team4099.lib.units.derived.inVolts +import org.team4099.lib.units.derived.rotations +import org.team4099.lib.units.derived.volts +import org.team4099.lib.units.perMinute + +object FeederIOSim : FeederIO { + private val feederSim: FlywheelSim = + FlywheelSim( + DCMotor.getNEO(1), + FeederConstants.FEEDER_GEAR_RATIO, + FeederConstants.FEEDER_INERTIA.inKilogramsMeterSquared + ) + + var appliedVoltage = 0.volts + + override fun updateInputs(inputs: FeederIO.FeederIOInputs) { + feederSim.update(Constants.Universal.LOOP_PERIOD_TIME.inSeconds) + + inputs.feederVelocity = feederSim.getAngularVelocityRPM().rotations.perMinute + inputs.feederAppliedVoltage = appliedVoltage + inputs.feederSupplyCurrent = 0.amps + inputs.feederStatorCurrent = feederSim.currentDrawAmps.amps + inputs.feederTemp = 0.0.celsius + } + + override fun setFeederVoltage(voltage: ElectricalPotential) { + appliedVoltage = voltage + feederSim.setInputVoltage( + clamp(voltage, -FeederConstants.VOLTAGE_COMPENSATION, FeederConstants.VOLTAGE_COMPENSATION) + .inVolts + ) + } +} 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 200f125b..6c8b29bd 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt @@ -1,14 +1,9 @@ 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 // typealias GroundIntakeRequest = SuperStructureState.GroundIntakeStructure.GroundIntakeRequest // typealias GroundIntakeState = SuperStructureState.GroundIntakeStructure.GroundIntakeState @@ -31,4 +26,9 @@ sealed interface Request { class ZeroSensors : DrivetrainRequest class Idle : DrivetrainRequest } + + sealed interface FeederRequest : Request { + class OpenLoopIntake(val feederVoltage: ElectricalPotential) : FeederRequest + class OpenLoopShoot(val feederVoltage: ElectricalPotential) : FeederRequest + } }