diff --git a/.gitignore b/.gitignore index 46631a5..5548bd2 100644 --- a/.gitignore +++ b/.gitignore @@ -186,4 +186,4 @@ compile_commands.json # Eclipse generated file for annotation processors .factorypath -gradle.properties \ No newline at end of file +gradle.properties diff --git a/advantagescope_assets/Robot_Chube/config.json b/advantagescope_assets/Robot_Chube/config.json new file mode 100644 index 0000000..b7238bc --- /dev/null +++ b/advantagescope_assets/Robot_Chube/config.json @@ -0,0 +1,51 @@ +{ + "name": "Chube", + "disableSimplification": false, + "rotations": [ + { + "axis": "x", + "degrees": 90 + }, + { + "axis": "z", + "degrees": 90 + } + ], + "position": [], + "cameras": [], + "components": [ + { + "zeroedRotations": [ + { + "axis": "x", + "degrees": 90 + }, + { + "axis": "z", + "degrees": 90 + } + ], + "zeroedPosition": [ + 0, + 0, + 0 + ] + }, + { + "zeroedRotations": [], + "zeroedPosition": [ + 0, + 0, + 0 + ] + }, + { + "zeroedRotations": [], + "zeroedPosition": [ + 0, + 0, + 0 + ] + } + ] +} \ No newline at end of file diff --git a/advantagescope_assets/Robot_Chube/model.glb b/advantagescope_assets/Robot_Chube/model.glb new file mode 100644 index 0000000..bc05d3a Binary files /dev/null and b/advantagescope_assets/Robot_Chube/model.glb differ diff --git a/advantagescope_assets/Robot_Chube/model_0.glb b/advantagescope_assets/Robot_Chube/model_0.glb new file mode 100644 index 0000000..5b81cc4 Binary files /dev/null and b/advantagescope_assets/Robot_Chube/model_0.glb differ diff --git a/advantagescope_assets/Robot_Chube/model_1.glb b/advantagescope_assets/Robot_Chube/model_1.glb new file mode 100644 index 0000000..894ab33 Binary files /dev/null and b/advantagescope_assets/Robot_Chube/model_1.glb differ diff --git a/advantagescope_assets/Robot_Chube/model_2.glb b/advantagescope_assets/Robot_Chube/model_2.glb new file mode 100644 index 0000000..7755aa3 Binary files /dev/null and b/advantagescope_assets/Robot_Chube/model_2.glb differ diff --git a/gradle.properties b/gradle.properties new file mode 100644 index 0000000..bb3db05 --- /dev/null +++ b/gradle.properties @@ -0,0 +1 @@ +org.gradle.java.home=C:\\Users\\Public\\wpilib\\2025\\jdk \ No newline at end of file diff --git a/simgui-ds.json b/simgui-ds.json new file mode 100644 index 0000000..690a4a2 --- /dev/null +++ b/simgui-ds.json @@ -0,0 +1,100 @@ +{ + "keyboardJoysticks": [ + { + "axisConfig": [ + { + "decKey": 65, + "incKey": 68 + }, + { + "decKey": 87, + "incKey": 83 + }, + { + "decKey": 69, + "decayRate": 0.0, + "incKey": 82, + "keyRate": 0.009999999776482582 + } + ], + "axisCount": 3, + "buttonCount": 4, + "buttonKeys": [ + 90, + 88, + 67, + 86 + ], + "povConfig": [ + { + "key0": 328, + "key135": 323, + "key180": 322, + "key225": 321, + "key270": 324, + "key315": 327, + "key45": 329, + "key90": 326 + } + ], + "povCount": 1 + }, + { + "axisConfig": [ + { + "decKey": 74, + "incKey": 76 + }, + { + "decKey": 73, + "incKey": 75 + } + ], + "axisCount": 2, + "buttonCount": 4, + "buttonKeys": [ + 77, + 44, + 46, + 47 + ], + "povCount": 0 + }, + { + "axisConfig": [ + { + "decKey": 263, + "incKey": 262 + }, + { + "decKey": 265, + "incKey": 264 + } + ], + "axisCount": 2, + "buttonCount": 6, + "buttonKeys": [ + 260, + 268, + 266, + 261, + 269, + 267 + ], + "povCount": 0 + }, + { + "axisCount": 0, + "buttonCount": 0, + "povCount": 0 + } + ], + "robotJoysticks": [ + {}, + {}, + { + "guid": "78696e70757401000000000000000000", + "useGamepad": true + } + ] +} diff --git a/src/main/java/frc/robot/Robot.kt b/src/main/java/frc/robot/Robot.kt index 64673ba..0d6abf9 100644 --- a/src/main/java/frc/robot/Robot.kt +++ b/src/main/java/frc/robot/Robot.kt @@ -5,6 +5,10 @@ import edu.wpi.first.hal.FRCNetComm.tInstances import edu.wpi.first.hal.FRCNetComm.tResourceType import edu.wpi.first.hal.HAL import edu.wpi.first.math.MathUtil +import edu.wpi.first.math.geometry.Pose2d +import edu.wpi.first.math.geometry.Pose3d +import edu.wpi.first.networktables.NetworkTableInstance +import edu.wpi.first.wpilibj.RobotController import edu.wpi.first.wpilibj.TimedRobot import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard @@ -12,10 +16,6 @@ import edu.wpi.first.wpilibj.util.WPILibVersion import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.CommandScheduler import edu.wpi.first.wpilibj2.command.Commands -import frc.robot.RobotContainer.ControlledAngularRate -import frc.robot.RobotContainer.ControlledSpeed -import frc.robot.RobotContainer.MaxAngularRateConst -import frc.robot.RobotContainer.MaxSpeedConst import frc.robot.RobotContainer.leftJoystick import frc.robot.RobotContainer.vision @@ -36,6 +36,11 @@ object Robot : TimedRobot() { *the AutoChooser on the dashboard. */ private var autonomousCommand: Command = Commands.runOnce({}) + val robotPosePublisher = NetworkTableInstance.getDefault().getStructTopic("Robot Pose", Pose2d.struct).publish(); + val elevatorPosePublisher = + NetworkTableInstance.getDefault().getStructTopic("Elevator Pose", Pose3d.struct).publish(); + val pivotPosePublisher = NetworkTableInstance.getDefault().getStructTopic("Pivot Pose", Pose3d.struct).publish(); + val wristPosePublisher = NetworkTableInstance.getDefault().getStructTopic("Wrist Pose", Pose3d.struct).publish(); /** @@ -66,6 +71,13 @@ object Robot : TimedRobot() { // and running subsystem periodic() methods. This must be called from the robot's periodic // block in order for anything in the Command-based framework to work. CommandScheduler.getInstance().run() + + robotPosePublisher.set(RobotContainer.drivetrain.getSwervePose()) + elevatorPosePublisher.set(Pose3d()) + pivotPosePublisher.set(Pose3d()) + wristPosePublisher.set(Pose3d()) + + SmartDashboard.putNumberArray("Test", arrayOf(0.0, 0.0, 0.0)) } /** This method is called once each time the robot enters Disabled mode. */ @@ -108,13 +120,20 @@ object Robot : TimedRobot() { } +// var lastLoopTime = 0.0 + /** This method is called once when the robot is first started up. */ override fun simulationInit() { - +// lastLoopTime = MiscCalculations.getCurrentTime() } /** This method is called periodically whilst in simulation. */ override fun simulationPeriodic() { - +// val currentTime = MiscCalculations.getCurrentTime() +// val dtSeconds = (currentTime - lastLoopTime) / 1000 +// +// RobotContainer.drivetrain.updateSimState(dtSeconds, RobotController.getBatteryVoltage()) +// +// lastLoopTime = currentTime } } diff --git a/src/main/java/frc/robot/RobotConfiguration.kt b/src/main/java/frc/robot/RobotConfiguration.kt new file mode 100644 index 0000000..56d10cd --- /dev/null +++ b/src/main/java/frc/robot/RobotConfiguration.kt @@ -0,0 +1,11 @@ +package frc.robot + +enum class RobotState { + Real, + Simulated, + Empty +} + +object RobotConfiguration { + val robotState = RobotState.Empty +} \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.kt b/src/main/java/frc/robot/RobotContainer.kt index b1f05bd..18c986c 100644 --- a/src/main/java/frc/robot/RobotContainer.kt +++ b/src/main/java/frc/robot/RobotContainer.kt @@ -8,57 +8,45 @@ import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.Commands import edu.wpi.first.wpilibj2.command.FunctionalCommand import edu.wpi.first.wpilibj2.command.button.CommandJoystick +import edu.wpi.first.wpilibj2.command.button.CommandXboxController +import frc.robot.commands.SimTeleopDriveCommand import frc.robot.commands.TeleopDriveCommand -import frc.robot.constants.TunerConstants -import frc.robot.subsystems.CommandSwerveDrivetrain +import frc.robot.subsystems.swerve.TunerConstants +import frc.robot.subsystems.swerve.CommandSwerveDrivetrain +import frc.robot.subsystems.swerve.SwerveIOBase +import frc.robot.subsystems.swerve.SwerveIOReal +import frc.robot.subsystems.swerve.SwerveIOSim +import frc.robot.util.Telemetry object RobotContainer { - public val MaxSpeedConst = TunerConstants.kSpeedAt12Volts.`in`(MetersPerSecond) // kSpeedAt12Volts desired top - // speed - public var ControlledSpeed = MaxSpeedConst - public val MaxAngularRateConst = - RotationsPerSecond.of(0.75).`in`(RadiansPerSecond) // 3/4 of a rotation per second max angular velocity - - public var ControlledAngularRate = MaxAngularRateConst - - /* Setting up bindings for necessary control of the swerve drive platform */ - public val drive: SwerveRequest.FieldCentric = SwerveRequest.FieldCentric() - .withDeadband(MaxSpeedConst * 0.1).withRotationalDeadband(MaxAngularRateConst * 0.1) // Add a 10% deadband - .withDriveRequestType(DriveRequestType.OpenLoopVoltage) // Use open-loop control for drive motors - - public val robotRelative: SwerveRequest.RobotCentric = SwerveRequest.RobotCentric() - .withDeadband(MaxSpeedConst * 0.1).withRotationalDeadband(MaxAngularRateConst * 0.1) // Add a 10% deadband - .withDriveRequestType(DriveRequestType.OpenLoopVoltage) // Use open-loop control for drive motors - - - private val brake = SwerveRequest.SwerveDriveBrake() - private val point = SwerveRequest.PointWheelsAt() - - private val logger: Telemetry = Telemetry(MaxSpeedConst) - -// private val xbox = CommandXboxController(0) - val leftJoystick: CommandJoystick = CommandJoystick(0) val rightJoystick: CommandJoystick = CommandJoystick(1) - val vision = VisionSystem() + val xbox = CommandXboxController(2) + val vision = VisionSystem() - val drivetrain: CommandSwerveDrivetrain = TunerConstants.createDrivetrain() + val drivetrain = when (RobotConfiguration.robotState) { + RobotState.Real -> SwerveIOReal() + RobotState.Simulated -> SwerveIOSim() + RobotState.Empty -> SwerveIOBase() + } - val teleopDriveCommand = TeleopDriveCommand() + val teleopDriveCommand = when (RobotConfiguration.robotState) { + RobotState.Real -> TeleopDriveCommand() + RobotState.Simulated -> SimTeleopDriveCommand() + RobotState.Empty -> Commands.run({}) + } init { configureBindings() } private fun configureBindings() { - drivetrain.setDefaultCommand(teleopDriveCommand) - drivetrain.registerTelemetry(logger::telemeterize) -// teleopDriveCommand.schedule() + drivetrain.registerTelemetry(logger::telemeterize) } val autonomousCommand: Command diff --git a/src/main/java/frc/robot/Vision.kt b/src/main/java/frc/robot/Vision.kt index 8d150d0..a77f01f 100644 --- a/src/main/java/frc/robot/Vision.kt +++ b/src/main/java/frc/robot/Vision.kt @@ -18,11 +18,12 @@ import kotlin.math.max class VisionSystem { val max_distance_m = 6.0 - val limelightNames: Array = - arrayOf("limelight-front") + val limelightNames: Array = when (RobotConfiguration.robotState) { + RobotState.Real -> arrayOf("limelight-front") + else -> emptyArray() + } fun updateOdometryFromDisabled() { - var namesToSearch: Array; @@ -41,7 +42,7 @@ class VisionSystem { if (llMeasure.pose.x != 0.0 && llMeasure.pose.y != 0.0) { val poseDifference = - llMeasure.pose.translation.getDistance(RobotContainer.drivetrain.state.Pose.translation) + llMeasure.pose.translation.getDistance(RobotContainer.drivetrain.getSwervePose().translation) val distanceToTag = llMeasure.avgTagDist @@ -81,7 +82,7 @@ class VisionSystem { var namesToSearch: Array; - namesToSearch = limelightNames + namesToSearch = limelightNames for (llName in namesToSearch) { if (DriverStation.getAlliance().isEmpty) { @@ -90,11 +91,11 @@ class VisionSystem { } var llMeasure: LimelightHelpers.PoseEstimate = - LimelightHelpers.getBotPoseEstimate_wpiBlue(llName) + LimelightHelpers.getBotPoseEstimate_wpiBlue(llName) if (llMeasure.tagCount >= tagCount && llMeasure.pose.x != 0.0 && llMeasure.pose.y != 0.0) { val poseDifference = - llMeasure.pose.translation.getDistance(RobotContainer.drivetrain.state.Pose.translation) + llMeasure.pose.translation.getDistance(RobotContainer.drivetrain.getSwervePose().translation) if (!poseDifferenceCheck || poseDifference < max_distance_m) { val distanceToTag = llMeasure.avgTagDist diff --git a/src/main/java/frc/robot/commands/SimTeleopDriveCommand.kt b/src/main/java/frc/robot/commands/SimTeleopDriveCommand.kt new file mode 100644 index 0000000..4e1a437 --- /dev/null +++ b/src/main/java/frc/robot/commands/SimTeleopDriveCommand.kt @@ -0,0 +1,73 @@ +package frc.robot.commands + +import MiscCalculations +import edu.wpi.first.math.MathUtil +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard +import edu.wpi.first.wpilibj2.command.Command +import frc.robot.RobotContainer +import frc.robot.subsystems.ExampleSubsystem.runOnce +import frc.robot.subsystems.swerve.SwerveConstants + + +class SimTeleopDriveCommand : Command() { + init { + addRequirements(RobotContainer.drivetrain) + } + + override fun initialize() { + RobotContainer.xbox.start() + .onTrue(runOnce { RobotContainer.drivetrain.seedFieldCentric() }) + + super.initialize() + } + + override fun execute() { + val deadzonedLeftY = MiscCalculations.calculateDeadzone(RobotContainer.xbox.rightX, .5) + + SwerveConstants.ControlledSpeed = MathUtil.clamp( + SwerveConstants.ControlledSpeed + (-deadzonedLeftY * SwerveConstants.MaxSpeedConst * .02), 0.0, + SwerveConstants.MaxSpeedConst + ); + SwerveConstants.ControlledAngularRate = MathUtil.clamp( + SwerveConstants.ControlledAngularRate + (-deadzonedLeftY * SwerveConstants.MaxAngularRateConst * .02), + 0.0, SwerveConstants.MaxAngularRateConst + ); + + SmartDashboard.putNumber("ControlledSpeed", SwerveConstants.ControlledSpeed) + SmartDashboard.putNumber("ControlledAngularRate", SwerveConstants.ControlledAngularRate) + + + val fieldOriented = !RobotContainer.xbox.a().asBoolean + + if (fieldOriented) { + RobotContainer.drivetrain.applyDriveRequest( + -MiscCalculations.calculateDeadzone( + RobotContainer.xbox.leftY, + .1 + ) * SwerveConstants.ControlledSpeed, -MiscCalculations.calculateDeadzone( + RobotContainer.xbox.leftX, + .1 + ) * SwerveConstants.ControlledSpeed, -MiscCalculations.calculateDeadzone( + RobotContainer.xbox.rightX, + .1 + ) * SwerveConstants.ControlledAngularRate + ) + } else { + RobotContainer.drivetrain.applyRobotRelativeDriveRequest( + -MiscCalculations.calculateDeadzone( + RobotContainer.xbox.leftY, + .1 + ) * SwerveConstants.ControlledSpeed, -MiscCalculations.calculateDeadzone( + RobotContainer.xbox.leftX, + .1 + ) * SwerveConstants.ControlledSpeed, -MiscCalculations.calculateDeadzone( + RobotContainer.xbox.rightX, + .1 + ) * SwerveConstants.ControlledAngularRate + ) + + } + + super.execute() + } +} diff --git a/src/main/java/frc/robot/commands/TeleopDriveCommand.kt b/src/main/java/frc/robot/commands/TeleopDriveCommand.kt index cf279f2..3fa5b09 100644 --- a/src/main/java/frc/robot/commands/TeleopDriveCommand.kt +++ b/src/main/java/frc/robot/commands/TeleopDriveCommand.kt @@ -5,103 +5,69 @@ import edu.wpi.first.math.MathUtil import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard import edu.wpi.first.wpilibj2.command.Command import frc.robot.RobotContainer -import frc.robot.RobotContainer.ControlledAngularRate -import frc.robot.RobotContainer.ControlledSpeed -import frc.robot.RobotContainer.MaxAngularRateConst -import frc.robot.RobotContainer.MaxSpeedConst -import frc.robot.RobotContainer.drive -import frc.robot.RobotContainer.drivetrain -import frc.robot.RobotContainer.leftJoystick -import frc.robot.RobotContainer.rightJoystick -import frc.robot.RobotContainer.robotRelative +import frc.robot.subsystems.ExampleSubsystem.runOnce +import frc.robot.subsystems.swerve.SwerveConstants + class TeleopDriveCommand : Command() { init { - addRequirements(drivetrain) + addRequirements(RobotContainer.drivetrain) } override fun initialize() { - rightJoystick.button(2).onTrue(drivetrain.runOnce { drivetrain.seedFieldCentric() }) + RobotContainer.rightJoystick.button(2) + .onTrue(runOnce { RobotContainer.drivetrain.seedFieldCentric() }) super.initialize() } override fun execute() { - var deadzonedLeftY = MiscCalculations.calculateDeadzone(leftJoystick.y, .5) + val deadzonedLeftY = MiscCalculations.calculateDeadzone(RobotContainer.leftJoystick.y, .5) - ControlledSpeed = MathUtil.clamp( - ControlledSpeed + (-deadzonedLeftY * MaxSpeedConst * .02), 0.0, - MaxSpeedConst + SwerveConstants.ControlledSpeed = MathUtil.clamp( + SwerveConstants.ControlledSpeed + (-deadzonedLeftY * SwerveConstants.MaxSpeedConst * .02), 0.0, + SwerveConstants.MaxSpeedConst ); - ControlledAngularRate = MathUtil.clamp( - ControlledAngularRate + (-deadzonedLeftY * MaxAngularRateConst * .02), - 0.0, MaxAngularRateConst + SwerveConstants.ControlledAngularRate = MathUtil.clamp( + SwerveConstants.ControlledAngularRate + (-deadzonedLeftY * SwerveConstants.MaxAngularRateConst * .02), + 0.0, SwerveConstants.MaxAngularRateConst ); - SmartDashboard.putNumber("ControlledSpeed", ControlledSpeed) - SmartDashboard.putNumber("ControlledAngularRate", ControlledAngularRate) - SmartDashboard.putNumber("Right Joy Y", rightJoystick.y) - SmartDashboard.putNumber("Drive X", -MiscCalculations.calculateDeadzone( - rightJoystick.y, - .1 - ) * ControlledSpeed) + SmartDashboard.putNumber("ControlledSpeed", SwerveConstants.ControlledSpeed) + SmartDashboard.putNumber("ControlledAngularRate", SwerveConstants.ControlledAngularRate) - val fieldOriented = !rightJoystick.button(2).asBoolean + val fieldOriented = !RobotContainer.rightJoystick.button(2).asBoolean if (fieldOriented) { - drivetrain.applyRequest { - drive.withVelocityX( - -MiscCalculations.calculateDeadzone( - rightJoystick.y, - .1 - ) * ControlledSpeed - ) // - // Drive forward with - // negative Y (forward) - .withVelocityY( - -MiscCalculations.calculateDeadzone( - rightJoystick.x, - .1 - ) * ControlledSpeed - ) // Drive - // left with negative X - // (left) - .withRotationalRate( - -MiscCalculations.calculateDeadzone( - leftJoystick.x, - .1 - ) * ControlledAngularRate - ) - }.execute() + + RobotContainer.drivetrain.applyDriveRequest( + -MiscCalculations.calculateDeadzone( + RobotContainer.rightJoystick.y, + .1 + ) * SwerveConstants.ControlledSpeed, -MiscCalculations.calculateDeadzone( + RobotContainer.rightJoystick.x, + .1 + ) * SwerveConstants.ControlledSpeed, -MiscCalculations.calculateDeadzone( + RobotContainer.leftJoystick.x, + .1 + ) * SwerveConstants.ControlledAngularRate + ) } else { - drivetrain.applyRequest { - robotRelative.withVelocityX( - -MiscCalculations.calculateDeadzone( - rightJoystick.y, - .1 - ) * ControlledSpeed - ) // - // Drive forward with - // negative Y (forward) - .withVelocityY( - -MiscCalculations.calculateDeadzone( - rightJoystick.x, - .1 - ) * ControlledSpeed - ) // Drive - // left with negative X - // (left) - .withRotationalRate( - -MiscCalculations.calculateDeadzone( - leftJoystick.x, - .1 - ) * ControlledAngularRate - ) - }.execute() + RobotContainer.drivetrain.applyRobotRelativeDriveRequest( + -MiscCalculations.calculateDeadzone( + RobotContainer.rightJoystick.y, + .1 + ) * SwerveConstants.ControlledSpeed, -MiscCalculations.calculateDeadzone( + RobotContainer.rightJoystick.x, + .1 + ) * SwerveConstants.ControlledSpeed, -MiscCalculations.calculateDeadzone( + RobotContainer.leftJoystick.x, + .1 + ) * SwerveConstants.ControlledAngularRate + ) } - super.execute() } } diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.kt b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.kt deleted file mode 100644 index fb8756c..0000000 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.kt +++ /dev/null @@ -1,282 +0,0 @@ -package frc.robot.subsystems - -import com.ctre.phoenix6.SignalLogger -import com.ctre.phoenix6.Utils -import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants -import com.ctre.phoenix6.swerve.SwerveModuleConstants -import com.ctre.phoenix6.swerve.SwerveRequest -import edu.wpi.first.math.Matrix -import edu.wpi.first.math.geometry.Rotation2d -import edu.wpi.first.math.numbers.N1 -import edu.wpi.first.math.numbers.N3 -import edu.wpi.first.units.Units -import edu.wpi.first.units.measure.Voltage -import edu.wpi.first.wpilibj.DriverStation -import edu.wpi.first.wpilibj.DriverStation.Alliance -import edu.wpi.first.wpilibj.Notifier -import edu.wpi.first.wpilibj.RobotController -import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog -import edu.wpi.first.wpilibj2.command.Command -import edu.wpi.first.wpilibj2.command.Subsystem -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Mechanism - import frc.robot.constants.TunerConstants.TunerSwerveDrivetrain -import java.util.function.Supplier - -/** - * Class that extends the Phoenix 6 SwerveDrivetrain class and implements - * Subsystem so it can easily be used in command-based projects. - */ - class CommandSwerveDrivetrain : TunerSwerveDrivetrain, Subsystem { - private var m_simNotifier: Notifier? = null - private var m_lastSimTime = 0.0 - - /* Keep track if we've ever applied the operator perspective before or not */ - private var m_hasAppliedOperatorPerspective = false - - /* Swerve requests to apply during SysId characterization */ - private val m_translationCharacterization = SwerveRequest.SysIdSwerveTranslation() - private val m_steerCharacterization = SwerveRequest.SysIdSwerveSteerGains() - private val m_rotationCharacterization = SwerveRequest.SysIdSwerveRotation() - - /* SysId routine for characterizing translation. This is used to find PID gains for the drive motors. */ - private val m_sysIdRoutineTranslation = SysIdRoutine( - SysIdRoutine.Config( - null, // Use default ramp rate (1 V/s) - Units.Volts.of(4.0), // Reduce dynamic step voltage to 4 V to prevent brownout - null - ) // Use default timeout (10 s) - // Log state with SignalLogger class - { state: SysIdRoutineLog.State -> - SignalLogger.writeString( - "SysIdTranslation_State", - state.toString() - ) - }, - Mechanism( - { output: Voltage? -> - setControl( - m_translationCharacterization.withVolts( - output - ) - ) - }, - null, - this - ) - ) - - /* SysId routine for characterizing steer. This is used to find PID gains for the steer motors. */ - private val m_sysIdRoutineSteer = SysIdRoutine( - SysIdRoutine.Config( - null, // Use default ramp rate (1 V/s) - Units.Volts.of(7.0), // Use dynamic voltage of 7 V - null - ) // Use default timeout (10 s) - // Log state with SignalLogger class - { state: SysIdRoutineLog.State -> - SignalLogger.writeString( - "SysIdSteer_State", - state.toString() - ) - }, - Mechanism( - { volts: Voltage? -> setControl(m_steerCharacterization.withVolts(volts)) }, - null, - this - ) - ) - - /* - * SysId routine for characterizing rotation. - * This is used to find PID gains for the FieldCentricFacingAngle HeadingController. - * See the documentation of SwerveRequest.SysIdSwerveRotation for info on importing the log to SysId. - */ - private val m_sysIdRoutineRotation = SysIdRoutine( - SysIdRoutine.Config( /* This is in radians per second², but SysId only supports "volts per second" */ - Units.Volts.of(Math.PI / 6) - .per(Units.Second), /* This is in radians per second, but SysId only supports "volts" */ - Units.Volts.of(Math.PI), - null - ) // Use default timeout (10 s) - // Log state with SignalLogger class - { state: SysIdRoutineLog.State -> - SignalLogger.writeString( - "SysIdRotation_State", - state.toString() - ) - }, - Mechanism( - { output: Voltage -> - /* output is actually radians per second, but SysId only supports "volts" */ - setControl(m_rotationCharacterization.withRotationalRate(output.`in`(Units.Volts))) - /* also log the requested output for SysId */ - SignalLogger.writeDouble("Rotational_Rate", output.`in`(Units.Volts)) - }, - null, - this - ) - ) - - /* The SysId routine to test */ - private val m_sysIdRoutineToApply = m_sysIdRoutineTranslation - - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - * - * - * This constructs the underlying hardware devices, so users should not construct - * the devices themselves. If they need the devices, they can access them through - * getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param modules Constants for each specific module - */ - constructor( - drivetrainConstants: SwerveDrivetrainConstants?, - vararg modules: SwerveModuleConstants<*, *, *>? - ) : super(drivetrainConstants, *modules) { - if (Utils.isSimulation()) { - startSimThread() - } - } - - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - * - * - * This constructs the underlying hardware devices, so users should not construct - * the devices themselves. If they need the devices, they can access them through - * getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param odometryUpdateFrequency The frequency to run the odometry loop. If - * unspecified or set to 0 Hz, this is 250 Hz on - * CAN FD, and 100 Hz on CAN 2.0. - * @param modules Constants for each specific module - */ - constructor( - drivetrainConstants: SwerveDrivetrainConstants?, - odometryUpdateFrequency: Double, - vararg modules: SwerveModuleConstants<*, *, *>? - ) : super(drivetrainConstants, odometryUpdateFrequency, *modules) { - if (Utils.isSimulation()) { - startSimThread() - } - } - - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - * - * - * This constructs the underlying hardware devices, so users should not construct - * the devices themselves. If they need the devices, they can access them through - * getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param odometryUpdateFrequency The frequency to run the odometry loop. If - * unspecified or set to 0 Hz, this is 250 Hz on - * CAN FD, and 100 Hz on CAN 2.0. - * @param odometryStandardDeviation The standard deviation for odometry calculation - * in the form [x, y, theta]ᵀ, with units in meters - * and radians - * @param visionStandardDeviation The standard deviation for vision calculation - * in the form [x, y, theta]ᵀ, with units in meters - * and radians - * @param modules Constants for each specific module - */ - constructor( - drivetrainConstants: SwerveDrivetrainConstants?, - odometryUpdateFrequency: Double, - odometryStandardDeviation: Matrix?, - visionStandardDeviation: Matrix?, - vararg modules: SwerveModuleConstants<*, *, *>? - ) : super( - drivetrainConstants, - odometryUpdateFrequency, - odometryStandardDeviation, - visionStandardDeviation, - *modules - ) { - if (Utils.isSimulation()) { - startSimThread() - } - } - - /** - * Returns a command that applies the specified control request to this swerve drivetrain. - * - * @param request Function returning the request to apply - * @return Command to run - */ - fun applyRequest(requestSupplier: Supplier): Command { - return run { this.setControl(requestSupplier.get()) } - } - - /** - * Runs the SysId Quasistatic test in the given direction for the routine - * specified by [.m_sysIdRoutineToApply]. - * - * @param direction Direction of the SysId Quasistatic test - * @return Command to run - */ - fun sysIdQuasistatic(direction: SysIdRoutine.Direction?): Command { - return m_sysIdRoutineToApply.quasistatic(direction) - } - - /** - * Runs the SysId Dynamic test in the given direction for the routine - * specified by [.m_sysIdRoutineToApply]. - * - * @param direction Direction of the SysId Dynamic test - * @return Command to run - */ - fun sysIdDynamic(direction: SysIdRoutine.Direction?): Command { - return m_sysIdRoutineToApply.dynamic(direction) - } - - override fun periodic() { - /* - * Periodically try to apply the operator perspective. - * If we haven't applied the operator perspective before, then we should apply it regardless of DS state. - * This allows us to correct the perspective in case the robot code restarts mid-match. - * Otherwise, only check and apply the operator perspective if the DS is disabled. - * This ensures driving behavior doesn't change until an explicit disable event occurs during testing. - */ - if (!m_hasAppliedOperatorPerspective || DriverStation.isDisabled()) { - DriverStation.getAlliance().ifPresent { allianceColor: Alliance -> - setOperatorPerspectiveForward( - if (allianceColor == Alliance.Red - ) kRedAlliancePerspectiveRotation - else kBlueAlliancePerspectiveRotation - ) - m_hasAppliedOperatorPerspective = true - } - } - } - - private fun startSimThread() { - m_lastSimTime = Utils.getCurrentTimeSeconds() - - /* Run simulation at a faster rate so PID gains behave more reasonably */ - m_simNotifier = Notifier { - val currentTime = Utils.getCurrentTimeSeconds() - val deltaTime = currentTime - m_lastSimTime - m_lastSimTime = currentTime - - /* use the measured time delta, get battery voltage from WPILib */ - updateSimState(deltaTime, RobotController.getBatteryVoltage()) - } - m_simNotifier!!.startPeriodic(kSimLoopPeriod) - } - - companion object { - private const val kSimLoopPeriod = 0.005 // 5 ms - - /* Blue alliance sees forward as 0 degrees (toward red alliance wall) */ - private val kBlueAlliancePerspectiveRotation: Rotation2d = Rotation2d.kZero - - /* Red alliance sees forward as 180 degrees (toward blue alliance wall) */ - private val kRedAlliancePerspectiveRotation: Rotation2d = Rotation2d.k180deg - } - } diff --git a/src/main/java/frc/robot/subsystems/Superstructure.kt b/src/main/java/frc/robot/subsystems/Superstructure.kt new file mode 100644 index 0000000..0099246 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Superstructure.kt @@ -0,0 +1,18 @@ +package frc.robot.subsystems + +import edu.wpi.first.wpilibj2.command.SubsystemBase +import frc.robot.subsystems.elevator.ElevatorSystem +import frc.robot.subsystems.elevator.implementation.ElevatorIOEmpty +import frc.robot.subsystems.intake.IntakeSystem +import frc.robot.subsystems.intake.implementation.IntakeIOEmpty +import frc.robot.subsystems.pivot.PivotSystem +import frc.robot.subsystems.pivot.implementation.PivotIOEmpty +import frc.robot.subsystems.wrist.WristSystem +import frc.robot.subsystems.wrist.implementation.WristIOEmpty + +class Superstructure() : SubsystemBase() { + val pivotSystem = PivotSystem(PivotIOEmpty()) + val wristSystem = WristSystem(WristIOEmpty()) + val elevatorSystem = ElevatorSystem(ElevatorIOEmpty()) + val intakeSystem = IntakeSystem(IntakeIOEmpty()) +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.kt b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.kt new file mode 100644 index 0000000..587baf7 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.kt @@ -0,0 +1,16 @@ +package frc.robot.subsystems.elevator + +object ElevatorConstants { + val kG = 0.0 + val kS = 0.0 + val kV = 0.0 + val kA = 0.0 + val kP = 0.0 + val kI = 0.0 + val kD = 0.0 + val targetVelocity = 0.0 + val targetAcceleration = 0.0 + val targetJerk = 0.0 + val id = 0 + val canbus = "" +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.kt b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.kt new file mode 100644 index 0000000..38e6995 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.kt @@ -0,0 +1,11 @@ +package frc.robot.subsystems.elevator + +import com.ctre.phoenix6.configs.TalonFXConfiguration +import com.ctre.phoenix6.hardware.TalonFX + +interface ElevatorIO { +// fun getElevatorEncoder(): Double + + // fun setElevatorMotor(rotations: Double) + fun setPosition(positionMeters: Double) +} diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorSystem.kt b/src/main/java/frc/robot/subsystems/elevator/ElevatorSystem.kt new file mode 100644 index 0000000..b6a83d8 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorSystem.kt @@ -0,0 +1,20 @@ +package frc.robot.subsystems.elevator + +import edu.wpi.first.wpilibj2.command.SubsystemBase +import edu.wpi.first.wpilibj2.command.Command + +// By making a subsystem a Kotlin object, we ensure there is only ever one instance of it. +// It also reduces the need to have reference variables for the subsystems to be passed around. +class ElevatorSystem(private val io: ElevatorIO) : SubsystemBase() { + fun setPosition(positionMeters: Double): Command = runOnce { + io.setPosition(positionMeters) + } + + override fun periodic() { + // This method will be called once per scheduler run + } + + override fun simulationPeriodic() { + // This method will be called once per scheduler run during simulation + } +} diff --git a/src/main/java/frc/robot/subsystems/elevator/implementation/ElevatorIOEmpty.kt b/src/main/java/frc/robot/subsystems/elevator/implementation/ElevatorIOEmpty.kt new file mode 100644 index 0000000..f814777 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/elevator/implementation/ElevatorIOEmpty.kt @@ -0,0 +1,7 @@ +package frc.robot.subsystems.elevator.implementation + +import frc.robot.subsystems.elevator.ElevatorIO + +class ElevatorIOEmpty() : ElevatorIO { + override fun setPosition(positionMeters: Double) {} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/elevator/implementation/ElevatorIOReal.kt b/src/main/java/frc/robot/subsystems/elevator/implementation/ElevatorIOReal.kt new file mode 100644 index 0000000..571a640 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/elevator/implementation/ElevatorIOReal.kt @@ -0,0 +1,44 @@ +package frc.robot.subsystems.elevator.implementation + +import com.ctre.phoenix6.configs.TalonFXConfiguration +import com.ctre.phoenix6.hardware.TalonFX +import com.ctre.phoenix6.controls.MotionMagicVoltage +import frc.robot.subsystems.elevator.ElevatorConstants +import frc.robot.subsystems.elevator.ElevatorIO + +class ElevatorIOReal : ElevatorIO { + + var talonFX = TalonFX(ElevatorConstants.id, ElevatorConstants.canbus) + var talonFXConfigurations = TalonFXConfiguration() + + init { + talonFXConfigurations.Slot0.kG = ElevatorConstants.kG + talonFXConfigurations.Slot0.kS = ElevatorConstants.kS + talonFXConfigurations.Slot0.kV = ElevatorConstants.kV + talonFXConfigurations.Slot0.kA = ElevatorConstants.kA + talonFXConfigurations.Slot0.kP = ElevatorConstants.kP + talonFXConfigurations.Slot0.kI = ElevatorConstants.kI + talonFXConfigurations.Slot0.kD = ElevatorConstants.kD + talonFXConfigurations.MotionMagic.MotionMagicCruiseVelocity = ElevatorConstants.targetVelocity + talonFXConfigurations.MotionMagic.MotionMagicAcceleration = ElevatorConstants.targetAcceleration + talonFXConfigurations.MotionMagic.MotionMagicJerk = ElevatorConstants.targetJerk + talonFX.getConfigurator().apply(talonFXConfigurations) + } + + fun getElevatorEncoder(): Double { + TODO("Not yet implemented") + return 0.0; + } + + fun setElevatorMotor(rotations: Double) { + TODO("Not yet implemented") + } + + fun setMotorPosition(rotations: Double) { + talonFX.setControl(MotionMagicVoltage(rotations)) + } + + override fun setPosition(positionMeters: Double) { + TODO("Not yet implemented") + } +} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.kt b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.kt new file mode 100644 index 0000000..b573eca --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.kt @@ -0,0 +1,16 @@ +package frc.robot.subsystems.intake + +object IntakeConstants { + val kG = 0.0 + val kS = 0.0 + val kV = 0.0 + val kA = 0.0 + val kP = 0.0 + val kI = 0.0 + val kD = 0.0 + val targetVelocity = 0.0 + val targetAcceleration = 0.0 + val targetJerk = 0.0 + val id = 0 + val canbus = "" +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIO.kt b/src/main/java/frc/robot/subsystems/intake/IntakeIO.kt new file mode 100644 index 0000000..81518ba --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIO.kt @@ -0,0 +1,12 @@ +package frc.robot.subsystems.intake + +import com.ctre.phoenix6.configs.TalonFXConfiguration +import com.ctre.phoenix6.hardware.TalonFX + +interface IntakeIO { +// fun getIntakeEncoder(): Double + + // fun setIntakeMotor(x: Double) + fun setCoralIntakeState(state: CoralIntakeState) + fun setAlgaeIntakeState(state: AlgaeIntakeState) +} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeState.kt b/src/main/java/frc/robot/subsystems/intake/IntakeState.kt new file mode 100644 index 0000000..ad28612 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/IntakeState.kt @@ -0,0 +1,13 @@ +package frc.robot.subsystems.intake + +enum class CoralIntakeState { + Intaking, + Idle, + Scoring +} + +enum class AlgaeIntakeState { + Intaking, + Idle, + Scoring +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeSystem.kt b/src/main/java/frc/robot/subsystems/intake/IntakeSystem.kt new file mode 100644 index 0000000..670c2df --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/IntakeSystem.kt @@ -0,0 +1,50 @@ +package frc.robot.subsystems.intake + +import edu.wpi.first.wpilibj2.command.SubsystemBase + +// By making a subsystem a Kotlin object, we ensure there is only ever one instance of it. +// It also reduces the need to have reference variables for the subsystems to be passed around. +class IntakeSystem(private val io: IntakeIO) : SubsystemBase() { + /** + * Example command factory method. + * + * @return a command + */ +// fun exampleMethodCommand(): Command = runOnce { +// // Subsystem.runOnce() implicitly add `this` as a required subsystem. +// // TODO: one-time action goes here +// } + + /** + * An example method querying a boolean state of the subsystem (for example, a digital sensor). + * + * @return value of some boolean subsystem state, such as a digital sensor. + */ +// fun exampleCondition(): Boolean { +// // Query some boolean state, such as a digital sensor. +// return false +// } + + override fun periodic() { + // This method will be called once per scheduler run + } + + override fun simulationPeriodic() { + // This method will be called once per scheduler run during simulation + } + +// fun exampleAction() +// { +// // This action is called by the ExampleCommand +// println("ExampleSubsystem.exampleAction has been called") +// } + + fun getPos(): Double { + TODO("Not yet implemented") + return 0.0; + } + + fun setPos(x: Double) { + TODO("Not yet implemented") + } +} diff --git a/src/main/java/frc/robot/subsystems/intake/implementation/IntakeIOEmpty.kt b/src/main/java/frc/robot/subsystems/intake/implementation/IntakeIOEmpty.kt new file mode 100644 index 0000000..b20f310 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/implementation/IntakeIOEmpty.kt @@ -0,0 +1,10 @@ +package frc.robot.subsystems.intake.implementation + +import frc.robot.subsystems.intake.AlgaeIntakeState +import frc.robot.subsystems.intake.CoralIntakeState +import frc.robot.subsystems.intake.IntakeIO + +class IntakeIOEmpty() : IntakeIO { + override fun setAlgaeIntakeState(state: AlgaeIntakeState) {} + override fun setCoralIntakeState(state: CoralIntakeState) {} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/intake/implementation/IntakeIOReal.kt b/src/main/java/frc/robot/subsystems/intake/implementation/IntakeIOReal.kt new file mode 100644 index 0000000..02df2ba --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/implementation/IntakeIOReal.kt @@ -0,0 +1,46 @@ +package frc.robot.subsystems.intake.implementation + +import com.ctre.phoenix6.configs.TalonFXConfiguration +import com.ctre.phoenix6.hardware.TalonFX +import com.ctre.phoenix6.controls.MotionMagicVoltage +import frc.robot.subsystems.intake.IntakeConstants +import frc.robot.subsystems.intake.AlgaeIntakeState +import frc.robot.subsystems.intake.CoralIntakeState +import frc.robot.subsystems.intake.IntakeIO + +class IntakeIOReal : IntakeIO { + + var talonFX = TalonFX(IntakeConstants.id, IntakeConstants.canbus) + var talonFXConfigurations = TalonFXConfiguration() + + init { + talonFXConfigurations.Slot0.kG = IntakeConstants.kG + talonFXConfigurations.Slot0.kS = IntakeConstants.kS + talonFXConfigurations.Slot0.kV = IntakeConstants.kV + talonFXConfigurations.Slot0.kA = IntakeConstants.kA + talonFXConfigurations.Slot0.kP = IntakeConstants.kP + talonFXConfigurations.Slot0.kI = IntakeConstants.kI + talonFXConfigurations.Slot0.kD = IntakeConstants.kD + talonFXConfigurations.MotionMagic.MotionMagicCruiseVelocity = IntakeConstants.targetVelocity + talonFXConfigurations.MotionMagic.MotionMagicAcceleration = IntakeConstants.targetAcceleration + talonFXConfigurations.MotionMagic.MotionMagicJerk = IntakeConstants.targetJerk + talonFX.getConfigurator().apply(talonFXConfigurations) + } + + fun getIntakeEncoder(): Double { + TODO("Not yet implemented") + return 0.0; + } + + fun setIntakeMotor(x: Double) { + talonFX.setControl(MotionMagicVoltage(x)) + } + + override fun setCoralIntakeState(state: CoralIntakeState) { + TODO("Not yet implemented") + } + + override fun setAlgaeIntakeState(state: AlgaeIntakeState) { + TODO("Not yet implemented") + } +} diff --git a/src/main/java/frc/robot/subsystems/pivot/PivotConstants.kt b/src/main/java/frc/robot/subsystems/pivot/PivotConstants.kt new file mode 100644 index 0000000..883a7d2 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/pivot/PivotConstants.kt @@ -0,0 +1,16 @@ +package frc.robot.subsystems.pivot + +object PivotConstants { + val kG = 0.0 + val kS = 0.0 + val kV = 0.0 + val kA = 0.0 + val kP = 0.0 + val kI = 0.0 + val kD = 0.0 + val targetVelocity = 0.0 + val targetAcceleration = 0.0 + val targetJerk = 0.0 + val id = 0 + val canbus = "" +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/pivot/PivotIO.kt b/src/main/java/frc/robot/subsystems/pivot/PivotIO.kt new file mode 100644 index 0000000..701b3d1 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/pivot/PivotIO.kt @@ -0,0 +1,11 @@ +package frc.robot.subsystems.pivot + +import com.ctre.phoenix6.configs.TalonFXConfiguration +import com.ctre.phoenix6.hardware.TalonFX + +interface PivotIO { +// fun getPivotEncoder(): Double + + // fun setPivotMotor(x: Double) + fun setAngle(angleDegrees: Double) +} diff --git a/src/main/java/frc/robot/subsystems/pivot/PivotSystem.kt b/src/main/java/frc/robot/subsystems/pivot/PivotSystem.kt new file mode 100644 index 0000000..8985407 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/pivot/PivotSystem.kt @@ -0,0 +1,50 @@ +package frc.robot.subsystems.pivot + +import edu.wpi.first.wpilibj2.command.SubsystemBase + +// By making a subsystem a Kotlin object, we ensure there is only ever one instance of it. +// It also reduces the need to have reference variables for the subsystems to be passed around. +class PivotSystem(private val io: PivotIO) : SubsystemBase() { + /** + * Example command factory method. + * + * @return a command + */ +// fun exampleMethodCommand(): Command = runOnce { +// // Subsystem.runOnce() implicitly add `this` as a required subsystem. +// // TODO: one-time action goes here +// } + + /** + * An example method querying a boolean state of the subsystem (for example, a digital sensor). + * + * @return value of some boolean subsystem state, such as a digital sensor. + */ +// fun exampleCondition(): Boolean { +// // Query some boolean state, such as a digital sensor. +// return false +// } + + override fun periodic() { + // This method will be called once per scheduler run + } + + override fun simulationPeriodic() { + // This method will be called once per scheduler run during simulation + } + +// fun exampleAction() +// { +// // This action is called by the ExampleCommand +// println("ExampleSubsystem.exampleAction has been called") +// } + + fun getPos(): Double { + TODO("Not yet implemented") + return 0.0; + } + + fun setPos(x: Double) { + TODO("Not yet implemented") + } +} diff --git a/src/main/java/frc/robot/subsystems/pivot/implementation/PivotIOEmpty.kt b/src/main/java/frc/robot/subsystems/pivot/implementation/PivotIOEmpty.kt new file mode 100644 index 0000000..1575120 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/pivot/implementation/PivotIOEmpty.kt @@ -0,0 +1,7 @@ +package frc.robot.subsystems.pivot.implementation + +import frc.robot.subsystems.pivot.PivotIO + +class PivotIOEmpty() : PivotIO { + override fun setAngle(angleDegrees: Double) {} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/pivot/implementation/PivotIOReal.kt b/src/main/java/frc/robot/subsystems/pivot/implementation/PivotIOReal.kt new file mode 100644 index 0000000..c0ed265 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/pivot/implementation/PivotIOReal.kt @@ -0,0 +1,40 @@ +package frc.robot.subsystems.pivot.implementation + +import com.ctre.phoenix6.configs.TalonFXConfiguration +import com.ctre.phoenix6.hardware.TalonFX +import com.ctre.phoenix6.controls.MotionMagicVoltage +import frc.robot.subsystems.pivot.PivotConstants +import frc.robot.subsystems.pivot.PivotIO + +class PivotIOReal : PivotIO { + + var talonFX = TalonFX(PivotConstants.id, PivotConstants.canbus) + var talonFXConfigurations = TalonFXConfiguration() + + init { + talonFXConfigurations.Slot0.kG = PivotConstants.kG + talonFXConfigurations.Slot0.kS = PivotConstants.kS + talonFXConfigurations.Slot0.kV = PivotConstants.kV + talonFXConfigurations.Slot0.kA = PivotConstants.kA + talonFXConfigurations.Slot0.kP = PivotConstants.kP + talonFXConfigurations.Slot0.kI = PivotConstants.kI + talonFXConfigurations.Slot0.kD = PivotConstants.kD + talonFXConfigurations.MotionMagic.MotionMagicCruiseVelocity = PivotConstants.targetVelocity + talonFXConfigurations.MotionMagic.MotionMagicAcceleration = PivotConstants.targetAcceleration + talonFXConfigurations.MotionMagic.MotionMagicJerk = PivotConstants.targetJerk + talonFX.getConfigurator().apply(talonFXConfigurations) + } + + fun getPivotEncoder(): Double { + TODO("Not yet implemented") + return 0.0; + } + + fun setPivotMotor(x: Double) { + talonFX.setControl(MotionMagicVoltage(x)) + } + + override fun setAngle(angleDegrees: Double) { + TODO("Not yet implemented") + } +} diff --git a/src/main/java/frc/robot/subsystems/swerve/CommandSwerveDrivetrain.kt b/src/main/java/frc/robot/subsystems/swerve/CommandSwerveDrivetrain.kt new file mode 100644 index 0000000..8715876 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/swerve/CommandSwerveDrivetrain.kt @@ -0,0 +1,282 @@ +package frc.robot.subsystems.swerve + +import com.ctre.phoenix6.SignalLogger +import com.ctre.phoenix6.Utils +import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants +import com.ctre.phoenix6.swerve.SwerveModuleConstants +import com.ctre.phoenix6.swerve.SwerveRequest +import edu.wpi.first.math.Matrix +import edu.wpi.first.math.geometry.Rotation2d +import edu.wpi.first.math.numbers.N1 +import edu.wpi.first.math.numbers.N3 +import edu.wpi.first.units.Units +import edu.wpi.first.units.measure.Voltage +import edu.wpi.first.wpilibj.DriverStation +import edu.wpi.first.wpilibj.DriverStation.Alliance +import edu.wpi.first.wpilibj.Notifier +import edu.wpi.first.wpilibj.RobotController +import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog +import edu.wpi.first.wpilibj2.command.Command +import edu.wpi.first.wpilibj2.command.Subsystem +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Mechanism +import frc.robot.subsystems.swerve.TunerConstants.TunerSwerveDrivetrain +import java.util.function.Supplier + +/** + * Class that extends the Phoenix 6 SwerveDrivetrain class and implements + * Subsystem so it can easily be used in command-based projects. + */ +class CommandSwerveDrivetrain : TunerSwerveDrivetrain, Subsystem { + private var m_simNotifier: Notifier? = null + private var m_lastSimTime = 0.0 + + /* Keep track if we've ever applied the operator perspective before or not */ + private var m_hasAppliedOperatorPerspective = false + + /* Swerve requests to apply during SysId characterization */ + private val m_translationCharacterization = SwerveRequest.SysIdSwerveTranslation() + private val m_steerCharacterization = SwerveRequest.SysIdSwerveSteerGains() + private val m_rotationCharacterization = SwerveRequest.SysIdSwerveRotation() + + /* SysId routine for characterizing translation. This is used to find PID gains for the drive motors. */ + private val m_sysIdRoutineTranslation = SysIdRoutine( + SysIdRoutine.Config( + null, // Use default ramp rate (1 V/s) + Units.Volts.of(4.0), // Reduce dynamic step voltage to 4 V to prevent brownout + null + ) // Use default timeout (10 s) + // Log state with SignalLogger class + { state: SysIdRoutineLog.State -> + SignalLogger.writeString( + "SysIdTranslation_State", + state.toString() + ) + }, + Mechanism( + { output: Voltage? -> + setControl( + m_translationCharacterization.withVolts( + output + ) + ) + }, + null, + this + ) + ) + + /* SysId routine for characterizing steer. This is used to find PID gains for the steer motors. */ + private val m_sysIdRoutineSteer = SysIdRoutine( + SysIdRoutine.Config( + null, // Use default ramp rate (1 V/s) + Units.Volts.of(7.0), // Use dynamic voltage of 7 V + null + ) // Use default timeout (10 s) + // Log state with SignalLogger class + { state: SysIdRoutineLog.State -> + SignalLogger.writeString( + "SysIdSteer_State", + state.toString() + ) + }, + Mechanism( + { volts: Voltage? -> setControl(m_steerCharacterization.withVolts(volts)) }, + null, + this + ) + ) + + /* + * SysId routine for characterizing rotation. + * This is used to find PID gains for the FieldCentricFacingAngle HeadingController. + * See the documentation of SwerveRequest.SysIdSwerveRotation for info on importing the log to SysId. + */ + private val m_sysIdRoutineRotation = SysIdRoutine( + SysIdRoutine.Config( /* This is in radians per second², but SysId only supports "volts per second" */ + Units.Volts.of(Math.PI / 6) + .per(Units.Second), /* This is in radians per second, but SysId only supports "volts" */ + Units.Volts.of(Math.PI), + null + ) // Use default timeout (10 s) + // Log state with SignalLogger class + { state: SysIdRoutineLog.State -> + SignalLogger.writeString( + "SysIdRotation_State", + state.toString() + ) + }, + Mechanism( + { output: Voltage -> + /* output is actually radians per second, but SysId only supports "volts" */ + setControl(m_rotationCharacterization.withRotationalRate(output.`in`(Units.Volts))) + /* also log the requested output for SysId */ + SignalLogger.writeDouble("Rotational_Rate", output.`in`(Units.Volts)) + }, + null, + this + ) + ) + + /* The SysId routine to test */ + private val m_sysIdRoutineToApply = m_sysIdRoutineTranslation + + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + * + * + * This constructs the underlying hardware devices, so users should not construct + * the devices themselves. If they need the devices, they can access them through + * getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param modules Constants for each specific module + */ + constructor( + drivetrainConstants: SwerveDrivetrainConstants?, + vararg modules: SwerveModuleConstants<*, *, *>? + ) : super(drivetrainConstants, *modules) { + if (Utils.isSimulation()) { + startSimThread() + } + } + + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + * + * + * This constructs the underlying hardware devices, so users should not construct + * the devices themselves. If they need the devices, they can access them through + * getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param odometryUpdateFrequency The frequency to run the odometry loop. If + * unspecified or set to 0 Hz, this is 250 Hz on + * CAN FD, and 100 Hz on CAN 2.0. + * @param modules Constants for each specific module + */ + constructor( + drivetrainConstants: SwerveDrivetrainConstants?, + odometryUpdateFrequency: Double, + vararg modules: SwerveModuleConstants<*, *, *>? + ) : super(drivetrainConstants, odometryUpdateFrequency, *modules) { + if (Utils.isSimulation()) { + startSimThread() + } + } + + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + * + * + * This constructs the underlying hardware devices, so users should not construct + * the devices themselves. If they need the devices, they can access them through + * getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param odometryUpdateFrequency The frequency to run the odometry loop. If + * unspecified or set to 0 Hz, this is 250 Hz on + * CAN FD, and 100 Hz on CAN 2.0. + * @param odometryStandardDeviation The standard deviation for odometry calculation + * in the form [x, y, theta]ᵀ, with units in meters + * and radians + * @param visionStandardDeviation The standard deviation for vision calculation + * in the form [x, y, theta]ᵀ, with units in meters + * and radians + * @param modules Constants for each specific module + */ + constructor( + drivetrainConstants: SwerveDrivetrainConstants?, + odometryUpdateFrequency: Double, + odometryStandardDeviation: Matrix?, + visionStandardDeviation: Matrix?, + vararg modules: SwerveModuleConstants<*, *, *>? + ) : super( + drivetrainConstants, + odometryUpdateFrequency, + odometryStandardDeviation, + visionStandardDeviation, + *modules + ) { + if (Utils.isSimulation()) { + startSimThread() + } + } + + /** + * Returns a command that applies the specified control request to this swerve drivetrain. + * + * @param request Function returning the request to apply + * @return Command to run + */ + fun applyRequest(requestSupplier: Supplier): Command { + return run { this.setControl(requestSupplier.get()) } + } + + /** + * Runs the SysId Quasistatic test in the given direction for the routine + * specified by [.m_sysIdRoutineToApply]. + * + * @param direction Direction of the SysId Quasistatic test + * @return Command to run + */ + fun sysIdQuasistatic(direction: SysIdRoutine.Direction?): Command { + return m_sysIdRoutineToApply.quasistatic(direction) + } + + /** + * Runs the SysId Dynamic test in the given direction for the routine + * specified by [.m_sysIdRoutineToApply]. + * + * @param direction Direction of the SysId Dynamic test + * @return Command to run + */ + fun sysIdDynamic(direction: SysIdRoutine.Direction?): Command { + return m_sysIdRoutineToApply.dynamic(direction) + } + + override fun periodic() { + /* + * Periodically try to apply the operator perspective. + * If we haven't applied the operator perspective before, then we should apply it regardless of DS state. + * This allows us to correct the perspective in case the robot code restarts mid-match. + * Otherwise, only check and apply the operator perspective if the DS is disabled. + * This ensures driving behavior doesn't change until an explicit disable event occurs during testing. + */ + if (!m_hasAppliedOperatorPerspective || DriverStation.isDisabled()) { + DriverStation.getAlliance().ifPresent { allianceColor: Alliance -> + setOperatorPerspectiveForward( + if (allianceColor == Alliance.Red + ) kRedAlliancePerspectiveRotation + else kBlueAlliancePerspectiveRotation + ) + m_hasAppliedOperatorPerspective = true + } + } + } + + private fun startSimThread() { + m_lastSimTime = Utils.getCurrentTimeSeconds() + + /* Run simulation at a faster rate so PID gains behave more reasonably */ + m_simNotifier = Notifier { + val currentTime = Utils.getCurrentTimeSeconds() + val deltaTime = currentTime - m_lastSimTime + m_lastSimTime = currentTime + + /* use the measured time delta, get battery voltage from WPILib */ + updateSimState(deltaTime, RobotController.getBatteryVoltage()) + } + m_simNotifier!!.startPeriodic(kSimLoopPeriod) + } + + companion object { + private const val kSimLoopPeriod = 0.005 // 5 ms + + /* Blue alliance sees forward as 0 degrees (toward red alliance wall) */ + private val kBlueAlliancePerspectiveRotation: Rotation2d = Rotation2d.kZero + + /* Red alliance sees forward as 180 degrees (toward blue alliance wall) */ + private val kRedAlliancePerspectiveRotation: Rotation2d = Rotation2d.k180deg + } +} diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.kt b/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.kt new file mode 100644 index 0000000..dc1c4e6 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.kt @@ -0,0 +1,15 @@ +package frc.robot.subsystems.swerve + +import edu.wpi.first.units.Units.* +import frc.robot.subsystems.swerve.TunerConstants + +object SwerveConstants { + val MaxSpeedConst = TunerConstants.kSpeedAt12Volts.`in`(MetersPerSecond) // kSpeedAt12Volts desired top + + // speed + var ControlledSpeed = MaxSpeedConst + val MaxAngularRateConst = + RotationsPerSecond.of(0.75).`in`(RadiansPerSecond) // 3/4 of a rotation per second max angular velocity + + var ControlledAngularRate = MaxAngularRateConst +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveIOBase.kt b/src/main/java/frc/robot/subsystems/swerve/SwerveIOBase.kt new file mode 100644 index 0000000..3f36618 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveIOBase.kt @@ -0,0 +1,25 @@ +package frc.robot.subsystems.swerve + +import com.ctre.phoenix6.swerve.SwerveDrivetrain +import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState +import com.ctre.phoenix6.swerve.SwerveRequest +import edu.wpi.first.math.geometry.Pose2d +import edu.wpi.first.wpilibj2.command.Command +import java.util.function.Consumer +import java.util.function.Supplier +import edu.wpi.first.math.numbers.N1 +import edu.wpi.first.math.numbers.N3 +import edu.wpi.first.math.Matrix +import edu.wpi.first.wpilibj2.command.SubsystemBase + +open class SwerveIOBase() : SubsystemBase() { + open fun seedFieldCentric() {} + + open fun getSwervePose(): Pose2d = Pose2d() + + open fun addVisionMeasurement(visionRobotPoseMeters: Pose2d, timestampSeconds: Double) {} + open fun setVisionMeasurementStdDevs(visionMeasurementStdDevs: Matrix) {} + + open fun applyDriveRequest(x: Double, y: Double, twistRadians: Double) {} + open fun applyRobotRelativeDriveRequest(x: Double, y: Double, twistRadians: Double) {} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveIOReal.kt b/src/main/java/frc/robot/subsystems/swerve/SwerveIOReal.kt new file mode 100644 index 0000000..c6054cd --- /dev/null +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveIOReal.kt @@ -0,0 +1,99 @@ +package frc.robot.subsystems.swerve + +import com.ctre.phoenix6.swerve.SwerveDrivetrain +import com.ctre.phoenix6.swerve.SwerveModule +import com.ctre.phoenix6.swerve.SwerveRequest +import edu.wpi.first.math.Matrix +import edu.wpi.first.math.geometry.Pose2d +import edu.wpi.first.math.numbers.N1 +import edu.wpi.first.math.numbers.N3 +import edu.wpi.first.units.Units.* +import edu.wpi.first.wpilibj2.command.Command +import edu.wpi.first.wpilibj2.command.SubsystemBase +import frc.robot.RobotContainer +import frc.robot.subsystems.swerve.TunerConstants +import frc.robot.subsystems.swerve.SwerveConstants +import frc.robot.subsystems.swerve.SwerveIOBase +import frc.robot.util.Telemetry +import java.util.function.Consumer +import java.util.function.Supplier + +class SwerveIOReal() : SwerveIOBase() { + private val drivetrain = TunerConstants.createDrivetrain() + + private val logger: Telemetry = Telemetry(SwerveConstants.MaxSpeedConst) + + + init { + drivetrain.registerTelemetry(logger::telemeterize) + } + +// override fun setDefaultCommand(command: Command) { +// drivetrain.defaultCommand = command +// } + + override fun seedFieldCentric() { + drivetrain.seedFieldCentric() + } + + override fun getSwervePose(): Pose2d = drivetrain.state.Pose + + override fun addVisionMeasurement(visionRobotPoseMeters: Pose2d, timestampSeconds: Double) { + drivetrain.addVisionMeasurement(visionRobotPoseMeters, timestampSeconds) + } + + override fun setVisionMeasurementStdDevs(visionMeasurementStdDevs: Matrix) { + drivetrain.setVisionMeasurementStdDevs(visionMeasurementStdDevs) + } + + /* Setting up bindings for necessary control of the swerve drive platform */ + private val fieldCentric: SwerveRequest.FieldCentric = SwerveRequest.FieldCentric() + .withDeadband(SwerveConstants.MaxSpeedConst * 0.1) + .withRotationalDeadband(SwerveConstants.MaxAngularRateConst * 0.1) // Add a 10% deadband + .withDriveRequestType(SwerveModule.DriveRequestType.OpenLoopVoltage) // Use open-loop control for drive motors + + private val robotRelative: SwerveRequest.RobotCentric = SwerveRequest.RobotCentric() + .withDeadband(SwerveConstants.MaxSpeedConst * 0.1) + .withRotationalDeadband(SwerveConstants.MaxAngularRateConst * 0.1) // Add a 10% deadband + .withDriveRequestType(SwerveModule.DriveRequestType.OpenLoopVoltage) // Use open-loop control for drive motors + + + private val brake = SwerveRequest.SwerveDriveBrake() + private val point = SwerveRequest.PointWheelsAt() + + override fun applyDriveRequest(x: Double, y: Double, twistRadians: Double) { + drivetrain.applyRequest { + fieldCentric.withVelocityX( + x + ) // + // Drive forward with + // negative Y (forward) + .withVelocityY( + y + ) // Drive + // left with negative X + // (left) + .withRotationalRate( + twistRadians + ) + }.execute() + } + + override fun applyRobotRelativeDriveRequest(x: Double, y: Double, twistRadians: Double) { + drivetrain.applyRequest { + robotRelative.withVelocityX( + x + ) // + // Drive forward with + // negative Y (forward) + .withVelocityY( + y + ) // Drive + // left with negative X + // (left) + .withRotationalRate( + twistRadians + ) + }.execute() + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveIOSim.kt b/src/main/java/frc/robot/subsystems/swerve/SwerveIOSim.kt new file mode 100644 index 0000000..8be4dad --- /dev/null +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveIOSim.kt @@ -0,0 +1,61 @@ +package frc.robot.subsystems.swerve + +import com.ctre.phoenix6.swerve.SwerveDrivetrain +import edu.wpi.first.math.Matrix +import edu.wpi.first.math.geometry.Pose2d +import edu.wpi.first.math.geometry.Rotation2d +import edu.wpi.first.math.geometry.Transform2d +import edu.wpi.first.math.kinematics.ChassisSpeeds +import edu.wpi.first.math.numbers.N1 +import edu.wpi.first.math.numbers.N3 +import edu.wpi.first.wpilibj.Timer +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard +import edu.wpi.first.wpilibj2.command.Command +import edu.wpi.first.wpilibj2.command.SubsystemBase +import frc.robot.subsystems.swerve.SwerveIOBase + +class SwerveIOSim() : SwerveIOBase() { + private var robotPose = Pose2d() + private var currentSpeed = ChassisSpeeds() + + private var lastLoopTime = 0.0 + + override fun seedFieldCentric() { + robotPose = robotPose.rotateBy(robotPose.rotation.unaryMinus()) + } + + override fun getSwervePose(): Pose2d = robotPose + + override fun addVisionMeasurement(visionRobotPoseMeters: Pose2d, timestampSeconds: Double) {} + + override fun setVisionMeasurementStdDevs(visionMeasurementStdDevs: Matrix) {} + + override fun applyDriveRequest(x: Double, y: Double, twistRadians: Double) { + // TODO Actually implement this + currentSpeed = + ChassisSpeeds(x, y, twistRadians) + } + + override fun applyRobotRelativeDriveRequest(x: Double, y: Double, twistRadians: Double) { + currentSpeed = + ChassisSpeeds(x, y, twistRadians) + } + + override fun periodic() { + val currentTime = Timer.getFPGATimestamp() + val dtSeconds = currentTime - lastLoopTime + + robotPose = robotPose.plus( + Transform2d( + currentSpeed.vxMetersPerSecond * dtSeconds, + currentSpeed.vyMetersPerSecond * dtSeconds, + Rotation2d(currentSpeed.omegaRadiansPerSecond * dtSeconds) + ) + ) + + SmartDashboard.putNumber("Robot ChassisSpeed X", currentSpeed.vxMetersPerSecond) + SmartDashboard.putNumber("Robot ChassisSpeed Y", currentSpeed.vyMetersPerSecond) + + lastLoopTime = currentTime + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/constants/TunerConstants.kt b/src/main/java/frc/robot/subsystems/swerve/TunerConstants.kt similarity index 98% rename from src/main/java/frc/robot/constants/TunerConstants.kt rename to src/main/java/frc/robot/subsystems/swerve/TunerConstants.kt index d67d009..53406da 100644 --- a/src/main/java/frc/robot/constants/TunerConstants.kt +++ b/src/main/java/frc/robot/subsystems/swerve/TunerConstants.kt @@ -1,4 +1,4 @@ -package frc.robot.constants +package frc.robot.subsystems.swerve import com.ctre.phoenix6.CANBus import com.ctre.phoenix6.configs.* @@ -17,9 +17,9 @@ import edu.wpi.first.units.measure.Distance import edu.wpi.first.units.measure.LinearVelocity import edu.wpi.first.units.measure.MomentOfInertia import edu.wpi.first.units.measure.Voltage -import frc.robot.subsystems.CommandSwerveDrivetrain +import frc.robot.subsystems.swerve.CommandSwerveDrivetrain -// import frc.robot.subsystems.CommandSwerveDrivetrain +// import frc.robot.subsystems.swerve.CommandSwerveDrivetrain object TunerConstants { diff --git a/src/main/java/frc/robot/subsystems/wrist/WristConstants.kt b/src/main/java/frc/robot/subsystems/wrist/WristConstants.kt new file mode 100644 index 0000000..ad57f9c --- /dev/null +++ b/src/main/java/frc/robot/subsystems/wrist/WristConstants.kt @@ -0,0 +1,16 @@ +package frc.robot.subsystems.wrist + +object WristConstants { + val kG = 0.0 + val kS = 0.0 + val kV = 0.0 + val kA = 0.0 + val kP = 0.0 + val kI = 0.0 + val kD = 0.0 + val targetVelocity = 0.0 + val targetAcceleration = 0.0 + val targetJerk = 0.0 + val id = 0 + val canbus = "" +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/wrist/WristIO.kt b/src/main/java/frc/robot/subsystems/wrist/WristIO.kt new file mode 100644 index 0000000..41f9414 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/wrist/WristIO.kt @@ -0,0 +1,10 @@ +package frc.robot.subsystems.wrist + +import com.ctre.phoenix6.configs.TalonFXConfiguration +import com.ctre.phoenix6.hardware.TalonFX + +interface WristIO { + // fun getWristEncoder(): Double +// fun setWristMotor(x: Double) + fun setAngle(angleDegrees: Double) +} diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSystem.kt b/src/main/java/frc/robot/subsystems/wrist/WristSystem.kt new file mode 100644 index 0000000..f75be48 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/wrist/WristSystem.kt @@ -0,0 +1,23 @@ +package frc.robot.subsystems.wrist + +import edu.wpi.first.wpilibj2.command.SubsystemBase + +class WristSystem(private val io: WristIO) : SubsystemBase() { + /** + * Example command factory method. + * + * @return a command + */ +// fun exampleMethodCommand(): Command = runOnce { +// // Subsystem.runOnce() implicitly add `this` as a required subsystem. +// // TODO: one-time action goes here +// } + + override fun periodic() { + // This method will be called once per scheduler run + } + + override fun simulationPeriodic() { + // This method will be called once per scheduler run during simulation + } +} diff --git a/src/main/java/frc/robot/subsystems/wrist/implementation/WristIOEmpty.kt b/src/main/java/frc/robot/subsystems/wrist/implementation/WristIOEmpty.kt new file mode 100644 index 0000000..e68f322 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/wrist/implementation/WristIOEmpty.kt @@ -0,0 +1,7 @@ +package frc.robot.subsystems.wrist.implementation + +import frc.robot.subsystems.wrist.WristIO + +class WristIOEmpty() : WristIO { + override fun setAngle(angleDegrees: Double) {} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/wrist/implementation/WristIOReal.kt b/src/main/java/frc/robot/subsystems/wrist/implementation/WristIOReal.kt new file mode 100644 index 0000000..e853073 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/wrist/implementation/WristIOReal.kt @@ -0,0 +1,40 @@ +package frc.robot.subsystems.wrist.implementation + +import com.ctre.phoenix6.configs.TalonFXConfiguration +import com.ctre.phoenix6.hardware.TalonFX +import com.ctre.phoenix6.controls.MotionMagicVoltage +import frc.robot.subsystems.wrist.WristConstants +import frc.robot.subsystems.wrist.WristIO + +class WristIOReal() : WristIO { + + var talonFX = TalonFX(WristConstants.id, WristConstants.canbus) + var talonFXConfigurations = TalonFXConfiguration() + + init { + talonFXConfigurations.Slot0.kG = WristConstants.kG + talonFXConfigurations.Slot0.kS = WristConstants.kS + talonFXConfigurations.Slot0.kV = WristConstants.kV + talonFXConfigurations.Slot0.kA = WristConstants.kA + talonFXConfigurations.Slot0.kP = WristConstants.kP + talonFXConfigurations.Slot0.kI = WristConstants.kI + talonFXConfigurations.Slot0.kD = WristConstants.kD + talonFXConfigurations.MotionMagic.MotionMagicCruiseVelocity = WristConstants.targetVelocity + talonFXConfigurations.MotionMagic.MotionMagicAcceleration = WristConstants.targetAcceleration + talonFXConfigurations.MotionMagic.MotionMagicJerk = WristConstants.targetJerk + talonFX.getConfigurator().apply(talonFXConfigurations) + } + + fun getWristEncoder(): Double { + TODO("Not yet implemented") + return 0.0; + } + + fun setWristMotor(x: Double) { + talonFX.setControl(MotionMagicVoltage(x)) + } + + override fun setAngle(angleDegrees: Double) { + TODO("Not yet implemented") + } +} diff --git a/src/main/java/frc/robot/LimelightHelpers.java b/src/main/java/frc/robot/util/LimelightHelpers.java similarity index 99% rename from src/main/java/frc/robot/LimelightHelpers.java rename to src/main/java/frc/robot/util/LimelightHelpers.java index dcd1c26..06d1530 100644 --- a/src/main/java/frc/robot/LimelightHelpers.java +++ b/src/main/java/frc/robot/util/LimelightHelpers.java @@ -7,8 +7,6 @@ import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.networktables.TimestampedDoubleArray; -import frc.robot.LimelightHelpers.LimelightResults; -import frc.robot.LimelightHelpers.PoseEstimate; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; diff --git a/src/main/java/frc/robot/Telemetry.kt b/src/main/java/frc/robot/util/Telemetry.kt similarity index 99% rename from src/main/java/frc/robot/Telemetry.kt rename to src/main/java/frc/robot/util/Telemetry.kt index bcb1046..6d17c4b 100644 --- a/src/main/java/frc/robot/Telemetry.kt +++ b/src/main/java/frc/robot/util/Telemetry.kt @@ -1,4 +1,4 @@ -package frc.robot +package frc.robot.util import com.ctre.phoenix6.SignalLogger import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState