Intake

.github/workflows/gradle.yml

@@ -8,7 +8,7 @@ jobs:
     runs-on: ubuntu-latest
 
     # This grabs the WPILib docker container
-    container: wpilib/roborio-cross-ubuntu:2024.1.1-beta-4
+    container: wpilib/roborio-cross-ubuntu:2024.1.1
     steps:
         # Checks-out your repository under $GITHUB_WORKSPACE, so your job can access it
         - uses: actions/checkout@v2

src/main/kotlin/com/team4099/lib/phoenix6/PositionVoltage.kt

@@ -0,0 +1,61 @@
+package com.team4099.lib.phoenix6
+
+import org.team4099.lib.units.AngularVelocity
+import org.team4099.lib.units.derived.Angle
+import org.team4099.lib.units.derived.ElectricalPotential
+import org.team4099.lib.units.derived.degrees
+import org.team4099.lib.units.derived.inRotations
+import org.team4099.lib.units.derived.inVolts
+import org.team4099.lib.units.derived.volts
+import org.team4099.lib.units.inRotationsPerSecond
+import org.team4099.lib.units.perSecond
+import com.ctre.phoenix6.controls.PositionVoltage as PositionVoltagePhoenix6
+
+class PositionVoltage(
+    private var position: Angle, // Assuming an AngularPosition type exists similar to AngularVelocity
+    private var enableFOC: Boolean = true,
+    private var feedforward: ElectricalPotential = 0.0.volts,
+    private var slot: Int = 0,
+    private var overrideBrakeDurNeutral: Boolean = false,
+    private var limitForwardMotion: Boolean = false,
+    private var limitReverseMotion: Boolean = false,
+    private var velocity: AngularVelocity = 0.0.degrees.perSecond,
+) {
+
+    val positionVoltagePhoenix6 = PositionVoltagePhoenix6(position.inRotations, velocity.inRotationsPerSecond, enableFOC, feedforward.inVolts, slot, overrideBrakeDurNeutral, limitForwardMotion, limitReverseMotion)
+
+    fun setPosition(new_position: Angle) {
+        position = new_position
+        positionVoltagePhoenix6.Position = new_position.inRotations
+    }
+
+    fun setEnableFOC(new_enableFOC: Boolean) {
+        enableFOC = new_enableFOC
+        positionVoltagePhoenix6.EnableFOC = new_enableFOC
+    }
+
+    fun setFeedforward(new_feedforward: ElectricalPotential) {
+        feedforward = new_feedforward
+        positionVoltagePhoenix6.FeedForward = new_feedforward.inVolts
+    }
+
+    fun setSlot(new_slot: Int) {
+        slot = new_slot
+        positionVoltagePhoenix6.Slot = new_slot
+    }
+
+    fun setOverrideBrakeDurNeutral(new_override: Boolean) {
+        overrideBrakeDurNeutral = new_override
+        positionVoltagePhoenix6.OverrideBrakeDurNeutral = new_override
+    }
+
+    fun setLimitForwardMotion(new_limitForward: Boolean) {
+        limitForwardMotion = new_limitForward
+        positionVoltagePhoenix6.LimitForwardMotion = new_limitForward
+    }
+
+    fun setLimitReverseMotion(new_limitReverse: Boolean) {
+        limitReverseMotion = new_limitReverse
+        positionVoltagePhoenix6.LimitReverseMotion = new_limitReverse
+    }
+}

src/main/kotlin/com/team4099/lib/phoenix6/VelocityVoltage.kt

@@ -0,0 +1,64 @@
+package com.team4099.lib.phoenix6
+
+import com.ctre.phoenix6.controls.VelocityVoltage as VelocityVoltagePhoenix6
+import org.team4099.lib.units.AngularAcceleration
+import org.team4099.lib.units.AngularVelocity
+import org.team4099.lib.units.derived.ElectricalPotential
+import org.team4099.lib.units.derived.degrees
+import org.team4099.lib.units.derived.inVolts
+import org.team4099.lib.units.derived.volts
+import org.team4099.lib.units.inRotationsPerSecond
+import org.team4099.lib.units.inRotationsPerSecondPerSecond
+import org.team4099.lib.units.perSecond
+
+class VelocityVoltage(private var velocity: AngularVelocity,
+                           private var acceleration: AngularAcceleration = 0.0.degrees.perSecond.perSecond,
+                           private var enableFOC:Boolean = true,
+                            private var feedforward: ElectricalPotential = 0.0.volts,
+                           private var slot:Int = 0,
+                           private var overrideBrakeDurNeutral: Boolean = false,
+                           private var limitForwardMotion: Boolean = false,
+                           private var limitReverseMotion: Boolean = false){
+
+    val velocityVoltagePhoenix6 = VelocityVoltagePhoenix6(velocity.inRotationsPerSecond, acceleration.inRotationsPerSecondPerSecond, enableFOC, feedforward.inVolts, slot, overrideBrakeDurNeutral, limitForwardMotion, limitReverseMotion)
+
+    fun setVelocity(new_velocity: AngularVelocity) {
+        velocity = new_velocity
+        velocityVoltagePhoenix6.Velocity = velocity.inRotationsPerSecond
+    }
+
+    fun setAcceleration(new_accel: AngularAcceleration) {
+        acceleration = new_accel
+        velocityVoltagePhoenix6.Acceleration = acceleration.inRotationsPerSecondPerSecond
+    }
+
+    fun setEnableFOC(new_enableFOC: Boolean) {
+        enableFOC = new_enableFOC
+        velocityVoltagePhoenix6.EnableFOC = new_enableFOC
+    }
+
+    fun setFeedforward(new_feedforward: ElectricalPotential) {
+        feedforward = new_feedforward
+        velocityVoltagePhoenix6.FeedForward = new_feedforward.inVolts
+    }
+
+    fun setSlot(new_slot: Int) {
+        slot = new_slot
+        velocityVoltagePhoenix6.Slot = new_slot
+    }
+
+    fun setOverrideBrakeDurNeutral(new_override: Boolean) {
+        overrideBrakeDurNeutral = new_override
+        velocityVoltagePhoenix6.OverrideBrakeDurNeutral = new_override
+    }
+
+    fun setLimitForwardMotion(new_limitForward: Boolean) {
+        limitForwardMotion = new_limitForward
+        velocityVoltagePhoenix6.LimitForwardMotion = new_limitForward
+    }
+
+    fun setLimitReverseMotion(new_limitReverse: Boolean) {
+        limitReverseMotion = new_limitReverse
+        velocityVoltagePhoenix6.LimitReverseMotion = new_limitReverse
+    }
+}

src/main/kotlin/com/team4099/robot2023/BuildConstants.kt

@@ -6,10 +6,10 @@ package com.team4099.robot2023
 const val MAVEN_GROUP = ""
 const val MAVEN_NAME = "Crescendo-2024"
 const val VERSION = "unspecified"
-const val GIT_REVISION = 26
-const val GIT_SHA = "73f4f3ef5bab17de6b7bbc919ece786ceb7b52ea"
-const val GIT_DATE = "2024-01-15T19:49:07Z"
+const val GIT_REVISION = 34
+const val GIT_SHA = "d5fd1849cf1454f10fbe3db431150aff1c19e8cc"
+const val GIT_DATE = "2024-01-19T23:40:38Z"
 const val GIT_BRANCH = "intake"
-const val BUILD_DATE = "2024-01-15T20:41:51Z"
-const val BUILD_UNIX_TIME = 1705369311220L
+const val BUILD_DATE = "2024-01-20T00:01:13Z"
+const val BUILD_UNIX_TIME = 1705726873776L
 const val DIRTY = 1

src/main/kotlin/com/team4099/robot2023/RobotContainer.kt

@@ -14,11 +14,14 @@ import com.team4099.robot2023.subsystems.intake.IntakeIONEO
 import com.team4099.robot2023.subsystems.intake.IntakeIOSim
 import com.team4099.robot2023.subsystems.limelight.LimelightVision
 import com.team4099.robot2023.subsystems.limelight.LimelightVisionIO
+import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest
 import com.team4099.robot2023.subsystems.vision.Vision
 import com.team4099.robot2023.subsystems.vision.camera.CameraIONorthstar
 import com.team4099.robot2023.util.driver.Ryan
 import edu.wpi.first.wpilibj.RobotBase
+import org.team4099.lib.geometry.Pose2d
 import org.team4099.lib.smoothDeadband
+import org.team4099.lib.units.base.feet
 import org.team4099.lib.units.derived.Angle
 import org.team4099.lib.units.derived.degrees
 
@@ -90,7 +93,7 @@ object RobotContainer {
   }
 
   fun zeroSensors() {
-    drivetrain.zeroSensors()
+    drivetrain.currentRequest = DrivetrainRequest.ZeroSensors()
   }
 
   fun zeroAngle(toAngle: Angle) {
@@ -114,7 +117,6 @@ object RobotContainer {
 
   fun mapTeleopControls() {
     ControlBoard.resetGyro.whileTrue(ResetGyroYawCommand(drivetrain, toAngle = 180.degrees))
-    ControlBoard.groundIntakeTest.whileTrue(intake.generateIntakeTestCommand())
     //    ControlBoard.autoLevel.whileActiveContinuous(
     //      GoToAngle(drivetrain).andThen(AutoLevel(drivetrain))
     //    )

src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DriveBrakeModeCommand.kt

@@ -1,6 +1,7 @@
 package com.team4099.robot2023.commands.drivetrain
 
 import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain
+import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest
 import edu.wpi.first.wpilibj2.command.Command
 import org.team4099.lib.units.base.meters
 import org.team4099.lib.units.derived.radians
@@ -12,7 +13,7 @@ class DriveBrakeModeCommand(val drivetrain: Drivetrain) : Command() {
   }
 
   override fun execute() {
-    drivetrain.setOpenLoop(
+    drivetrain.currentRequest = DrivetrainRequest.OpenLoop(
         0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond)
       )
     drivetrain.swerveModules.forEach() { it.setDriveBrakeMode(true) }

src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt

@@ -6,6 +6,7 @@ import com.team4099.lib.trajectory.CustomTrajectoryGenerator
 import com.team4099.lib.trajectory.Waypoint
 import com.team4099.robot2023.config.constants.DrivetrainConstants
 import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain
+import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest
 import com.team4099.robot2023.util.AllianceFlipUtil
 import com.team4099.robot2023.util.Velocity2d
 import edu.wpi.first.math.kinematics.ChassisSpeeds
@@ -215,7 +216,7 @@ class DrivePathCommand(
       Pose2dWPILIB(desiredState.poseMeters.translation, desiredRotation.position)
     )
 
-    drivetrain.setClosedLoop(
+    drivetrain.currentRequest = DrivetrainRequest.ClosedLoop(
         nextDriveState,
         ChassisAccels(xAccel, yAccel, 0.0.radians.perSecond.perSecond).chassisAccelsWPILIB
       )
@@ -280,16 +281,16 @@ class DrivePathCommand(
     Logger.recordOutput("ActiveCommands/DrivePathCommand", false)
     if (interrupted) {
       // Stop where we are if interrupted
-      drivetrain.setOpenLoop(
-          0.degrees.perSecond, Pair(0.meters.perSecond, 0.meters.perSecond)
-        )
+      drivetrain.currentRequest = DrivetrainRequest.OpenLoop(
+        0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond)
+      )
     } else {
       // Execute one last time to end up in the final state of the trajectory
       // Since we weren't interrupted, we know curTime > endTime
       execute()
-      drivetrain.setOpenLoop(
-          0.degrees.perSecond, Pair(0.meters.perSecond, 0.meters.perSecond)
-        )
+      drivetrain.currentRequest = DrivetrainRequest.OpenLoop(
+        0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond)
+      )
     }
   }
 }

src/main/kotlin/com/team4099/robot2023/commands/drivetrain/GoToAngle.kt

@@ -3,6 +3,7 @@ package com.team4099.robot2023.commands.drivetrain
 import com.team4099.lib.logging.LoggedTunableValue
 import com.team4099.robot2023.config.constants.DrivetrainConstants
 import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain
+import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest
 import edu.wpi.first.wpilibj.RobotBase
 import edu.wpi.first.wpilibj2.command.Command
 import org.littletonrobotics.junction.Logger
@@ -81,7 +82,7 @@ class GoToAngle(val drivetrain: Drivetrain) : Command() {
 
     val thetaFeedback = thetaPID.calculate(drivetrain.odometryPose.rotation, desiredAngle)
 
-    drivetrain.setOpenLoop(
+    drivetrain.currentRequest = DrivetrainRequest.OpenLoop(
         thetaFeedback, Pair(0.0.meters.perSecond, 0.0.meters.perSecond), fieldOriented = true
       )
 
@@ -97,8 +98,8 @@ class GoToAngle(val drivetrain: Drivetrain) : Command() {
   }
 
   override fun end(interrupted: Boolean) {
-    drivetrain.setOpenLoop(
-        0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond)
-      )
+    drivetrain.currentRequest = DrivetrainRequest.OpenLoop(
+      0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond)
+    )
   }
 }

src/main/kotlin/com/team4099/robot2023/commands/drivetrain/GyroAutoLevel.kt

@@ -4,6 +4,7 @@ import com.team4099.lib.hal.Clock
 import com.team4099.lib.logging.LoggedTunableValue
 import com.team4099.robot2023.config.constants.DrivetrainConstants
 import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain
+import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest
 import edu.wpi.first.wpilibj2.command.Command
 import org.littletonrobotics.junction.Logger
 import org.team4099.lib.controller.ProfiledPIDController
@@ -91,7 +92,7 @@ class GyroAutoLevel(val drivetrain: Drivetrain) : Command() {
   override fun execute() {
     Logger.recordOutput("ActiveCommands/AutoLevelCommand", true)
 
-    drivetrain.setOpenLoop(
+    drivetrain.currentRequest = DrivetrainRequest.OpenLoop(
         0.0.radians.perSecond, gyroFeedback, fieldOriented = true
       )
 
@@ -119,8 +120,8 @@ class GyroAutoLevel(val drivetrain: Drivetrain) : Command() {
   }
 
   override fun end(interrupted: Boolean) {
-    drivetrain.setOpenLoop(
-        0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond)
-      )
+    drivetrain.currentRequest = DrivetrainRequest.OpenLoop(
+      0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond)
+    )
   }
 }

src/main/kotlin/com/team4099/robot2023/commands/drivetrain/OpenLoopReverseCommand.kt

@@ -1,6 +1,7 @@
 package com.team4099.robot2023.commands.drivetrain
 
 import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain
+import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest
 import edu.wpi.first.wpilibj2.command.Command
 import org.team4099.lib.units.base.feet
 import org.team4099.lib.units.derived.degrees
@@ -12,7 +13,7 @@ class OpenLoopReverseCommand(val drivetrain: Drivetrain) : Command() {
   }
 
   override fun execute() {
-    drivetrain.setOpenLoop(
+    drivetrain.currentRequest = DrivetrainRequest.OpenLoop(
         0.degrees.perSecond,
         Pair(-5.0.feet.perSecond, 0.0.feet.perSecond),
         fieldOriented = false

src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TeleopDriveCommand.kt

@@ -1,6 +1,7 @@
 package com.team4099.robot2023.commands.drivetrain
 
 import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain
+import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest
 import com.team4099.robot2023.util.driver.DriverProfile
 import edu.wpi.first.wpilibj2.command.Command
 import org.littletonrobotics.junction.Logger
@@ -23,7 +24,7 @@ class TeleopDriveCommand(
   override fun execute() {
     val speed = driver.driveSpeedClampedSupplier(driveX, driveY, slowMode)
     val rotation = driver.rotationSpeedClampedSupplier(turn, slowMode)
-    drivetrain.setOpenLoop(rotation, speed)
+    drivetrain.currentRequest = DrivetrainRequest.OpenLoop(rotation, speed)
     Logger.recordOutput("ActiveCommands/TeleopDriveCommand", true)
   }
   override fun isFinished(): Boolean {

src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ZeroSensorsCommand.kt

@@ -1,6 +1,7 @@
 package com.team4099.robot2023.commands.drivetrain
 
 import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain
+import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest
 import edu.wpi.first.wpilibj2.command.Command
 import org.littletonrobotics.junction.Logger
 
@@ -11,7 +12,7 @@ class ZeroSensorsCommand(val drivetrain: Drivetrain) : Command() {
   }
 
   override fun initialize() {
-    drivetrain.zeroSensors()
+    drivetrain.currentRequest = DrivetrainRequest.ZeroSensors()
   }
 
   override fun isFinished(): Boolean {

src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt

@@ -97,6 +97,4 @@ object ControlBoard {
 
   // for testing
   val setArmCommand = Trigger { technician.yButton }
-
-  val groundIntakeTest = Trigger { driver.aButton }
 }

src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt

@@ -12,6 +12,6 @@ object IntakeConstants {
   const val ROLLER_MOTOR_INVERTED = true
   const val ROLLER_GEAR_RATIO = 24.0 / 12.0 // this one has been updated
 
-  // TODO: Update value
-  val IDLE_ROLLER_VOLTAGE = 2.0.volts
+  // TODO: Update the idle roller voltage later
+  val IDLE_ROLLER_VOLTAGE = 1.0.volts
 }