Merge pull request #98 from Galaxia5987/dev

Merge to main

.gitignore

@@ -164,10 +164,9 @@ annotation/build/
 # Simulation GUI and other tools window save file
 *-window.json
 
-/src/main/kotlin/frc/robot/BuildConstants.java
-
 simgui*
 networktables.json
 /.idea
 /ctre_sim/
 /log-downloader/venv/
+/src/main/kotlin/frc/robot/BuildConstants.java

src/main/kotlin/frc/robot/Initializer.kt

@@ -1,10 +1,46 @@
 package frc.robot
 
-import frc.robot.subsystems.drive.*
+import frc.robot.subsystems.climber.Climber
+import frc.robot.subsystems.climber.ClimberIO
+import frc.robot.subsystems.climber.ClimberIOReal
+import frc.robot.subsystems.climber.ClimberIOSim
+import frc.robot.subsystems.climber.LoggedClimberInputs
+import frc.robot.subsystems.drive.Drive
+import frc.robot.subsystems.drive.GyroIO
+import frc.robot.subsystems.drive.GyroIONavX
+import frc.robot.subsystems.drive.ModuleIO
+import frc.robot.subsystems.drive.ModuleIOSim
+import frc.robot.subsystems.drive.ModuleIOTalonFX
+import frc.robot.subsystems.drive.TunerConstants
+import frc.robot.subsystems.elevator.Elevator
+import frc.robot.subsystems.elevator.ElevatorIO
+import frc.robot.subsystems.elevator.ElevatorIOReal
+import frc.robot.subsystems.elevator.ElevatorIOSim
+import frc.robot.subsystems.elevator.LoggedElevatorInputs
+import frc.robot.subsystems.gripper.Gripper
+import frc.robot.subsystems.gripper.GripperIO
+import frc.robot.subsystems.gripper.GripperIOReal
+import frc.robot.subsystems.gripper.GripperIOSim
+import frc.robot.subsystems.gripper.LoggedGripperInputs
+import frc.robot.subsystems.intake.extender.Extender
+import frc.robot.subsystems.intake.extender.ExtenderIO
+import frc.robot.subsystems.intake.extender.ExtenderIOReal
+import frc.robot.subsystems.intake.extender.ExtenderIOSim
+import frc.robot.subsystems.intake.extender.LoggedExtenderInputs
+import frc.robot.subsystems.intake.roller.LoggedRollerInputs
+import frc.robot.subsystems.intake.roller.Roller
+import frc.robot.subsystems.intake.roller.RollerIO
+import frc.robot.subsystems.intake.roller.RollerIOReal
+import frc.robot.subsystems.intake.roller.RollerIOSim
 import frc.robot.subsystems.vision.Vision
 import frc.robot.subsystems.vision.VisionConstants
 import frc.robot.subsystems.vision.VisionIOPhotonVision
 import frc.robot.subsystems.vision.VisionIOPhotonVisionSim
+import frc.robot.subsystems.wrist.LoggedWristInputs
+import frc.robot.subsystems.wrist.Wrist
+import frc.robot.subsystems.wrist.WristIO
+import frc.robot.subsystems.wrist.WristIOReal
+import frc.robot.subsystems.wrist.WristIOSim
 
 private val swerveModuleIOs =
     arrayOf(
@@ -44,3 +80,75 @@ private val visionIOs =
     }.toTypedArray()
 
 val vision = Vision(swerveDrive::addVisionMeasurement, *visionIOs)
+
+val climber =
+    Climber(
+        when (CURRENT_MODE) {
+            Mode.REAL -> ClimberIOReal()
+            Mode.SIM -> ClimberIOSim()
+            Mode.REPLAY ->
+                object : ClimberIO {
+                    override var inputs = LoggedClimberInputs()
+                }
+        }
+    )
+
+val elevator =
+    Elevator(
+        when (CURRENT_MODE) {
+            Mode.REAL -> ElevatorIOReal()
+            Mode.SIM -> ElevatorIOSim()
+            Mode.REPLAY ->
+                object : ElevatorIO {
+                    override var inputs = LoggedElevatorInputs()
+                }
+        }
+    )
+
+val gripper =
+    Gripper(
+        when (CURRENT_MODE) {
+            Mode.REAL -> GripperIOReal()
+            Mode.SIM -> GripperIOSim()
+            Mode.REPLAY ->
+                object : GripperIO {
+                    override var inputs = LoggedGripperInputs()
+                }
+        }
+    )
+
+val extender =
+    Extender(
+        when (CURRENT_MODE) {
+            Mode.REAL -> ExtenderIOReal()
+            Mode.SIM -> ExtenderIOSim()
+            Mode.REPLAY ->
+                object : ExtenderIO {
+                    override var inputs = LoggedExtenderInputs()
+                }
+        }
+    )
+
+val roller =
+    Roller(
+        when (CURRENT_MODE) {
+            Mode.REAL -> RollerIOReal()
+            Mode.SIM -> RollerIOSim()
+            Mode.REPLAY ->
+                object : RollerIO {
+                    override var inputs = LoggedRollerInputs()
+                }
+        }
+    )
+
+val wrist =
+    Wrist(
+        when (CURRENT_MODE) {
+            Mode.REAL -> WristIOReal()
+            Mode.SIM -> WristIOSim()
+            Mode.REPLAY ->
+                object : WristIO {
+                    override var inputs = LoggedWristInputs()
+                }
+        }
+    )

src/main/kotlin/frc/robot/RobotContainer.kt

@@ -21,6 +21,13 @@ object RobotContainer {
     private val testController = CommandXboxController(2)
 
     private val swerveDrive = frc.robot.swerveDrive
+    private val vision = frc.robot.vision
+    private val climber = frc.robot.climber
+    private val elevator = frc.robot.elevator
+    private val gripper = frc.robot.gripper
+    private val extender = frc.robot.extender
+    private val roller = frc.robot.roller
+    private val wrist = frc.robot.wrist
 
     init {
         registerAutoCommands()

src/main/kotlin/frc/robot/lib/motors/LinearServo.kt

@@ -0,0 +1,84 @@
+package frc.robot.lib.motors
+
+import edu.wpi.first.math.MathUtil
+import edu.wpi.first.units.Units
+import edu.wpi.first.units.measure.Distance
+import edu.wpi.first.wpilibj.Servo
+import edu.wpi.first.wpilibj.Timer
+import edu.wpi.first.wpilibj2.command.button.Trigger
+
+/**
+ * Parameters for L16-R Actuonix Linear Actuators
+ *
+ * @param channel PWM channel used to control the servo
+ * @param length max length of the servo [mm]
+ * @param speed max speed of the servo [mm/second]
+ */
+class LinearServo(
+    channel: Int,
+    length: Int,
+    speed: Int,
+    positionTolerance: Double
+) : Servo(channel) {
+    private var speed: Double
+    private var length: Double
+    private var setpoint = 0.0
+    private var position = 0.0
+    private var lastTime = 0.0
+
+    /**
+     * Miniature Linear Servo Actuators - User Guide (Rev 1) Page 10Table of
+     * Contentswcproducts.com Run this method in any periodic function to update
+     * the position estimation of your servo
+     *
+     * @param setpoint the target position of the servo [mm]
+     */
+    override fun setPosition(setpoint: Double) {
+        this.setpoint = MathUtil.clamp(setpoint, 0.0, length)
+        setSpeed((this.setpoint / length * 2.0) - 1.0)
+    }
+
+    fun setPosition(setpoint: Distance) {
+        setPosition(setpoint.`in`(Units.Millimeters))
+    }
+
+    init {
+        setBoundsMicroseconds(2000, 0, 0, 0, 1000)
+        this.length = length.toDouble()
+        this.speed = speed.toDouble()
+    }
+
+    /**
+     * Run this method in any periodic function to update the position
+     * estimation of your servo
+     */
+    fun updatePosition() {
+        val dt = Timer.getFPGATimestamp() - lastTime
+        if (position > setpoint + speed * dt) {
+            position -= speed * dt
+        } else if (position < setpoint - speed * dt) {
+            position += speed * dt
+        } else {
+            position = setpoint
+        }
+    }
+
+    /**
+     * Current position of the servo, must be calling
+     * [updatePosition()][.updatePosition] periodically
+     *
+     * @return Servo Position [mm]
+     */
+    override fun getPosition(): Double {
+        return position
+    }
+
+    /**
+     * Checks if the servo is at its target position, must be calling
+     * [updatePosition()][.updatePosition] periodically
+     * @return true when servo is at its target
+     */
+    val reachedSetpoint = Trigger {
+        MathUtil.isNear(setpoint, position, positionTolerance)
+    }
+}

src/main/kotlin/frc/robot/lib/motors/TalonFXSim.java

@@ -74,8 +74,8 @@ public void setControl(VelocityVoltage request) {
         voltageRequest =
                 () ->
                         controller.calculate(
-                                getVelocity().in(Units.RotationsPerSecond),
-                                request.Velocity)
+                                        getVelocity().in(Units.RotationsPerSecond),
+                                        request.Velocity)
                                 + request.FeedForward;
     }
 

src/main/kotlin/frc/robot/subsystems/AUTO-SPEC.md

@@ -1,38 +1,61 @@
 # Autonomous Specifications
+
 ## Linked waypoints
+
 - `Lower position`: Lower starting position
 - `Intake line start`
 - `Intake line end`
+
 ## Paths
+
 - `Intaking zone to high position`
-  - Drive from the intaking zone, pick up a cone, wait for a second, and drive close to the second alliance's loading zone.
-    ![image](https://github.com/Galaxia5987/Robot-template/assets/31829093/6c310a67-afb8-44f6-a652-35aaf70c5238)
-  - Special constraints (if relevant)
-    - Near loading zone: decrease speed so the red dragon won't notice us.
-      ![image](https://github.com/Galaxia5987/Robot-template/assets/31829093/82f75dd9-9c13-4dbc-913d-741f211f3972)
+    - Drive from the intaking zone, pick up a cone, wait for a second, and drive close to the second alliance's loading
+      zone.
+      ![image](https://github.com/Galaxia5987/Robot-template/assets/31829093/6c310a67-afb8-44f6-a652-35aaf70c5238)
+    - Special constraints (if relevant)
+        - Near loading zone: decrease speed so the red dragon won't notice us.
+          ![image](https://github.com/Galaxia5987/Robot-template/assets/31829093/82f75dd9-9c13-4dbc-913d-741f211f3972)
 - Another one...
+
 ## Autos
+
 - `Intaking zone to high position while shooting`
-  - Uses `Intaking zone to high position` path
-  - Shoots after passing the second waypoint, near the Charge Station
-    ![image](https://github.com/Galaxia5987/Robot-template/assets/31829093/5e5d017e-a853-4621-8156-03d409aa8542)
+    - Uses `Intaking zone to high position` path
+    - Shoots after passing the second waypoint, near the Charge Station
+      ![image](https://github.com/Galaxia5987/Robot-template/assets/31829093/5e5d017e-a853-4621-8156-03d409aa8542)
 - Another one...
 
 # Choosers
-> Could be as simple as "Using PathPlanner's built-in chooser" (BTW use [this](https://www.chiefdelphi.com/t/pathplanner-2024-beta/442364/149?u=dan)) or more complex based on the game, like what's been done [here](https://www.chiefdelphi.com/t/frc-6328-mechanical-advantage-2023-build-thread/420691/179#autos-the-questionnaire-2):
+
+> Could be as simple as "Using PathPlanner's built-in chooser" (BTW
+> use [this](https://www.chiefdelphi.com/t/pathplanner-2024-beta/442364/149?u=dan)) or more complex based on the game,
+> like what's been
+>
+done [here](https://www.chiefdelphi.com/t/frc-6328-mechanical-advantage-2023-build-thread/420691/179#autos-the-questionnaire-2):
+
 ## Side chooser
+
 Choose which side the robot's on:
+
 - Left
 - Center
 - Right
+
 ## Game piece chooser
+
 Choose which game piece type the robot is preloaded with:
+
 - Cube
 - Cone
+
 ## State machine
+
 > Mermaid state graph which shows which auto to run based on the chooser goes here.
 
 # Dashboard
-> Include here fields that will be sent over NetworkTables and shown to the drivers during the autonomous period of the match.
+
+> Include here fields that will be sent over NetworkTables and shown to the drivers during the autonomous period of the
+> match.
+
 - Robot location: `Robot/Odometry`
 - Time left until teleop starts: `Robot/TimeLeftUntilTeleop`