diff --git a/simgui-ds.json b/simgui-ds.json new file mode 100644 index 00000000..69b1a3cb --- /dev/null +++ b/simgui-ds.json @@ -0,0 +1,97 @@ +{ + "keyboardJoysticks": [ + { + "axisConfig": [ + { + "decKey": 65, + "incKey": 68 + }, + { + "decKey": 87, + "incKey": 83 + }, + { + "decKey": 69, + "decayRate": 0.0, + "incKey": 82, + "keyRate": 0.009999999776482582 + } + ], + "axisCount": 3, + "buttonCount": 4, + "buttonKeys": [ + 90, + 88, + 67, + 86 + ], + "povConfig": [ + { + "key0": 328, + "key135": 323, + "key180": 322, + "key225": 321, + "key270": 324, + "key315": 327, + "key45": 329, + "key90": 326 + } + ], + "povCount": 1 + }, + { + "axisConfig": [ + { + "decKey": 74, + "incKey": 76 + }, + { + "decKey": 73, + "incKey": 75 + } + ], + "axisCount": 2, + "buttonCount": 4, + "buttonKeys": [ + 77, + 44, + 46, + 47 + ], + "povCount": 0 + }, + { + "axisConfig": [ + { + "decKey": 263, + "incKey": 262 + }, + { + "decKey": 265, + "incKey": 264 + } + ], + "axisCount": 2, + "buttonCount": 6, + "buttonKeys": [ + 260, + 268, + 266, + 261, + 269, + 267 + ], + "povCount": 0 + }, + { + "axisCount": 0, + "buttonCount": 0, + "povCount": 0 + } + ], + "robotJoysticks": [ + { + "guid": "Keyboard0" + } + ] +} diff --git a/simgui.json b/simgui.json new file mode 100644 index 00000000..21c47b41 --- /dev/null +++ b/simgui.json @@ -0,0 +1,9 @@ +{ + "NTProvider": { + "types": { + "/FMSInfo": "FMSInfo", + "/Shuffleboard/Pre-match/Mode": "String Chooser", + "/SmartDashboard/AutonomousMode": "String Chooser" + } + } +} diff --git a/src/main/kotlin/com/team4099/lib/phoenix6/PositionVoltage.kt b/src/main/kotlin/com/team4099/lib/phoenix6/PositionVoltage.kt new file mode 100644 index 00000000..939bf44c --- /dev/null +++ b/src/main/kotlin/com/team4099/lib/phoenix6/PositionVoltage.kt @@ -0,0 +1,72 @@ +package com.team4099.lib.phoenix6 + +import org.team4099.lib.units.AngularVelocity +import org.team4099.lib.units.derived.Angle +import org.team4099.lib.units.derived.ElectricalPotential +import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.inRotations +import org.team4099.lib.units.derived.inVolts +import org.team4099.lib.units.derived.volts +import org.team4099.lib.units.inRotationsPerSecond +import org.team4099.lib.units.perSecond +import com.ctre.phoenix6.controls.PositionVoltage as PositionVoltagePhoenix6 + +class PositionVoltage( + private var position: + Angle, // Assuming an AngularPosition type exists similar to AngularVelocity + private var enableFOC: Boolean = true, + private var feedforward: ElectricalPotential = 0.0.volts, + private var slot: Int = 0, + private var overrideBrakeDurNeutral: Boolean = false, + private var limitForwardMotion: Boolean = false, + private var limitReverseMotion: Boolean = false, + var velocity: AngularVelocity = 0.0.degrees.perSecond, +) { + + 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 new file mode 100644 index 00000000..dde287dc --- /dev/null +++ b/src/main/kotlin/com/team4099/lib/phoenix6/VelocityVoltage.kt @@ -0,0 +1,76 @@ +package com.team4099.lib.phoenix6 + +import org.team4099.lib.units.AngularAcceleration +import org.team4099.lib.units.AngularVelocity +import org.team4099.lib.units.derived.ElectricalPotential +import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.inVolts +import org.team4099.lib.units.derived.volts +import org.team4099.lib.units.inRotationsPerSecond +import org.team4099.lib.units.inRotationsPerSecondPerSecond +import org.team4099.lib.units.perSecond +import com.ctre.phoenix6.controls.VelocityVoltage as VelocityVoltagePhoenix6 + +class VelocityVoltage( + private var velocity: AngularVelocity, + private var acceleration: AngularAcceleration = 0.0.degrees.perSecond.perSecond, + private var enableFOC: Boolean = true, + private var feedforward: ElectricalPotential = 0.0.volts, + private var slot: Int = 0, + private var overrideBrakeDurNeutral: Boolean = false, + private var limitForwardMotion: Boolean = false, + private var limitReverseMotion: Boolean = false +) { + + 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 setAcceleration(new_accel: AngularAcceleration) { + acceleration = new_accel + velocityVoltagePhoenix6.Acceleration = acceleration.inRotationsPerSecondPerSecond + } + + 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 setSlot(new_slot: Int) { + slot = new_slot + velocityVoltagePhoenix6.Slot = new_slot + } + + 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 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 126e75c1..fab5921e 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 = -1 -const val GIT_SHA = "UNKNOWN" -const val GIT_DATE = "UNKNOWN" -const val GIT_BRANCH = "UNKNOWN" -const val BUILD_DATE = "2023-12-16T19:11:21Z" -const val BUILD_UNIX_TIME = 1702771881467L -const val DIRTY = 129 +const val GIT_REVISION = 53 +const val GIT_SHA = "d075a80c020aa3cd40bc5368148d5a6c2c73458a" +const val GIT_DATE = "2024-01-23T20:30:14Z" +const val GIT_BRANCH = "elevator" +const val BUILD_DATE = "2024-01-23T20:50:22Z" +const val BUILD_UNIX_TIME = 1706061022253L +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 6bfb14cc..5d96cd2b 100644 --- a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt +++ b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt @@ -6,13 +6,14 @@ import com.team4099.robot2023.commands.drivetrain.TeleopDriveCommand import com.team4099.robot2023.config.ControlBoard import com.team4099.robot2023.config.constants.Constants import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain -import com.team4099.robot2023.subsystems.drivetrain.drive.DrivetrainIOReal +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.drivetrain.gyro.GyroIOPigeon2 +import com.team4099.robot2023.subsystems.elevator.Elevator +import com.team4099.robot2023.subsystems.elevator.ElevatorIONEO +import com.team4099.robot2023.subsystems.elevator.ElevatorIOSim import com.team4099.robot2023.subsystems.limelight.LimelightVision import com.team4099.robot2023.subsystems.limelight.LimelightVisionIO -import com.team4099.robot2023.subsystems.superstructure.Request import com.team4099.robot2023.subsystems.vision.Vision import com.team4099.robot2023.subsystems.vision.camera.CameraIONorthstar import com.team4099.robot2023.util.driver.Ryan @@ -20,17 +21,19 @@ import edu.wpi.first.wpilibj.RobotBase import org.team4099.lib.smoothDeadband import org.team4099.lib.units.derived.Angle import org.team4099.lib.units.derived.degrees +import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest object RobotContainer { private val drivetrain: Drivetrain private val vision: Vision private val limelight: LimelightVision + private val elevator: Elevator init { if (RobotBase.isReal()) { // Real Hardware Implementations // drivetrain = Drivetrain(object: GyroIO {},object: DrivetrainIO {} - drivetrain = Drivetrain(GyroIOPigeon2, DrivetrainIOReal) + drivetrain = Drivetrain(object : GyroIO {}, object : DrivetrainIO {}) vision = Vision( // object: CameraIO {} @@ -42,6 +45,7 @@ object RobotContainer { // CameraIONorthstar("backward") ) limelight = LimelightVision(object : LimelightVisionIO {}) + elevator = Elevator(ElevatorIONEO) } else { // Simulation implementations drivetrain = Drivetrain(object : GyroIO {}, DrivetrainIOSim) @@ -52,6 +56,7 @@ object RobotContainer { CameraIONorthstar("northstar_3"), ) limelight = LimelightVision(object : LimelightVisionIO {}) + elevator = Elevator(ElevatorIOSim) } vision.setDataInterfaces({ drivetrain.odometryPose }, { drivetrain.addVisionData(it) }) @@ -86,7 +91,7 @@ object RobotContainer { } fun zeroSensors() { - drivetrain.currentRequest = Request.DrivetrainRequest.ZeroSensors() + drivetrain.currentRequest = DrivetrainRequest.ZeroSensors() } fun zeroAngle(toAngle: Angle) { @@ -128,6 +133,11 @@ object RobotContainer { // Constants.Universal.Substation.SINGLE_SUBSTATION // ) // ) + + ControlBoard.elevatorOpenLoopExtend.whileTrue(elevator.testElevatorOpenLoopExtendCommand()) + ControlBoard.elevatorOpenLoopRetract.whileTrue(elevator.testElevatorOpenLoopRetractCommand()) + ControlBoard.elevatorClosedLoopHigh.whileTrue(elevator.testElevatorClosedLoopExtendCommand()) + ControlBoard.elevatorClosedLoopLow.whileTrue(elevator.elevatorClosedLoopRetractCommand()) } fun mapTestControls() {} diff --git a/src/main/kotlin/com/team4099/robot2023/commands/SysIdCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/SysIdCommand.kt index 61999fe2..bd56d9ed 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/SysIdCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/SysIdCommand.kt @@ -2,14 +2,14 @@ package com.team4099.robot2023.commands import edu.wpi.first.wpilibj.Timer import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard -import edu.wpi.first.wpilibj2.command.CommandBase +import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.Subsystem import org.littletonrobotics.junction.Logger import java.util.function.BiConsumer import java.util.function.Consumer import java.util.function.Supplier -class SysIdCommand : CommandBase { +class SysIdCommand : Command { private val isDriveTrain: Boolean private lateinit var driveTrainSetter: BiConsumer private lateinit var mechanismSetter: Consumer 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 8d3024d0..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,20 +1,20 @@ package com.team4099.robot2023.commands.drivetrain import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain -import com.team4099.robot2023.subsystems.superstructure.Request -import edu.wpi.first.wpilibj2.command.CommandBase +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) : CommandBase() { +class DriveBrakeModeCommand(val drivetrain: Drivetrain) : Command() { init { addRequirements(drivetrain) } override fun execute() { drivetrain.currentRequest = - Request.DrivetrainRequest.OpenLoop( + 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 92bf07df..12a8841c 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt @@ -6,7 +6,6 @@ import com.team4099.lib.trajectory.CustomTrajectoryGenerator import com.team4099.lib.trajectory.Waypoint import com.team4099.robot2023.config.constants.DrivetrainConstants import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain -import com.team4099.robot2023.subsystems.superstructure.Request import com.team4099.robot2023.util.AllianceFlipUtil import com.team4099.robot2023.util.Velocity2d import edu.wpi.first.math.kinematics.ChassisSpeeds @@ -15,7 +14,7 @@ import edu.wpi.first.math.trajectory.TrajectoryParameterizer.TrajectoryGeneratio import edu.wpi.first.math.trajectory.constraint.CentripetalAccelerationConstraint import edu.wpi.first.math.trajectory.constraint.TrajectoryConstraint import edu.wpi.first.wpilibj.DriverStation -import edu.wpi.first.wpilibj2.command.CommandBase +import edu.wpi.first.wpilibj2.command.Command import org.littletonrobotics.junction.Logger import org.team4099.lib.controller.PIDController import org.team4099.lib.geometry.Pose2d @@ -55,6 +54,7 @@ import org.team4099.lib.units.inRadiansPerSecondPerSecond import org.team4099.lib.units.perSecond import java.util.function.Supplier import kotlin.math.PI +import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest class DrivePathCommand( val drivetrain: Drivetrain, @@ -65,7 +65,7 @@ class DrivePathCommand( val endPathOnceAtReference: Boolean = true, val leaveOutYAdjustment: Boolean = false, val endVelocity: Velocity2d = Velocity2d(), -) : CommandBase() { +) : Command() { private val xPID: PIDController> private val yPID: PIDController> @@ -245,7 +245,7 @@ class DrivePathCommand( ) drivetrain.currentRequest = - Request.DrivetrainRequest.ClosedLoop( + DrivetrainRequest.ClosedLoop( nextDriveState, ChassisAccels(xAccel, yAccel, 0.0.radians.perSecond.perSecond).chassisAccelsWPILIB ) @@ -311,16 +311,16 @@ class DrivePathCommand( if (interrupted) { // Stop where we are if interrupted drivetrain.currentRequest = - Request.DrivetrainRequest.OpenLoop( - 0.degrees.perSecond, Pair(0.meters.perSecond, 0.meters.perSecond) + 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 = - Request.DrivetrainRequest.OpenLoop( - 0.degrees.perSecond, Pair(0.meters.perSecond, 0.meters.perSecond) + 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 f46e6956..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,9 +3,8 @@ package com.team4099.robot2023.commands.drivetrain import com.team4099.lib.logging.LoggedTunableValue import com.team4099.robot2023.config.constants.DrivetrainConstants import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain -import com.team4099.robot2023.subsystems.superstructure.Request import edu.wpi.first.wpilibj.RobotBase -import edu.wpi.first.wpilibj2.command.CommandBase +import edu.wpi.first.wpilibj2.command.Command import org.littletonrobotics.junction.Logger import org.team4099.lib.controller.ProfiledPIDController import org.team4099.lib.controller.TrapezoidProfile @@ -25,8 +24,9 @@ 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) : CommandBase() { +class GoToAngle(val drivetrain: Drivetrain) : Command() { private val thetaPID: ProfiledPIDController> val thetakP = @@ -93,7 +93,7 @@ class GoToAngle(val drivetrain: Drivetrain) : CommandBase() { val thetaFeedback = thetaPID.calculate(drivetrain.odometryPose.rotation, desiredAngle) drivetrain.currentRequest = - Request.DrivetrainRequest.OpenLoop( + DrivetrainRequest.OpenLoop( thetaFeedback, Pair(0.0.meters.perSecond, 0.0.meters.perSecond), fieldOriented = true ) @@ -110,7 +110,7 @@ class GoToAngle(val drivetrain: Drivetrain) : CommandBase() { override fun end(interrupted: Boolean) { drivetrain.currentRequest = - Request.DrivetrainRequest.OpenLoop( + 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 7978de7f..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,8 +4,7 @@ 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 -import edu.wpi.first.wpilibj2.command.CommandBase +import edu.wpi.first.wpilibj2.command.Command import org.littletonrobotics.junction.Logger import org.team4099.lib.controller.ProfiledPIDController import org.team4099.lib.controller.TrapezoidProfile @@ -22,8 +21,9 @@ 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) : CommandBase() { +class GyroAutoLevel(val drivetrain: Drivetrain) : Command() { private val gyroPID: ProfiledPIDController> var alignmentAngle = drivetrain.odometryPose.rotation @@ -101,9 +101,7 @@ class GyroAutoLevel(val drivetrain: Drivetrain) : CommandBase() { Logger.recordOutput("ActiveCommands/AutoLevelCommand", true) drivetrain.currentRequest = - Request.DrivetrainRequest.OpenLoop( - 0.0.radians.perSecond, gyroFeedback, fieldOriented = true - ) + DrivetrainRequest.OpenLoop(0.0.radians.perSecond, gyroFeedback, fieldOriented = true) Logger.recordOutput( "AutoLevel/DesiredPitchDegrees", DrivetrainConstants.DOCKING_GYRO_SETPOINT.inDegrees @@ -130,7 +128,7 @@ class GyroAutoLevel(val drivetrain: Drivetrain) : CommandBase() { override fun end(interrupted: Boolean) { drivetrain.currentRequest = - Request.DrivetrainRequest.OpenLoop( + 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 8267b398..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,20 +1,20 @@ package com.team4099.robot2023.commands.drivetrain import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain -import com.team4099.robot2023.subsystems.superstructure.Request -import edu.wpi.first.wpilibj2.command.CommandBase +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) : CommandBase() { +class OpenLoopReverseCommand(val drivetrain: Drivetrain) : Command() { init { addRequirements(drivetrain) } override fun execute() { drivetrain.currentRequest = - Request.DrivetrainRequest.OpenLoop( + 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/ResetGyroPitchCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetGyroPitchCommand.kt index 26ae9102..09f48993 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetGyroPitchCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetGyroPitchCommand.kt @@ -1,13 +1,13 @@ package com.team4099.robot2023.commands.drivetrain import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain -import edu.wpi.first.wpilibj2.command.CommandBase +import edu.wpi.first.wpilibj2.command.Command import org.littletonrobotics.junction.Logger import org.team4099.lib.units.derived.Angle import org.team4099.lib.units.derived.degrees class ResetGyroPitchCommand(val drivetrain: Drivetrain, val toAngle: Angle = 0.0.degrees) : - CommandBase() { + Command() { init { addRequirements(drivetrain) } diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetGyroYawCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetGyroYawCommand.kt index d4f34fb9..1c84301e 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetGyroYawCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetGyroYawCommand.kt @@ -1,13 +1,13 @@ package com.team4099.robot2023.commands.drivetrain import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain -import edu.wpi.first.wpilibj2.command.CommandBase +import edu.wpi.first.wpilibj2.command.Command import org.littletonrobotics.junction.Logger import org.team4099.lib.units.derived.Angle import org.team4099.lib.units.derived.degrees class ResetGyroYawCommand(val drivetrain: Drivetrain, val toAngle: Angle = 0.0.degrees) : - CommandBase() { + Command() { init { addRequirements(drivetrain) } diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetPoseCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetPoseCommand.kt index 1ca8a687..eccb3790 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetPoseCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetPoseCommand.kt @@ -2,11 +2,11 @@ package com.team4099.robot2023.commands.drivetrain import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain import com.team4099.robot2023.util.AllianceFlipUtil -import edu.wpi.first.wpilibj2.command.CommandBase +import edu.wpi.first.wpilibj2.command.Command import org.littletonrobotics.junction.Logger import org.team4099.lib.geometry.Pose2d -class ResetPoseCommand(val drivetrain: Drivetrain, val pose: Pose2d) : CommandBase() { +class ResetPoseCommand(val drivetrain: Drivetrain, val pose: Pose2d) : Command() { init { addRequirements(drivetrain) } diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetZeroCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetZeroCommand.kt index df186518..9e3cd36d 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetZeroCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetZeroCommand.kt @@ -1,10 +1,10 @@ package com.team4099.robot2023.commands.drivetrain import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain -import edu.wpi.first.wpilibj2.command.CommandBase +import edu.wpi.first.wpilibj2.command.Command import org.littletonrobotics.junction.Logger -class ResetZeroCommand(val drivetrain: Drivetrain) : CommandBase() { +class ResetZeroCommand(val drivetrain: Drivetrain) : Command() { init { addRequirements(drivetrain) } diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/SwerveModuleTuningCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/SwerveModuleTuningCommand.kt index 70e0c257..6f0403e0 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/SwerveModuleTuningCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/SwerveModuleTuningCommand.kt @@ -2,12 +2,12 @@ package com.team4099.robot2023.commands.drivetrain import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain import edu.wpi.first.math.kinematics.SwerveModuleState -import edu.wpi.first.wpilibj2.command.CommandBase +import edu.wpi.first.wpilibj2.command.Command import org.team4099.lib.units.derived.Angle import org.team4099.lib.units.derived.inRotation2ds class SwerveModuleTuningCommand(val drivetrain: Drivetrain, val steeringPosition: () -> Angle) : - CommandBase() { + Command() { init { addRequirements(drivetrain) } 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 d369cb5a..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 import com.team4099.robot2023.util.driver.DriverProfile -import edu.wpi.first.wpilibj2.command.CommandBase +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, @@ -13,7 +13,7 @@ class TeleopDriveCommand( val turn: () -> Double, val slowMode: () -> Boolean, val drivetrain: Drivetrain -) : CommandBase() { +) : Command() { init { addRequirements(drivetrain) @@ -24,7 +24,7 @@ class TeleopDriveCommand( override fun execute() { val speed = driver.driveSpeedClampedSupplier(driveX, driveY, slowMode) val rotation = driver.rotationSpeedClampedSupplier(turn, slowMode) - drivetrain.currentRequest = Request.DrivetrainRequest.OpenLoop(rotation, speed) + drivetrain.currentRequest = DrivetrainRequest.OpenLoop(rotation, speed) Logger.recordOutput("ActiveCommands/TeleopDriveCommand", true) } override fun isFinished(): Boolean { 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 d47b882c..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,11 +1,11 @@ package com.team4099.robot2023.commands.drivetrain import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain -import edu.wpi.first.wpilibj2.command.CommandBase +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) : CommandBase() { +class ZeroSensorsCommand(val drivetrain: Drivetrain) : Command() { init { addRequirements(drivetrain) diff --git a/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt b/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt index 2a3446ac..7ace6cad 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt @@ -97,4 +97,9 @@ object ControlBoard { // for testing val setArmCommand = Trigger { technician.yButton } + + val elevatorOpenLoopExtend = Trigger { driver.aButton } + val elevatorOpenLoopRetract = Trigger { driver.bButton } + val elevatorClosedLoopHigh = Trigger { driver.xButton } + val elevatorClosedLoopLow = Trigger { driver.yButton } } diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt new file mode 100644 index 00000000..071adc8d --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt @@ -0,0 +1,88 @@ +package com.team4099.robot2023.config.constants + +import org.team4099.lib.units.base.Meter +import org.team4099.lib.units.base.amps +import org.team4099.lib.units.base.inches +import org.team4099.lib.units.base.meters +import org.team4099.lib.units.base.percent +import org.team4099.lib.units.base.pounds +import org.team4099.lib.units.base.seconds +import org.team4099.lib.units.derived.DerivativeGain +import org.team4099.lib.units.derived.IntegralGain +import org.team4099.lib.units.derived.ProportionalGain +import org.team4099.lib.units.derived.Volt +import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.volts +import org.team4099.lib.units.perSecond + +object ElevatorConstants { + // TODO: Change values later based on CAD + val REAL_KP = 0.0.volts / 1.inches + val REAL_KI = 0.0.volts / (1.inches * 1.seconds) + val REAL_KD = 0.0.volts / (1.inches.perSecond) + + val CARRIAGE_MASS = 30.892.pounds + + val ELEVATOR_MAX_RETRACTION = 0.0.inches + val ELEVATOR_MAX_EXTENSION = 18.0.inches + + val LEADER_INVERTED = false + val FOLLOWER_INVERTED = true + + val LEADER_KP: ProportionalGain = 0.0.volts / 1.inches + val LEADER_KI: IntegralGain = 0.0.volts / (1.inches * 1.seconds) + val LEADER_KD: DerivativeGain = 0.0.volts / (1.inches.perSecond) + + val FOLLOWER_KP: ProportionalGain = 0.0.volts / 1.inches + val FOLLOWER_KI: IntegralGain = 0.0.volts / (1.inches * 1.seconds) + val FOLLOWER_KD: DerivativeGain = 0.0.volts / (1.inches.perSecond) + + val SIM_KP = 3.0.volts / 1.inches + val SIM_KI = 0.0.volts / (1.inches * 1.seconds) + val SIM_KD = 0.0.volts / (1.inches.perSecond) + + val ELEVATOR_KS = 0.0.volts + val ELEVATOR_KG = 0.35.volts + val ELEVATOR_KV = 0.39.volts / 1.inches.perSecond + val ELEVATOR_KA = 0.00083.volts / 1.inches.perSecond.perSecond + val ELEVATOR_OPEN_LOOP_EXTEND_VOLTAGE = 8.0.volts + val ELEVATOR_OPEN_LOOP_RETRACT_VOLTAGE = -12.0.volts + + val ENABLE_ELEVATOR = true + val ELEVATOR_IDLE_HEIGHT = 0.0.inches + val ELEVATOR_SOFT_LIMIT_EXTENSION = 17.5.inches + val ELEVATOR_SOFT_LIMIT_RETRACTION = -1.0.inches + val ELEVATOR_OPEN_LOOP_SOFT_LIMIT_EXTENSION = 0.0.inches + val ELEVATOR_OPEN_LOOP_SOFT_LIMIT_RETRACTION = 0.0.inches + val ELEVATOR_SAFE_THRESHOLD = 5.0.inches + + + val ELEVATOR_TOLERANCE = 0.2.inches + + val MAX_VELOCITY = 0.82.meters.perSecond + val MAX_ACCELERATION = 2.meters.perSecond.perSecond + + val SHOOT_SPEAKER_POSITION = 0.0.inches + val SHOOT_AMP_POSITION = 0.0.inches + val SOURCE_NOTE_OFFSET = 0.0.inches + val ELEVATOR_THETA_POS = 0.0.degrees + val HOMING_STATOR_CURRENT = 0.0.amps + val HOMING_STALL_TIME_THRESHOLD = 0.0.seconds + val HOMING_APPLIED_VOLTAGE = 0.0.volts + val ELEVATOR_GROUND_OFFSET = 0.0.inches + + val VOLTAGE_COMPENSATION = 12.0.volts + val ELEVATOR_PULLEY_TO_MOTOR = 4.0 / 1 * 4.0 / 1 + val SPOOL_DIAMETER = 1.591.inches + + val LEADER_SUPPLY_CURRENT_LIMIT = 0.0.amps + val LEADER_THRESHOLD_CURRENT_LIMIT = 0.0.amps + val LEADER_SUPPLY_TIME_THRESHOLD = 0.0.seconds + val LEADER_STATOR_CURRENT_LIMIT = 0.0.amps + + val FOLLOWER_SUPPLY_TIME_THRESHOLD = 0.0.seconds + val FOLLOWER_STATOR_CURRENT_LIMIT = 0.0.amps + val FOLLOWER_SUPPLY_CURRENT_LIMIT = 0.0.amps + val FOLLOWER_THRESHOLD_CURRENT_LIMIT = 0.0.amps + +} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/GroundIntake/GroundIntake.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/GroundIntake/GroundIntake.kt deleted file mode 100644 index 5fdc7677..00000000 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/GroundIntake/GroundIntake.kt +++ /dev/null @@ -1,4 +0,0 @@ -package com.team4099.robot2023.subsystems.GroundIntake - -class GroundIntake { -} \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt index df493fe8..c1ef2b6f 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt @@ -1,4 +1,3 @@ package com.team4099.robot2023.subsystems.Shooter -class Shooter { -} \ No newline at end of file +class Shooter diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/TelescopingArm/TelescopingArm.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/TelescopingArm/TelescopingArm.kt index a1af62e2..53e9e9ed 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/TelescopingArm/TelescopingArm.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/TelescopingArm/TelescopingArm.kt @@ -1,4 +1,3 @@ package com.team4099.robot2023.subsystems.TelescopingArm -class TelescopingArm { -} \ No newline at end of file +class TelescopingArm 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 484c9b0f..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 import com.team4099.robot2023.util.Alert import com.team4099.robot2023.util.FMSData import com.team4099.robot2023.util.PoseEstimator @@ -32,6 +31,7 @@ import org.team4099.lib.units.AngularVelocity import org.team4099.lib.units.LinearAcceleration import org.team4099.lib.units.LinearVelocity import org.team4099.lib.units.base.Length +import org.team4099.lib.units.base.feet import org.team4099.lib.units.base.inMeters import org.team4099.lib.units.base.inMilliseconds import org.team4099.lib.units.base.inSeconds @@ -44,8 +44,6 @@ import org.team4099.lib.units.derived.inRotation2ds import org.team4099.lib.units.derived.radians import org.team4099.lib.units.inMetersPerSecond import org.team4099.lib.units.perSecond -import java.util.concurrent.locks.Lock -import java.util.concurrent.locks.ReentrantLock import java.util.function.Supplier import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest @@ -75,35 +73,6 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB var elevatorHeightSupplier = Supplier { 0.0.inches } - var velocityTarget = 0.degrees.perSecond - - var targetedDriveVector = Pair(0.meters.perSecond, 0.meters.perSecond) - - var fieldOrientation = true // true denotes that when driving, it'll be field oriented. - - var targetedChassisSpeeds = edu.wpi.first.math.kinematics.ChassisSpeeds(0.0, 0.0, 0.0) - - var targetedChassisAccels = edu.wpi.first.math.kinematics.ChassisSpeeds(0.0, 0.0, 0.0) - - 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 - } - init { // Wheel speeds @@ -143,7 +112,7 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB backRightWheelLocation.translation2d ) - var swerveDrivePoseEstimator = PoseEstimator(VecBuilder.fill(0.003, 0.003, 0.0001)) + var swerveDrivePoseEstimator = PoseEstimator(VecBuilder.fill(0.003, 0.003, 0.00001)) var swerveDriveOdometry = SwerveDriveOdometry( @@ -159,13 +128,6 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB var odometryPose: Pose2d get() = swerveDrivePoseEstimator.getLatestPose() - // get() { - // return Pose2d( - // 42.875.inches + 79.centi.meters + 14.inches + 2.75.inches, - // 113.25.inches + 1.inches, - // 180.degrees - // ) - // } set(value) { swerveDrivePoseEstimator.resetPose(value) @@ -207,20 +169,48 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB var lastGyroYaw = 0.0.radians - override fun periodic() { - val startTime = Clock.realTimestamp + var velocityTarget = 0.degrees.perSecond - odometryLock.lock(); // Prevents odometry updates while reading data + var targetedDriveVector = Pair(0.meters.perSecond, 0.meters.perSecond) - gyroIO.updateInputs(gyroInputs) - swerveModules.forEach { it.updateInputs() } + var fieldOrientation = true + + var targetedChassisSpeeds = edu.wpi.first.math.kinematics.ChassisSpeeds(0.0, 0.0, 0.0) + + var targetedChassisAccels = edu.wpi.first.math.kinematics.ChassisSpeeds(0.0, 0.0, 0.0) + + 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 + } - odometryLock.unlock() + init { + zeroSteering() + } + override fun periodic() { + val startTime = Clock.realTimestamp gyroNotConnectedAlert.set(!gyroInputs.gyroConnected) + gyroIO.updateInputs(gyroInputs) swerveModules.forEach { it.periodic() } + // TODO: add logic to reduce setpoint based on elevator/arm position + DrivetrainConstants.DRIVE_SETPOINT_MAX = 15.feet.perSecond + Logger.recordOutput( "Drivetrain/maxSetpointMetersPerSecond", DrivetrainConstants.DRIVE_SETPOINT_MAX.inMetersPerSecond @@ -263,7 +253,12 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB Logger.recordOutput("Drivetrain/ModuleStates", *measuredStates) Logger.recordOutput("Drivetrain/SetPointStates", *setPointStates.toTypedArray()) - Logger.recordOutput(VisionConstants.POSE_TOPIC_NAME, odometryPose.pose2d) + Logger.recordOutput( + VisionConstants.POSE_TOPIC_NAME, + doubleArrayOf( + odometryPose.x.inMeters, odometryPose.y.inMeters, odometryPose.rotation.inRadians + ) + ) Logger.recordOutput( "Odometry/pose3d", Pose3d( @@ -324,31 +319,35 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB } private fun updateOdometry() { - - var deltaCount = - if (gyroInputs.gyroConnected) gyroInputs.odometryYawPositions.size else Integer.MAX_VALUE - for (i in 0..4) { - deltaCount = Math.min(deltaCount, swerveModules[i].positionDeltas.size) + 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 + ) } - for (deltaIndex in 0..deltaCount) { - // Read wheel deltas from each module - val wheelDeltas = arrayOfNulls(4) - for (moduleIndex in 0..4) { - wheelDeltas[moduleIndex] = swerveModules[moduleIndex].positionDeltas[deltaIndex] - } - - var driveTwist = swerveDriveKinematics.toTwist2d(*wheelDeltas) - if (gyroInputs.gyroConnected) { - driveTwist = - edu.wpi.first.math.geometry.Twist2d( - driveTwist.dx, driveTwist.dy, (gyroInputs.rawGyroYaw - lastGyroYaw).inRadians - ) - lastGyroYaw = gyroInputs.rawGyroYaw - } + var driveTwist = swerveDriveKinematics.toTwist2d(*wheelDeltas.toTypedArray()) - swerveDrivePoseEstimator.addDriveData(Clock.fpgaTime.inSeconds, Twist2d(driveTwist)) - odometryPose = odometryPose.exp(Twist2d(driveTwist)) + if (gyroInputs.gyroConnected) { + driveTwist = + edu.wpi.first.math.geometry.Twist2d( + driveTwist.dx, driveTwist.dy, (gyroInputs.rawGyroYaw - lastGyroYaw).inRadians + ) + lastGyroYaw = gyroInputs.rawGyroYaw } // reversing the drift to store the ground truth pose @@ -365,12 +364,14 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB swerveModules[i].modulePosition.angle ) } - swerveDriveOdometry.update((gyroInputs.gyroYaw).inRotation2ds, undriftedModules) + swerveDriveOdometry.update(gyroInputs.gyroYaw.inRotation2ds, undriftedModules) drift = undriftedPose.minus(odometryPose) Logger.recordOutput(VisionConstants.SIM_POSE_TOPIC_NAME, undriftedPose.pose2d) } + + swerveDrivePoseEstimator.addDriveData(Clock.fpgaTime.inSeconds, Twist2d(driveTwist)) } /** @@ -391,6 +392,7 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB var desiredChassisSpeeds: ChassisSpeeds if (fieldOriented) { + Logger.recordOutput("drivetrain/isFieldOriented", true) desiredChassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds( allianceFlippedDriveVector.first, @@ -629,17 +631,6 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB } companion object { - // Drivetrain multithreading - var odometryLock: Lock = ReentrantLock() - fun setOdometryLock(Locked: Boolean) { - if (Locked) { - odometryLock.lock() - } else { - odometryLock.unlock() - } - } - - // Drivetrain states for state machine. enum class DrivetrainState { UNINITIALIZED, IDLE, @@ -647,7 +638,7 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB OPEN_LOOP, CLOSED_LOOP; - inline fun equivalentToRequest(request: Request.DrivetrainRequest): Boolean { + inline fun equivalentToRequest(request: DrivetrainRequest): Boolean { return ( (request is DrivetrainRequest.ZeroSensors && this == ZEROING_SENSORS) || (request is DrivetrainRequest.OpenLoop && this == OPEN_LOOP) || @@ -657,7 +648,7 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB } } - inline fun fromRequestToState(request: Request.DrivetrainRequest): DrivetrainState { + inline fun fromRequestToState(request: DrivetrainRequest): DrivetrainState { return when (request) { is DrivetrainRequest.OpenLoop -> DrivetrainState.OPEN_LOOP is DrivetrainRequest.ClosedLoop -> DrivetrainState.CLOSED_LOOP diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/DrivetrainIOReal.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/DrivetrainIOReal.kt deleted file mode 100644 index e86fea20..00000000 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/DrivetrainIOReal.kt +++ /dev/null @@ -1,55 +0,0 @@ -package com.team4099.robot2023.subsystems.drivetrain.drive - -import com.ctre.phoenix6.hardware.TalonFX -import com.team4099.robot2023.config.constants.Constants -import com.team4099.robot2023.config.constants.Constants.Universal.CANIVORE_NAME -import com.team4099.robot2023.config.constants.DrivetrainConstants -import com.team4099.robot2023.subsystems.drivetrain.swervemodule.SwerveModule -import com.team4099.robot2023.subsystems.drivetrain.swervemodule.SwerveModuleIOFalcon -import edu.wpi.first.wpilibj.AnalogInput - -object DrivetrainIOReal : DrivetrainIO { - override fun getSwerveModules(): List { - return listOf( - SwerveModule( - SwerveModuleIOFalcon( - TalonFX(Constants.Drivetrain.FRONT_LEFT_STEERING_ID, CANIVORE_NAME), - TalonFX(Constants.Drivetrain.FRONT_LEFT_DRIVE_ID, CANIVORE_NAME), - AnalogInput(Constants.Drivetrain.FRONT_LEFT_ANALOG_POTENTIOMETER), - DrivetrainConstants.FRONT_LEFT_MODULE_ZERO, - Constants.Drivetrain.FRONT_LEFT_MODULE_NAME - ) - ), - SwerveModule( - SwerveModuleIOFalcon( - TalonFX(Constants.Drivetrain.FRONT_RIGHT_STEERING_ID, CANIVORE_NAME), - TalonFX(Constants.Drivetrain.FRONT_RIGHT_DRIVE_ID, CANIVORE_NAME), - AnalogInput(Constants.Drivetrain.FRONT_RIGHT_ANALOG_POTENTIOMETER), - DrivetrainConstants.FRONT_RIGHT_MODULE_ZERO, - Constants.Drivetrain.FRONT_RIGHT_MODULE_NAME - ) - ), - SwerveModule( - SwerveModuleIOFalcon( - TalonFX(Constants.Drivetrain.BACK_LEFT_STEERING_ID, CANIVORE_NAME), - TalonFX(Constants.Drivetrain.BACK_LEFT_DRIVE_ID, CANIVORE_NAME), - AnalogInput(Constants.Drivetrain.BACK_LEFT_ANALOG_POTENTIOMETER), - DrivetrainConstants.BACK_LEFT_MODULE_ZERO, - Constants.Drivetrain.BACK_LEFT_MODULE_NAME - ) - ), - SwerveModule( - // object: SwerveModuleIO { - // override val label: String = Constants.Drivetrain.BACK_RIGHT_MODULE_NAME - // } - SwerveModuleIOFalcon( - TalonFX(Constants.Drivetrain.BACK_RIGHT_STEERING_ID, CANIVORE_NAME), - TalonFX(Constants.Drivetrain.BACK_RIGHT_DRIVE_ID, CANIVORE_NAME), - AnalogInput(Constants.Drivetrain.BACK_RIGHT_ANALOG_POTENTIOMETER), - DrivetrainConstants.BACK_RIGHT_MODULE_ZERO, - Constants.Drivetrain.BACK_RIGHT_MODULE_NAME - ) - ) - ) - } -} 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 ce8a4d09..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 @@ -63,6 +63,8 @@ object GyroIONavx : GyroIO { } 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 diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/gyro/GyroIOPigeon2.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/gyro/GyroIOPigeon2.kt deleted file mode 100644 index 6a493d64..00000000 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/gyro/GyroIOPigeon2.kt +++ /dev/null @@ -1,144 +0,0 @@ -package com.team4099.robot2023.subsystems.drivetrain.gyro - -import com.ctre.phoenix6.configs.Pigeon2Configuration -import com.ctre.phoenix6.hardware.Pigeon2 -import com.team4099.robot2023.config.constants.Constants -import com.team4099.robot2023.config.constants.DrivetrainConstants -import com.team4099.robot2023.config.constants.GyroConstants -import com.team4099.robot2023.subsystems.drivetrain.swervemodule.threads.PhoenixOdometryThread -import com.team4099.robot2023.subsystems.drivetrain.swervemodule.threads.SparkMaxOdometryThread -import org.littletonrobotics.junction.Logger -import org.team4099.lib.units.AngularVelocity -import org.team4099.lib.units.derived.Angle -import org.team4099.lib.units.derived.degrees -import org.team4099.lib.units.derived.inDegrees -import org.team4099.lib.units.derived.inRadians -import org.team4099.lib.units.inDegreesPerSecond -import org.team4099.lib.units.perSecond -import java.util.Queue -import kotlin.math.IEEErem - -object GyroIOPigeon2 : GyroIO { - private var pigeon2 = Pigeon2(Constants.Gyro.PIGEON_2_ID, Constants.Universal.CANIVORE_NAME) - - private val isConnected: Boolean = pigeon2.upTime.value > 0.0 - - var gyroYawOffset: Angle = 0.0.degrees - var gyroPitchOffset: Angle = 0.0.degrees - var gyroRollOffset: Angle = 0.0.degrees - - val yawPositionQueue: Queue - - val rawGyro: Angle = 0.0.degrees - - /** The current angle of the drivetrain. */ - val gyroYaw: Angle - get() { - if (isConnected) { - var rawYaw = pigeon2.yaw.value + gyroYawOffset.inDegrees - rawYaw += DrivetrainConstants.GYRO_RATE_COEFFICIENT * gyroYawRate.inDegreesPerSecond - return rawYaw.IEEErem(360.0).degrees - } else { - return (-1.337).degrees - } - } - - val gyroPitch: Angle - get() { - if (isConnected) { - val rawPitch = pigeon2.pitch.value + gyroPitchOffset.inDegrees - return rawPitch.IEEErem(360.0).degrees - } else { - return (-1.337).degrees - } - } - - val gyroRoll: Angle - get() { - if (isConnected) { - val rawRoll = pigeon2.roll.value + gyroRollOffset.inDegrees - return rawRoll.IEEErem(360.0).degrees - } else { - return -1.337.degrees - } - } - - val gyroYawRate: AngularVelocity - get() { - if (isConnected) { - return pigeon2.angularVelocityZWorld.value.degrees.perSecond - } else { - return -1.337.degrees.perSecond - } - } - - val gyroPitchRate: AngularVelocity - get() { - if (isConnected) { - return pigeon2.angularVelocityYWorld.value.degrees.perSecond - } else { - return -1.337.degrees.perSecond - } - } - - val gyroRollRate: AngularVelocity - get() { - if (isConnected) { - return pigeon2.angularVelocityXWorld.value.degrees.perSecond - } else { - return -1.337.degrees.perSecond - } - } - - init { - val pigeon2Configuration = Pigeon2Configuration() - pigeon2Configuration.MountPose.MountPosePitch = GyroConstants.mountPitch.inRadians - pigeon2Configuration.MountPose.MountPoseYaw = GyroConstants.mountYaw.inRadians - pigeon2Configuration.MountPose.MountPoseRoll = GyroConstants.mountRoll.inRadians - - yawPositionQueue = - if (Constants.Drivetrain.DRIVETRAIN_TYPE == - Constants.Drivetrain.DrivetrainType.PHOENIX_TALON - ) { - PhoenixOdometryThread.getInstance().registerSignal(pigeon2, pigeon2.getYaw()) - } else { - SparkMaxOdometryThread.getInstance().registerSignal { pigeon2.yaw.getValueAsDouble() } - } - - // TODO look into more pigeon configuration stuff - pigeon2.configurator.apply(pigeon2Configuration) - } - - override fun updateInputs(inputs: GyroIO.GyroIOInputs) { - - inputs.rawGyroYaw = pigeon2.yaw.value.degrees - - inputs.gyroConnected = isConnected - - inputs.gyroYaw = gyroYaw - inputs.gyroPitch = gyroPitch - inputs.gyroRoll = gyroRoll - - inputs.gyroYawRate = gyroYawRate - inputs.gyroPitchRate = gyroPitchRate - inputs.gyroRollRate = gyroRollRate - - inputs.odometryYawPositions = - yawPositionQueue.stream().map { value: Double -> value.degrees }.toArray() as Array - yawPositionQueue.clear() - - Logger.recordOutput("Gyro/rawYawDegrees", pigeon2.yaw.value) - } - - override fun zeroGyroYaw(toAngle: Angle) { - gyroYawOffset = toAngle - pigeon2.yaw.value.IEEErem(360.0).degrees - } - - override fun zeroGyroPitch(toAngle: Angle) { - gyroPitchOffset = toAngle - pigeon2.pitch.value.IEEErem(360.0).degrees - } - - override fun zeroGyroRoll(toAngle: Angle) { - gyroRollOffset = toAngle - pigeon2.roll.value.IEEErem(360.0).degrees - } -} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModule.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/servemodule/SwerveModule.kt similarity index 76% rename from src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModule.kt rename to src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/servemodule/SwerveModule.kt index 34d71a78..f43e426f 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModule.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/servemodule/SwerveModule.kt @@ -15,7 +15,6 @@ import org.team4099.lib.units.base.seconds 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.inDegrees import org.team4099.lib.units.derived.inRadians import org.team4099.lib.units.derived.inRotation2ds import org.team4099.lib.units.derived.inVoltsPerDegree @@ -31,11 +30,8 @@ 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.inMetersPerSecond -import org.team4099.lib.units.inMetersPerSecondPerSecond import org.team4099.lib.units.perSecond import kotlin.math.IEEErem -import kotlin.math.abs import kotlin.math.withSign class SwerveModule(val io: SwerveModuleIO) { @@ -43,15 +39,11 @@ class SwerveModule(val io: SwerveModuleIO) { var modulePosition = SwerveModulePosition() - var positionDeltas = mutableListOf() - private var speedSetPoint: LinearVelocity = 0.feet.perSecond private var accelerationSetPoint: LinearAcceleration = 0.feet.perSecond.perSecond private var steeringSetPoint: Angle = 0.degrees - private var lastDrivePosition = 0.meters - private var shouldInvert = false private val steeringkP = @@ -116,27 +108,9 @@ class SwerveModule(val io: SwerveModuleIO) { } } - fun updateInputs() { - io.updateInputs(inputs) - } - fun periodic() { io.updateInputs(inputs) - val deltaCount = - Math.min(inputs.odometryDrivePositions.size, inputs.odometrySteeringPositions.size) - - for (i in 0..deltaCount) { - val newDrivePosition = inputs.odometryDrivePositions[i] - val newSteeringAngle = inputs.odometrySteeringPositions[i] - positionDeltas.add( - SwerveModulePosition( - (newDrivePosition - lastDrivePosition).inMeters, newSteeringAngle.inRotation2ds - ) - ) - lastDrivePosition = newDrivePosition - } - // Updating SwerveModulePosition every loop cycle modulePosition.distanceMeters = inputs.drivePosition.inMeters modulePosition.angle = inputs.steeringPosition.inRotation2ds @@ -154,27 +128,24 @@ class SwerveModule(val io: SwerveModuleIO) { } Logger.processInputs(io.label, inputs) - Logger.recordOutput( - "${io.label}/driveSpeedSetpointMetersPerSecond", - if (!shouldInvert) speedSetPoint.inMetersPerSecond else -speedSetPoint.inMetersPerSecond - ) - Logger.recordOutput( - "${io.label}/driveAccelSetpointMetersPerSecondSq", - accelerationSetPoint.inMetersPerSecondPerSecond - ) - Logger.recordOutput("${io.label}/steeringSetpointRadians", steeringSetPoint.inRadians) - Logger.recordOutput( - "${io.label}/steeringValueDegreesWithMod", inputs.steeringPosition.inDegrees.IEEErem(360.0) - ) - - Logger.recordOutput("SwerveModule/SpeedSetPoint", speedSetPoint.inMetersPerSecond) - Logger.recordOutput("SwerveModule/SteeringSetPoint", steeringSetPoint.inRadians) - Logger.recordOutput( - "SwerveModule/AccelerationSetPoint", accelerationSetPoint.inMetersPerSecondPerSecond - ) - Logger.recordOutput( - "SwerveModule/SteeringError", (steeringSetPoint - inputs.steeringPosition).inRadians - ) + // 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) + // ) } /** @@ -251,22 +222,15 @@ class SwerveModule(val io: SwerveModuleIO) { SwerveModuleState.optimize(desiredState, inputs.steeringPosition.inRotation2ds) io.setOpenLoop( optimizedState.angle.angle, - optimizedState.speedMetersPerSecond.meters.perSecond * - Math.cos( - abs( - (optimizedState.angle.angle - inputs.steeringPosition) - .inRadians - ) - ) // consider desaturating wheel speeds here if it doesn't work + 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 * - Math.cos(abs((desiredState.angle.angle - inputs.steeringPosition).inRadians)) - ) + io.setOpenLoop(desiredState.angle.angle, desiredState.speedMetersPerSecond.meters.perSecond) Logger.recordOutput("${io.label}/steeringSetpointNonOptimized", desiredState.angle.degrees) } } @@ -290,18 +254,17 @@ class SwerveModule(val io: SwerveModuleIO) { SwerveModuleState.optimize(desiredVelState, inputs.steeringPosition.inRotation2ds) val optimizedAccelState = SwerveModuleState.optimize(desiredAccelState, inputs.steeringPosition.inRotation2ds) - - steeringSetPoint = optimizedVelState.angle.angle - speedSetPoint = optimizedVelState.speedMetersPerSecond.meters.perSecond - accelerationSetPoint = optimizedAccelState.speedMetersPerSecond.meters.perSecond.perSecond - - io.setClosedLoop(steeringSetPoint, speedSetPoint, accelerationSetPoint) + io.setClosedLoop( + optimizedVelState.angle.angle, + optimizedVelState.speedMetersPerSecond.meters.perSecond, + optimizedAccelState.speedMetersPerSecond.meters.perSecond.perSecond + ) } else { - steeringSetPoint = desiredVelState.angle.angle - speedSetPoint = desiredVelState.speedMetersPerSecond.meters.perSecond - accelerationSetPoint = desiredAccelState.speedMetersPerSecond.meters.perSecond.perSecond - - io.setClosedLoop(steeringSetPoint, speedSetPoint, accelerationSetPoint) + io.setClosedLoop( + desiredVelState.angle.angle, + desiredVelState.speedMetersPerSecond.meters.perSecond, + desiredAccelState.speedMetersPerSecond.meters.perSecond.perSecond + ) } } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/servemodule/SwerveModuleIO.kt similarity index 76% rename from src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIO.kt rename to src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/servemodule/SwerveModuleIO.kt index 95173245..bbc82e41 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/servemodule/SwerveModuleIO.kt @@ -7,7 +7,6 @@ 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 @@ -49,9 +48,6 @@ interface SwerveModuleIO { var driveTemp = 0.0.celsius var steeringTemp = 0.0.celsius - var odometryDrivePositions = arrayOf() - var odometrySteeringPositions = arrayOf() - var potentiometerOutputRaw = 0.0 var potentiometerOutputRadians = 0.0.radians @@ -76,43 +72,46 @@ interface SwerveModuleIO { } override fun fromLog(table: LogTable?) { - table?.get("driveAppliedVoltage", driveAppliedVoltage.inVolts)?.let { + table?.getDouble("driveAppliedVoltage", driveAppliedVoltage.inVolts)?.let { driveAppliedVoltage = it.volts } - table?.get("steeringAppliedVoltage", steeringAppliedVoltage.inVolts)?.let { + table?.getDouble("steeringAppliedVoltage", steeringAppliedVoltage.inVolts)?.let { steeringAppliedVoltage = it.volts } - table?.get("driveStatorCurrentAmps", driveStatorCurrent.inAmperes)?.let { + table?.getDouble("driveStatorCurrentAmps", driveStatorCurrent.inAmperes)?.let { driveStatorCurrent = it.amps } - table?.get("driveSupplyCurrentAmps", driveSupplyCurrent.inAmperes)?.let { + table?.getDouble("driveSupplyCurrentAmps", driveSupplyCurrent.inAmperes)?.let { driveSupplyCurrent = it.amps } - table?.get("steeringStatorCurrentAmps", steeringStatorCurrent.inAmperes)?.let { + table?.getDouble("steeringStatorCurrentAmps", steeringStatorCurrent.inAmperes)?.let { steeringStatorCurrent = it.amps } - table?.get("steeringSupplyCurrentAmps", steeringSupplyCurrent.inAmperes)?.let { + table?.getDouble("steeringSupplyCurrentAmps", steeringSupplyCurrent.inAmperes)?.let { steeringSupplyCurrent = it.amps } - table?.get("drivePositionMeters", drivePosition.inMeters)?.let { drivePosition = it.meters } - table?.get("steeringPositionRadians", steeringPosition.inRadians)?.let { + table?.getDouble("drivePositionMeters", drivePosition.inMeters)?.let { + drivePosition = it.meters + } + table?.getDouble("steeringPositionRadians", steeringPosition.inRadians)?.let { steeringPosition = it.radians } - table?.get("driveVelocityMetersPerSecond", driveVelocity.inMetersPerSecond)?.let { + table?.getDouble("driveVelocityMetersPerSecond", driveVelocity.inMetersPerSecond)?.let { driveVelocity = it.meters.perSecond } - table?.get("steeringVelocityRadiansPerSecond", steeringVelocity.inRadiansPerSecond)?.let { - steeringVelocity = it.radians.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?.get("driveTempCelcius", driveTemp.inCelsius)?.let { driveTemp = it.celsius } - table?.get("steeringTempCelcius", steeringTemp.inCelsius)?.let { steeringTemp = it.celsius } - table?.get("potentiometerOutputRaw", potentiometerOutputRaw)?.let { + table?.getDouble("potentiometerOutputRaw", potentiometerOutputRaw)?.let { potentiometerOutputRaw = it } - table?.get("potentiometerOutputRaw", potentiometerOutputRadians.inRadians)?.let { + table?.getDouble("potentiometerOutputRaw", potentiometerOutputRadians.inRadians)?.let { potentiometerOutputRadians = it.radians } - table?.get("driftPositionMeters", drift.inMeters)?.let { drift = it.meters } + table?.getDouble("driftPositionMeters", drift.inMeters)?.let { drift = it.meters } } } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOSim.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/servemodule/SwerveModuleIOSim.kt similarity index 100% rename from src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOSim.kt rename to src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/servemodule/SwerveModuleIOSim.kt diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOFalcon.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOFalcon.kt deleted file mode 100644 index b655a562..00000000 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOFalcon.kt +++ /dev/null @@ -1,458 +0,0 @@ -package com.team4099.robot2023.subsystems.drivetrain.swervemodule - -import com.ctre.phoenix6.StatusSignal -import com.ctre.phoenix6.configs.MotionMagicConfigs -import com.ctre.phoenix6.configs.MotorOutputConfigs -import com.ctre.phoenix6.configs.Slot0Configs -import com.ctre.phoenix6.configs.TalonFXConfiguration -import com.ctre.phoenix6.controls.DutyCycleOut -import com.ctre.phoenix6.controls.PositionDutyCycle -import com.ctre.phoenix6.controls.VelocityDutyCycle -import com.ctre.phoenix6.hardware.TalonFX -import com.ctre.phoenix6.signals.NeutralModeValue -import com.team4099.robot2023.config.constants.Constants -import com.team4099.robot2023.config.constants.DrivetrainConstants -import com.team4099.robot2023.subsystems.drivetrain.swervemodule.threads.PhoenixOdometryThread -import com.team4099.robot2023.subsystems.falconspin.Falcon500 -import com.team4099.robot2023.subsystems.falconspin.MotorChecker -import com.team4099.robot2023.subsystems.falconspin.MotorCollection -import edu.wpi.first.wpilibj.AnalogInput -import edu.wpi.first.wpilibj.RobotController -import org.littletonrobotics.junction.Logger -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.ctreAngularMechanismSensor -import org.team4099.lib.units.ctreLinearMechanismSensor -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.inRadians -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.perSecond -import java.lang.Math.PI -import java.util.Queue - -class SwerveModuleIOFalcon( - private val steeringFalcon: TalonFX, - private val driveFalcon: TalonFX, - private val potentiometer: AnalogInput, - private val zeroOffset: Angle, - override val label: String -) : SwerveModuleIO { - private val steeringSensor = - ctreAngularMechanismSensor( - steeringFalcon, - DrivetrainConstants.STEERING_SENSOR_CPR, - DrivetrainConstants.STEERING_SENSOR_GEAR_RATIO, - DrivetrainConstants.STEERING_COMPENSATION_VOLTAGE - ) - - private val driveSensor = - ctreLinearMechanismSensor( - driveFalcon, - DrivetrainConstants.DRIVE_SENSOR_CPR, - DrivetrainConstants.DRIVE_SENSOR_GEAR_RATIO, - DrivetrainConstants.WHEEL_DIAMETER, - DrivetrainConstants.DRIVE_COMPENSATION_VOLTAGE - ) - - // motor params - private val steeringConfiguration: TalonFXConfiguration = TalonFXConfiguration() - private val driveConfiguration: TalonFXConfiguration = TalonFXConfiguration() - - private val potentiometerOutput: Double - get() { - return if (label != Constants.Drivetrain.BACK_RIGHT_MODULE_NAME) { - potentiometer.voltage / RobotController.getVoltage5V() * 2.0 * Math.PI - } else { - 2 * PI - potentiometer.voltage / RobotController.getVoltage5V() * 2.0 * Math.PI - } - } - - init { - driveFalcon.configurator.apply(TalonFXConfiguration()) - steeringFalcon.configurator.apply(TalonFXConfiguration()) - - driveFalcon.clearStickyFaults() - steeringFalcon.clearStickyFaults() - - steeringConfiguration.Slot0.kP = - steeringSensor.proportionalPositionGainToRawUnits(DrivetrainConstants.PID.STEERING_KP) - steeringConfiguration.Slot0.kI = - steeringSensor.integralPositionGainToRawUnits(DrivetrainConstants.PID.STEERING_KI) - steeringConfiguration.Slot0.kD = - steeringSensor.derivativePositionGainToRawUnits(DrivetrainConstants.PID.STEERING_KD) - steeringConfiguration.Slot0.kV = - steeringSensor.velocityFeedforwardToRawUnits(DrivetrainConstants.PID.STEERING_KFF) - steeringConfiguration.MotionMagic.MotionMagicCruiseVelocity = - steeringSensor.velocityToRawUnits(DrivetrainConstants.STEERING_VEL_MAX) - steeringConfiguration.MotionMagic.MotionMagicAcceleration = - steeringSensor.accelerationToRawUnits(DrivetrainConstants.STEERING_ACCEL_MAX) - steeringConfiguration.CurrentLimits.SupplyCurrentLimit = - DrivetrainConstants.STEERING_SUPPLY_CURRENT_LIMIT.inAmperes - steeringConfiguration.CurrentLimits.SupplyCurrentLimitEnable = true - - steeringConfiguration.MotorOutput.NeutralMode = - NeutralModeValue.Brake // change back to coast maybe? - steeringFalcon.inverted = true - steeringFalcon.configurator.apply(steeringConfiguration) - - driveConfiguration.Slot0.kP = - driveSensor.proportionalVelocityGainToRawUnits(DrivetrainConstants.PID.DRIVE_KP) - driveConfiguration.Slot0.kI = - driveSensor.integralVelocityGainToRawUnits(DrivetrainConstants.PID.DRIVE_KI) - driveConfiguration.Slot0.kD = - driveSensor.derivativeVelocityGainToRawUnits(DrivetrainConstants.PID.DRIVE_KD) - driveConfiguration.Slot0.kV = 0.05425 - // driveSensor.velocityFeedforwardToRawUnits(DrivetrainConstants.PID.DRIVE_KFF) - driveConfiguration.CurrentLimits.SupplyCurrentLimit = - DrivetrainConstants.DRIVE_SUPPLY_CURRENT_LIMIT.inAmperes - driveConfiguration.CurrentLimits.SupplyCurrentThreshold = - DrivetrainConstants.DRIVE_THRESHOLD_CURRENT_LIMIT.inAmperes - driveConfiguration.CurrentLimits.SupplyTimeThreshold = - DrivetrainConstants.DRIVE_TRIGGER_THRESHOLD_TIME.inSeconds - driveConfiguration.CurrentLimits.SupplyCurrentLimitEnable = true - driveConfiguration.CurrentLimits.StatorCurrentLimit = - DrivetrainConstants.DRIVE_STATOR_CURRENT_LIMIT.inAmperes - driveConfiguration.CurrentLimits.StatorCurrentLimitEnable = false // TODO tune - - driveConfiguration.MotorOutput.NeutralMode = NeutralModeValue.Brake - driveFalcon.configurator.apply(driveConfiguration) - - MotorChecker.add( - "Drivetrain", - "Drive", - MotorCollection( - mutableListOf(Falcon500(driveFalcon, "$label Drive Motor")), - DrivetrainConstants.DRIVE_SUPPLY_CURRENT_LIMIT, - 90.celsius, - DrivetrainConstants.DRIVE_SUPPLY_CURRENT_LIMIT - 30.amps, - 110.celsius - ) - ) - - MotorChecker.add( - "Drivetrain", - "Steering", - MotorCollection( - mutableListOf(Falcon500(steeringFalcon, "$label Steering Motor")), - DrivetrainConstants.STEERING_SUPPLY_CURRENT_LIMIT, - 90.celsius, - DrivetrainConstants.STEERING_SUPPLY_CURRENT_LIMIT - 10.amps, - 110.celsius - ) - ) - } - val driveStatorCurrentSignal: StatusSignal - val driveSupplyCurrentSignal: StatusSignal - val steeringStatorCurrentSignal: StatusSignal - val steeringSupplyCurrentSignal: StatusSignal - val driveTempSignal: StatusSignal - val steeringTempSignal: StatusSignal - val drivePositionQueue: Queue - val steeringPositionQueue: Queue - - init { - driveFalcon.configurator.apply(TalonFXConfiguration()) - steeringFalcon.configurator.apply(TalonFXConfiguration()) - - driveFalcon.clearStickyFaults() - steeringFalcon.clearStickyFaults() - - steeringConfiguration.Slot0.kP = - steeringSensor.proportionalPositionGainToRawUnits(DrivetrainConstants.PID.STEERING_KP) - steeringConfiguration.Slot0.kI = - steeringSensor.integralPositionGainToRawUnits(DrivetrainConstants.PID.STEERING_KI) - steeringConfiguration.Slot0.kD = - steeringSensor.derivativePositionGainToRawUnits(DrivetrainConstants.PID.STEERING_KD) - steeringConfiguration.Slot0.kV = - steeringSensor.velocityFeedforwardToRawUnits(DrivetrainConstants.PID.STEERING_KFF) - steeringConfiguration.MotionMagic.MotionMagicCruiseVelocity = - steeringSensor.velocityToRawUnits(DrivetrainConstants.STEERING_VEL_MAX) - steeringConfiguration.MotionMagic.MotionMagicAcceleration = - steeringSensor.accelerationToRawUnits(DrivetrainConstants.STEERING_ACCEL_MAX) - steeringConfiguration.CurrentLimits.SupplyCurrentLimit = - DrivetrainConstants.STEERING_SUPPLY_CURRENT_LIMIT.inAmperes - steeringConfiguration.CurrentLimits.SupplyCurrentLimitEnable = true - - steeringConfiguration.MotorOutput.NeutralMode = - NeutralModeValue.Brake // change back to coast maybe? - steeringFalcon.inverted = true - steeringFalcon.configurator.apply(steeringConfiguration) - - driveConfiguration.Slot0.kP = - driveSensor.proportionalVelocityGainToRawUnits(DrivetrainConstants.PID.DRIVE_KP) - driveConfiguration.Slot0.kI = - driveSensor.integralVelocityGainToRawUnits(DrivetrainConstants.PID.DRIVE_KI) - driveConfiguration.Slot0.kD = - driveSensor.derivativeVelocityGainToRawUnits(DrivetrainConstants.PID.DRIVE_KD) - driveConfiguration.Slot0.kV = 0.05425 - // driveSensor.velocityFeedforwardToRawUnits(DrivetrainConstants.PID.DRIVE_KFF) - driveConfiguration.CurrentLimits.SupplyCurrentLimit = - DrivetrainConstants.DRIVE_SUPPLY_CURRENT_LIMIT.inAmperes - driveConfiguration.CurrentLimits.SupplyCurrentThreshold = - DrivetrainConstants.DRIVE_THRESHOLD_CURRENT_LIMIT.inAmperes - driveConfiguration.CurrentLimits.SupplyTimeThreshold = - DrivetrainConstants.DRIVE_TRIGGER_THRESHOLD_TIME.inSeconds - driveConfiguration.CurrentLimits.SupplyCurrentLimitEnable = true - driveConfiguration.CurrentLimits.StatorCurrentLimit = - DrivetrainConstants.DRIVE_STATOR_CURRENT_LIMIT.inAmperes - driveConfiguration.CurrentLimits.StatorCurrentLimitEnable = false // TODO tune - - driveConfiguration.MotorOutput.NeutralMode = NeutralModeValue.Brake - driveFalcon.configurator.apply(driveConfiguration) - - driveStatorCurrentSignal = driveFalcon.statorCurrent - driveSupplyCurrentSignal = driveFalcon.supplyCurrent - steeringStatorCurrentSignal = steeringFalcon.statorCurrent - steeringSupplyCurrentSignal = steeringFalcon.statorCurrent - driveTempSignal = driveFalcon.deviceTemp - steeringTempSignal = steeringFalcon.deviceTemp - - drivePositionQueue = - PhoenixOdometryThread.getInstance().registerSignal(driveFalcon, driveFalcon.getPosition()) - steeringPositionQueue = - PhoenixOdometryThread.getInstance().registerSignal(driveFalcon, driveFalcon.getPosition()) - - MotorChecker.add( - "Drivetrain", - "Drive", - MotorCollection( - mutableListOf(Falcon500(driveFalcon, "$label Drive Motor")), - DrivetrainConstants.DRIVE_SUPPLY_CURRENT_LIMIT, - 90.celsius, - DrivetrainConstants.DRIVE_SUPPLY_CURRENT_LIMIT - 30.amps, - 110.celsius - ) - ) - - MotorChecker.add( - "Drivetrain", - "Steering", - MotorCollection( - mutableListOf(Falcon500(steeringFalcon, "$label Steering Motor")), - DrivetrainConstants.STEERING_SUPPLY_CURRENT_LIMIT, - 90.celsius, - DrivetrainConstants.STEERING_SUPPLY_CURRENT_LIMIT - 10.amps, - 110.celsius - ) - ) - } - - override fun updateInputs(inputs: SwerveModuleIO.SwerveModuleIOInputs) { - inputs.driveAppliedVoltage = (driveFalcon.get() * RobotController.getBatteryVoltage()).volts - inputs.steeringAppliedVoltage = - (steeringFalcon.get() * RobotController.getBatteryVoltage()).volts - - inputs.driveStatorCurrent = driveStatorCurrentSignal.value.amps - inputs.driveSupplyCurrent = driveSupplyCurrentSignal.value.amps - inputs.steeringStatorCurrent = steeringStatorCurrentSignal.value.amps - inputs.steeringSupplyCurrent = steeringSupplyCurrentSignal.value.amps - - inputs.drivePosition = driveSensor.position - inputs.steeringPosition = steeringSensor.position - - inputs.driveVelocity = driveSensor.velocity - inputs.steeringVelocity = steeringSensor.velocity - - // processor temp is also something we may want to log ? - inputs.driveTemp = driveTempSignal.value.celsius - inputs.steeringTemp = steeringTempSignal.value.celsius - - inputs.odometryDrivePositions = - drivePositionQueue - .stream() - .map { value: Double -> - ( - DrivetrainConstants.WHEEL_DIAMETER * 2 * Math.PI * value / - DrivetrainConstants.DRIVE_SENSOR_GEAR_RATIO - ) - } - .toArray() as - Array - inputs.odometrySteeringPositions = - steeringPositionQueue - .stream() - .map { value: Double -> - (value / DrivetrainConstants.STEERING_SENSOR_GEAR_RATIO).rotations - } - .toArray() as - Array - drivePositionQueue.clear() - steeringPositionQueue.clear() - - inputs.potentiometerOutputRaw = - potentiometer.voltage / RobotController.getVoltage5V() * 2.0 * Math.PI - inputs.potentiometerOutputRadians = potentiometerOutput.radians - - Logger.recordOutput( - "$label/potentiometerRadiansWithOffset", - (inputs.potentiometerOutputRadians - zeroOffset).inRadians - ) - - Logger.recordOutput("$label/motorOutput", driveFalcon.get()) - } - - override fun setSteeringSetpoint(angle: Angle) { - steeringFalcon.setControl( - PositionDutyCycle( - steeringSensor.positionToRawUnits(angle), - steeringSensor.velocityToRawUnits(0.0.radians.perSecond), - DrivetrainConstants.FOC_ENABLED, - 0.0, - 0, - false, - false, - false - ) - ) - } - - override fun setClosedLoop( - steering: Angle, - speed: LinearVelocity, - acceleration: LinearAcceleration - ) { - val feedforward = DrivetrainConstants.PID.DRIVE_KS * speed.sign - driveFalcon.setControl( - VelocityDutyCycle( - driveSensor.velocityToRawUnits(speed), - driveSensor.accelerationToRawUnits(acceleration), - DrivetrainConstants.FOC_ENABLED, - feedforward.inVolts / DrivetrainConstants.DRIVE_COMPENSATION_VOLTAGE.inVolts, - 0, - false, - false, - false - ) - ) - - setSteeringSetpoint(steering) - } - - /** - * Open Loop Control using PercentOutput control on a Falcon - * - * @param steering: Desired angle - * @param speed: Desired speed - */ - override fun setOpenLoop(steering: Angle, speed: LinearVelocity) { - driveFalcon.setControl( - DutyCycleOut( - speed / DrivetrainConstants.DRIVE_SETPOINT_MAX, - DrivetrainConstants.FOC_ENABLED, - false, - false, - false - ) - ) - setSteeringSetpoint(steering) - } - - override fun resetModuleZero() { - println("Absolute Potentiometer Value $label ($potentiometerOutput") - } - - override fun zeroSteering() { - steeringFalcon.setPosition( - steeringSensor.positionToRawUnits( - if (label != Constants.Drivetrain.BACK_RIGHT_MODULE_NAME) - (potentiometerOutput.radians) - zeroOffset - else (2 * PI).radians - (potentiometerOutput.radians - zeroOffset) - ) - ) - - drivePositionQueue.clear() - steeringPositionQueue.clear() - Logger.recordOutput("$label/zeroPositionRadians", steeringSensor.position.inRadians) - println("Loading Zero for Module $label (${steeringFalcon.position.value})") - } - - override fun zeroDrive() { - driveFalcon.setPosition(0.0) - drivePositionQueue.clear() - steeringPositionQueue.clear() - } - - override fun configureDrivePID( - kP: ProportionalGain, Volt>, - kI: IntegralGain, Volt>, - kD: DerivativeGain, Volt> - ) { - val PIDConfig = Slot0Configs() - - PIDConfig.kP = driveSensor.proportionalVelocityGainToRawUnits(kP) - PIDConfig.kI = driveSensor.integralVelocityGainToRawUnits(kI) - PIDConfig.kD = driveSensor.derivativeVelocityGainToRawUnits(kD) - PIDConfig.kV = 0.05425 - - driveFalcon.configurator.apply(PIDConfig) - } - - override fun configureSteeringPID( - kP: ProportionalGain, - kI: IntegralGain, - kD: DerivativeGain - ) { - val PIDConfig = Slot0Configs() - - PIDConfig.kP = steeringSensor.proportionalPositionGainToRawUnits(kP) - PIDConfig.kI = steeringSensor.integralPositionGainToRawUnits(kI) - PIDConfig.kD = steeringSensor.derivativePositionGainToRawUnits(kD) - PIDConfig.kV = 0.05425 - - driveFalcon.configurator.apply(PIDConfig) - } - - override fun configureSteeringMotionMagic( - maxVel: AngularVelocity, - maxAccel: AngularAcceleration - ) { - - val motionMagicConfig = MotionMagicConfigs() - - motionMagicConfig.MotionMagicCruiseVelocity = steeringSensor.velocityToRawUnits(maxVel) - motionMagicConfig.MotionMagicAcceleration = steeringSensor.accelerationToRawUnits(maxAccel) - - steeringFalcon.configurator.apply(motionMagicConfig) - } - - override fun setDriveBrakeMode(brake: Boolean) { - val motorOutputConfig = MotorOutputConfigs() - - if (brake) { - motorOutputConfig.NeutralMode = NeutralModeValue.Brake - } else { - motorOutputConfig.NeutralMode = NeutralModeValue.Coast - } - driveFalcon.configurator.apply(motorOutputConfig) - } - - override fun setSteeringBrakeMode(brake: Boolean) { - val motorOutputConfig = MotorOutputConfigs() - - if (brake) { - motorOutputConfig.NeutralMode = NeutralModeValue.Brake - } else { - motorOutputConfig.NeutralMode = NeutralModeValue.Coast - } - steeringFalcon.configurator.apply(motorOutputConfig) - // motor output configs might overwrite invert? - steeringFalcon.inverted = true - } -} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/threads/PhoenixOdometryThread.java b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/threads/PhoenixOdometryThread.java deleted file mode 100644 index e9a0a4a1..00000000 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/threads/PhoenixOdometryThread.java +++ /dev/null @@ -1,110 +0,0 @@ -// Copyright 2021-2023 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. - -package com.team4099.robot2023.subsystems.drivetrain.swervemodule.threads; - -import com.ctre.phoenix6.BaseStatusSignal; -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.hardware.ParentDevice; -import com.ctre.phoenix6.unmanaged.Unmanaged; -import com.team4099.robot2023.config.constants.DrivetrainConstants; -import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain; - -import java.util.ArrayList; -import java.util.List; -import java.util.Queue; -import java.util.concurrent.ArrayBlockingQueue; -import java.util.concurrent.locks.Lock; -import java.util.concurrent.locks.ReentrantLock; - -/** - * Provides an interface for asynchronously reading high-frequency measurements to a set of queues. - * - *

This version is intended for Phoenix 6 devices on both the RIO and CANivore buses. When using - * a CANivore, the thread uses the "waitForAll" blocking method to enable more consistent sampling. - * This also allows Phoenix Pro users to benefit from lower latency between devices using CANivore - * time synchronization. - */ -public class PhoenixOdometryThread extends Thread { - private final Lock signalsLock = - new ReentrantLock(); // Prevents conflicts when registering signals - private BaseStatusSignal[] signals = new BaseStatusSignal[0]; - private final List> queues = new ArrayList<>(); - private boolean isCANFD = false; - - private static PhoenixOdometryThread instance = null; - - public static PhoenixOdometryThread getInstance() { - if (instance == null) { - instance = new PhoenixOdometryThread(); - } - return instance; - } - - private PhoenixOdometryThread() { - setName("PhoenixOdometryThread"); - setDaemon(true); - start(); - } - - public Queue registerSignal(ParentDevice device, StatusSignal signal) { - isCANFD = Unmanaged.isNetworkFD(device.getNetwork()); - Queue queue = new ArrayBlockingQueue<>(100); - signalsLock.lock(); - try { - BaseStatusSignal[] newSignals = new BaseStatusSignal[signals.length + 1]; - System.arraycopy(signals, 0, newSignals, 0, signals.length); - newSignals[signals.length] = signal; - signals = newSignals; - queues.add(queue); - - } finally { - signalsLock.unlock(); - } - return queue; - } - - @Override - public void run() { - while (true) { - // Wait for updates from all signals - signalsLock.lock(); - try { - if (isCANFD && signals.length > 0) { - BaseStatusSignal.waitForAll(2.0 / DrivetrainConstants.OMOMETRY_UPDATE_FREQUENCY, signals); - } else { - // "waitForAll" does not support blocking on multiple - // signals with a bus that is not CAN FD, regardless - // of Pro licensing. No reasoning for this behavior - // is provided by the documentation. - Thread.sleep((long) (1000.0 / DrivetrainConstants.OMOMETRY_UPDATE_FREQUENCY)); - BaseStatusSignal.refreshAll(signals); - } - } catch (InterruptedException e) { - e.printStackTrace(); - } finally { - signalsLock.unlock(); - } - - // Save new data to queues - Drivetrain.Companion.setOdometryLock(true); - try { - for (int i = 0; i < signals.length; i++) { - queues.get(i).offer(signals[i].getValueAsDouble()); - } - } finally { - Drivetrain.Companion.setOdometryLock(false); - } - } - } -} \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/threads/SparkMaxOdometryThread.java b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/threads/SparkMaxOdometryThread.java deleted file mode 100644 index 07d34186..00000000 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/threads/SparkMaxOdometryThread.java +++ /dev/null @@ -1,73 +0,0 @@ -// Copyright 2021-2023 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. - -package com.team4099.robot2023.subsystems.drivetrain.swervemodule.threads; - -import com.team4099.robot2023.config.constants.DrivetrainConstants; -import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain; -import edu.wpi.first.wpilibj.Notifier; -import java.util.ArrayList; -import java.util.List; -import java.util.Queue; -import java.util.concurrent.ArrayBlockingQueue; -import java.util.function.DoubleSupplier; - -/** - * Provides an interface for asynchronously reading high-frequency measurements to a set of queues. - * - *

This version is intended for devices like the SparkMax that require polling rather than a - * blocking thread. A Notifier thread is used to gather samples with consistent timing. - */ -public class SparkMaxOdometryThread { - private List signals = new ArrayList<>(); - private List> queues = new ArrayList<>(); - - private final Notifier notifier; - private static SparkMaxOdometryThread instance = null; - - public static SparkMaxOdometryThread getInstance() { - if (instance == null) { - instance = new SparkMaxOdometryThread(); - } - return instance; - } - - private SparkMaxOdometryThread() { - notifier = new Notifier(this::periodic); - notifier.setName("SparkMaxOdometryThread"); - notifier.startPeriodic(1.0 / DrivetrainConstants.OMOMETRY_UPDATE_FREQUENCY ); - } - - public Queue registerSignal(DoubleSupplier signal) { - Queue queue = new ArrayBlockingQueue<>(100); - Drivetrain.Companion.setOdometryLock(true); - try { - signals.add(signal); - queues.add(queue); - } finally { - Drivetrain.Companion.setOdometryLock(false); - } - return queue; - } - - private void periodic() { - Drivetrain.Companion.setOdometryLock(true); - try { - for (int i = 0; i < signals.size(); i++) { - queues.get(i).offer(signals.get(i).getAsDouble()); - } - } finally { - Drivetrain.Companion.setOdometryLock(false); - } - } -} \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt new file mode 100644 index 00000000..26b64436 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt @@ -0,0 +1,406 @@ +package com.team4099.robot2023.subsystems.elevator + +import com.team4099.lib.hal.Clock +import com.team4099.lib.logging.LoggedTunableNumber +import com.team4099.lib.logging.LoggedTunableValue +import com.team4099.robot2023.config.constants.Constants +import com.team4099.robot2023.config.constants.ElevatorConstants +import edu.wpi.first.wpilibj.RobotBase +import edu.wpi.first.wpilibj2.command.Command +import edu.wpi.first.wpilibj2.command.Commands.runOnce +import edu.wpi.first.wpilibj2.command.SubsystemBase +import org.littletonrobotics.junction.Logger +import org.team4099.lib.controller.ElevatorFeedforward +import org.team4099.lib.controller.TrapezoidProfile +import org.team4099.lib.units.base.Meter +import org.team4099.lib.units.base.inInches +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.derived.ElectricalPotential +import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.inDegrees +import org.team4099.lib.units.derived.inVolts +import org.team4099.lib.units.derived.inVoltsPerInch +import org.team4099.lib.units.derived.inVoltsPerInchPerSecond +import org.team4099.lib.units.derived.inVoltsPerInchSeconds +import org.team4099.lib.units.derived.inVoltsPerMeterPerSecondPerSecond +import org.team4099.lib.units.derived.perInch +import org.team4099.lib.units.derived.perInchSeconds +import org.team4099.lib.units.derived.volts +import org.team4099.lib.units.inInchesPerSecond +import org.team4099.lib.units.perSecond +import kotlin.time.Duration.Companion.seconds +import com.team4099.robot2023.subsystems.superstructure.Request.ElevatorRequest as ElevatorRequest + +class Elevator(val io: ElevatorIO) : SubsystemBase() { + val inputs = ElevatorIO.ElevatorInputs() + private var elevatorFeedforward: ElevatorFeedforward = + ElevatorFeedforward( + ElevatorConstants.ELEVATOR_KS, + ElevatorConstants.ELEVATOR_KG, + ElevatorConstants.ELEVATOR_KV, + ElevatorConstants.ELEVATOR_KA + ) + + private val kP = + LoggedTunableValue("Elevator/kP", Pair({ it.inVoltsPerInch }, { it.volts.perInch })) + private val kI = + LoggedTunableValue( + "Elevator/kI", Pair({ it.inVoltsPerInchSeconds }, { it.volts.perInchSeconds }) + ) + private val kD = + LoggedTunableValue( + "Elevator/kD", Pair({ it.inVoltsPerInchPerSecond }, { it.volts / 1.0.inches.perSecond }) + ) + private val kS = + LoggedTunableValue( + "Elevator/kS", Pair({ it.inVolts}, {it.volts}) + ) + private val kG = + LoggedTunableValue( + "Elevator/kG", Pair({ it.inVolts}, {it.volts}) + ) + private val kV = + LoggedTunableValue( + "Elevator/kG", Pair({it.inVoltsPerInchPerSecond}, {it.volts / 1.0.inches.perSecond}) + ) + private val kA = + LoggedTunableValue( + "Elevator/kA", Pair({it.inVoltsPerMeterPerSecondPerSecond}, {it.volts / 1.0.meters.perSecond.perSecond}) + ) + + object TunableElevatorHeights { + val enableElevator = + LoggedTunableNumber("Elevator/enableMovementElevator", + if (ElevatorConstants.ENABLE_ELEVATOR) 1.0 else 0.0) + + val minPosition = + LoggedTunableValue( + "Elevator/minPosition", + ElevatorConstants.ELEVATOR_IDLE_HEIGHT, + Pair({ it.inInches }, { it.inches }) + ) + val maxPosition = + LoggedTunableValue( + "Elevator/maxPosition", + ElevatorConstants.ELEVATOR_SOFT_LIMIT_EXTENSION, + Pair({ it.inInches }, { it.inches }) + ) + + // TODO: change voltages + val openLoopExtendVoltage = + LoggedTunableValue( + "Elevator/openLoopExtendVoltage", ElevatorConstants.ELEVATOR_OPEN_LOOP_EXTEND_VOLTAGE, Pair({ it.inVolts }, { it.volts }) + ) + val openLoopRetractVoltage = + LoggedTunableValue( + "Elevator/openLoopRetractVoltage", ElevatorConstants.ELEVATOR_OPEN_LOOP_RETRACT_VOLTAGE, Pair({ it.inVolts }, { it.volts }) + ) + + val shootSpeakerPosition = + LoggedTunableValue( + "Elevator/shootSpeakerPosition", ElevatorConstants.SHOOT_SPEAKER_POSITION + ) + val shootAmpPosition = + LoggedTunableValue("Elevator/shootAmpPosition", ElevatorConstants.SHOOT_AMP_POSITION) + val sourceNoteOffset = + LoggedTunableValue("Elevator/sourceNoteOffset", ElevatorConstants.SOURCE_NOTE_OFFSET) + + val xPos = LoggedTunableValue("Elevator/xPos", 0.0.inches) + val yPos = LoggedTunableValue("Elevator/yPos", 0.0.inches) + val zPos = LoggedTunableValue("Elevator/zPos", 0.0.inches) + val thetaPos = LoggedTunableValue("Elevator/thetaPos", 0.0.degrees) + val xPos1 = LoggedTunableValue("Elevator/xPos1", 0.0.inches) + val yPos1 = LoggedTunableValue("Elevator/yPos1", 0.0.inches) + val zPos1 = LoggedTunableValue("Elevator/zPos1", 0.0.inches) + val thetaPos1 = + LoggedTunableValue( + "Elevator/thetaPos1", + ElevatorConstants.ELEVATOR_THETA_POS, + Pair({ it.inDegrees }, { it.degrees }) + ) + } + + val forwardLimitReached: Boolean + get() = inputs.elevatorPosition >= ElevatorConstants.ELEVATOR_SOFT_LIMIT_EXTENSION + val reverseLimitReached: Boolean + get() = inputs.elevatorPosition <= ElevatorConstants.ELEVATOR_SOFT_LIMIT_RETRACTION + + val forwardOpenLoopLimitReached: Boolean + get() = inputs.elevatorPosition >= ElevatorConstants.ELEVATOR_OPEN_LOOP_SOFT_LIMIT_EXTENSION + val reverseOpenLoopLimitReached: Boolean + get() = inputs.elevatorPosition <= ElevatorConstants.ELEVATOR_OPEN_LOOP_SOFT_LIMIT_RETRACTION + + var isHomed = false + + var currentState: ElevatorState = ElevatorState.UNINITIALIZED + var currentRequest: ElevatorRequest = ElevatorRequest.OpenLoop(0.0.volts) + set(value) { + when (value) { + is ElevatorRequest.OpenLoop -> elevatorVoltageTarget = value.voltage + is ElevatorRequest.TargetingPosition -> { + elevatorPositionTarget = value.position + } + else -> {} + } + field = value + } + + var elevatorPositionTarget = 0.0.inches + private set + var elevatorVelocityTarget = 0.0.inches.perSecond + private set + var elevatorVoltageTarget = 0.0.volts + private set + + private var lastRequestedPosition = -9999.inches + private var lastRequestedVelocity = -9999.inches.perSecond + private var lastRequestedVoltage = -9999.volts + + private var timeProfileGeneratedAt = Clock.fpgaTime + + private var lastHomingStatorCurrentTripTime = Clock.fpgaTime + + // Trapezoidal Profile Constraints + private var elevatorConstraints: TrapezoidProfile.Constraints = + TrapezoidProfile.Constraints( + ElevatorConstants.MAX_VELOCITY, ElevatorConstants.MAX_ACCELERATION + ) + private var elevatorSetpoint: TrapezoidProfile.State = + TrapezoidProfile.State(inputs.elevatorPosition, inputs.elevatorVelocity) + private var elevatorProfile = + TrapezoidProfile( + elevatorConstraints, + TrapezoidProfile.State(-9999.inches, -9999.inches.perSecond), + TrapezoidProfile.State(-9999.inches, -9999.inches.perSecond) + ) + + val isAtTargetedPosition: Boolean + get() = + ( + currentRequest is ElevatorRequest.TargetingPosition && + elevatorProfile.isFinished(Clock.fpgaTime - timeProfileGeneratedAt) && + (inputs.elevatorPosition - elevatorPositionTarget).absoluteValue <= + ElevatorConstants.ELEVATOR_TOLERANCE + ) || + (TunableElevatorHeights.enableElevator.get() != 1.0) + + val canContinueSafely: Boolean + get() = + currentRequest is ElevatorRequest.TargetingPosition && + ( + ((inputs.elevatorPosition - elevatorPositionTarget).absoluteValue <= ElevatorConstants.ELEVATOR_SAFE_THRESHOLD) || + elevatorProfile.isFinished(Clock.fpgaTime - timeProfileGeneratedAt) + ) && + lastRequestedPosition == elevatorPositionTarget + + init { + TunableElevatorHeights + + // Initializes PID constants and changes FF depending on if sim or real + if (RobotBase.isReal()) { + isHomed = false + + kP.initDefault(ElevatorConstants.REAL_KP) + kI.initDefault(ElevatorConstants.REAL_KI) + kD.initDefault(ElevatorConstants.REAL_KD) + } else { + isHomed = true + + kP.initDefault(ElevatorConstants.SIM_KP) + kI.initDefault(ElevatorConstants.SIM_KI) + kD.initDefault(ElevatorConstants.SIM_KD) + + io.configPID(kP.get(), kI.get(), kD.get()) + } + + elevatorFeedforward = + ElevatorFeedforward( + ElevatorConstants.ELEVATOR_KS, + ElevatorConstants.ELEVATOR_KG, + ElevatorConstants.ELEVATOR_KV, + ElevatorConstants.ELEVATOR_KA + ) + + + } + + override fun periodic() { + io.updateInputs(inputs) + if ((kP.hasChanged()) || (kI.hasChanged()) || (kD.hasChanged())) { + io.configPID(kP.get(), kI.get(), kD.get()) + } + Logger.processInputs("Elevator", inputs) + Logger.recordOutput("Elevator/currentState", currentState.name) + Logger.recordOutput("Elevator/currentRequest", currentRequest.javaClass.simpleName) + Logger.recordOutput( + "Elevator/elevatorHeight", + inputs.elevatorPosition - ElevatorConstants.ELEVATOR_GROUND_OFFSET + ) + if (Constants.Tuning.DEBUGING_MODE) { + Logger.recordOutput("Elevator/isHomed", isHomed) + Logger.recordOutput("Elevator/canContinueSafely", canContinueSafely) + + Logger.recordOutput("Elevator/isAtTargetPosition", isAtTargetedPosition) + Logger.recordOutput("Elevator/lastGeneratedAt", timeProfileGeneratedAt.inSeconds) + + Logger.recordOutput("Elevator/elevatorPositionTarget", elevatorPositionTarget.inInches) + Logger.recordOutput( + "Elevator/elevatorVelocityTarget", elevatorVelocityTarget.inInchesPerSecond + ) + Logger.recordOutput("Elevator/elevatorVoltageTarget", elevatorVoltageTarget.inVolts) + + Logger.recordOutput("Elevator/lastElevatorPositionTarget", lastRequestedPosition.inInches) + Logger.recordOutput( + "Elevator/lastElevatorVelocityTarget", lastRequestedVelocity.inInchesPerSecond + ) + Logger.recordOutput("Elevator/lastElevatorVoltageTarget", lastRequestedVoltage.inVolts) + + Logger.recordOutput("Elevator/forwardLimitReached", forwardLimitReached) + Logger.recordOutput("Elevator/reverseLimitReached", reverseLimitReached) + } + var nextState = currentState + when (currentState) { + ElevatorState.UNINITIALIZED -> { + nextState = fromElevatorRequestToState(currentRequest) + } + ElevatorState.OPEN_LOOP -> { + setOutputVoltage(elevatorVoltageTarget) + nextState = fromElevatorRequestToState(currentRequest) + } + ElevatorState.TARGETING_POSITION -> { + if ((elevatorPositionTarget != lastRequestedPosition) || + (elevatorVelocityTarget != lastRequestedVelocity) + ) { + val preProfileGenerate = Clock.realTimestamp + elevatorProfile = + TrapezoidProfile( + elevatorConstraints, + TrapezoidProfile.State(elevatorPositionTarget, elevatorVelocityTarget), + TrapezoidProfile.State(inputs.elevatorPosition, inputs.elevatorVelocity) + ) + val postProfileGenerate = Clock.realTimestamp + Logger.recordOutput( + "/Elevator/profileGenerationMS", + postProfileGenerate.inSeconds - preProfileGenerate.inSeconds + ) + Logger.recordOutput("Elevator/initialPosition", elevatorProfile.initial.position.inInches) + Logger.recordOutput( + "Elevator/initialVelocity", elevatorProfile.initial.velocity.inInchesPerSecond + ) + timeProfileGeneratedAt = Clock.fpgaTime + lastRequestedPosition = elevatorPositionTarget + lastRequestedVelocity = elevatorVelocityTarget + } + val timeElapsed = Clock.fpgaTime - timeProfileGeneratedAt + val profilePosition = elevatorProfile.calculate(timeElapsed) + setPosition(profilePosition) + Logger.recordOutput( + "Elevator/completedMotionProfile", elevatorProfile.isFinished(timeElapsed) + ) + Logger.recordOutput( + "Elevator/profileTargetVelocity", profilePosition.velocity.inInchesPerSecond + ) + Logger.recordOutput("Elevator/profileTargetPosition", profilePosition.position.inInches) + nextState = fromElevatorRequestToState(currentRequest) + if (!(currentState.equivalentToRequest(currentRequest))) { + lastRequestedVelocity = -999.inches.perSecond + lastRequestedPosition = -999.inches + } + } + ElevatorState.HOME -> { + if (inputs.leaderStatorCurrent < ElevatorConstants.HOMING_STATOR_CURRENT) { + lastHomingStatorCurrentTripTime = Clock.fpgaTime + } + if (!inputs.isSimulating && + ( + !isHomed && + inputs.leaderStatorCurrent < ElevatorConstants.HOMING_STATOR_CURRENT && + Clock.fpgaTime - lastHomingStatorCurrentTripTime < + ElevatorConstants.HOMING_STALL_TIME_THRESHOLD + ) + ) { + io.setOutputVoltage(ElevatorConstants.HOMING_APPLIED_VOLTAGE) + } else { + zeroEncoder() + isHomed = true + } + if (isHomed) { + nextState = fromElevatorRequestToState(currentRequest) + } + } + } + currentState = nextState + } + + fun setOutputVoltage(voltage: ElectricalPotential) { + if ((forwardLimitReached) && (voltage > 0.volts) || + (reverseLimitReached) && (voltage < 0.volts) + ) { + io.setOutputVoltage(0.volts) + } else { + io.setOutputVoltage(voltage) + } + } + + fun setHomeVoltage(voltage: ElectricalPotential) { + io.setOutputVoltage(voltage) + } + + fun zeroEncoder() { + io.zeroEncoder() + } + + fun setPosition(setpoint: TrapezoidProfile.State) { + val elevatorAcceleration = + (setpoint.velocity - elevatorSetpoint.velocity) / Constants.Universal.LOOP_PERIOD_TIME + elevatorSetpoint = setpoint + val feedForward = elevatorFeedforward.calculate(setpoint.velocity, elevatorAcceleration) + if ((forwardLimitReached) && (setpoint.position > inputs.elevatorPosition) || + (reverseLimitReached) && (setpoint.position < inputs.elevatorPosition) + ) { + io.setPosition(inputs.elevatorPosition, feedForward) + } else { + io.setPosition(setpoint.position, feedForward) + } + } + + companion object { + enum class ElevatorState { + UNINITIALIZED, + TARGETING_POSITION, + OPEN_LOOP, + HOME; + inline fun equivalentToRequest(request: ElevatorRequest): Boolean { + return (request is ElevatorRequest.Home && this == HOME) || + (request is ElevatorRequest.OpenLoop && this == OPEN_LOOP) || + (request is ElevatorRequest.TargetingPosition && this == TARGETING_POSITION) + } + } + + inline fun fromElevatorRequestToState(request: ElevatorRequest): ElevatorState { + return when (request) { + is ElevatorRequest.Home -> ElevatorState.HOME + is ElevatorRequest.OpenLoop -> ElevatorState.OPEN_LOOP + is ElevatorRequest.TargetingPosition -> ElevatorState.TARGETING_POSITION + } + } + } + + fun testElevatorOpenLoopRetractCommand(): Command { + return runOnce({ currentRequest = ElevatorRequest.OpenLoop(-10.volts) }) + } + + fun testElevatorOpenLoopExtendCommand(): Command { + return runOnce({ currentRequest = ElevatorRequest.OpenLoop(10.volts) }) + } + + fun elevatorClosedLoopRetractCommand(): Command { + return runOnce({ currentRequest = ElevatorRequest.TargetingPosition(4.inches) }) + } + + fun testElevatorClosedLoopExtendCommand(): Command { + return runOnce({ currentRequest = ElevatorRequest.TargetingPosition(16.inches) }) + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIO.kt new file mode 100644 index 00000000..a5e20665 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIO.kt @@ -0,0 +1,102 @@ +package com.team4099.robot2023.subsystems.elevator + +import org.littletonrobotics.junction.LogTable +import org.littletonrobotics.junction.inputs.LoggableInputs +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.inCelsius +import org.team4099.lib.units.base.inInches +import org.team4099.lib.units.base.inches +import org.team4099.lib.units.derived.DerivativeGain +import org.team4099.lib.units.derived.ElectricalPotential +import org.team4099.lib.units.derived.IntegralGain +import org.team4099.lib.units.derived.ProportionalGain +import org.team4099.lib.units.derived.Volt +import org.team4099.lib.units.derived.inVolts +import org.team4099.lib.units.derived.volts +import org.team4099.lib.units.inInchesPerSecond +import org.team4099.lib.units.perSecond + +interface ElevatorIO { + class ElevatorInputs : LoggableInputs { + var elevatorPosition = 0.0.inches + var elevatorVelocity = 0.0.inches.perSecond + + var leaderAppliedVoltage = 0.0.volts + var followerAppliedVoltage = 0.0.volts + + var leaderSupplyCurrent = 0.0.amps + var leaderStatorCurrent = 0.0.amps + + var followerSupplyCurrent = 0.0.amps + var followerStatorCurrent = 0.0.amps + + var leaderTempCelcius = 0.0.celsius + var followerTempCelcius = 0.0.celsius + + var isSimulating = false + + override fun toLog(table: LogTable) { + table?.put("elevatorPositionInches", elevatorPosition.inInches) + table?.put("elevatorVelocityInchesPerSec", elevatorVelocity.inInchesPerSecond) + table?.put("elevatorLeaderAppliedVolts", leaderAppliedVoltage.inVolts) + table?.put("elevatorFollowerAppliedVolts", followerAppliedVoltage.inVolts) + table?.put("elevatorLeaderStatorCurrentAmps", leaderStatorCurrent.inAmperes) + table?.put("elevatorFollowerStatorCurrentAmps", followerStatorCurrent.inAmperes) + table?.put("elevatorLeaderSupplyCurrentAmps", leaderSupplyCurrent.inAmperes) + table?.put("elevatorFollowerSupplyCurrentAmps", followerSupplyCurrent.inAmperes) + table?.put("elevatorLeaderTempCelsius", leaderTempCelcius.inCelsius) + table?.put("elevatorFollowerTempCelsius", followerTempCelcius.inCelsius) + } + + override fun fromLog(table: LogTable?) { + table?.get("elevatorPositionInches", elevatorPosition.inInches)?.let { + elevatorPosition = it.inches + } + table?.get("elevatorVelocityInchesPerSec", elevatorVelocity.inInchesPerSecond)?.let { + elevatorVelocity = it.inches.perSecond + } + table?.get("elevatorLeaderAppliedVolts", leaderAppliedVoltage.inVolts)?.let { + leaderAppliedVoltage = it.volts + } + table?.get("elevatorFollowerAppliedVolts", followerAppliedVoltage.inVolts)?.let { + followerAppliedVoltage = it.volts + } + table?.get("elevatorLeaderStatorCurrentAmps", leaderStatorCurrent.inAmperes)?.let { + leaderStatorCurrent = it.amps + } + table?.get("elevatorLeaderSupplyCurrentAmps", leaderSupplyCurrent.inAmperes)?.let { + leaderSupplyCurrent = it.amps + } + table?.get("elevatorLeaderTempCelcius", leaderTempCelcius.inCelsius)?.let { + leaderTempCelcius = it.celsius + } + + table?.get("elevatorFollowerAppliedVolts", followerAppliedVoltage.inVolts)?.let { + followerAppliedVoltage = it.volts + } + table?.get("elevatorFollowerStatorCurrentAmps", followerStatorCurrent.inAmperes)?.let { + followerStatorCurrent = it.amps + } + table?.get("elevatorFollowerSupplyCurrentAmps", followerSupplyCurrent.inAmperes)?.let { + followerSupplyCurrent = it.amps + } + table?.get("elevatorFollowerTempCelcius", followerTempCelcius.inCelsius)?.let { + followerTempCelcius = it.celsius + } + } + } + + fun updateInputs(inputs: ElevatorInputs) {} + fun setOutputVoltage(voltage: ElectricalPotential) {} + fun setPosition(position: Length, feedForward: ElectricalPotential) {} + fun zeroEncoder() {} + fun configPID( + kP: ProportionalGain, + kI: IntegralGain, + kD: DerivativeGain + ) {} +} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOKraken.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOKraken.kt new file mode 100644 index 00000000..16b8bd07 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOKraken.kt @@ -0,0 +1,185 @@ +package com.team4099.robot2023.subsystems.elevator + +import com.ctre.phoenix6.StatusSignal +import com.ctre.phoenix6.configs.Slot0Configs +import com.ctre.phoenix6.configs.TalonFXConfiguration +import com.ctre.phoenix6.controls.PositionDutyCycle +import com.ctre.phoenix6.controls.PositionVoltage +import com.ctre.phoenix6.hardware.TalonFX +import com.ctre.phoenix6.signals.NeutralModeValue +import com.team4099.robot2023.config.constants.Constants +import com.team4099.robot2023.config.constants.ElevatorConstants +import com.team4099.robot2023.subsystems.falconspin.Falcon500 +import com.team4099.robot2023.subsystems.falconspin.MotorChecker +import com.team4099.robot2023.subsystems.falconspin.MotorCollection +import 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.inches +import org.team4099.lib.units.ctreLinearMechanismSensor +import org.team4099.lib.units.derived.DerivativeGain +import org.team4099.lib.units.derived.ElectricalPotential +import org.team4099.lib.units.derived.IntegralGain +import org.team4099.lib.units.derived.ProportionalGain +import org.team4099.lib.units.derived.Volt +import org.team4099.lib.units.derived.inVolts +import org.team4099.lib.units.derived.rotations +import org.team4099.lib.units.derived.volts + +object ElevatorIOKraken : ElevatorIO { + private val elevatorLeaderKraken: TalonFX = TalonFX(Constants.Elevator.LEADER_MOTOR_ID) + private val elevatorFollowerKraken: TalonFX = TalonFX(Constants.Elevator.FOLLOWER_MOTOR_ID) + private val leaderSensor = + ctreLinearMechanismSensor( + elevatorLeaderKraken, + ElevatorConstants.ELEVATOR_PULLEY_TO_MOTOR, + ElevatorConstants.SPOOL_DIAMETER, + ElevatorConstants.VOLTAGE_COMPENSATION + ) + private val followerSensor = + ctreLinearMechanismSensor( + elevatorLeaderKraken, + ElevatorConstants.ELEVATOR_PULLEY_TO_MOTOR, + ElevatorConstants.SPOOL_DIAMETER, + ElevatorConstants.VOLTAGE_COMPENSATION + ) + private val elevatorLeaderConfiguration: TalonFXConfiguration = TalonFXConfiguration() + private val elevatorFollowerConfiguration: TalonFXConfiguration = TalonFXConfiguration() + + var elevatorLeaderStatorCurrentSignal: StatusSignal + var elevatorLeaderSupplyCurrentSignal: StatusSignal + var elevatorLeadertempSignal: StatusSignal + var elevatorLeaderDutyCycle: StatusSignal + var elevatorFollowerStatorCurrentSignal: StatusSignal + var elevatorFollowerSupplyCurrentSignal: StatusSignal + var elevatorFollowertempSignal: StatusSignal + var elevatorFollowerDutyCycle: StatusSignal + + init { + elevatorLeaderKraken.clearStickyFaults() + elevatorFollowerKraken.clearStickyFaults() + elevatorLeaderConfiguration.Slot0.kP = + leaderSensor.proportionalPositionGainToRawUnits(ElevatorConstants.LEADER_KP) + elevatorLeaderConfiguration.Slot0.kI = + leaderSensor.integralPositionGainToRawUnits(ElevatorConstants.LEADER_KI) + elevatorLeaderConfiguration.Slot0.kD = + leaderSensor.derivativePositionGainToRawUnits(ElevatorConstants.LEADER_KD) + + elevatorFollowerConfiguration.Slot0.kP = + followerSensor.proportionalPositionGainToRawUnits(ElevatorConstants.FOLLOWER_KP) + elevatorFollowerConfiguration.Slot0.kI = + followerSensor.integralPositionGainToRawUnits(ElevatorConstants.FOLLOWER_KI) + elevatorFollowerConfiguration.Slot0.kD = + followerSensor.derivativePositionGainToRawUnits(ElevatorConstants.FOLLOWER_KD) + + elevatorLeaderConfiguration.CurrentLimits.SupplyCurrentLimit = + ElevatorConstants.LEADER_SUPPLY_CURRENT_LIMIT.inAmperes + elevatorLeaderConfiguration.CurrentLimits.SupplyCurrentThreshold = + ElevatorConstants.LEADER_THRESHOLD_CURRENT_LIMIT.inAmperes + elevatorLeaderConfiguration.CurrentLimits.SupplyTimeThreshold = + ElevatorConstants.LEADER_SUPPLY_TIME_THRESHOLD.inSeconds + elevatorLeaderConfiguration.CurrentLimits.SupplyCurrentLimitEnable = true + elevatorLeaderConfiguration.CurrentLimits.StatorCurrentLimit = + ElevatorConstants.LEADER_STATOR_CURRENT_LIMIT.inAmperes + elevatorLeaderConfiguration.CurrentLimits.StatorCurrentLimitEnable = false + + elevatorFollowerConfiguration.CurrentLimits.SupplyCurrentLimit = + ElevatorConstants.FOLLOWER_SUPPLY_CURRENT_LIMIT.inAmperes + elevatorFollowerConfiguration.CurrentLimits.SupplyCurrentThreshold = + ElevatorConstants.FOLLOWER_THRESHOLD_CURRENT_LIMIT.inAmperes + elevatorFollowerConfiguration.CurrentLimits.SupplyTimeThreshold = + ElevatorConstants.FOLLOWER_SUPPLY_TIME_THRESHOLD.inSeconds + elevatorFollowerConfiguration.CurrentLimits.SupplyCurrentLimitEnable = true + elevatorFollowerConfiguration.CurrentLimits.StatorCurrentLimit = + ElevatorConstants.FOLLOWER_STATOR_CURRENT_LIMIT.inAmperes + elevatorFollowerConfiguration.CurrentLimits.StatorCurrentLimitEnable = false + + elevatorLeaderConfiguration.MotorOutput.NeutralMode = NeutralModeValue.Brake + elevatorFollowerConfiguration.MotorOutput.NeutralMode = NeutralModeValue.Brake + elevatorLeaderKraken.configurator.apply(elevatorLeaderConfiguration) + elevatorFollowerKraken.configurator.apply(elevatorFollowerConfiguration) + elevatorLeaderKraken.inverted = true + elevatorFollowerKraken.inverted = false + MotorChecker.add( + "Elevator", + "Extension", + MotorCollection( + mutableListOf( + Falcon500(elevatorLeaderKraken, "Leader Extension Motor"), + (Falcon500(elevatorFollowerKraken, "Follower Extension Motor")) + ), + ElevatorConstants.LEADER_STATOR_CURRENT_LIMIT, + 30.celsius, + ElevatorConstants.LEADER_STATOR_CURRENT_LIMIT - 30.amps, + 110.celsius + ) + ) + elevatorLeaderStatorCurrentSignal = elevatorLeaderKraken.statorCurrent + elevatorLeaderSupplyCurrentSignal = elevatorLeaderKraken.supplyCurrent + elevatorLeadertempSignal = elevatorLeaderKraken.deviceTemp + elevatorLeaderDutyCycle = elevatorLeaderKraken.dutyCycle + elevatorFollowerStatorCurrentSignal = elevatorFollowerKraken.statorCurrent + elevatorFollowerSupplyCurrentSignal = elevatorFollowerKraken.supplyCurrent + elevatorFollowertempSignal = elevatorFollowerKraken.deviceTemp + elevatorFollowerDutyCycle = elevatorFollowerKraken.dutyCycle + } + + override fun configPID( + kP: ProportionalGain, + kI: IntegralGain, + kD: DerivativeGain + ) { + val pidConfiguration = Slot0Configs() + pidConfiguration.kP = leaderSensor.proportionalPositionGainToRawUnits(kP) + pidConfiguration.kI = leaderSensor.integralPositionGainToRawUnits(kI) + pidConfiguration.kD = leaderSensor.derivativePositionGainToRawUnits(kD) + elevatorLeaderKraken.configurator.apply(pidConfiguration) + elevatorFollowerKraken.configurator.apply(pidConfiguration) + } + + override fun updateInputs(inputs: ElevatorIO.ElevatorInputs) { + inputs.elevatorPosition = leaderSensor.position + inputs.elevatorVelocity = leaderSensor.velocity + inputs.leaderAppliedVoltage = elevatorLeaderDutyCycle.value.volts + inputs.followerAppliedVoltage = elevatorFollowerDutyCycle.value.volts + inputs.leaderSupplyCurrent = elevatorLeaderSupplyCurrentSignal.value.amps + inputs.leaderStatorCurrent = elevatorLeaderStatorCurrentSignal.value.amps + inputs.followerSupplyCurrent = elevatorFollowerSupplyCurrentSignal.value.amps + inputs.followerStatorCurrent = elevatorFollowerStatorCurrentSignal.value.amps + inputs.leaderTempCelcius = elevatorLeadertempSignal.value.celsius + inputs.followerTempCelcius = elevatorFollowertempSignal.value.celsius + } + + override fun setOutputVoltage(voltage: ElectricalPotential) { + if (((leaderSensor.position < 0.5.inches) && (voltage < 0.volts)) || + (leaderSensor.position > ElevatorConstants.ELEVATOR_MAX_EXTENSION - 0.5.inches && (voltage > 0.volts))) { + elevatorLeaderKraken.setVoltage(0.0) + } + else { + elevatorLeaderKraken.setVoltage(voltage.inVolts) + } + } + + override fun setPosition(position: Length, feedForward: ElectricalPotential) { + elevatorLeaderKraken.setControl( + PositionVoltage( + leaderSensor.getRawPosition(), + 0.0, + true, + feedForward.inVolts, + 0, + false, + false, + false + ) + ) + } + + override fun zeroEncoder() { + elevatorLeaderKraken.setPosition(0.0) + elevatorFollowerKraken.setPosition(0.0) + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIONEO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIONEO.kt new file mode 100644 index 00000000..57e88ff9 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIONEO.kt @@ -0,0 +1,203 @@ +package com.team4099.robot2023.subsystems.elevator + +import com.revrobotics.CANSparkMax +import com.revrobotics.CANSparkMaxLowLevel +import com.revrobotics.SparkMaxPIDController +import com.team4099.lib.math.clamp +import com.team4099.robot2023.config.constants.Constants +import com.team4099.robot2023.config.constants.ElevatorConstants +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.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.inPercentOutputPerSecond +import org.team4099.lib.units.base.inches +import org.team4099.lib.units.derived.DerivativeGain +import org.team4099.lib.units.derived.ElectricalPotential +import org.team4099.lib.units.derived.IntegralGain +import org.team4099.lib.units.derived.ProportionalGain +import org.team4099.lib.units.derived.Volt +import org.team4099.lib.units.derived.inVolts +import org.team4099.lib.units.derived.volts +import org.team4099.lib.units.sparkMaxLinearMechanismSensor +import kotlin.math.absoluteValue + +object ElevatorIONEO : ElevatorIO { + + private val leaderSparkMax = + CANSparkMax(Constants.Elevator.LEADER_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless) + + private val leaderSensor = + sparkMaxLinearMechanismSensor( + leaderSparkMax, + ElevatorConstants.ELEVATOR_PULLEY_TO_MOTOR, + ElevatorConstants.SPOOL_DIAMETER, + ElevatorConstants.VOLTAGE_COMPENSATION + ) + + private val followerSparkMax = + CANSparkMax(Constants.Elevator.FOLLOWER_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless) + + private val followerSensor = + sparkMaxLinearMechanismSensor( + followerSparkMax, + ElevatorConstants.ELEVATOR_PULLEY_TO_MOTOR, + ElevatorConstants.SPOOL_DIAMETER, + ElevatorConstants.VOLTAGE_COMPENSATION + ) + + private val leaderPIDController: SparkMaxPIDController = leaderSparkMax.pidController + private val followerPIDController: SparkMaxPIDController = followerSparkMax.pidController + init { + + // reseting motor + leaderSparkMax.restoreFactoryDefaults() + followerSparkMax.restoreFactoryDefaults() + + leaderSparkMax.clearFaults() + followerSparkMax.clearFaults() + + // basic settings + leaderSparkMax.enableVoltageCompensation(ElevatorConstants.VOLTAGE_COMPENSATION.inVolts) + followerSparkMax.enableVoltageCompensation(ElevatorConstants.VOLTAGE_COMPENSATION.inVolts) + + leaderSparkMax.inverted = true + + leaderSparkMax.setSmartCurrentLimit( + ElevatorConstants.LEADER_STATOR_CURRENT_LIMIT.inAmperes.toInt() + ) + followerSparkMax.setSmartCurrentLimit( + ElevatorConstants.FOLLOWER_STATOR_CURRENT_LIMIT.inAmperes.toInt() + ) + + leaderSparkMax.idleMode = CANSparkMax.IdleMode.kBrake + followerSparkMax.idleMode = CANSparkMax.IdleMode.kBrake + + // makes follower motor output exact same power as leader + followerSparkMax.follow(leaderSparkMax, ElevatorConstants.FOLLOWER_INVERTED) + + leaderPIDController.ff = 0.0 + + leaderSparkMax.burnFlash() + followerSparkMax.burnFlash() + + MotorChecker.add( + "Elevator", + "Extension", + MotorCollection( + mutableListOf( + Neo(leaderSparkMax, "Leader Extension Motor"), + Neo(followerSparkMax, "Follower Extension Motor") + ), + ElevatorConstants.LEADER_STATOR_CURRENT_LIMIT, + 30.celsius, + ElevatorConstants.LEADER_STATOR_CURRENT_LIMIT - 30.amps, + 90.celsius + ), + ) + } + + override fun updateInputs(inputs: ElevatorIO.ElevatorInputs) { + inputs.elevatorPosition = leaderSensor.position + + inputs.elevatorVelocity = leaderSensor.velocity + + // voltage in * percent out + inputs.leaderAppliedVoltage = leaderSparkMax.busVoltage.volts * leaderSparkMax.appliedOutput + + inputs.leaderStatorCurrent = leaderSparkMax.outputCurrent.amps + + // BatteryVoltage * SupplyCurrent = percentOutput * BatteryVoltage * StatorCurrent + // AppliedVoltage = percentOutput * BatteryVoltage + // SupplyCurrent = (percentOutput * BatteryVoltage / BatteryVoltage) * StatorCurrent = + // percentOutput * statorCurrent + + inputs.leaderSupplyCurrent = + inputs.leaderStatorCurrent * leaderSparkMax.appliedOutput.absoluteValue + + inputs.leaderTempCelcius = leaderSparkMax.motorTemperature.celsius + + // voltage in * percent out + inputs.followerAppliedVoltage = leaderSparkMax.busVoltage.volts * followerSparkMax.appliedOutput + + inputs.followerStatorCurrent = followerSparkMax.outputCurrent.amps + + inputs.followerSupplyCurrent = + inputs.followerStatorCurrent * followerSparkMax.appliedOutput.absoluteValue + + inputs.followerTempCelcius = followerSparkMax.motorTemperature.celsius + + Logger.recordOutput("Elevator/leaderRawRotations", leaderSparkMax.encoder.position) + } + + /** + * Sets the voltage of the elevator motors but also checks to make sure elevator doesn't exceed + * limit + * + * @param voltage the voltage to set the motor to + */ + override fun setOutputVoltage(voltage: ElectricalPotential) { + // divide by 2 cause full power elevator is scary + if (((leaderSensor.position < 0.5.inches) && (voltage < 0.volts)) || + (leaderSensor.position > ElevatorConstants.ELEVATOR_MAX_EXTENSION - 0.5.inches && (voltage > 0.volts)) + ) { + leaderSparkMax.setVoltage(0.0) + } else { + leaderSparkMax.setVoltage(voltage.inVolts) + } + } + + /** + * Sets the voltage of the elevator motors but also checks to make sure elevator doesn't exceed + * limit + * + * @param position the target position the PID controller will use + * @param feedforward change in voltage to account for external forces on the system + */ + override fun setPosition(position: Length, feedforward: ElectricalPotential) { + leaderPIDController.setReference( + leaderSensor.positionToRawUnits( + clamp( + position, + ElevatorConstants.ELEVATOR_SOFT_LIMIT_RETRACTION, + ElevatorConstants.ELEVATOR_SOFT_LIMIT_EXTENSION + ) + ), + CANSparkMax.ControlType.kPosition, + 0, + feedforward.inVolts, + ) + } + + /** set the current encoder position to be the encoders zero value */ + override fun zeroEncoder() { + leaderSparkMax.encoder.position = 0.0 + followerSparkMax.encoder.position = 0.0 + } + + /** + * updates the PID controller values using the sensor measurement for proportional intregral and + * derivative gain multiplied by the 3 PID constants + * + * @param kP a constant which will be used to scale the proportion gain + * @param kI a constant which will be used to scale the integral gain + * @param kD a constant which will be used to scale the derivative gain + */ + override fun configPID( + kP: ProportionalGain, + kI: IntegralGain, + kD: DerivativeGain + ) { + leaderPIDController.p = leaderSensor.proportionalPositionGainToRawUnits(kP) + leaderPIDController.i = leaderSensor.integralPositionGainToRawUnits(kI) + leaderPIDController.d = leaderSensor.derivativePositionGainToRawUnits(kD) + followerPIDController.p = followerSensor.proportionalPositionGainToRawUnits(kP) + followerPIDController.i = followerSensor.integralPositionGainToRawUnits(kI) + followerPIDController.d = followerSensor.derivativePositionGainToRawUnits(kD) + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOSim.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOSim.kt new file mode 100644 index 00000000..d81e1d26 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOSim.kt @@ -0,0 +1,152 @@ +package com.team4099.robot2023.subsystems.elevator + +import com.team4099.lib.math.clamp +import com.team4099.robot2023.config.constants.Constants +import com.team4099.robot2023.config.constants.ElevatorConstants +import com.team4099.robot2023.subsystems.falconspin.MotorChecker +import com.team4099.robot2023.subsystems.falconspin.MotorCollection +import com.team4099.robot2023.subsystems.falconspin.SimulatedMotor +import edu.wpi.first.math.system.plant.DCMotor +import edu.wpi.first.wpilibj.simulation.BatterySim +import edu.wpi.first.wpilibj.simulation.ElevatorSim +import edu.wpi.first.wpilibj.simulation.RoboRioSim +import org.littletonrobotics.junction.Logger +import org.team4099.lib.controller.PIDController +import org.team4099.lib.units.base.Length +import org.team4099.lib.units.base.Meter +import org.team4099.lib.units.base.amps +import org.team4099.lib.units.base.celsius +import org.team4099.lib.units.base.inKilograms +import org.team4099.lib.units.base.inMeters +import org.team4099.lib.units.base.inSeconds +import org.team4099.lib.units.base.inches +import org.team4099.lib.units.base.meters +import org.team4099.lib.units.derived.DerivativeGain +import org.team4099.lib.units.derived.ElectricalPotential +import org.team4099.lib.units.derived.IntegralGain +import org.team4099.lib.units.derived.ProportionalGain +import org.team4099.lib.units.derived.Volt +import org.team4099.lib.units.derived.inVolts +import org.team4099.lib.units.derived.volts +import org.team4099.lib.units.perSecond + +object ElevatorIOSim : ElevatorIO { + + val elevatorSim: ElevatorSim = + ElevatorSim( + DCMotor.getNEO(2), + ElevatorConstants.ELEVATOR_PULLEY_TO_MOTOR, + ElevatorConstants.CARRIAGE_MASS.inKilograms, + ElevatorConstants.SPOOL_DIAMETER.inMeters / 2, + ElevatorConstants.ELEVATOR_MAX_RETRACTION.inMeters, + ElevatorConstants.ELEVATOR_MAX_EXTENSION.inMeters, + true, + 0.0 + ) + + init { + MotorChecker.add( + "Elevator", + "Extension", + MotorCollection( + mutableListOf( + SimulatedMotor( + elevatorSim, + "Extension Motor", + ) + ), + baseCurrentLimit = 60.amps, + firstStageTemperatureLimit = 10.celsius, + firstStageCurrentLimit = 45.amps, + motorShutDownThreshold = 20.celsius + ) + ) + } + + private val elevatorController = + PIDController(ElevatorConstants.SIM_KP, ElevatorConstants.SIM_KI, ElevatorConstants.SIM_KD) + + private var lastAppliedVoltage = 0.0.volts + + override fun updateInputs(inputs: ElevatorIO.ElevatorInputs) { + elevatorSim.update(Constants.Universal.LOOP_PERIOD_TIME.inSeconds) + + inputs.elevatorPosition = elevatorSim.positionMeters.meters + inputs.elevatorVelocity = elevatorSim.velocityMetersPerSecond.meters.perSecond + + inputs.leaderTempCelcius = 0.0.celsius + inputs.leaderStatorCurrent = 0.0.amps + inputs.leaderSupplyCurrent = elevatorSim.currentDrawAmps.amps / 2 + inputs.leaderAppliedVoltage = lastAppliedVoltage + + inputs.followerTempCelcius = 0.0.celsius + inputs.followerStatorCurrent = 0.0.amps + inputs.followerSupplyCurrent = elevatorSim.currentDrawAmps.amps / 2 + inputs.followerAppliedVoltage = lastAppliedVoltage + + inputs.isSimulating = true + + RoboRioSim.setVInVoltage( + BatterySim.calculateDefaultBatteryLoadedVoltage(elevatorSim.currentDrawAmps) + ) + } + + /** + * Sets the voltage of the elevator motors but also checks to make sure elevator doesn't exceed + * limit + * + * @param voltage the voltage to set the motor to + */ + override fun setOutputVoltage(voltage: ElectricalPotential) { + Logger.recordOutput("Elevator/OutputTest", voltage) + val clampedVoltage = + clamp( + voltage, + -ElevatorConstants.VOLTAGE_COMPENSATION, + ElevatorConstants.VOLTAGE_COMPENSATION + ) + lastAppliedVoltage = clampedVoltage + + elevatorSim.setInputVoltage(clampedVoltage.inVolts) + } + + /** + * Sets the voltage of the elevator motors but also checks to make sure elevator doesn't exceed + * limit + * + * @param position the target position the PID controller will use + * @param feedforward change in voltage to account for external forces on the system + */ + override fun setPosition(position: Length, feedforward: ElectricalPotential) { + val ff = + clamp( + feedforward, + -ElevatorConstants.VOLTAGE_COMPENSATION, + ElevatorConstants.VOLTAGE_COMPENSATION + ) + val feedback = elevatorController.calculate(elevatorSim.positionMeters.meters, position) + + setOutputVoltage(ff + feedback) + elevatorSim.setInputVoltage((ff + feedback).inVolts) + } + + /** set the current encoder position to be the encoders zero value */ + override fun zeroEncoder() { + } + + /** + * updates the PID controller values using the sensor measurement for proportional intregral and + * derivative gain multiplied by the 3 PID constants + * + * @param kP a constant which will be used to scale the proportion gain + * @param kI a constant which will be used to scale the integral gain + * @param kD a constant which will be used to scale the derivative gain + */ + override fun configPID( + kP: ProportionalGain, + kI: IntegralGain, + kD: DerivativeGain + ) { + elevatorController.setPID(kP, kI, kD) + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt index bb51b835..f7209b2a 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt @@ -1,6 +1,5 @@ package com.team4099.robot2023.subsystems.feeder import edu.wpi.first.wpilibj2.command.SubsystemBase -class Feeder(val io: FeederIO) : SubsystemBase() { -} \ No newline at end of file +class Feeder(val io: FeederIO) : SubsystemBase() 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 41ddbf24..780fc386 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt @@ -1,4 +1,3 @@ package com.team4099.robot2023.subsystems.feeder -interface FeederIO { -} \ No newline at end of file +interface FeederIO 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 65fe9e78..6ae241f6 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt @@ -1,70 +1,17 @@ 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.Angle import org.team4099.lib.units.derived.ElectricalPotential -import org.team4099.lib.units.perSecond - -// typealias GroundIntakeRequest = SuperStructureState.GroundIntakeStructure.GroundIntakeRequest -// typealias GroundIntakeState = SuperStructureState.GroundIntakeStructure.GroundIntakeState -// typealiasing for nested interfaces and sealed classes doesn't work -// https://youtrack.jetbrains.com/issue/KT-34281/Access-nested-classes-including-sealed-class-subclasses-through-typealias sealed interface Request { - sealed interface SuperstructureRequest : Request { - class Idle() : SuperstructureRequest - class Home() : SuperstructureRequest - - class GroundIntakeCube() : SuperstructureRequest - class GroundIntakeCone() : SuperstructureRequest - - class EjectGamePiece() : SuperstructureRequest - - class DoubleSubstationIntakePrep(val gamePiece: GamePiece) : SuperstructureRequest - class SingleSubstationIntakePrep(val gamePiece: GamePiece) : SuperstructureRequest - - class DoubleSubstationIntake() : SuperstructureRequest - class SingleSubstationIntake(val gamePiece: GamePiece) : SuperstructureRequest - - class PrepScore(val gamePiece: GamePiece, val nodeTier: NodeTier) : SuperstructureRequest - - class Score() : SuperstructureRequest - - class Tuning() : SuperstructureRequest - } - - // Implements RequestStructure to ensure standardized structure - sealed interface ManipulatorRequest : Request { - class TargetingPosition(val position: Length, val rollerVoltage: ElectricalPotential) : - ManipulatorRequest - class OpenLoop(val voltage: ElectricalPotential, val rollerVoltage: ElectricalPotential) : - ManipulatorRequest - class Home() : ManipulatorRequest - } - sealed interface ElevatorRequest : Request { - class TargetingPosition( - val position: Length, - val finalVelocity: LinearVelocity = 0.0.inches.perSecond, - val canContinueBuffer: Length = 5.0.inches - ) : ElevatorRequest + class TargetingPosition(val position: Length) : ElevatorRequest class OpenLoop(val voltage: ElectricalPotential) : ElevatorRequest - class Home : ElevatorRequest - } - - sealed interface GroundIntakeRequest : Request { - class TargetingPosition(val position: Angle, val rollerVoltage: ElectricalPotential) : - GroundIntakeRequest - class OpenLoop(val voltage: ElectricalPotential, val rollerVoltage: ElectricalPotential) : - GroundIntakeRequest - class ZeroArm() : GroundIntakeRequest + class Home() : ElevatorRequest } sealed interface DrivetrainRequest : Request { diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json index eb271997..0bf11fbf 100644 --- a/vendordeps/PathplannerLib.json +++ b/vendordeps/PathplannerLib.json @@ -1,7 +1,7 @@ { "fileName": "PathplannerLib.json", "name": "PathplannerLib", - "version": "2024.0.0-beta-6", + "version": "2024.1.4", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2024", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2024.0.0-beta-6" + "version": "2024.1.4" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2024.0.0-beta-6", + "version": "2024.1.4", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false,