diff --git a/src/main/kotlin/com/frcteam3636/frc2024/CAN.kt b/src/main/kotlin/com/frcteam3636/frc2024/CAN.kt index 91f027b..95ff0f3 100644 --- a/src/main/kotlin/com/frcteam3636/frc2024/CAN.kt +++ b/src/main/kotlin/com/frcteam3636/frc2024/CAN.kt @@ -1,5 +1,6 @@ package com.frcteam3636.frc2024 +import com.ctre.phoenix6.hardware.Pigeon2 import com.ctre.phoenix6.hardware.TalonFX import com.revrobotics.CANSparkFlex import com.revrobotics.CANSparkLowLevel @@ -29,13 +30,15 @@ fun CANSparkMax(id: REVMotorControllerId, type: CANSparkLowLevel.MotorType) = fun CANSparkFlex(id: REVMotorControllerId, type: CANSparkLowLevel.MotorType) = CANSparkFlex(id.num, type) -enum class CTREMotorControllerId(val num: Int, val bus: String) { +enum class CTREDeviceId(val num: Int, val bus: String) { FrontLeftDrivingMotor(1, "*"), BackLeftDrivingMotor(2, "*"), BackRightDrivingMotor(3, "*"), FrontRightDrivingMotor(4, "*"), RightArmMotor(10, "*"), LeftArmMotor(11, "*"), + PigeonGyro(20, "*"), } -fun TalonFX(id: CTREMotorControllerId) = TalonFX(id.num, id.bus) \ No newline at end of file +fun TalonFX(id: CTREDeviceId) = TalonFX(id.num, id.bus) +fun Pigeon2(id: CTREDeviceId) = Pigeon2(id.num, id.bus) \ No newline at end of file diff --git a/src/main/kotlin/com/frcteam3636/frc2024/LoggedRobot.kt b/src/main/kotlin/com/frcteam3636/frc2024/LoggedRobot.kt new file mode 100644 index 0000000..629aee9 --- /dev/null +++ b/src/main/kotlin/com/frcteam3636/frc2024/LoggedRobot.kt @@ -0,0 +1,136 @@ +package org.littletonrobotics.junction + +import edu.wpi.first.hal.DriverStationJNI +import edu.wpi.first.hal.FRCNetComm +import edu.wpi.first.hal.HAL +import edu.wpi.first.hal.NotifierJNI +import edu.wpi.first.wpilibj.IterativeRobotBase +import org.littletonrobotics.junction.AutoLogOutputManager +import org.littletonrobotics.junction.CheckInstall +import org.littletonrobotics.junction.Logger +import java.lang.management.GarbageCollectorMXBean +import java.lang.management.ManagementFactory + + +/** + * LoggedRobot implements the IterativeRobotBase robot program framework. + * + * + * + * The LoggedRobot class is intended to be subclassed by a user creating a robot + * program, and will call all required AdvantageKit periodic methods. + * + * + * + * periodic() functions from the base class are called on an interval by a + * Notifier instance. + */ +open class MyLoggedRobot protected constructor(period: Double = defaultPeriodSecs) : + IterativeRobotBase(period) { + private val notifier = NotifierJNI.initializeNotifier() + private val periodUs = (period * 1000000).toLong() + private var nextCycleUs: Long = 0 + private val gcStatsCollector = GcStatsCollector() + + private var useTiming = true + + /** + * Constructor for LoggedRobot. + * + * @param period Period in seconds. + */ + /** Constructor for LoggedRobot. */ + init { + NotifierJNI.setNotifierName(notifier, "LoggedRobot") + + HAL.report(FRCNetComm.tResourceType.kResourceType_Framework, FRCNetComm.tInstances.kFramework_AdvantageKit) + } + + protected fun finalize() { + NotifierJNI.stopNotifier(notifier) + NotifierJNI.cleanNotifier(notifier) + } + + /** Provide an alternate "main loop" via startCompetition(). */ + override fun startCompetition() { + // Robot init methods + val initStart = Logger.getRealTimestamp() + robotInit() + if (isSimulation()) { + simulationInit() + } + val initEnd = Logger.getRealTimestamp() + + // Register auto logged outputs + AutoLogOutputManager.registerFields(this) + + // Save data from init cycle + Logger.periodicAfterUser(initEnd - initStart, 0) + + // Tell the DS that the robot is ready to be enabled + println("********** Robot program startup complete **********") + DriverStationJNI.observeUserProgramStarting() + + // Loop forever, calling the appropriate mode-dependent function + while (true) { + if (useTiming) { + val currentTimeUs = Logger.getRealTimestamp() + if (nextCycleUs < currentTimeUs) { + // Loop overrun, start next cycle immediately + nextCycleUs = currentTimeUs + } else { + // Wait before next cycle + NotifierJNI.updateNotifierAlarm(notifier, nextCycleUs) + NotifierJNI.waitForNotifierAlarm(notifier) + } + nextCycleUs += periodUs + } + + val periodicBeforeStart = Logger.getRealTimestamp() + Logger.periodicBeforeUser() + val userCodeStart = Logger.getRealTimestamp() + loopFunc() + val userCodeEnd = Logger.getRealTimestamp() + + gcStatsCollector.update() + Logger.periodicAfterUser(userCodeEnd - userCodeStart, userCodeStart - periodicBeforeStart) + } + } + + /** Ends the main loop in startCompetition(). */ + override fun endCompetition() { + NotifierJNI.stopNotifier(notifier) + } + + /** Sets whether to use standard timing or run as fast as possible. */ + fun setUseTiming(useTiming: Boolean) { + this.useTiming = useTiming + } + + private class GcStatsCollector { + private val gcBeans: List = ManagementFactory.getGarbageCollectorMXBeans() + private val lastTimes = LongArray(gcBeans.size) + private val lastCounts = LongArray(gcBeans.size) + + fun update() { + var accumTime: Long = 0 + var accumCounts: Long = 0 + for (i in gcBeans.indices) { + val gcTime = gcBeans[i].collectionTime + val gcCount = gcBeans[i].collectionCount + accumTime += gcTime - lastTimes[i] + accumCounts += gcCount - lastCounts[i] + + lastTimes[i] = gcTime + lastCounts[i] = gcCount + } + + Logger.recordOutput("LoggedRobot/GCTimeMS", accumTime.toDouble()) + Logger.recordOutput("LoggedRobot/GCCounts", accumCounts.toDouble()) + } + } + + companion object { + const val defaultPeriodSecs: Double = 0.02 + } +} diff --git a/src/main/kotlin/com/frcteam3636/frc2024/Robot.kt b/src/main/kotlin/com/frcteam3636/frc2024/Robot.kt index bcd5594..1bddd21 100644 --- a/src/main/kotlin/com/frcteam3636/frc2024/Robot.kt +++ b/src/main/kotlin/com/frcteam3636/frc2024/Robot.kt @@ -23,8 +23,8 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController import edu.wpi.first.wpilibj2.command.button.JoystickButton import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine import org.littletonrobotics.junction.LogFileUtil -import org.littletonrobotics.junction.LoggedRobot import org.littletonrobotics.junction.Logger +import org.littletonrobotics.junction.MyLoggedRobot import org.littletonrobotics.junction.networktables.NT4Publisher import org.littletonrobotics.junction.wpilog.WPILOGReader import org.littletonrobotics.junction.wpilog.WPILOGWriter @@ -41,7 +41,7 @@ import org.littletonrobotics.junction.wpilog.WPILOGWriter * update the `Main.kt` file in the project. (If you use the IDE's Rename or Move refactorings when * renaming the object or package, it will get changed everywhere.) */ -object Robot : LoggedRobot() { +object Robot : MyLoggedRobot() { private val controller = CommandXboxController(2) private val joystickLeft = Joystick(0) private val joystickRight = Joystick(1) @@ -124,6 +124,7 @@ object Robot : LoggedRobot() { Drivetrain.register() Indexer.register() Intake.register() + Arm.register() } /** Expose commands for autonomous routines to use and display an auto picker in Shuffleboard. */ diff --git a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/Arm.kt b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/Arm.kt index 1c5f517..1b8538f 100644 --- a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/Arm.kt +++ b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/Arm.kt @@ -2,16 +2,25 @@ package com.frcteam3636.frc2024.subsystems.arm import com.ctre.phoenix6.SignalLogger import com.frcteam3636.frc2024.Robot +import com.frcteam3636.frc2024.subsystems.intake.Intake +import com.frcteam3636.frc2024.subsystems.intake.Intake.indexerAngleLigament +import com.frcteam3636.frc2024.subsystems.intake.Intake.intakeAngleLigament import edu.wpi.first.units.Angle import edu.wpi.first.units.Measure import edu.wpi.first.units.Units.Degrees import edu.wpi.first.units.Units.Volts import edu.wpi.first.wpilibj.Timer +import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d +import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d +import edu.wpi.first.wpilibj.util.Color +import edu.wpi.first.wpilibj.util.Color8Bit import edu.wpi.first.wpilibj2.command.Subsystem import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Mechanism import org.littletonrobotics.junction.Logger +import kotlin.math.cos +import kotlin.math.sin private const val SECONDS_BETWEEN_ARM_UPDATES = 0.5 @@ -23,7 +32,17 @@ object Arm : Subsystem { Robot.Model.PROTOTYPE -> ArmIOPrototype() } - var inputs = ArmIO.ArmInputs() + var inputs = LoggedArmInputs() + var mechanism = Mechanism2d(200.0, 200.0) + var armAngleLigament = MechanismLigament2d("Arm Ligament", 50.0, 90.0, 5.0, Color8Bit(Color.kGreen)) + var armWristAngleLigament = MechanismLigament2d("Arm Wrist Ligament", 20.0, 90.0, 5.0, Color8Bit(Color.kGreen)) + + init { + mechanism.getRoot("Arm", 100.0, 100.0).apply{ + append(armAngleLigament) + } + armAngleLigament.append(armWristAngleLigament) + } private var sysID = SysIdRoutine( SysIdRoutine.Config( @@ -41,9 +60,13 @@ object Arm : Subsystem { override fun periodic() { io.updateInputs(inputs) Logger.processInputs("/Arm", inputs) + armAngleLigament.angle = inputs.position.`in`(Degrees) + armWristAngleLigament.angle = 90.0 - inputs.position.`in`(Degrees) + if (timer.advanceIfElapsed(SECONDS_BETWEEN_ARM_UPDATES) && inputs.absoluteEncoderConnected){ io.updatePosition(inputs.absoluteEncoderPosition) } + Logger.recordOutput("/Arm/Mechanism", mechanism) } diff --git a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/ArmIO.kt b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/ArmIO.kt index 3c5910d..395ff52 100644 --- a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/ArmIO.kt +++ b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/ArmIO.kt @@ -5,7 +5,7 @@ import com.ctre.phoenix6.controls.VoltageOut import com.ctre.phoenix6.signals.GravityTypeValue import com.ctre.phoenix6.signals.InvertedValue import com.ctre.phoenix6.signals.NeutralModeValue -import com.frcteam3636.frc2024.CTREMotorControllerId +import com.frcteam3636.frc2024.CTREDeviceId import com.frcteam3636.frc2024.Robot import com.frcteam3636.frc2024.TalonFX import com.frcteam3636.frc2024.utils.math.* @@ -20,52 +20,26 @@ import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim import org.littletonrobotics.junction.LogTable import org.littletonrobotics.junction.Logger import org.littletonrobotics.junction.inputs.LoggableInputs +import org.team9432.annotation.Logged +@Logged +open class ArmInputs { + var rightPosition = Radians.zero()!! + var leftPosition = Radians.zero()!! + var position = Radians.zero()!! -interface ArmIO{ - class ArmInputs : LoggableInputs { - - var rightPosition = Radians.zero()!! - var leftPosition = Radians.zero()!! - var position = Radians.zero()!! - - var absoluteEncoderPosition = Radians.zero()!! - - var rightCurrent = Volts.zero()!! - var leftCurrent = Volts.zero()!! + var absoluteEncoderPosition = Radians.zero()!! - var rightVelocity = RadiansPerSecond.zero()!! - var leftVelocity = RadiansPerSecond.zero()!! + var rightCurrent = Volts.zero()!! + var leftCurrent = Volts.zero()!! - var absoluteEncoderConnected = false + var rightVelocity = RadiansPerSecond.zero()!! + var leftVelocity = RadiansPerSecond.zero()!! + var absoluteEncoderConnected = false +} - override fun toLog(table: LogTable) { - table.put("Right Arm Motor Position", rightPosition) - table.put("Left Arm Motor Position", leftPosition) - - table.put("Right Arm Motor Current", rightCurrent) - table.put("Left Arm Motor Current", leftCurrent) - - table.put("Right Arm Motor Velocity", rightVelocity) - table.put("Left Arm Motor Velocity", leftVelocity) - - table.put("Absolute Encoder Connected",absoluteEncoderConnected ) - } - - override fun fromLog(table: LogTable) { - rightPosition = table.get("Right Arm Motor Position", rightPosition) - leftPosition = table.get("Left Arm Motor Position", leftPosition) - - rightCurrent = table.get("Right Arm Motor Current", rightCurrent) - leftCurrent = table.get("Left Arm Motor Current", leftCurrent) - - rightVelocity = table.get("Right Arm Motor Velocity", rightVelocity) - leftVelocity = table.get("Left Arm Motor Velocity", leftVelocity) - - absoluteEncoderConnected = table.get("Absolute Encoder Connected", absoluteEncoderConnected) - } - } +interface ArmIO{ fun updateInputs(inputs: ArmInputs) @@ -79,13 +53,13 @@ interface ArmIO{ class ArmIOReal: ArmIO{ - private val leftMotor = TalonFX(CTREMotorControllerId.LeftArmMotor) + private val leftMotor = TalonFX(CTREDeviceId.LeftArmMotor) - private val rightMotor = TalonFX(CTREMotorControllerId.RightArmMotor) + private val rightMotor = TalonFX(CTREDeviceId.RightArmMotor) private val absoluteEncoder = DutyCycleEncoder(DigitalInput(8)) - override fun updateInputs(inputs: ArmIO.ArmInputs) { + override fun updateInputs(inputs: ArmInputs) { inputs.position = Rotations.of(absoluteEncoder.absolutePosition) inputs.absoluteEncoderConnected = absoluteEncoder.isConnected @@ -179,7 +153,7 @@ interface ArmIO{ val armSim = SingleJointedArmSim( DCMotor.getKrakenX60(1), 3.0, - 0.0301907551, + 0.244, 0.331414, -0.142921, 2.32814365359, @@ -187,15 +161,16 @@ interface ArmIO{ -0.142921 ) - val pidController = PIDController(ArmIOReal.PID_GAINS) +// val pidController = PIDController(ArmIOReal.PID_GAINS) var setPoint = 0.0 - override fun updateInputs(inputs: ArmIO.ArmInputs) { + override fun updateInputs(inputs: ArmInputs) { armSim.update(Robot.period) inputs.rightVelocity = RadiansPerSecond.of(armSim.velocityRadPerSec) inputs.leftVelocity = RadiansPerSecond.of(armSim.velocityRadPerSec) - val pidVoltage = pidController.calculate(inputs.position.`in`(Radians),setPoint) - armSim.setInputVoltage(pidVoltage) + inputs.position = Radians.of(armSim.angleRads) +// val pidVoltage = pidController.calculate(inputs.position.`in`(Radians),setPoint) +// armSim.setInputVoltage(pidVoltage) } override fun pivotToPosition(position: Measure) { @@ -204,6 +179,7 @@ interface ArmIO{ override fun setVoltage(volts: Measure) { armSim.setInputVoltage(volts.`in`(Volts)) + Logger.recordOutput("/Arm/OutVolt", volts) } override fun updatePosition(position: Measure) { @@ -213,7 +189,7 @@ interface ArmIO{ class ArmIOPrototype: ArmIO { //placeholder - override fun updateInputs(inputs: ArmIO.ArmInputs) { + override fun updateInputs(inputs: ArmInputs) { } override fun pivotToPosition(position: Measure) { diff --git a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/drivetrain/Drivetrain.kt b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/drivetrain/Drivetrain.kt index f7e2bac..ae592f2 100644 --- a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/drivetrain/Drivetrain.kt +++ b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/drivetrain/Drivetrain.kt @@ -1,6 +1,6 @@ package com.frcteam3636.frc2024.subsystems.drivetrain -import com.frcteam3636.frc2024.CTREMotorControllerId +import com.frcteam3636.frc2024.CTREDeviceId import com.frcteam3636.frc2024.REVMotorControllerId import com.frcteam3636.frc2024.Robot import com.frcteam3636.frc2024.subsystems.drivetrain.Drivetrain.Constants.BRAKE_POSITION @@ -336,22 +336,22 @@ object Drivetrain : Subsystem, Sendable { PerCorner( frontLeft = Pair( - CTREMotorControllerId.FrontLeftDrivingMotor, + CTREDeviceId.FrontLeftDrivingMotor, REVMotorControllerId.FrontLeftTurningMotor ), frontRight = Pair( - CTREMotorControllerId.FrontRightDrivingMotor, + CTREDeviceId.FrontRightDrivingMotor, REVMotorControllerId.FrontRightTurningMotor ), backRight = Pair( - CTREMotorControllerId.BackRightDrivingMotor, + CTREDeviceId.BackRightDrivingMotor, REVMotorControllerId.BackRightTurningMotor ), backLeft = Pair( - CTREMotorControllerId.BackLeftDrivingMotor, + CTREDeviceId.BackLeftDrivingMotor, REVMotorControllerId.BackLeftTurningMotor ), ) diff --git a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/drivetrain/DrivetrainIO.kt b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/drivetrain/DrivetrainIO.kt index 88d9d77..f57ac10 100644 --- a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/drivetrain/DrivetrainIO.kt +++ b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/drivetrain/DrivetrainIO.kt @@ -1,7 +1,8 @@ package com.frcteam3636.frc2024.subsystems.drivetrain +import com.frcteam3636.frc2024.CTREDeviceId +import com.frcteam3636.frc2024.Pigeon2 import com.frcteam3636.frc2024.utils.swerve.PerCorner -import com.kauailabs.navx.frc.AHRS import edu.wpi.first.math.geometry.Rotation3d import edu.wpi.first.math.kinematics.SwerveModulePosition import edu.wpi.first.math.kinematics.SwerveModuleState @@ -44,7 +45,7 @@ abstract class DrivetrainIO { /** Drivetrain I/O layer that uses real swerve modules along with a NavX gyro. */ class DrivetrainIOReal(override val modules: PerCorner) : DrivetrainIO() { - override val gyro = GyroNavX(AHRS()) + override val gyro = GyroPigeon(Pigeon2(CTREDeviceId.PigeonGyro)) companion object { fun fromKrakenSwerve() = diff --git a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/drivetrain/Gyro.kt b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/drivetrain/Gyro.kt index b7fa865..ba8a973 100644 --- a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/drivetrain/Gyro.kt +++ b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/drivetrain/Gyro.kt @@ -1,5 +1,8 @@ package com.frcteam3636.frc2024.subsystems.drivetrain +import com.ctre.phoenix6.BaseStatusSignal +import com.ctre.phoenix6.configs.Pigeon2Configuration +import com.ctre.phoenix6.hardware.Pigeon2 import com.frcteam3636.frc2024.Robot import com.frcteam3636.frc2024.utils.swerve.PerCorner import com.frcteam3636.frc2024.utils.swerve.translation2dPerSecond @@ -27,20 +30,40 @@ class GyroNavX(private val ahrs: AHRS) : Gyro { private var offset: Rotation3d = Rotation3d() init { - Logger.recordOutput("Gyro/Offset", offset) + Logger.recordOutput("NavXGyro/Offset", offset) } override var rotation: Rotation3d get() = offset + ahrs.rotation3d set(goal) { offset = goal - ahrs.rotation3d - Logger.recordOutput("Gyro/Offset", offset) + Logger.recordOutput("NavXGyro/Offset", offset) } override val connected get() = ahrs.isConnected } +class GyroPigeon(private val pigeon: Pigeon2) : Gyro { + + private var offset: Rotation3d = Rotation3d() + + init { + Logger.recordOutput("PigeonGyro/Offset", offset) + BaseStatusSignal.setUpdateFrequencyForAll(100.0, pigeon.yaw, pigeon.pitch, pigeon.roll); + } + + override var rotation: Rotation3d + get() = offset + pigeon.rotation3d + set(goal) { + offset = goal - pigeon.rotation3d + Logger.recordOutput("PigeonGyro/Offset", offset) + } + + override val connected + get() = pigeon.yaw.status.isOK +} + class GyroSim(private val modules: PerCorner) : Gyro { override var rotation = Rotation3d() override val connected = true diff --git a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/drivetrain/Module.kt b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/drivetrain/Module.kt index e59a4c6..d35af4c 100644 --- a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/drivetrain/Module.kt +++ b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/drivetrain/Module.kt @@ -109,7 +109,7 @@ interface DrivingMotor { var velocity: Double } -class DrivingTalon(id: CTREMotorControllerId) : DrivingMotor { +class DrivingTalon(id: CTREDeviceId) : DrivingMotor { private val inner = TalonFX(id).apply { configurator.apply(Slot0Configs().apply {