finish feeder

.gitignore

@@ -172,3 +172,4 @@ out/
 
 # Simulation GUI and other tools window save file
 *-window.json
+

build.gradle

@@ -83,7 +83,7 @@ dependencies {
 	simulationRelease wpi.sim.enableRelease()
 
 	implementation 'org.jetbrains.kotlin:kotlin-test-junit5'
-	implementation 'com.github.team4099:FalconUtils:1.1.28'
+	implementation 'com.github.team4099:FalconUtils:1.1.29'
 	implementation 'org.apache.commons:commons-collections4:4.0'
 	implementation 'com.google.code.gson:gson:2.10.1'
 	implementation "io.javalin:javalin:5.3.2"

simgui-ds.json

@@ -88,5 +88,10 @@
       "buttonCount": 0,
       "povCount": 0
     }
+  ],
+  "robotJoysticks": [
+    {
+      "guid": "Keyboard0"
+    }
   ]
 }

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

@@ -12,50 +12,61 @@ import org.team4099.lib.units.perSecond
 import com.ctre.phoenix6.controls.PositionVoltage as PositionVoltagePhoenix6
 
 class PositionVoltage(
-    var position: Angle, // Assuming an AngularPosition type exists similar to AngularVelocity
-    var enableFOC: Boolean = true,
-    var feedforward: ElectricalPotential = 0.0.volts,
-    var slot: Int = 0,
-    var overrideBrakeDurNeutral: Boolean = false,
-    var limitForwardMotion: Boolean = false,
-    var limitReverseMotion: Boolean = false,
-    var velocity: AngularVelocity = 0.0.degrees.perSecond,
+  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
-    }
-}
+  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

@@ -1,6 +1,5 @@
 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
@@ -10,55 +9,68 @@ 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
+import com.ctre.phoenix6.controls.VelocityVoltage as VelocityVoltagePhoenix6
 
-class VelocityVoltage(var velocity: AngularVelocity,
-                           var acceleration: AngularAcceleration = 0.0.degrees.perSecond.perSecond,
-                           var enableFOC:Boolean = true,
-                           var feedforward: ElectricalPotential = 0.0.volts,
-                           var slot:Int = 0,
-                           var overrideBrakeDurNeutral: Boolean = false,
-                           var limitForwardMotion: Boolean = false,
-                           var limitReverseMotion: Boolean = false){
+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)
+  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 setVelocity(new_velocity: AngularVelocity) {
+    velocity = new_velocity
+    velocityVoltagePhoenix6.Velocity = velocity.inRotationsPerSecond
+  }
 
-    fun setAcceleration(new_accel: AngularAcceleration) {
-        acceleration = new_accel
-        velocityVoltagePhoenix6.Acceleration = acceleration.inRotationsPerSecondPerSecond
-    }
+  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 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 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 setSlot(new_slot: Int) {
+    slot = new_slot
+    velocityVoltagePhoenix6.Slot = new_slot
+  }
 
-    fun setOverrideBrakeDurNeutral(new_override: Boolean) {
-        overrideBrakeDurNeutral = new_override
-        velocityVoltagePhoenix6.OverrideBrakeDurNeutral = new_override
-    }
+  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 setLimitForwardMotion(new_limitForward: Boolean) {
+    limitForwardMotion = new_limitForward
+    velocityVoltagePhoenix6.LimitForwardMotion = new_limitForward
+  }
 
-    fun setLimitReverseMotion(new_limitReverse: Boolean) {
-        limitReverseMotion = new_limitReverse
-        velocityVoltagePhoenix6.LimitReverseMotion = new_limitReverse
-    }
-}
+  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 = 28
-const val GIT_SHA = "eca16395eb62f3d6d367479ab32e985e6472149b"
-const val GIT_DATE = "2024-01-16T14:49:50Z"
-const val GIT_BRANCH = "targeting-command-with-drivetrain"
-const val BUILD_DATE = "2024-01-16T15:27:50Z"
-const val BUILD_UNIX_TIME = 1705436870465L
+const val GIT_REVISION = 102
+const val GIT_SHA = "177b056f2e17e0175cc3fd37903303defea08ea0"
+const val GIT_DATE = "2024-01-20T20:08:22Z"
+const val GIT_BRANCH = "feeder"
+const val BUILD_DATE = "2024-01-20T23:40:50Z"
+const val BUILD_UNIX_TIME = 1705812050035L
 const val DIRTY = 1

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

@@ -9,34 +9,31 @@ import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain
 import com.team4099.robot2023.subsystems.drivetrain.drive.DrivetrainIO
 import com.team4099.robot2023.subsystems.drivetrain.drive.DrivetrainIOSim
 import com.team4099.robot2023.subsystems.drivetrain.gyro.GyroIO
-import com.team4099.robot2023.subsystems.intake.Intake
-import com.team4099.robot2023.subsystems.intake.IntakeIONEO
-import com.team4099.robot2023.subsystems.intake.IntakeIOSim
+import com.team4099.robot2023.subsystems.feeder.Feeder
+import com.team4099.robot2023.subsystems.feeder.FeederIONeo
+import com.team4099.robot2023.subsystems.feeder.FeederIOSim
 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
+import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest
 
 object RobotContainer {
   private val drivetrain: Drivetrain
-  private val intake: Intake
   private val vision: Vision
   private val limelight: LimelightVision
+  private val feeder: Feeder
 
   init {
     if (RobotBase.isReal()) {
       // Real Hardware Implementations
       // drivetrain = Drivetrain(object: GyroIO {},object: DrivetrainIO {}
       drivetrain = Drivetrain(object : GyroIO {}, object : DrivetrainIO {})
-      intake = Intake(IntakeIONEO)
       vision =
         Vision(
           //          object: CameraIO {}
@@ -48,17 +45,18 @@ object RobotContainer {
           //        CameraIONorthstar("backward")
         )
       limelight = LimelightVision(object : LimelightVisionIO {})
+      feeder = Feeder(FeederIONeo)
     } else {
       // Simulation implementations
       drivetrain = Drivetrain(object : GyroIO {}, DrivetrainIOSim)
-      intake = Intake(IntakeIOSim)
       vision =
         Vision(
           CameraIONorthstar("northstar_1"),
           CameraIONorthstar("northstar_2"),
           CameraIONorthstar("northstar_3"),
         )
       limelight = LimelightVision(object : LimelightVisionIO {})
+      feeder = Feeder(FeederIOSim)
     }
 
     vision.setDataInterfaces({ drivetrain.odometryPose }, { drivetrain.addVisionData(it) })
@@ -135,6 +133,9 @@ object RobotContainer {
     //        Constants.Universal.Substation.SINGLE_SUBSTATION
     //      )
     //    )
+
+    ControlBoard.feederShootTest.whileTrue(feeder.feederOpenLoopIntakeTestCommand())
+    ControlBoard.feederIntakeTest.whileTrue(feeder.feederOpenLoopShootTestCommand())
   }
 
   fun mapTestControls() {}

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

@@ -1,19 +1,20 @@
 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
 import org.team4099.lib.units.perSecond
+import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest
 
 class DriveBrakeModeCommand(val drivetrain: Drivetrain) : Command() {
   init {
     addRequirements(drivetrain)
   }
 
   override fun execute() {
-    drivetrain.currentRequest = DrivetrainRequest.OpenLoop(
+    drivetrain.currentRequest =
+      DrivetrainRequest.OpenLoop(
         0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond)
       )
     drivetrain.swerveModules.forEach() { it.setDriveBrakeMode(true) }