diff --git a/simgui.json b/simgui.json index 21c47b41..efe77a4e 100644 --- a/simgui.json +++ b/simgui.json @@ -1,9 +1,29 @@ { + "HALProvider": { + "Other Devices": { + "window": { + "visible": false + } + } + }, "NTProvider": { "types": { "/FMSInfo": "FMSInfo", "/Shuffleboard/Pre-match/Mode": "String Chooser", "/SmartDashboard/AutonomousMode": "String Chooser" } + }, + "NetworkTables": { + "transitory": { + "SmartDashboard": { + "TunableNumbers": { + "Flywheel": { + "open": true + }, + "open": true + }, + "open": true + } + } } } diff --git a/src/main/kotlin/com/team4099/lib/phoenix6/PositionVoltage.kt b/src/main/kotlin/com/team4099/lib/phoenix6/PositionVoltage.kt index 2efb09ec..4bebe8fa 100644 --- a/src/main/kotlin/com/team4099/lib/phoenix6/PositionVoltage.kt +++ b/src/main/kotlin/com/team4099/lib/phoenix6/PositionVoltage.kt @@ -20,7 +20,7 @@ class PositionVoltage( private var overrideBrakeDurNeutral: Boolean = false, private var limitForwardMotion: Boolean = false, private var limitReverseMotion: Boolean = false, - private var velocity: AngularVelocity = 0.0.degrees.perSecond, + private var velocity: AngularVelocity = 0.0.degrees.perSecond ) { val positionVoltagePhoenix6 = diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt index 680529cd..e4ca5403 100644 --- a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt +++ b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt @@ -15,10 +15,16 @@ import com.team4099.robot2023.subsystems.elevator.ElevatorIOSim import com.team4099.robot2023.subsystems.feeder.Feeder import com.team4099.robot2023.subsystems.feeder.FeederIONeo import com.team4099.robot2023.subsystems.feeder.FeederIOSim +import com.team4099.robot2023.subsystems.flywheel.Flywheel +import com.team4099.robot2023.subsystems.flywheel.FlywheelIOSim +import com.team4099.robot2023.subsystems.flywheel.FlywheelIOTalon import com.team4099.robot2023.subsystems.limelight.LimelightVision import com.team4099.robot2023.subsystems.limelight.LimelightVisionIO import com.team4099.robot2023.subsystems.vision.Vision import com.team4099.robot2023.subsystems.vision.camera.CameraIONorthstar +import com.team4099.robot2023.subsystems.wrist.Wrist +import com.team4099.robot2023.subsystems.wrist.WristIONeo +import com.team4099.robot2023.subsystems.wrist.WristIOSim import com.team4099.robot2023.util.driver.Ryan import edu.wpi.first.wpilibj.RobotBase import org.team4099.lib.smoothDeadband @@ -32,6 +38,8 @@ object RobotContainer { private val limelight: LimelightVision private val feeder: Feeder private val elevator: Elevator + private val flywheel: Flywheel + private val wrist: Wrist init { if (RobotBase.isReal()) { @@ -51,6 +59,8 @@ object RobotContainer { limelight = LimelightVision(object : LimelightVisionIO {}) feeder = Feeder(FeederIONeo) elevator = Elevator(ElevatorIONEO) + flywheel = Flywheel(FlywheelIOTalon) + wrist = Wrist(WristIONeo) } else { // Simulation implementations drivetrain = Drivetrain(object : GyroIO {}, DrivetrainIOSim) @@ -63,6 +73,8 @@ object RobotContainer { limelight = LimelightVision(object : LimelightVisionIO {}) feeder = Feeder(FeederIOSim) elevator = Elevator(ElevatorIOSim) + flywheel = Flywheel(FlywheelIOSim) + wrist = Wrist(WristIOSim) } vision.setDataInterfaces({ drivetrain.odometryPose }, { drivetrain.addVisionData(it) }) 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 12a8841c..cf53f4e4 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt @@ -239,11 +239,6 @@ class DrivePathCommand( drivetrain.targetPose = Pose2d(Pose2dWPILIB(desiredState.poseMeters.translation, desiredRotation.position)) - Logger.recordOutput( - "Pathfollow/target", - Pose2dWPILIB(desiredState.poseMeters.translation, desiredRotation.position) - ) - drivetrain.currentRequest = DrivetrainRequest.ClosedLoop( nextDriveState, @@ -275,7 +270,6 @@ class DrivePathCommand( desiredRotation.velocityRadiansPerSec.radians.perSecond.inDegreesPerSecond ) - Logger.recordOutput("Pathfollow/trajectory", trajectory) Logger.recordOutput("Pathfollow/isAtReference", swerveDriveController.atReference()) Logger.recordOutput("Pathfollow/trajectoryTimeSeconds", trajectory.totalTimeSeconds) diff --git a/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt b/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt index abd31737..6298ac5e 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt @@ -94,6 +94,4 @@ object ControlBoard { val extendIntake = Trigger { technician.aButton } val retractIntake = Trigger { technician.bButton } val characterizeIntake = Trigger { technician.xButton } - - // for testing } diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt index 75c5a2f6..f65f7460 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt @@ -116,10 +116,13 @@ object Constants { } object Shooter { - const val FLYWHEEL_MOTOR_ID = 51 - // TODO find wrist motor id - const val SHOOTER_WRIST_MOTOR_ID = 999 - const val FEEDER_MOTOR_ID = 999 + const val FLYWHEEL_LEFT_MOTOR_ID = 51 + const val FLYWHEEL_RIGHT_MOTOR_ID = 52 + } + + object WRIST { + const val WRIST_MOTOR_ID = 41 + const val CANCODER_ID = 42 } object Alert { diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/FlywheelConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/FlywheelConstants.kt new file mode 100644 index 00000000..68199689 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/FlywheelConstants.kt @@ -0,0 +1,85 @@ +package com.team4099.robot2023.config.constants + +import org.team4099.lib.units.Velocity +import org.team4099.lib.units.base.amps +import org.team4099.lib.units.base.grams +import org.team4099.lib.units.base.meters +import org.team4099.lib.units.base.seconds +import org.team4099.lib.units.derived.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.radians +import org.team4099.lib.units.derived.rotations +import org.team4099.lib.units.derived.volts +import org.team4099.lib.units.kilo +import org.team4099.lib.units.perMinute +import org.team4099.lib.units.perSecond + +object FlywheelConstants { + val LEFT_GEAR_RATIO = 1.0 + + val RIGHT_MOTOR_REVOLUTIONS_PER_FLYWHEEL_REVOLUTIONS = 24.0 / 48.0 + + val VOLTAGE_COMPENSATION = 12.volts + + val INERTIA = 0.0014550597.kilo.grams * 1.0.meters.squared + + val RIGHT_FLYWHEEL_SUPPLY_CURRENT_LIMIT = 50.0.amps + val RIGHT_FLYWHEEL_THRESHOLD_CURRENT_LIMIT = 1.0.amps + val RIGHT_flywheel_TRIGGER_THRESHOLD_TIME = 10.0.seconds + val RIGHT_FLYWHEEL_STATOR_CURRENT_LIMIT = 50.0.amps + + val LEFT_FLYWHEEL_SUPPLY_CURRENT_LIMIT = 50.0.amps + val LEFT_FLYWHEEL_THRESHOLD_CURRENT_LIMIT = 1.0.amps + val LEFT_flywheel_TRIGGER_THRESHOLD_TIME = 10.0.seconds + val LEFT_FLYWHEEL_STATOR_CURRENT_LIMIT = 50.0.amps + + val FLYWHEEL_TOLERANCE = 50.0.rotations.perMinute + object PID { + val RIGHT_REAL_KP: ProportionalGain, Volt> = + 0.01.volts / 1.0.rotations.perMinute + val RIGHT_REAL_KI: IntegralGain, Volt> = + 0.0.volts / (1.0.rotations.perMinute * 1.0.seconds) + val RIGHT_REAL_KD: DerivativeGain, Volt> = + 0.0.volts / (1.0.rotations.perMinute.perSecond) + + val RIGHT_SIM_KP: ProportionalGain, Volt> = + 0.015.volts / 1.0.rotations.perMinute + val RIGHT_SIM_KI: IntegralGain, Volt> = + 0.0.volts / (1.0.rotations.perMinute * 1.0.seconds) + val RIGHT_SIM_KD: DerivativeGain, Volt> = + 0.0.volts / (1.0.rotations.perMinute.perSecond) + + val LEFT_REAL_KP: ProportionalGain, Volt> = + 0.015.volts / 1.0.rotations.perMinute + val LEFT_REAL_KI: IntegralGain, Volt> = + 0.0.volts / (1.0.rotations.perMinute * 1.0.seconds) + val LEFT_REAL_KD: DerivativeGain, Volt> = + 0.0.volts / (1.0.rotations.perMinute.perSecond) + + val LEFT_SIM_KP: ProportionalGain, Volt> = + 0.0015.volts / 1.0.rotations.perMinute + val LEFT_SIM_KI: IntegralGain, Volt> = + 0.0.volts / (1.0.rotations.perMinute * 1.0.seconds) + val LEFT_SIM_KD: DerivativeGain, Volt> = + 0.0.volts / (1.0.rotations.perMinute.perSecond) + + val RIGHT_REAL_FLYWHEEL_KS = 0.0.volts + val RIGHT_REAL_FLYWHEEL_KV = 0.0099.volts / 1.radians.perSecond + val RIGHT_REAL_FLYWHEEL_KA = 0.09.volts / 1.radians.perSecond.perSecond + + val LEFT_REAL_FLYWHEEL_KS = 0.0.volts + val LEFT_REAL_FLYWHEEL_KV = 0.0197.volts / 1.radians.perSecond + val LEFT_REAL_FLYWHEEL_KA = 0.03.volts / 1.radians.perSecond.perSecond + + val RIGHT_SIM_FLYWHEEL_KS = 0.0.volts + val RIGHT_SIM_FLYWHEEL_KV = 0.0099.volts / 1.radians.perSecond + val RIGHT_SIM_FLYWHEEL_KA = 0.09.volts / 1.radians.perSecond.perSecond + + val LEFT_SIM_FLYWHEEL_KS = 0.0.volts + val LEFT_SIM_FLYWHEEL_KV = 0.0197.volts / 1.radians.perSecond + val LEFT_SIM_FLYWHEEL_KA = 0.03.volts / 1.radians.perSecond.perSecond + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/WristConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/WristConstants.kt new file mode 100644 index 00000000..12d6ecc1 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/WristConstants.kt @@ -0,0 +1,57 @@ +package com.team4099.robot2023.config.constants + +import org.team4099.lib.units.base.amps +import org.team4099.lib.units.base.grams +import org.team4099.lib.units.base.inches +import org.team4099.lib.units.base.meters +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.Radian +import org.team4099.lib.units.derived.Volt +import org.team4099.lib.units.derived.degrees +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.kilo +import org.team4099.lib.units.perSecond + +object WristConstants { + // val ROLLER_GEAR_RATIO = 0.0 + // val ROLLER_VOLTAGE_COMPENSATION = 0.0.volts + // val ROLLER_STATOR_CURRENT_LIMIT = 0.0.amps + + val VOLTAGE_COMPENSATION = 12.0.volts + val ABSOLUTE_ENCODER_OFFSET = 0.degrees + val WRIST_LENGTH = 18.6.inches + val WRIST_INERTIA = 0.7181257183.kilo.grams * 1.0.meters.squared + + val WRIST_ENCODER_GEAR_RATIO = 0.0 + + val WRIST_GEAR_RATIO = 90.0 / 12 * 90 / 24 * 90 / 30 + val WRIST_VOLTAGE_COMPENSATION = 12.0.volts + val WRIST_STATOR_CURRENT_LIMIT = 40.0.amps + + val WRIST_MAX_ROTATION = 45.degrees + val WRIST_MIN_ROTATION = (-47.0).degrees + + val MAX_WRIST_VELOCITY = 1.radians.perSecond + val MAX_WRIST_ACCELERATION = 2.radians.perSecond.perSecond + object PID { + val REAL_KP: ProportionalGain = 0.001.volts / 1.0.degrees + val REAL_KI: IntegralGain = 0.0.volts / (1.0.degrees * 1.0.seconds) + val REAL_KD: DerivativeGain = 0.0.volts / (1.0.rotations / 1.0.seconds) + + val SIM_KP: ProportionalGain = 1.volts / 1.0.degrees + val SIM_KI: IntegralGain = 0.0.volts / (1.0.degrees * 1.0.seconds) + val SIM_KD: DerivativeGain = 0.00.volts / (1.0.degrees / 1.0.seconds) + + val WRIST_KG = 1.25.volts + val WRIST_KV = 1.61.volts / 1.0.radians.perSecond + val WRIST_KA = 0.03.volts / 1.0.radians.perSecond.perSecond + val WRIST_KS = 0.0.volts + } + + val WRIST_TOLERANCE = 0.01.degrees +} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt deleted file mode 100644 index c1ef2b6f..00000000 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt +++ /dev/null @@ -1,3 +0,0 @@ -package com.team4099.robot2023.subsystems.Shooter - -class Shooter diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/servemodule/SwerveModule.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/servemodule/SwerveModule.kt index 0eb3f017..f503a1bf 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/servemodule/SwerveModule.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/servemodule/SwerveModule.kt @@ -169,6 +169,7 @@ class SwerveModule(val io: SwerveModuleIO) { (steering - inputs.steeringPosition).inRadians.IEEErem(2 * Math.PI).radians shouldInvert = (steeringDifference.absoluteValue > (Math.PI / 2).radians) && optimize + if (shouldInvert) { steeringDifference -= Math.PI.withSign(steeringDifference.inRadians).radians } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/Flywheel.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/Flywheel.kt new file mode 100644 index 00000000..37374786 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/Flywheel.kt @@ -0,0 +1,329 @@ +package com.team4099.robot2023.subsystems.flywheel + +import com.team4099.lib.hal.Clock +import com.team4099.lib.logging.LoggedTunableValue +import com.team4099.robot2023.config.constants.Constants +import com.team4099.robot2023.config.constants.FlywheelConstants +import com.team4099.robot2023.subsystems.superstructure.Request +import edu.wpi.first.wpilibj.RobotBase +import edu.wpi.first.wpilibj2.command.Command +import edu.wpi.first.wpilibj2.command.SubsystemBase +import org.littletonrobotics.junction.Logger +import org.team4099.lib.controller.SimpleMotorFeedforward +import org.team4099.lib.units.AngularVelocity +import org.team4099.lib.units.base.seconds +import org.team4099.lib.units.derived.ElectricalPotential +import org.team4099.lib.units.derived.Radian +import org.team4099.lib.units.derived.Volt +import org.team4099.lib.units.derived.inVolts +import org.team4099.lib.units.derived.inVoltsPerRadianPerSecond +import org.team4099.lib.units.derived.inVoltsPerRadianPerSecondPerSecond +import org.team4099.lib.units.derived.inVoltsPerRotations +import org.team4099.lib.units.derived.inVoltsPerRotationsPerMinute +import org.team4099.lib.units.derived.inVoltsPerRotationsPerMinutePerSecond +import org.team4099.lib.units.derived.perRadianPerSecond +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.inRotationsPerMinute +import org.team4099.lib.units.perMinute +import org.team4099.lib.units.perSecond + +class Flywheel(val io: FlywheelIO) : SubsystemBase() { + private val rightkP = + LoggedTunableValue( + "Flywheel/Right kP", + Pair({ it.inVoltsPerRotationsPerMinute }, { it.volts / 1.0.rotations.perMinute }) + ) + private val rightkI = + LoggedTunableValue( + "Flywheel/Right kI", + Pair({ it.inVoltsPerRotations }, { it.volts / (1.0.rotations.perMinute * 1.0.seconds) }) + ) + private val rightkD = + LoggedTunableValue( + "Flywheel/Right kD", + Pair( + { it.inVoltsPerRotationsPerMinutePerSecond }, + { it.volts / 1.0.rotations.perMinute.perSecond } + ) + ) + + private val leftkP = + LoggedTunableValue( + "Flywheel/Left kP", + Pair({ it.inVoltsPerRotationsPerMinute }, { it.volts / 1.0.rotations.perMinute }) + ) + private val leftkI = + LoggedTunableValue( + "Flywheel/Left kI", + Pair({ it.inVoltsPerRotations }, { it.volts / (1.0.rotations.perMinute * 1.0.seconds) }) + ) + private val leftkD = + LoggedTunableValue( + "Flywheel/Left kD", + Pair( + { it.inVoltsPerRotationsPerMinutePerSecond }, + { it.volts / 1.0.rotations.perMinute.perSecond } + ) + ) + + val inputs = FlywheelIO.FlywheelIOInputs() + private val flywheelRightkS = + LoggedTunableValue("Flywheel/Right kS", Pair({ it.inVolts }, { it.volts })) + private val flywheelRightkV = + LoggedTunableValue( + "Flywheel/Right kV", + Pair({ it.inVoltsPerRadianPerSecond }, { it.volts.perRadianPerSecond }) + ) + private val flywheelRightkA = + LoggedTunableValue( + "Flywheel/Right kA", + Pair( + { it.inVoltsPerRadianPerSecondPerSecond }, + { it.volts / 1.0.radians.perSecond.perSecond } + ) + ) + + private val flywheelLeftkS = + LoggedTunableValue("Flywheel/Left kS", Pair({ it.inVolts }, { it.volts })) + private val flywheelLeftkV = + LoggedTunableValue( + "Flywheel/Left kV", + Pair({ it.inVoltsPerRadianPerSecond }, { it.volts.perRadianPerSecond }) + ) + private val flywheelLeftkA = + LoggedTunableValue( + "Flywheel/Left kA", + Pair( + { it.inVoltsPerRadianPerSecondPerSecond }, + { it.volts / 1.0.radians.perSecond.perSecond } + ) + ) + + var flywheelRightFeedForward: SimpleMotorFeedforward + var flywheelLeftFeedForward: SimpleMotorFeedforward + + var lastFlywheelRunTime = 0.0.seconds + private var lastRightFlywheelVoltage = 0.0.volts + + var flywheelRightTargetVoltage = 0.0.volts + var flywheelRightTargetVelocity: AngularVelocity = 0.0.rotations.perMinute + + var flywheelLeftTargetVoltage = 0.0.volts + var flywheelLeftTargetVelocity: AngularVelocity = 0.0.rotations.perMinute + + val isAtTargetedVelocity: Boolean + get() = + ( + currentState == FlywheelStates.TARGETING_VELOCITY && + (inputs.rightFlywheelVelocity - flywheelRightTargetVelocity).absoluteValue <= + FlywheelConstants.FLYWHEEL_TOLERANCE && + (inputs.leftFlywheelVelocity - flywheelLeftTargetVelocity).absoluteValue <= + FlywheelConstants.FLYWHEEL_TOLERANCE + ) + + var currentState = Companion.FlywheelStates.UNINITIALIZED + + var currentRequest: Request.FlywheelRequest = Request.FlywheelRequest.OpenLoop(0.0.volts) + set(value) { + when (value) { + is Request.FlywheelRequest.OpenLoop -> { + flywheelRightTargetVoltage = value.flywheelVoltage + flywheelLeftTargetVoltage = value.flywheelVoltage + } + is Request.FlywheelRequest.TargetingVelocity -> { + flywheelRightTargetVelocity = value.flywheelVelocity + // left needs to be half of the right one + flywheelLeftTargetVelocity = value.flywheelVelocity / 2 + } + else -> {} + } + field = value + } + + init { + if (RobotBase.isReal()) { + rightkP.initDefault(FlywheelConstants.PID.RIGHT_REAL_KP) + rightkI.initDefault(FlywheelConstants.PID.RIGHT_REAL_KI) + rightkD.initDefault(FlywheelConstants.PID.RIGHT_REAL_KD) + + leftkP.initDefault(FlywheelConstants.PID.LEFT_REAL_KP) + leftkI.initDefault(FlywheelConstants.PID.LEFT_REAL_KI) + leftkD.initDefault(FlywheelConstants.PID.LEFT_REAL_KD) + + flywheelRightFeedForward = + SimpleMotorFeedforward( + FlywheelConstants.PID.RIGHT_REAL_FLYWHEEL_KS, + FlywheelConstants.PID.RIGHT_REAL_FLYWHEEL_KV, + FlywheelConstants.PID.RIGHT_REAL_FLYWHEEL_KA + ) + + flywheelLeftFeedForward = + SimpleMotorFeedforward( + FlywheelConstants.PID.LEFT_REAL_FLYWHEEL_KS, + FlywheelConstants.PID.LEFT_REAL_FLYWHEEL_KV, + FlywheelConstants.PID.LEFT_REAL_FLYWHEEL_KA + ) + + flywheelLeftkS.initDefault(FlywheelConstants.PID.LEFT_REAL_FLYWHEEL_KS) + flywheelLeftkV.initDefault(FlywheelConstants.PID.LEFT_REAL_FLYWHEEL_KV) + flywheelLeftkA.initDefault(FlywheelConstants.PID.LEFT_REAL_FLYWHEEL_KA) + + flywheelRightkS.initDefault(FlywheelConstants.PID.RIGHT_REAL_FLYWHEEL_KS) + flywheelRightkV.initDefault(FlywheelConstants.PID.RIGHT_REAL_FLYWHEEL_KV) + flywheelRightkA.initDefault(FlywheelConstants.PID.RIGHT_REAL_FLYWHEEL_KA) + } else { + rightkP.initDefault(FlywheelConstants.PID.RIGHT_SIM_KP) + rightkI.initDefault(FlywheelConstants.PID.RIGHT_SIM_KI) + rightkD.initDefault(FlywheelConstants.PID.RIGHT_SIM_KD) + + leftkP.initDefault(FlywheelConstants.PID.LEFT_SIM_KP) + leftkI.initDefault(FlywheelConstants.PID.LEFT_SIM_KI) + leftkD.initDefault(FlywheelConstants.PID.LEFT_SIM_KD) + + flywheelLeftkS.initDefault(FlywheelConstants.PID.LEFT_SIM_FLYWHEEL_KS) + flywheelLeftkV.initDefault(FlywheelConstants.PID.LEFT_SIM_FLYWHEEL_KV) + flywheelLeftkA.initDefault(FlywheelConstants.PID.LEFT_SIM_FLYWHEEL_KA) + + flywheelRightkS.initDefault(FlywheelConstants.PID.RIGHT_SIM_FLYWHEEL_KS) + flywheelRightkV.initDefault(FlywheelConstants.PID.RIGHT_SIM_FLYWHEEL_KV) + flywheelRightkA.initDefault(FlywheelConstants.PID.RIGHT_SIM_FLYWHEEL_KA) + + flywheelRightFeedForward = + SimpleMotorFeedforward( + FlywheelConstants.PID.RIGHT_SIM_FLYWHEEL_KS, + FlywheelConstants.PID.RIGHT_SIM_FLYWHEEL_KV, + FlywheelConstants.PID.RIGHT_SIM_FLYWHEEL_KA + ) + + flywheelLeftFeedForward = + SimpleMotorFeedforward( + FlywheelConstants.PID.LEFT_SIM_FLYWHEEL_KS, + FlywheelConstants.PID.LEFT_SIM_FLYWHEEL_KV, + FlywheelConstants.PID.LEFT_SIM_FLYWHEEL_KA + ) + } + + io.configPID( + rightkP.get(), rightkI.get(), rightkD.get(), leftkP.get(), leftkI.get(), leftkD.get() + ) + } + override fun periodic() { + io.updateInputs(inputs) + Logger.processInputs("Flywheel", inputs) + Logger.recordOutput("Flywheel/currentState", currentState.name) + + Logger.recordOutput("Flywheel/requestedState", currentRequest.javaClass.simpleName) + + Logger.recordOutput("Flywheel/isAtTargetedVelocity", isAtTargetedVelocity) + + if (Constants.Tuning.DEBUGING_MODE) { + Logger.recordOutput("Flywheel/FlywheelTargetVoltage", flywheelRightTargetVoltage.inVolts) + Logger.recordOutput( + "Flywheel/FlywheelTargetVelocity", flywheelRightTargetVelocity.inRotationsPerMinute + ) + Logger.recordOutput("Flywheel/FlywheelLastVoltage", lastRightFlywheelVoltage.inVolts) + } + + if (rightkP.hasChanged() || + rightkI.hasChanged() || + rightkD.hasChanged() || + leftkP.hasChanged() || + leftkI.hasChanged() || + leftkD.hasChanged() + ) { + io.configPID( + rightkP.get(), rightkI.get(), rightkD.get(), leftkP.get(), leftkI.get(), leftkD.get() + ) + } + + if (flywheelRightkA.hasChanged() || + flywheelRightkV.hasChanged() || + flywheelRightkS.hasChanged() + ) { + flywheelRightFeedForward = + SimpleMotorFeedforward( + flywheelRightkS.get(), flywheelRightkV.get(), flywheelRightkA.get() + ) + } + + if (flywheelLeftkA.hasChanged() || flywheelLeftkV.hasChanged() || flywheelLeftkS.hasChanged()) { + flywheelLeftFeedForward = + SimpleMotorFeedforward(flywheelLeftkS.get(), flywheelLeftkV.get(), flywheelLeftkA.get()) + } + + var nextState = currentState + when (currentState) { + Companion.FlywheelStates.UNINITIALIZED -> { + nextState = Companion.FlywheelStates.OPEN_LOOP + } + Companion.FlywheelStates.OPEN_LOOP -> { + setFlywheelVoltage(flywheelRightTargetVoltage, flywheelLeftTargetVoltage) + lastFlywheelRunTime = Clock.fpgaTime + + nextState = fromRequestToState(currentRequest) + } + Companion.FlywheelStates.TARGETING_VELOCITY -> { + setFlywheelVelocity(flywheelRightTargetVelocity, flywheelLeftTargetVelocity) + lastFlywheelRunTime = Clock.fpgaTime + nextState = fromRequestToState(currentRequest) + } + } + currentState = nextState + } + + fun setFlywheelVoltage( + appliedVoltageRight: ElectricalPotential, + appliedVoltageLeft: ElectricalPotential + ) { + io.setFlywheelVoltage(appliedVoltageRight, appliedVoltageLeft) + } + + fun setFlywheelVelocity( + flywheelRightVelocity: AngularVelocity, + flywheelLeftVelocity: AngularVelocity + ) { + val rightFeedForward = flywheelRightFeedForward.calculate(flywheelRightVelocity) + val leftFeedForward = flywheelLeftFeedForward.calculate(flywheelLeftVelocity) + + Logger.recordOutput("Flywheel/rightFeedForward", rightFeedForward.inVolts) + Logger.recordOutput("Flywheel/leftFeedForward", leftFeedForward.inVolts) + + io.setFlywheelVelocity( + flywheelRightVelocity, flywheelLeftVelocity, leftFeedForward, rightFeedForward + ) + } + + fun flywheelSpinUpCommand(): Command { + return runOnce({ + currentRequest = Request.FlywheelRequest.TargetingVelocity(6000.rotations.perMinute) + }) + } + + fun flywheelOpenLoopCommand(): Command { + return runOnce({ currentRequest = Request.FlywheelRequest.OpenLoop(12.volts) }) + } + + fun flywheelResetCommand(): Command { + return runOnce({ currentRequest = Request.FlywheelRequest.OpenLoop(0.volts) }) + } + + companion object { + enum class FlywheelStates { + UNINITIALIZED, + OPEN_LOOP, + TARGETING_VELOCITY; + fun equivalentToRequest(request: Request.FlywheelRequest): Boolean { + return (request is Request.FlywheelRequest.OpenLoop && this == OPEN_LOOP) || + (request is Request.FlywheelRequest.TargetingVelocity && this == TARGETING_VELOCITY) + } + } + inline fun fromRequestToState(request: Request.FlywheelRequest): FlywheelStates { + return when (request) { + is Request.FlywheelRequest.OpenLoop -> FlywheelStates.OPEN_LOOP + is Request.FlywheelRequest.TargetingVelocity -> FlywheelStates.TARGETING_VELOCITY + } + } + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/FlywheelIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/FlywheelIO.kt new file mode 100644 index 00000000..f4a9d336 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/FlywheelIO.kt @@ -0,0 +1,133 @@ +package com.team4099.robot2023.subsystems.flywheel + +import org.littletonrobotics.junction.LogTable +import org.littletonrobotics.junction.inputs.LoggableInputs +import org.team4099.lib.units.AngularVelocity +import org.team4099.lib.units.Velocity +import org.team4099.lib.units.base.amps +import org.team4099.lib.units.base.celsius +import org.team4099.lib.units.base.inAmperes +import org.team4099.lib.units.base.inCelsius +import org.team4099.lib.units.derived.DerivativeGain +import org.team4099.lib.units.derived.ElectricalPotential +import org.team4099.lib.units.derived.IntegralGain +import org.team4099.lib.units.derived.ProportionalGain +import org.team4099.lib.units.derived.Radian +import org.team4099.lib.units.derived.Volt +import org.team4099.lib.units.derived.inNewtons +import org.team4099.lib.units.derived.inVolts +import org.team4099.lib.units.derived.newtons +import org.team4099.lib.units.derived.rotations +import org.team4099.lib.units.derived.volts +import org.team4099.lib.units.inRotationsPerMinute +import org.team4099.lib.units.perMinute + +interface FlywheelIO { + class FlywheelIOInputs : LoggableInputs { + var rightFlywheelVelocity = 0.0.rotations.perMinute + var rightFlywheelAppliedVoltage = 0.volts + var rightFlywheelStatorCurrent = 0.amps + var rightFlywheelSupplyCurrent = 0.amps + var rightFlywheelTemperature = 0.celsius + var rightFlywheelDutyCycle = 0.0.volts + var rightFlywheelTorque = 0.0.newtons + + var leftFlywheelVelocity = 0.0.rotations.perMinute + var leftFlywheelAppliedVoltage = 0.volts + var leftFlywheelStatorCurrent = 0.amps + var leftFlywheelSupplyCurrent = 0.amps + var leftFlywheelTemperature = 0.celsius + var leftFlywheelDutyCycle = 0.0.volts + var leftFlywheelTorque = 0.0.newtons + + var isSimulated = false + + override fun toLog(table: LogTable) { + table.put("flywheelRightVelocityRPM", rightFlywheelVelocity.inRotationsPerMinute) + table.put("flywheelRightAppliedVoltage", rightFlywheelAppliedVoltage.inVolts) + table.put("flywheelRightStatorCurrent", rightFlywheelStatorCurrent.inAmperes) + table.put("flywheelRightSupplyCurrent", rightFlywheelSupplyCurrent.inAmperes) + table.put("flywheelRightTemperature", rightFlywheelTemperature.inCelsius) + table.put("rightFlywheelDutyCycle", rightFlywheelDutyCycle.inVolts) + table.put("rightFlywheelTorque", rightFlywheelTorque.inNewtons) + + table.put("flywheelLeftVelocityRPM", leftFlywheelVelocity.inRotationsPerMinute) + table.put("flywheelLeftAppliedVoltage", leftFlywheelAppliedVoltage.inVolts) + table.put("flywheelLeftStatorCurrent", leftFlywheelStatorCurrent.inAmperes) + table.put("flywheelLeftSupplyCurrent", leftFlywheelSupplyCurrent.inAmperes) + table.put("flywheelLeftTemperature", leftFlywheelTemperature.inCelsius) + table.put("leftFlywheelDutyCycle", leftFlywheelDutyCycle.inVolts) + table.put("leftFlywheelTorque", leftFlywheelTorque.inNewtons) + } + + override fun fromLog(table: LogTable) { + // Flywheel logs + table.get("rightFlywheelVelocityRPM", rightFlywheelVelocity.inRotationsPerMinute).let { + rightFlywheelVelocity = it.rotations.perMinute + } + table.get("rightFlywheelAppliedVoltage", rightFlywheelAppliedVoltage.inVolts).let { + rightFlywheelAppliedVoltage = it.volts + } + table.get("rightFlywheelStatorCurrent", rightFlywheelStatorCurrent.inAmperes).let { + rightFlywheelStatorCurrent = it.amps + } + table.get("rightFlywheelSupplyCurrent", rightFlywheelSupplyCurrent.inAmperes).let { + rightFlywheelSupplyCurrent = it.amps + } + table.get("rightFlywheelTemperature", rightFlywheelTemperature.inCelsius).let { + rightFlywheelTemperature = it.celsius + } + table.get("rightFlywheelDutyCycle", rightFlywheelDutyCycle.inVolts).let { + rightFlywheelDutyCycle = it.volts + } + table.get("rightFlywheelTorque", rightFlywheelTorque.inNewtons).let { + rightFlywheelTorque = it.newtons + } + + // Left motor + table.get("leftFlywheelVelocityRPM", leftFlywheelVelocity.inRotationsPerMinute).let { + leftFlywheelVelocity = it.rotations.perMinute + } + table.get("leftFlywheelAppliedVoltage", leftFlywheelAppliedVoltage.inVolts).let { + leftFlywheelAppliedVoltage = it.volts + } + table.get("leftFlywheelStatorCurrent", leftFlywheelStatorCurrent.inAmperes).let { + leftFlywheelStatorCurrent = it.amps + } + table.get("leftFlywheelSupplyCurrent", leftFlywheelSupplyCurrent.inAmperes).let { + leftFlywheelSupplyCurrent = it.amps + } + table.get("leftFlywheelTemperature", leftFlywheelTemperature.inCelsius).let { + leftFlywheelTemperature = it.celsius + } + table.get("leftFlywheelDutyCycle", leftFlywheelDutyCycle.inVolts).let { + leftFlywheelDutyCycle = it.volts + } + table.get("leftFlywheelTorque", leftFlywheelTorque.inNewtons).let { + leftFlywheelTorque = it.newtons + } + } + } + + fun setFlywheelVoltage(voltageRight: ElectricalPotential, voltageLeft: ElectricalPotential) {} + + fun setFlywheelVelocity( + rightVelocity: AngularVelocity, + leftVelocity: AngularVelocity, + feedforwardLeft: ElectricalPotential, + feedforwardRight: ElectricalPotential + ) {} + + fun setFlywheelBrakeMode(brake: Boolean) {} + + fun updateInputs(inputs: FlywheelIOInputs) {} + + fun configPID( + rightkP: ProportionalGain, Volt>, + rightkI: IntegralGain, Volt>, + rightkD: DerivativeGain, Volt>, + leftkP: ProportionalGain, Volt>, + leftkI: IntegralGain, Volt>, + leftkD: DerivativeGain, Volt> + ) {} +} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/FlywheelIOSim.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/FlywheelIOSim.kt new file mode 100644 index 00000000..03f771cc --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/FlywheelIOSim.kt @@ -0,0 +1,137 @@ +package com.team4099.robot2023.subsystems.flywheel + +import com.team4099.lib.math.clamp +import com.team4099.robot2023.config.constants.Constants +import com.team4099.robot2023.config.constants.FlywheelConstants +import edu.wpi.first.math.system.plant.DCMotor +import edu.wpi.first.wpilibj.simulation.FlywheelSim +import org.team4099.lib.controller.PIDController +import org.team4099.lib.units.AngularVelocity +import org.team4099.lib.units.Velocity +import org.team4099.lib.units.base.amps +import org.team4099.lib.units.base.celsius +import org.team4099.lib.units.base.inSeconds +import org.team4099.lib.units.derived.DerivativeGain +import org.team4099.lib.units.derived.ElectricalPotential +import org.team4099.lib.units.derived.IntegralGain +import org.team4099.lib.units.derived.ProportionalGain +import org.team4099.lib.units.derived.Radian +import org.team4099.lib.units.derived.Volt +import org.team4099.lib.units.derived.inKilogramsMeterSquared +import org.team4099.lib.units.derived.inVolts +import org.team4099.lib.units.derived.rotations +import org.team4099.lib.units.derived.volts +import org.team4099.lib.units.perMinute + +object FlywheelIOSim : FlywheelIO { + private val flywheelRightSim: FlywheelSim = + FlywheelSim( + DCMotor.getKrakenX60Foc(2), + FlywheelConstants.RIGHT_MOTOR_REVOLUTIONS_PER_FLYWHEEL_REVOLUTIONS, + FlywheelConstants.INERTIA.inKilogramsMeterSquared + ) + + private val flywheelLeftSim: FlywheelSim = + FlywheelSim( + DCMotor.getKrakenX60Foc(2), + FlywheelConstants.LEFT_GEAR_RATIO, + FlywheelConstants.INERTIA.inKilogramsMeterSquared + ) + + private var appliedRightVoltage = 0.0.volts + private var appliedLeftVoltage = 0.0.volts + + private val rightFlywheelController = + PIDController( + FlywheelConstants.PID.RIGHT_SIM_KP, + FlywheelConstants.PID.RIGHT_SIM_KI, + FlywheelConstants.PID.RIGHT_SIM_KD + ) + + private val leftFlywheelController = + PIDController( + FlywheelConstants.PID.LEFT_SIM_KP, + FlywheelConstants.PID.LEFT_SIM_KI, + FlywheelConstants.PID.LEFT_SIM_KD + ) + + override fun updateInputs(inputs: FlywheelIO.FlywheelIOInputs) { + flywheelLeftSim.update(Constants.Universal.LOOP_PERIOD_TIME.inSeconds) + flywheelRightSim.update(Constants.Universal.LOOP_PERIOD_TIME.inSeconds) + + inputs.leftFlywheelVelocity = flywheelLeftSim.getAngularVelocityRPM().rotations.perMinute + inputs.leftFlywheelAppliedVoltage = appliedLeftVoltage + inputs.leftFlywheelSupplyCurrent = 0.amps + inputs.leftFlywheelStatorCurrent = flywheelLeftSim.currentDrawAmps.amps + inputs.leftFlywheelTemperature = 0.0.celsius + + inputs.rightFlywheelVelocity = flywheelRightSim.getAngularVelocityRPM().rotations.perMinute + inputs.rightFlywheelAppliedVoltage = appliedRightVoltage + inputs.rightFlywheelSupplyCurrent = 0.amps + inputs.rightFlywheelStatorCurrent = flywheelRightSim.currentDrawAmps.amps + inputs.rightFlywheelTemperature = 0.0.celsius + + inputs.isSimulated = true + } + + override fun setFlywheelVoltage( + voltageRight: ElectricalPotential, + voltageLeft: ElectricalPotential + ) { + appliedRightVoltage = voltageRight + flywheelRightSim.setInputVoltage( + clamp( + voltageRight, + -FlywheelConstants.VOLTAGE_COMPENSATION, + FlywheelConstants.VOLTAGE_COMPENSATION + ) + .inVolts + ) + + appliedLeftVoltage = voltageLeft + flywheelLeftSim.setInputVoltage( + clamp( + voltageLeft, + -FlywheelConstants.VOLTAGE_COMPENSATION, + FlywheelConstants.VOLTAGE_COMPENSATION + ) + .inVolts + ) + } + + override fun setFlywheelVelocity( + rightVelocity: AngularVelocity, + leftVelocity: AngularVelocity, + feedforwardLeft: ElectricalPotential, + feedforwardRight: ElectricalPotential + ) { + val rightFeedback = + rightFlywheelController.calculate( + flywheelRightSim.getAngularVelocityRPM().rotations.perMinute, rightVelocity + ) + val leftFeedback = + leftFlywheelController.calculate( + flywheelLeftSim.getAngularVelocityRPM().rotations.perMinute, leftVelocity + ) + + setFlywheelVoltage(rightFeedback + feedforwardRight, leftFeedback + feedforwardLeft) + + appliedRightVoltage = rightFeedback + feedforwardRight + appliedLeftVoltage = leftFeedback + feedforwardLeft + } + + override fun setFlywheelBrakeMode(brake: Boolean) {} + + override fun configPID( + rightkP: ProportionalGain, Volt>, + rightkI: IntegralGain, Volt>, + rightkD: DerivativeGain, Volt>, + leftkP: ProportionalGain, Volt>, + leftkI: IntegralGain, Volt>, + leftkD: DerivativeGain, Volt> + ) { + rightFlywheelController.setPID(rightkP, rightkI, rightkD) + + leftFlywheelController.setPID(leftkP, leftkI, leftkD) + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/FlywheelIOTalon.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/FlywheelIOTalon.kt new file mode 100644 index 00000000..0101a00c --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/FlywheelIOTalon.kt @@ -0,0 +1,253 @@ +package com.team4099.robot2023.subsystems.flywheel + +import com.ctre.phoenix6.StatusSignal +import com.ctre.phoenix6.configs.MotorOutputConfigs +import com.ctre.phoenix6.configs.Slot0Configs +import com.ctre.phoenix6.configs.TalonFXConfiguration +import com.ctre.phoenix6.controls.Follower +import com.ctre.phoenix6.hardware.TalonFX +import com.ctre.phoenix6.signals.NeutralModeValue +import com.team4099.lib.phoenix6.VelocityVoltage +import com.team4099.robot2023.config.constants.Constants +import com.team4099.robot2023.config.constants.Constants.Shooter.FLYWHEEL_LEFT_MOTOR_ID +import com.team4099.robot2023.config.constants.Constants.Shooter.FLYWHEEL_RIGHT_MOTOR_ID +import com.team4099.robot2023.config.constants.FlywheelConstants +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.AngularVelocity +import org.team4099.lib.units.Velocity +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.derived.DerivativeGain +import org.team4099.lib.units.derived.ElectricalPotential +import org.team4099.lib.units.derived.IntegralGain +import org.team4099.lib.units.derived.ProportionalGain +import org.team4099.lib.units.derived.Radian +import org.team4099.lib.units.derived.Volt +import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.inVolts +import org.team4099.lib.units.derived.newtons +import org.team4099.lib.units.derived.volts +import org.team4099.lib.units.perSecond + +object FlywheelIOTalon : FlywheelIO { + + private val flywheelLeftTalon: TalonFX = TalonFX(FLYWHEEL_LEFT_MOTOR_ID) + + private val flywheelRightTalon: TalonFX = TalonFX(FLYWHEEL_RIGHT_MOTOR_ID) + + private val flywheelLeftConfiguration: TalonFXConfiguration = TalonFXConfiguration() + + private val flywheelRightConfiguration: TalonFXConfiguration = TalonFXConfiguration() + + var leftVelocityRequest = + VelocityVoltage(-1337.degrees.perSecond, slot = 0, feedforward = -1337.volts) + var rightVelocityRequest = + VelocityVoltage(-1337.degrees.perSecond, slot = 0, feedforward = -1337.volts) + + private val flywheelLeftSensor = + ctreAngularMechanismSensor( + flywheelLeftTalon, + FlywheelConstants.LEFT_GEAR_RATIO, + FlywheelConstants.VOLTAGE_COMPENSATION, + ) + + private val flywheelRightSensor = + ctreAngularMechanismSensor( + flywheelRightTalon, + FlywheelConstants.RIGHT_MOTOR_REVOLUTIONS_PER_FLYWHEEL_REVOLUTIONS, + FlywheelConstants.VOLTAGE_COMPENSATION, + ) + + var leftFlywheelStatorCurrentSignal: StatusSignal + var leftFlywheelSupplyCurrentSignal: StatusSignal + var leftFlywheelTempSignal: StatusSignal + var leftFlywheelDutyCycle: StatusSignal + + var rightFlywheelStatorCurrentSignal: StatusSignal + var rightFlywheelSupplyCurrentSignal: StatusSignal + var rightFlywheelTempSignal: StatusSignal + var rightFlywheelDutyCycle: StatusSignal + var motorVoltage: StatusSignal + var motorTorque: StatusSignal + + init { + + flywheelLeftTalon.configurator.apply(TalonFXConfiguration()) + flywheelRightTalon.configurator.apply(TalonFXConfiguration()) + + flywheelLeftTalon.clearStickyFaults() + flywheelRightTalon.clearStickyFaults() + + flywheelLeftConfiguration.Slot0.kP = + flywheelLeftSensor.proportionalVelocityGainToRawUnits(FlywheelConstants.PID.LEFT_REAL_KP) + flywheelLeftConfiguration.Slot0.kI = + flywheelLeftSensor.integralVelocityGainToRawUnits(FlywheelConstants.PID.LEFT_REAL_KI) + flywheelLeftConfiguration.Slot0.kD = + flywheelLeftSensor.derivativeVelocityGainToRawUnits(FlywheelConstants.PID.LEFT_REAL_KD) + + flywheelRightConfiguration.Slot0.kP = + flywheelRightSensor.proportionalVelocityGainToRawUnits(FlywheelConstants.PID.RIGHT_REAL_KP) + flywheelRightConfiguration.Slot0.kI = + flywheelRightSensor.integralVelocityGainToRawUnits(FlywheelConstants.PID.RIGHT_REAL_KI) + flywheelRightConfiguration.Slot0.kD = + flywheelRightSensor.derivativeVelocityGainToRawUnits(FlywheelConstants.PID.RIGHT_REAL_KD) + // + // flywheelSensor.velocityFeedforwardToRawUnits(FlywheelConstantsConstants.PID.flywheel_KFF) + + flywheelLeftConfiguration.CurrentLimits.SupplyCurrentLimit = + FlywheelConstants.LEFT_FLYWHEEL_SUPPLY_CURRENT_LIMIT.inAmperes + flywheelLeftConfiguration.CurrentLimits.SupplyCurrentThreshold = + FlywheelConstants.LEFT_FLYWHEEL_THRESHOLD_CURRENT_LIMIT.inAmperes + flywheelLeftConfiguration.CurrentLimits.SupplyTimeThreshold = + FlywheelConstants.LEFT_flywheel_TRIGGER_THRESHOLD_TIME.inSeconds + flywheelLeftConfiguration.CurrentLimits.SupplyCurrentLimitEnable = true + flywheelLeftConfiguration.CurrentLimits.StatorCurrentLimit = + FlywheelConstants.LEFT_FLYWHEEL_STATOR_CURRENT_LIMIT.inAmperes + flywheelLeftConfiguration.CurrentLimits.StatorCurrentLimitEnable = false + + flywheelRightConfiguration.CurrentLimits.SupplyCurrentLimit = + FlywheelConstants.RIGHT_FLYWHEEL_SUPPLY_CURRENT_LIMIT.inAmperes + flywheelRightConfiguration.CurrentLimits.SupplyCurrentThreshold = + FlywheelConstants.RIGHT_FLYWHEEL_THRESHOLD_CURRENT_LIMIT.inAmperes + flywheelRightConfiguration.CurrentLimits.SupplyTimeThreshold = + FlywheelConstants.RIGHT_flywheel_TRIGGER_THRESHOLD_TIME.inSeconds + flywheelRightConfiguration.CurrentLimits.SupplyCurrentLimitEnable = true + flywheelRightConfiguration.CurrentLimits.StatorCurrentLimit = + FlywheelConstants.RIGHT_FLYWHEEL_STATOR_CURRENT_LIMIT.inAmperes + flywheelRightConfiguration.CurrentLimits.StatorCurrentLimitEnable = false + + flywheelLeftConfiguration.MotorOutput.NeutralMode = NeutralModeValue.Brake + flywheelRightConfiguration.MotorOutput.NeutralMode = NeutralModeValue.Brake + + flywheelRightTalon.configurator.apply(flywheelRightConfiguration) + + flywheelLeftTalon.setControl(Follower(Constants.Shooter.FLYWHEEL_RIGHT_MOTOR_ID, true)) + + rightFlywheelStatorCurrentSignal = flywheelRightTalon.statorCurrent + rightFlywheelSupplyCurrentSignal = flywheelRightTalon.supplyCurrent + rightFlywheelTempSignal = flywheelRightTalon.deviceTemp + rightFlywheelDutyCycle = flywheelRightTalon.dutyCycle + motorVoltage = flywheelRightTalon.motorVoltage + motorTorque = flywheelRightTalon.torqueCurrent + + leftFlywheelStatorCurrentSignal = flywheelLeftTalon.statorCurrent + leftFlywheelSupplyCurrentSignal = flywheelLeftTalon.supplyCurrent + leftFlywheelTempSignal = flywheelLeftTalon.deviceTemp + leftFlywheelDutyCycle = flywheelLeftTalon.dutyCycle + + MotorChecker.add( + "Shooter", + "Flywheel", + MotorCollection( + mutableListOf(Falcon500(flywheelRightTalon, "Flywheel Right Motor")), + FlywheelConstants.RIGHT_FLYWHEEL_SUPPLY_CURRENT_LIMIT, + 90.celsius, + FlywheelConstants.RIGHT_FLYWHEEL_SUPPLY_CURRENT_LIMIT - 30.amps, + 110.celsius + ) + ) + MotorChecker.add( + "Shooter", + "Flywheel", + MotorCollection( + mutableListOf(Falcon500(flywheelLeftTalon, "Flywheel Left Motor")), + FlywheelConstants.LEFT_FLYWHEEL_SUPPLY_CURRENT_LIMIT, + 90.celsius, + FlywheelConstants.LEFT_FLYWHEEL_SUPPLY_CURRENT_LIMIT - 30.amps, + 110.celsius + ) + ) + MotorChecker.add( + "Shooter", + "Flywheel", + MotorCollection( + mutableListOf(Falcon500(flywheelRightTalon, "Flywheel Right Motor")), + FlywheelConstants.RIGHT_FLYWHEEL_SUPPLY_CURRENT_LIMIT, + 90.celsius, + FlywheelConstants.RIGHT_FLYWHEEL_SUPPLY_CURRENT_LIMIT - 30.amps, + 110.celsius + ) + ) + } + + override fun configPID( + rightkP: ProportionalGain, Volt>, + rightkI: IntegralGain, Volt>, + rightkD: DerivativeGain, Volt>, + leftkP: ProportionalGain, Volt>, + leftkI: IntegralGain, Volt>, + leftkD: DerivativeGain, Volt> + ) { + val PIDRightConfig = Slot0Configs() + PIDRightConfig.kP = flywheelRightSensor.proportionalVelocityGainToRawUnits(rightkP) + PIDRightConfig.kI = flywheelRightSensor.integralVelocityGainToRawUnits(rightkI) + PIDRightConfig.kD = flywheelRightSensor.derivativeVelocityGainToRawUnits(rightkD) + + val PIDLeftConfig = Slot0Configs() + PIDLeftConfig.kP = flywheelLeftSensor.proportionalVelocityGainToRawUnits(leftkP) + PIDLeftConfig.kI = flywheelLeftSensor.integralVelocityGainToRawUnits(leftkI) + PIDLeftConfig.kD = flywheelLeftSensor.derivativeVelocityGainToRawUnits(leftkD) + + flywheelLeftTalon.configurator.apply(PIDLeftConfig) + flywheelRightTalon.configurator.apply(PIDRightConfig) + } + + override fun setFlywheelVoltage( + voltageRight: ElectricalPotential, + voltageLeft: ElectricalPotential + ) { + flywheelRightTalon.setVoltage(voltageRight.inVolts) + flywheelLeftTalon.setVoltage(voltageLeft.inVolts) + } + + override fun setFlywheelVelocity( + rightVelocity: AngularVelocity, + leftVelocity: AngularVelocity, + feedforwardLeft: ElectricalPotential, + feedforwardRight: ElectricalPotential + ) { + FlywheelIOTalon.rightVelocityRequest.setFeedforward(feedforwardRight) + FlywheelIOTalon.rightVelocityRequest.setVelocity(rightVelocity) + FlywheelIOTalon.flywheelRightTalon.setControl( + FlywheelIOTalon.rightVelocityRequest.velocityVoltagePhoenix6 + ) + FlywheelIOTalon.leftVelocityRequest.setFeedforward(feedforwardLeft) + FlywheelIOTalon.leftVelocityRequest.setVelocity(leftVelocity) + FlywheelIOTalon.flywheelRightTalon.setControl( + FlywheelIOTalon.leftVelocityRequest.velocityVoltagePhoenix6 + ) + } + override fun updateInputs(inputs: FlywheelIO.FlywheelIOInputs) { + inputs.rightFlywheelVelocity = flywheelRightSensor.velocity + inputs.rightFlywheelAppliedVoltage = motorVoltage.value.volts + inputs.rightFlywheelStatorCurrent = rightFlywheelStatorCurrentSignal.value.amps + inputs.rightFlywheelSupplyCurrent = rightFlywheelSupplyCurrentSignal.value.amps + inputs.rightFlywheelTemperature = rightFlywheelTempSignal.value.celsius + // TODO fix unit for torque + inputs.rightFlywheelTorque = motorTorque.value.newtons + inputs.rightFlywheelDutyCycle = rightFlywheelDutyCycle.value.volts + + inputs.leftFlywheelVelocity = flywheelLeftSensor.velocity + inputs.leftFlywheelAppliedVoltage = leftFlywheelDutyCycle.value.volts + inputs.leftFlywheelStatorCurrent = leftFlywheelStatorCurrentSignal.value.amps + inputs.leftFlywheelSupplyCurrent = leftFlywheelSupplyCurrentSignal.value.amps + inputs.leftFlywheelTemperature = leftFlywheelTempSignal.value.celsius + } + + override fun setFlywheelBrakeMode(brake: Boolean) { + val motorOutputConfig = MotorOutputConfigs() + + if (brake) { + motorOutputConfig.NeutralMode = NeutralModeValue.Brake + } else { + motorOutputConfig.NeutralMode = NeutralModeValue.Coast + } + + flywheelRightTalon.configurator.apply(motorOutputConfig) + } +} 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 1bc41506..ce6eeb14 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt @@ -4,6 +4,7 @@ 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.derived.Angle import org.team4099.lib.units.derived.ElectricalPotential sealed interface Request { @@ -39,4 +40,13 @@ sealed interface Request { class OpenLoop(val voltage: ElectricalPotential) : ElevatorRequest class Home() : ElevatorRequest } + sealed interface WristRequest : Request { + class OpenLoop(val wristVoltage: ElectricalPotential) : WristRequest + class TargetingPosition(val wristPosition: Angle) : WristRequest + class Zero() : WristRequest + } + sealed interface FlywheelRequest : Request { + class OpenLoop(val flywheelVoltage: ElectricalPotential) : FlywheelRequest + class TargetingVelocity(val flywheelVelocity: AngularVelocity) : FlywheelRequest + } } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/Wrist.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/Wrist.kt new file mode 100644 index 00000000..2edab156 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/Wrist.kt @@ -0,0 +1,292 @@ +package com.team4099.robot2023.subsystems.wrist + +import com.team4099.lib.hal.Clock +import com.team4099.lib.logging.LoggedTunableValue +import com.team4099.robot2023.config.constants.Constants +import com.team4099.robot2023.config.constants.WristConstants +import com.team4099.robot2023.subsystems.superstructure.Request +import edu.wpi.first.wpilibj.RobotBase +import edu.wpi.first.wpilibj2.command.Command +import edu.wpi.first.wpilibj2.command.Commands +import edu.wpi.first.wpilibj2.command.SubsystemBase +import org.littletonrobotics.junction.Logger +import org.team4099.lib.controller.ArmFeedforward +import org.team4099.lib.controller.TrapezoidProfile +import org.team4099.lib.units.AngularVelocity +import org.team4099.lib.units.base.inSeconds +import org.team4099.lib.units.base.seconds +import org.team4099.lib.units.derived.ElectricalPotential +import org.team4099.lib.units.derived.Radian +import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.inDegrees +import org.team4099.lib.units.derived.inVolts +import org.team4099.lib.units.derived.inVoltsPerDegree +import org.team4099.lib.units.derived.inVoltsPerDegreePerSecond +import org.team4099.lib.units.derived.inVoltsPerDegreeSeconds +import org.team4099.lib.units.derived.inVoltsPerRadianPerSecond +import org.team4099.lib.units.derived.inVoltsPerRadianPerSecondPerSecond +import org.team4099.lib.units.derived.perDegree +import org.team4099.lib.units.derived.perDegreePerSecond +import org.team4099.lib.units.derived.perDegreeSeconds +import org.team4099.lib.units.derived.perRadianPerSecond +import org.team4099.lib.units.derived.perRadianPerSecondPerSecond +import org.team4099.lib.units.derived.radians +import org.team4099.lib.units.derived.volts +import org.team4099.lib.units.inDegreesPerSecond +import org.team4099.lib.units.perSecond + +class Wrist(val io: WristIO) : SubsystemBase() { + val inputs = WristIO.WristIOInputs() + + private val wristkS = + LoggedTunableValue( + "Wrist/kS", WristConstants.PID.WRIST_KS, Pair({ it.inVolts }, { it.volts }) + ) + private val wristkV = + LoggedTunableValue( + "Wrist/kV", Pair({ it.inVoltsPerRadianPerSecond }, { it.volts.perRadianPerSecond }) + ) + private val wristkA = + LoggedTunableValue( + "Wrist/kA", + Pair({ it.inVoltsPerRadianPerSecondPerSecond }, { it.volts.perRadianPerSecondPerSecond }) + ) + private val wristkG = + LoggedTunableValue( + "Wrist/kG", WristConstants.PID.WRIST_KG, Pair({ it.inVolts }, { it.volts }) + ) + + var wristFeedForward: ArmFeedforward + + private val wristkP = + LoggedTunableValue("Wrist/kP", Pair({ it.inVoltsPerDegree }, { it.volts.perDegree })) + private val wristkI = + LoggedTunableValue( + "Wrist/kI", Pair({ it.inVoltsPerDegreeSeconds }, { it.volts.perDegreeSeconds }) + ) + private val wristkD = + LoggedTunableValue( + "Wrist/kD", Pair({ it.inVoltsPerDegreePerSecond }, { it.volts.perDegreePerSecond }) + ) + + var currentRequest: Request.WristRequest = Request.WristRequest.Zero() + set(value) { + when (value) { + is Request.WristRequest.OpenLoop -> { + wristTargetVoltage = value.wristVoltage + } + is Request.WristRequest.TargetingPosition -> { + wristPositionTarget = value.wristPosition + } + else -> {} + } + field = value + } + + var currentState: WristStates = WristStates.UNINITIALIZED + + var wristTargetVoltage: ElectricalPotential = 0.0.volts + + private var lastWristPositionTarget = 0.0.degrees + private var wristPositionTarget = 0.0.degrees + + private var timeProfileGeneratedAt = 0.0.seconds + + private var wristConstraints: TrapezoidProfile.Constraints = + TrapezoidProfile.Constraints( + WristConstants.MAX_WRIST_VELOCITY, WristConstants.MAX_WRIST_ACCELERATION + ) + + private var wristProfile = + TrapezoidProfile( + wristConstraints, + TrapezoidProfile.State(-1337.radians, -1337.radians.perSecond), + TrapezoidProfile.State(-1337.radians, -1337.radians.perSecond) + ) + + private var prevWristSetpoint: TrapezoidProfile.State = + TrapezoidProfile.State(inputs.wristPostion, inputs.wristVelocity) + val forwardLimitReached: Boolean + get() = inputs.wristPostion >= WristConstants.WRIST_MAX_ROTATION + val reverseLimitReached: Boolean + get() = inputs.wristPostion <= WristConstants.WRIST_MIN_ROTATION + + private fun isOutOfBounds(velocity: AngularVelocity): Boolean { + return (velocity > 0.0.degrees.perSecond && forwardLimitReached) || + (velocity < 0.0.degrees.perSecond && reverseLimitReached) + } + + init { + if (RobotBase.isReal()) { + wristkP.initDefault(WristConstants.PID.REAL_KP) + wristkI.initDefault(WristConstants.PID.REAL_KI) + wristkD.initDefault(WristConstants.PID.REAL_KD) + } else { + wristkP.initDefault(WristConstants.PID.SIM_KP) + wristkI.initDefault(WristConstants.PID.SIM_KI) + wristkD.initDefault(WristConstants.PID.SIM_KD) + } + + wristkS.initDefault(WristConstants.PID.WRIST_KS) + wristkG.initDefault(WristConstants.PID.WRIST_KG) + wristkV.initDefault(WristConstants.PID.WRIST_KV) + wristkA.initDefault(WristConstants.PID.WRIST_KA) + + wristFeedForward = + ArmFeedforward( + WristConstants.PID.WRIST_KS, + WristConstants.PID.WRIST_KG, + WristConstants.PID.WRIST_KV, + WristConstants.PID.WRIST_KA + ) + } + + override fun periodic() { + io.updateInputs(inputs) + if (wristkP.hasChanged() || wristkI.hasChanged() || wristkD.hasChanged()) { + io.configPID(wristkP.get(), wristkI.get(), wristkD.get()) + } + if (wristkA.hasChanged() || + wristkV.hasChanged() || + wristkG.hasChanged() || + wristkS.hasChanged() + ) { + print(wristkV.get().inVoltsPerRadianPerSecond) + wristFeedForward = ArmFeedforward(wristkS.get(), wristkG.get(), wristkV.get(), wristkA.get()) + } + + Logger.processInputs("Wrist", inputs) + + Logger.recordOutput("Wrist/currentState", currentState.name) + + Logger.recordOutput("Wrist/requestedState", currentRequest.javaClass.simpleName) + + Logger.recordOutput("Wrist/isAtTargetedPosition", isAtTargetedPosition) + + if (Constants.Tuning.DEBUGING_MODE) {} + + var nextState = currentState + when (currentState) { + WristStates.UNINITIALIZED -> { + nextState = fromRequestToState(currentRequest) + } + WristStates.ZERO -> { + io.zeroEncoder() + nextState = fromRequestToState(currentRequest) + } + WristStates.OPEN_LOOP -> { + setWristVoltage(wristTargetVoltage) + + nextState = fromRequestToState(currentRequest) + } + WristStates.TARGETING_POSITION -> { + + if (wristPositionTarget != lastWristPositionTarget) { + val preProfileGenerate = Clock.fpgaTime + wristProfile = + TrapezoidProfile( + wristConstraints, + TrapezoidProfile.State(wristPositionTarget, 0.0.radians.perSecond), + TrapezoidProfile.State(inputs.wristPostion, inputs.wristVelocity) + ) + + val postProfileGenerate = Clock.fpgaTime + Logger.recordOutput( + "/Wrist/ProfileGenerationMS", + postProfileGenerate.inSeconds - preProfileGenerate.inSeconds + ) + + timeProfileGeneratedAt = Clock.fpgaTime + lastWristPositionTarget = wristPositionTarget + } + val timeElapsed = Clock.fpgaTime - timeProfileGeneratedAt + val setPoint: TrapezoidProfile.State = wristProfile.calculate(timeElapsed) + setWristPosition(setPoint) + Logger.recordOutput("Wrist/completedMotionProfile", wristProfile.isFinished(timeElapsed)) + nextState = fromRequestToState(currentRequest) + // if we're transitioning out of targeting position, we want to make sure the next time we + // enter targeting position, we regenerate profile (even if the arm setpoint is the same as + // the previous time we ran it) + if (!(currentState.equivalentToRequest(currentRequest))) { + // setting the last target to something unreasonable so the profile is generated next loop + // cycle + lastWristPositionTarget = (-1337).degrees + } + } + } + currentState = nextState + } + + private fun setWristPosition(setPoint: TrapezoidProfile.State) { + val WristAngularAcceleration = + (setPoint.velocity - prevWristSetpoint.velocity) / Constants.Universal.LOOP_PERIOD_TIME + prevWristSetpoint = setPoint + val feedforward = + wristFeedForward.calculate(setPoint.position, setPoint.velocity, WristAngularAcceleration) + + // When the forward or reverse limit is reached, set the voltage to 0 + // Else move the arm to the setpoint position + if (isOutOfBounds(setPoint.velocity)) { + io.setWristVoltage(wristFeedForward.calculate(inputs.wristPostion, 0.degrees.perSecond)) + } else { + io.setWristPosition(setPoint.position, feedforward) + } + + Logger.recordOutput("Wrist/profileIsOutOfBounds", isOutOfBounds(setPoint.velocity)) + Logger.recordOutput("Wrist/armFeedForward", feedforward.inVolts) + Logger.recordOutput("Wrist/armTargetPosition", setPoint.position.inDegrees) + Logger.recordOutput("Wrist/armTargetVelocity", setPoint.velocity.inDegreesPerSecond) + } + + val isAtTargetedPosition: Boolean + get() = + ( + currentState == WristStates.TARGETING_POSITION && + wristProfile.isFinished(Clock.fpgaTime - timeProfileGeneratedAt) && + (inputs.wristPostion - wristPositionTarget).absoluteValue <= + WristConstants.WRIST_TOLERANCE + ) + + fun setWristVoltage(appliedVoltage: ElectricalPotential) { + io.setWristVoltage(appliedVoltage) + } + + fun wristPositionCommand(): Command { + return Commands.runOnce({ + currentRequest = Request.WristRequest.TargetingPosition(-15.degrees) + }) + } + + fun wristOpenLoopCommand(): Command { + return Commands.runOnce({ currentRequest = Request.WristRequest.OpenLoop(10.volts) }) + } + + fun wristResetCommand(): Command { + return Commands.runOnce({ currentRequest = Request.WristRequest.OpenLoop(-10.volts) }) + } + + companion object { + enum class WristStates { + UNINITIALIZED, + ZERO, + OPEN_LOOP, + TARGETING_POSITION; + + inline fun equivalentToRequest(request: Request.WristRequest): Boolean { + return ( + (request is Request.WristRequest.Zero && this == ZERO) || + (request is Request.WristRequest.OpenLoop && this == OPEN_LOOP) || + (request is Request.WristRequest.TargetingPosition && this == TARGETING_POSITION) + ) + } + } + + inline fun fromRequestToState(request: Request.WristRequest): WristStates { + return when (request) { + is Request.WristRequest.OpenLoop -> WristStates.OPEN_LOOP + is Request.WristRequest.TargetingPosition -> WristStates.TARGETING_POSITION + is Request.WristRequest.Zero -> WristStates.ZERO + } + } + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/WristIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/WristIO.kt new file mode 100644 index 00000000..f5936e56 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/WristIO.kt @@ -0,0 +1,96 @@ +package com.team4099.robot2023.subsystems.wrist + +import org.littletonrobotics.junction.LogTable +import org.littletonrobotics.junction.inputs.LoggableInputs +import org.team4099.lib.units.base.amps +import org.team4099.lib.units.base.celsius +import org.team4099.lib.units.base.inAmperes +import org.team4099.lib.units.base.inCelsius +import org.team4099.lib.units.derived.Angle +import org.team4099.lib.units.derived.DerivativeGain +import org.team4099.lib.units.derived.ElectricalPotential +import org.team4099.lib.units.derived.IntegralGain +import org.team4099.lib.units.derived.ProportionalGain +import org.team4099.lib.units.derived.Radian +import org.team4099.lib.units.derived.Volt +import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.inDegrees +import org.team4099.lib.units.derived.inVolts +import org.team4099.lib.units.derived.radians +import org.team4099.lib.units.derived.volts +import org.team4099.lib.units.inRadiansPerSecond +import org.team4099.lib.units.perSecond + +interface WristIO { + class WristIOInputs : LoggableInputs { + + var wristPostion = 0.0.degrees + var wristVelocity = 0.0.radians.perSecond + var wristAppliedVoltage = 0.0.volts + var wristDutyCycle = 0.0.volts + var wristTorque = 0.0 + var wristStatorCurrent = 0.0.amps + var wristSupplyCurrent = 0.0.amps + var wristTemperature = 0.0.celsius + + var isSimulated = false + + override fun toLog(table: LogTable) { + table.put("wristPostion", wristPostion.inDegrees) + table.put("wristVelocityRPM", wristVelocity.inRadiansPerSecond) + table.put("wristAppliedVoltage", wristAppliedVoltage.inVolts) + table.put("wristStatorCurrent", wristStatorCurrent.inAmperes) + table.put("wristSupplyCurrent", wristSupplyCurrent.inAmperes) + table.put("wristTemperature", wristTemperature.inCelsius) + } + + override fun fromLog(table: LogTable) { + + // wrist logs + table.get("wristPostion", wristPostion.inDegrees).let { wristPostion = it.degrees } + table.get("wristVelocity", wristVelocity.inRadiansPerSecond).let { + wristVelocity = it.radians.perSecond + } + table.get("wristAppliedVoltage", wristAppliedVoltage.inVolts).let { + wristAppliedVoltage = it.volts + } + table.get("wristStatorCurrent", wristStatorCurrent.inAmperes).let { + wristStatorCurrent = it.amps + } + table.get("wristSupplyCurrent", wristSupplyCurrent.inAmperes).let { + wristSupplyCurrent = it.amps + } + table.get("wristTemperature", wristTemperature.inCelsius).let { + wristTemperature = it.celsius + } + } + } + + fun updateInputs(inputs: WristIOInputs) {} + + /*fun setRollerVoltage (voltage: ElectricalPotential){ + + }*/ + fun setWristVoltage(voltage: ElectricalPotential) {} + + // fun setFeederVoltage (voltage: ElectricalPotential){ + + // } + fun setWristPosition(position: Angle, feedforward: ElectricalPotential) {} + + // fun setRollerBrakeMode (brake: Boolean){ + + // } + // fun setFeederBrakeMode (brake: Boolean){ + + // } + fun setWristBrakeMode(brake: Boolean) {} + + fun configPID( + kP: ProportionalGain, + kI: IntegralGain, + kD: DerivativeGain + ) {} + + fun zeroEncoder() {} +} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/WristIONeo.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/WristIONeo.kt new file mode 100644 index 00000000..e6723175 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/WristIONeo.kt @@ -0,0 +1,174 @@ +package com.team4099.robot2023.subsystems.wrist + +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.WristConstants +import edu.wpi.first.wpilibj.DutyCycleEncoder +import org.team4099.lib.units.base.amps +import org.team4099.lib.units.base.celsius +import org.team4099.lib.units.base.inAmperes +import org.team4099.lib.units.derived.Angle +import org.team4099.lib.units.derived.DerivativeGain +import org.team4099.lib.units.derived.ElectricalPotential +import org.team4099.lib.units.derived.IntegralGain +import org.team4099.lib.units.derived.ProportionalGain +import org.team4099.lib.units.derived.Radian +import org.team4099.lib.units.derived.Volt +import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.inDegrees +import org.team4099.lib.units.derived.inVolts +import org.team4099.lib.units.derived.rotations +import org.team4099.lib.units.derived.volts +import org.team4099.lib.units.sparkMaxAngularMechanismSensor +import kotlin.math.IEEErem +import kotlin.math.absoluteValue + +object WristIONeo : WristIO { + // private val rollerSparkMax = CANSparkMax(Constants.Shooter.ROLLER_MOTOR_ID, + // CANSparkMaxLowLevel.MotorType.kBrushless) + // private val rollerSensor = sparkMaxAngularMechanismSensor( rollerSparkMax, + // ShooterConstants.ROLLER_GEAR_RATIO, + // ShooterConstants.ROLLER_VOLTAGE_COMPENSATION + // ) + + private val wristSparkMax = + CANSparkMax(Constants.WRIST.WRIST_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless) + private val wristSensor = + sparkMaxAngularMechanismSensor( + wristSparkMax, WristConstants.WRIST_GEAR_RATIO, WristConstants.WRIST_VOLTAGE_COMPENSATION + ) + private val wristPIDController: SparkMaxPIDController = wristSparkMax.pidController + // private val feederSparkMax = CANSparkMax(Constants.Shooter.FEEDER_MOTOR_ID, + // CANSparkMaxLowLevel.MotorType.kBrushless) + // private val feederSensor = sparkMaxAngularMechanismSensor( feederSparkMax, + // ShooterConstants.FEEDER_GEAR_RATIO, + // ShooterConstants.FEEDER_VOLTAGE_COMPENSATION + // ) + + private val throughBoreEncoder = DutyCycleEncoder(Constants.Intake.REV_ENCODER_PORT) + + private val encoderAbsolutePosition: Angle + get() { + var output = + ( + (-throughBoreEncoder.absolutePosition.rotations) * + WristConstants.WRIST_ENCODER_GEAR_RATIO + ) + + if (output in (-55).degrees..0.0.degrees) { + output -= 180.degrees + } + + return output + } + + // uses the absolute encoder position to calculate the arm position + private val armAbsolutePosition: Angle + get() { + return (encoderAbsolutePosition - WristConstants.ABSOLUTE_ENCODER_OFFSET).inDegrees.IEEErem( + 360.0 + ) + .degrees + } + + init { + // reset the motors + // rollerSparkMax.restoreFactoryDefaults() + // rollerSparkMax.clearFaults() + + wristSparkMax.restoreFactoryDefaults() + wristSparkMax.clearFaults() + + // feederSparkMax.restoreFactoryDefaults() + // feederSparkMax.clearFaults() + + // voltage and current limits + // rollerSparkMax.enableVoltageCompensation(ShooterConstants.ROLLER_VOLTAGE_COMPENSATION.inVolts) + // rollerSparkMax.setSmartCurrentLimit(ShooterConstants.ROLLER_STATOR_CURRENT_LIMIT.inAmperes.toInt()) + + wristSparkMax.enableVoltageCompensation(WristConstants.WRIST_VOLTAGE_COMPENSATION.inVolts) + wristSparkMax.setSmartCurrentLimit(WristConstants.WRIST_STATOR_CURRENT_LIMIT.inAmperes.toInt()) + + // feederSparkMax.enableVoltageCompensation(ShooterConstants.FEEDER_VOLTAGE_COMPENSATION.inVolts) + // feederSparkMax.setSmartCurrentLimit(ShooterConstants.FEEDER_STATOR_CURRENT_LIMIT.inAmperes.toInt()) + } + + override fun updateInputs(inputs: WristIO.WristIOInputs) { + /* inputs.rollerVelocity = rollerSensor.velocity + inputs.rollerAppliedVoltage = rollerSparkMax.busVoltage.volts * rollerSparkMax.appliedOutput + inputs.rollerStatorCurrent = rollerSparkMax.outputCurrent.amps + // BatteryVoltage * SupplyCurrent = AppliedVoltage * StatorCurrent + // AppliedVoltage = percentOutput * BatteryVoltage + // SupplyCurrent = (percentOutput * BatteryVoltage / BatteryVoltage) * StatorCurrent = + // percentOutput * statorCurrent + inputs.rollerSupplyCurrent = inputs.rollerStatorCurrent * rollerSparkMax.appliedOutput.absoluteValue + inputs.rollerTempreature = rollerSparkMax.motorTemperature.celsius + */ + inputs.wristPostion = wristSensor.position + inputs.wristAppliedVoltage = wristSparkMax.busVoltage.volts * wristSparkMax.appliedOutput + inputs.wristStatorCurrent = wristSparkMax.outputCurrent.amps + // BatteryVoltage * SupplyCurrent = AppliedVoltage * StatorCurrent + // AppliedVoltage = percentOutput * BatteryVoltage + // SupplyCurrent = (percentOutput * BatteryVoltage / BatteryVoltage) * StatorCurrent = + // percentOutput * statorCurrent + inputs.wristSupplyCurrent = + inputs.wristStatorCurrent * wristSparkMax.appliedOutput.absoluteValue + inputs.wristTemperature = wristSparkMax.motorTemperature.celsius + + /* inputs.feederVelocity = feederSensor.velocity + inputs.feederAppliedVoltage = feederSparkMax.busVoltage.volts * feederSparkMax.appliedOutput + inputs.feederStatorCurrent = feederSparkMax.outputCurrent.amps + // BatteryVoltage * SupplyCurrent = AppliedVoltage * StatorCurrent + // AppliedVoltage = percentOutput * BatteryVoltage + // SupplyCurrent = (percentOutput * BatteryVoltage / BatteryVoltage) * StatorCurrent = + // percentOutput * statorCurrent + inputs.feederSupplyCurrent = inputs.wristStatorCurrent * feederSparkMax.appliedOutput.absoluteValue + inputs.feederTemperature = feederSparkMax.motorTemperature.celsius*/ + } + override fun configPID( + kP: ProportionalGain, + kI: IntegralGain, + kD: DerivativeGain + ) { + wristPIDController.p = wristSensor.proportionalPositionGainToRawUnits(kP) + wristPIDController.i = wristSensor.integralPositionGainToRawUnits(kI) + wristPIDController.d = wristSensor.derivativePositionGainToRawUnits(kD) + } + + override fun setWristPosition(position: Angle, feedforward: ElectricalPotential) { + wristPIDController.setReference( + wristSensor.positionToRawUnits( + clamp(position, WristConstants.WRIST_MIN_ROTATION, WristConstants.WRIST_MAX_ROTATION) + ), + CANSparkMax.ControlType.kPosition, + 0, + feedforward.inVolts + ) + } + + override fun setWristVoltage(voltage: ElectricalPotential) { + wristSparkMax.setVoltage( + clamp( + voltage, + -WristConstants.WRIST_VOLTAGE_COMPENSATION, + WristConstants.WRIST_VOLTAGE_COMPENSATION + ) + .inVolts + ) + } + + override fun setWristBrakeMode(brake: Boolean) { + if (brake) { + wristSparkMax.setIdleMode(CANSparkMax.IdleMode.kBrake) + } else { + wristSparkMax.setIdleMode(CANSparkMax.IdleMode.kCoast) + } + } + + override fun zeroEncoder() { + wristSparkMax.encoder.position = wristSensor.positionToRawUnits(armAbsolutePosition) + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/WristIOSim.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/WristIOSim.kt new file mode 100644 index 00000000..b7456f07 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/WristIOSim.kt @@ -0,0 +1,134 @@ +package com.team4099.robot2023.subsystems.wrist + +import com.team4099.lib.math.clamp +import com.team4099.robot2023.config.constants.Constants +import com.team4099.robot2023.config.constants.WristConstants +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.SingleJointedArmSim +import org.team4099.lib.controller.PIDController +import org.team4099.lib.units.base.amps +import org.team4099.lib.units.base.celsius +import org.team4099.lib.units.base.inMeters +import org.team4099.lib.units.base.inSeconds +import org.team4099.lib.units.derived.Angle +import org.team4099.lib.units.derived.DerivativeGain +import org.team4099.lib.units.derived.ElectricalPotential +import org.team4099.lib.units.derived.IntegralGain +import org.team4099.lib.units.derived.ProportionalGain +import org.team4099.lib.units.derived.Radian +import org.team4099.lib.units.derived.Volt +import org.team4099.lib.units.derived.inKilogramsMeterSquared +import org.team4099.lib.units.derived.inRadians +import org.team4099.lib.units.derived.inVolts +import org.team4099.lib.units.derived.radians +import org.team4099.lib.units.derived.volts +import org.team4099.lib.units.perSecond + +object WristIOSim : WristIO { + + val wristSim = + SingleJointedArmSim( + DCMotor.getNEO(1), + WristConstants.WRIST_GEAR_RATIO, + WristConstants.WRIST_INERTIA.inKilogramsMeterSquared, + WristConstants.WRIST_LENGTH.inMeters, + WristConstants.WRIST_MIN_ROTATION.inRadians, + WristConstants.WRIST_MAX_ROTATION.inRadians, + true, + 0.0 + ) + + init { + MotorChecker.add( + "Ground Intake", + "Rotation", + MotorCollection( + mutableListOf( + SimulatedMotor( + wristSim, + "Arm Motor", + ), + ), + 60.amps, + 10.celsius, + 45.amps, + 20.celsius + ) + ) + + MotorChecker.add( + "Ground Intake", + "Roller", + MotorCollection( + mutableListOf( + SimulatedMotor( + wristSim, + "Roller Motor", + ) + ), + 60.amps, + 10.celsius, + 45.amps, + 20.celsius + ) + ) + } + + private val wristController = + PIDController(WristConstants.PID.SIM_KP, WristConstants.PID.SIM_KI, WristConstants.PID.SIM_KD) + + var appliedVoltage = 0.volts + + override fun updateInputs(inputs: WristIO.WristIOInputs) { + wristSim.update(Constants.Universal.LOOP_PERIOD_TIME.inSeconds) + wristSim.update(Constants.Universal.LOOP_PERIOD_TIME.inSeconds) + + inputs.wristPostion = wristSim.angleRads.radians + inputs.wristVelocity = wristSim.velocityRadPerSec.radians.perSecond + inputs.wristSupplyCurrent = 0.amps + inputs.wristAppliedVoltage = appliedVoltage + inputs.wristStatorCurrent = wristSim.currentDrawAmps.amps + inputs.wristTemperature = 0.0.celsius + + inputs.isSimulated = true + } + + /** + * Sets the roller motor voltage, ensures the voltage is limited to battery voltage compensation + * + * @param voltage the voltage to set the roller motor to + */ + override fun setWristVoltage(voltage: ElectricalPotential) { + val clampedVoltage = + clamp(voltage, -WristConstants.VOLTAGE_COMPENSATION, WristConstants.VOLTAGE_COMPENSATION) + wristSim.setInputVoltage(clampedVoltage.inVolts) + appliedVoltage = clampedVoltage + } + + override fun setWristPosition(position: Angle, feedforward: ElectricalPotential) { + val feedback = wristController.calculate(wristSim.angleRads.radians, position) + setWristVoltage(feedback + feedforward) + } + + /** + * Updates the PID constants using the implementation controller, uses arm sensor to convert from + * PID constants to motor controller units + * + * @param kP accounts for linear error + * @param kI accounts for integral error + * @param kD accounts for derivative error + */ + override fun configPID( + kP: ProportionalGain, + kI: IntegralGain, + kD: DerivativeGain + ) { + wristController.setPID(kP, kI, kD) + } + + /** recalculates the current position of the neo encoder using value from the absolute encoder */ + override fun zeroEncoder() {} +} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/WristIOTalon.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/WristIOTalon.kt new file mode 100644 index 00000000..5d10a20b --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/WristIOTalon.kt @@ -0,0 +1,168 @@ +package com.team4099.robot2023.subsystems.wrist + +import com.ctre.phoenix6.StatusSignal +import com.ctre.phoenix6.configs.MagnetSensorConfigs +import com.ctre.phoenix6.configs.MotorOutputConfigs +import com.ctre.phoenix6.configs.Slot0Configs +import com.ctre.phoenix6.configs.TalonFXConfiguration +import com.ctre.phoenix6.hardware.CANcoder +import com.ctre.phoenix6.hardware.TalonFX +import com.ctre.phoenix6.signals.AbsoluteSensorRangeValue +import com.ctre.phoenix6.signals.FeedbackSensorSourceValue +import com.ctre.phoenix6.signals.NeutralModeValue +import com.ctre.phoenix6.signals.SensorDirectionValue +import com.team4099.lib.phoenix6.PositionVoltage +import com.team4099.robot2023.config.constants.Constants +import com.team4099.robot2023.config.constants.FlywheelConstants +import com.team4099.robot2023.config.constants.WristConstants +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.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.derived.Angle +import org.team4099.lib.units.derived.DerivativeGain +import org.team4099.lib.units.derived.ElectricalPotential +import org.team4099.lib.units.derived.IntegralGain +import org.team4099.lib.units.derived.ProportionalGain +import org.team4099.lib.units.derived.Radian +import org.team4099.lib.units.derived.Volt +import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.inDegrees +import org.team4099.lib.units.derived.inVolts +import org.team4099.lib.units.derived.volts + +object WristIOTalon : WristIO { + + private val wristTalon: TalonFX = TalonFX(Constants.WRIST.WRIST_MOTOR_ID) + + private val wristConfiguration: TalonFXConfiguration = TalonFXConfiguration() + + private val absoluteEncoder: CANcoder = CANcoder(Constants.WRIST.CANCODER_ID) + private val absoluteEncoderConfiguration: MagnetSensorConfigs = MagnetSensorConfigs() + + var positionRequest = PositionVoltage(-1337.degrees, slot = 0, feedforward = -1337.volts) + + private val wristSensor = + ctreAngularMechanismSensor( + wristTalon, + FlywheelConstants.RIGHT_MOTOR_REVOLUTIONS_PER_FLYWHEEL_REVOLUTIONS, + FlywheelConstants.VOLTAGE_COMPENSATION, + ) + + var statorCurrentSignal: StatusSignal + var supplyCurrentSignal: StatusSignal + var tempSignal: StatusSignal + var dutyCycle: StatusSignal + var motorVoltage: StatusSignal + var motorTorque: StatusSignal + init { + wristTalon.configurator.apply(TalonFXConfiguration()) + + wristTalon.clearStickyFaults() + + absoluteEncoderConfiguration.AbsoluteSensorRange = AbsoluteSensorRangeValue.Signed_PlusMinusHalf + absoluteEncoderConfiguration.SensorDirection = SensorDirectionValue.CounterClockwise_Positive + absoluteEncoderConfiguration.MagnetOffset = + WristConstants.ABSOLUTE_ENCODER_OFFSET.inDegrees / 180 + + absoluteEncoder.configurator.apply(absoluteEncoderConfiguration) + + wristConfiguration.Feedback.FeedbackRemoteSensorID = absoluteEncoder.deviceID + wristConfiguration.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.FusedCANcoder + + wristConfiguration.Feedback.SensorToMechanismRatio = WristConstants.WRIST_GEAR_RATIO + wristConfiguration.Feedback.RotorToSensorRatio = WristConstants.WRIST_ENCODER_GEAR_RATIO + + wristConfiguration.Slot0.kP = + wristSensor.proportionalPositionGainToRawUnits(WristConstants.PID.REAL_KP) + wristConfiguration.Slot0.kI = + wristSensor.integralPositionGainToRawUnits(WristConstants.PID.REAL_KI) + wristConfiguration.Slot0.kD = + wristSensor.derivativePositionGainToRawUnits(WristConstants.PID.REAL_KD) + + wristConfiguration.CurrentLimits.SupplyCurrentLimit = + FlywheelConstants.LEFT_FLYWHEEL_SUPPLY_CURRENT_LIMIT.inAmperes + wristConfiguration.CurrentLimits.SupplyCurrentThreshold = + FlywheelConstants.LEFT_FLYWHEEL_THRESHOLD_CURRENT_LIMIT.inAmperes + wristConfiguration.CurrentLimits.SupplyTimeThreshold = + FlywheelConstants.LEFT_flywheel_TRIGGER_THRESHOLD_TIME.inSeconds + wristConfiguration.CurrentLimits.SupplyCurrentLimitEnable = true + wristConfiguration.CurrentLimits.StatorCurrentLimit = + FlywheelConstants.LEFT_FLYWHEEL_STATOR_CURRENT_LIMIT.inAmperes + wristConfiguration.CurrentLimits.StatorCurrentLimitEnable = false + + wristConfiguration.MotorOutput.NeutralMode = NeutralModeValue.Brake + + wristTalon.configurator.apply(wristConfiguration) + + statorCurrentSignal = wristTalon.statorCurrent + supplyCurrentSignal = wristTalon.supplyCurrent + tempSignal = wristTalon.deviceTemp + dutyCycle = wristTalon.dutyCycle + motorVoltage = wristTalon.motorVoltage + motorTorque = wristTalon.torqueCurrent + + MotorChecker.add( + "Wrist", + "Wrist", + MotorCollection( + mutableListOf(Falcon500(wristTalon, "Wrist Motor")), + WristConstants.WRIST_STATOR_CURRENT_LIMIT, + 90.celsius, + WristConstants.WRIST_STATOR_CURRENT_LIMIT - 30.amps, + 110.celsius + ) + ) + } + + override fun configPID( + kP: ProportionalGain, + kI: IntegralGain, + kD: DerivativeGain + ) { + val PIDConfig = Slot0Configs() + PIDConfig.kP = wristSensor.proportionalPositionGainToRawUnits(kP) + PIDConfig.kI = wristSensor.integralPositionGainToRawUnits(kI) + PIDConfig.kD = wristSensor.derivativePositionGainToRawUnits(kD) + + wristTalon.configurator.apply(PIDConfig) + } + + override fun setWristVoltage(voltage: ElectricalPotential) { + wristTalon.setVoltage(voltage.inVolts) + } + + override fun setWristPosition(position: Angle, feedforward: ElectricalPotential) { + positionRequest.setFeedforward(feedforward) + positionRequest.setPosition(position) + wristTalon.setControl(positionRequest.positionVoltagePhoenix6) + } + override fun updateInputs(inputs: WristIO.WristIOInputs) { + inputs.wristPostion = wristSensor.position + inputs.wristVelocity = wristSensor.velocity + // TODO fix unit for torque + inputs.wristTorque = motorTorque.value + inputs.wristAppliedVoltage = motorVoltage.value.volts + inputs.wristDutyCycle = dutyCycle.value.volts + inputs.wristStatorCurrent = statorCurrentSignal.value.amps + inputs.wristSupplyCurrent = supplyCurrentSignal.value.amps + inputs.wristTemperature = tempSignal.value.celsius + } + + override fun setWristBrakeMode(brake: Boolean) { + val motorOutputConfig = MotorOutputConfigs() + + if (brake) { + motorOutputConfig.NeutralMode = NeutralModeValue.Brake + } else { + motorOutputConfig.NeutralMode = NeutralModeValue.Coast + } + wristTalon.configurator.apply(motorOutputConfig) + } + + override fun zeroEncoder() {} +}