Add elevator code

.github/workflows/build.yml

@@ -0,0 +1,40 @@
+# Adapted from WPILib docs: https://docs.wpilib.org/en/stable/docs/software/advanced-gradlerio/robot-code-ci.html
+
+name: Build, tests, formatting
+
+# Controls when the action will run. Triggers the workflow on pull request events.
+on:
+  pull_request
+
+# A workflow run is made up of one or more jobs that can run sequentially or in parallel
+jobs:
+  # This workflow contains a single job called "build"
+  build:
+    # The type of runner that the job will run on
+    runs-on: ubuntu-latest
+
+    # Grab the WPILib docker container
+    container: wpilib/roborio-cross-ubuntu:2025-22.04
+
+    # Steps represent a sequence of tasks that will be executed as part of the job
+    steps:
+      # Check-out your repository under $GITHUB_WORKSPACE, so your job can access it
+      - uses: actions/checkout@v4
+
+      # Declare the repository safe and not under dubious ownership.
+      - name: Add repository to git safe directories
+        run: git config --global --add safe.directory $GITHUB_WORKSPACE
+
+      # Grant execute permission for gradlew
+      - name: Grant execute permission for gradlew
+        run: chmod +x gradlew
+
+      # Run Gradle commands
+      - name: Check that the code builds
+        run: ./gradlew compileKotlin compileJava
+
+      - name: Run tests
+        run: ./gradlew test
+
+      - name: Check formatting
+        run: ./gradlew spotlessCheck

.gitignore

@@ -164,7 +164,7 @@ annotation/build/
 # Simulation GUI and other tools window save file
 *-window.json
 
-.\src\main\kotlin\frc\robot\BuildConstants.java
+/src/main/kotlin/frc/robot/BuildConstants.java
 
 simgui*
 networktables.json

files/cad/Robot_2025Bot/config.json

@@ -0,0 +1,115 @@
+{
+  "name": "2025Bot",
+  "disableSimplification": true,
+  "rotations": [
+    {
+      "axis": "x",
+      "degrees": 90
+    },
+    {
+      "axis": "z",
+      "degrees": -90
+    }
+  ],
+  "position": [
+    0,
+    0,
+    0.05545625
+  ],
+  "cameras": [],
+  "components": [
+    {
+      "zeroedRotations": [
+        {
+          "axis": "x",
+          "degrees": 90
+        },
+        {
+          "axis": "z",
+          "degrees": 90
+        }
+      ],
+      "zeroedPosition": [
+        0,
+        0,
+        0.05545625
+      ]
+    },
+    {
+      "zeroedRotations": [
+        {
+          "axis": "x",
+          "degrees": 90
+        },
+        {
+          "axis": "z",
+          "degrees": -90
+        }
+      ],
+      "zeroedPosition": [
+        -0.65,
+        0,
+        -0.48
+      ]
+    },
+    {
+      "zeroedRotations": [
+        {
+          "axis": "x",
+          "degrees": 90
+        },
+        {
+          "axis": "z",
+          "degrees": -90
+        }
+      ],
+      "zeroedPosition": [
+        -0.65,
+        0,
+        0.0
+      ]
+    },
+    {
+      "zeroedRotations": [
+        {
+          "axis": "x",
+          "degrees": -90
+        },
+        {
+          "axis": "z",
+          "degrees": -90
+        },
+        {
+          "axis": "y",
+          "degrees": 180
+        }
+      ],
+      "zeroedPosition": [
+        -0.79,
+        0,
+        -0.84
+      ]
+    },
+    {
+      "zeroedRotations": [
+        {
+          "axis": "x",
+          "degrees": -90
+        },
+        {
+          "axis": "z",
+          "degrees": -90
+        },
+        {
+          "axis": "y",
+          "degrees": 180
+        }
+      ],
+      "zeroedPosition": [
+        0,
+        0,
+        0.05
+      ]
+    }
+  ]
+}

files/cad/Robot_BlockBot/config.json

@@ -0,0 +1,118 @@
+{
+  "name": "2025 BlockBot",
+  "rotations": [
+    {
+      "axis": "x",
+      "degrees": 90
+    },
+    {
+      "axis": "z",
+      "degrees": -90
+    }
+  ],
+  "position": [
+    -0.651,
+    0,
+    0
+  ],
+  "cameras": [],
+  "components": [
+    {
+      "zeroedRotations": [
+        {
+          "axis": "x",
+          "degrees": 90
+        },
+        {
+          "axis": "z",
+          "degrees": -90
+        }
+      ],
+      "zeroedPosition": [
+        -0.61,
+        0,
+        0
+      ]
+    },
+    {
+      "zeroedRotations": [
+        {
+          "axis": "y",
+          "degrees": -90
+        },
+        {
+          "axis": "x",
+          "degrees": -90
+        }
+      ],
+      "zeroedPosition": [
+        -0.44,
+        0,
+        0.265
+      ]
+    },
+    {
+      "zeroedRotations": [
+        {
+          "axis": "x",
+          "degrees": -90
+        },
+        {
+          "axis": "z",
+          "degrees": -90
+        },
+        {
+          "axis": "y",
+          "degrees": 180
+        }
+      ],
+      "zeroedPosition": [
+        -0.80,
+        0,
+        -0.12
+      ]
+    },
+    {
+      "zeroedRotations": [
+        {
+          "axis": "x",
+          "degrees": -90
+        },
+        {
+          "axis": "z",
+          "degrees": -90
+        },
+        {
+          "axis": "y",
+          "degrees": 180
+        }
+      ],
+      "zeroedPosition": [
+        -0.80,
+        0,
+        -0.15
+      ]
+    },
+    {
+      "zeroedRotations": [
+        {
+          "axis": "x",
+          "degrees": -90
+        },
+        {
+          "axis": "z",
+          "degrees": -90
+        },
+        {
+          "axis": "y",
+          "degrees": 180
+        }
+      ],
+      "zeroedPosition": [
+        -0.80,
+        0,
+        -0.80
+      ]
+    }
+  ]
+}

src/main/kotlin/frc/robot/generated/TunerConstants.java

@@ -0,0 +1 @@
+

src/main/kotlin/frc/robot/lib/ExtensionFunctions.kt

@@ -2,14 +2,16 @@ package frc.robot.lib
 
 import com.pathplanner.lib.util.FlippingUtil
 import edu.wpi.first.math.geometry.Pose2d
-import edu.wpi.first.math.geometry.Pose3d
 import edu.wpi.first.math.geometry.Rotation2d
-import edu.wpi.first.math.geometry.Rotation3d
 import edu.wpi.first.math.geometry.Translation2d
 import edu.wpi.first.math.kinematics.ChassisSpeeds
+import edu.wpi.first.units.Units
+import edu.wpi.first.units.measure.Angle
+import edu.wpi.first.units.measure.Distance
 import edu.wpi.first.wpilibj2.command.Command
 import edu.wpi.first.wpilibj2.command.WrapperCommand
 import frc.robot.IS_RED
+import kotlin.math.PI
 import kotlin.math.hypot
 import org.littletonrobotics.junction.LogTable
 
@@ -81,9 +83,6 @@ fun Command.finallyDo(command: Command): WrapperCommand =
         }
     )
 
-fun Pose2d.toPose3d(): Pose3d =
-    Pose3d(x, y, 0.0, Rotation3d(0.0, 0.0, rotation.radians))
-
 fun Pose2d.flip(): Pose2d = FlippingUtil.flipFieldPose(this)
 
 fun Pose2d.flipIfNeeded(): Pose2d = if (IS_RED) this.flip() else this
@@ -96,3 +95,15 @@ fun Translation2d.flipIfNeeded(): Translation2d =
 fun Rotation2d.flip(): Rotation2d = FlippingUtil.flipFieldRotation(this)
 
 fun Rotation2d.flipIfNeeded(): Rotation2d = if (IS_RED) this.flip() else this
+
+fun Distance.toAngle(radius: Distance, gearRatio: Double): Angle =
+    this.timesConversionFactor(
+        Units.Rotations.per(Units.Meters)
+            .of(1.0 / radius.`in`(Units.Meters) * gearRatio * 2.0 * PI)
+    )
+
+fun Angle.toDistance(radius: Distance, gearRatio: Double): Distance =
+    this.timesConversionFactor(
+        Units.Meters.per(Units.Rotations)
+            .of(radius.`in`(Units.Meters) * gearRatio * 2.0 * PI)
+    )