Merge pull request #12 from team4099/working-drivetrain

Working drivetrain
team4099 · Jan 16, 2024 · 23919b4 · 23919b4
2 parents deb1873 + 41ca731
commit 23919b4
Show file tree

Hide file tree

Showing 44 changed files with 1,850 additions and 2,420 deletions.
diff --git a/simgui-ds.json b/simgui-ds.json
@@ -0,0 +1,92 @@
+{
+  "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
+    }
+  ]
+}
diff --git a/simgui.json b/simgui.json
@@ -0,0 +1,9 @@
+{
+  "NTProvider": {
+    "types": {
+      "/FMSInfo": "FMSInfo",
+      "/Shuffleboard/Pre-match/Mode": "String Chooser",
+      "/SmartDashboard/AutonomousMode": "String Chooser"
+    }
+  }
+}
diff --git a/src/main/kotlin/com/team4099/robot2023/BuildConstants.kt b/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 = -1
-const val GIT_SHA = "UNKNOWN"
-const val GIT_DATE = "UNKNOWN"
-const val GIT_BRANCH = "UNKNOWN"
-const val BUILD_DATE = "2023-12-16T19:11:21Z"
-const val BUILD_UNIX_TIME = 1702771881467L
-const val DIRTY = 129
+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 DIRTY = 1
diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt
@@ -6,31 +6,37 @@ import com.team4099.robot2023.commands.drivetrain.TeleopDriveCommand
 import com.team4099.robot2023.config.ControlBoard
 import com.team4099.robot2023.config.constants.Constants
 import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain
-import com.team4099.robot2023.subsystems.drivetrain.drive.DrivetrainIOReal
+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.drivetrain.gyro.GyroIOPigeon2
+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.limelight.LimelightVision
 import com.team4099.robot2023.subsystems.limelight.LimelightVisionIO
-import com.team4099.robot2023.subsystems.superstructure.Request
+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
 
 object RobotContainer {
   private val drivetrain: Drivetrain
+  private val intake: Intake
   private val vision: Vision
   private val limelight: LimelightVision
 
   init {
     if (RobotBase.isReal()) {
       // Real Hardware Implementations
       // drivetrain = Drivetrain(object: GyroIO {},object: DrivetrainIO {}
-      drivetrain = Drivetrain(GyroIOPigeon2, DrivetrainIOReal)
+      drivetrain = Drivetrain(object : GyroIO {}, object : DrivetrainIO {})
+      intake = Intake(IntakeIONEO)
       vision =
         Vision(
           //          object: CameraIO {}
@@ -45,6 +51,7 @@ object RobotContainer {
     } else {
       // Simulation implementations
       drivetrain = Drivetrain(object : GyroIO {}, DrivetrainIOSim)
+      intake = Intake(IntakeIOSim)
       vision =
         Vision(
           CameraIONorthstar("northstar_1"),
@@ -86,7 +93,7 @@ object RobotContainer {
   }
 
   fun zeroSensors() {
-    drivetrain.currentRequest = Request.DrivetrainRequest.ZeroSensors()
+    drivetrain.currentRequest = DrivetrainRequest.ZeroSensors()
   }
 
   fun zeroAngle(toAngle: Angle) {

diff --git a/src/main/kotlin/com/team4099/robot2023/commands/SysIdCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/SysIdCommand.kt
@@ -2,14 +2,14 @@ package com.team4099.robot2023.commands
 
 import edu.wpi.first.wpilibj.Timer
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard
-import edu.wpi.first.wpilibj2.command.CommandBase
+import edu.wpi.first.wpilibj2.command.Command
 import edu.wpi.first.wpilibj2.command.Subsystem
 import org.littletonrobotics.junction.Logger
 import java.util.function.BiConsumer
 import java.util.function.Consumer
 import java.util.function.Supplier
 
-class SysIdCommand : CommandBase {
+class SysIdCommand : Command {
   private val isDriveTrain: Boolean
   private lateinit var driveTrainSetter: BiConsumer<Double, Double>
   private lateinit var mechanismSetter: Consumer<Double>

diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DriveBrakeModeCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DriveBrakeModeCommand.kt
@@ -1,20 +1,19 @@
 package com.team4099.robot2023.commands.drivetrain
 
 import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain
-import com.team4099.robot2023.subsystems.superstructure.Request
-import edu.wpi.first.wpilibj2.command.CommandBase
+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
 
-class DriveBrakeModeCommand(val drivetrain: Drivetrain) : CommandBase() {
+class DriveBrakeModeCommand(val drivetrain: Drivetrain) : Command() {
   init {
     addRequirements(drivetrain)
   }
 
   override fun execute() {
-    drivetrain.currentRequest =
-      Request.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) }

diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt
@@ -6,7 +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
+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
@@ -15,44 +15,16 @@ import edu.wpi.first.math.trajectory.TrajectoryParameterizer.TrajectoryGeneratio
 import edu.wpi.first.math.trajectory.constraint.CentripetalAccelerationConstraint
 import edu.wpi.first.math.trajectory.constraint.TrajectoryConstraint
 import edu.wpi.first.wpilibj.DriverStation
-import edu.wpi.first.wpilibj2.command.CommandBase
+import edu.wpi.first.wpilibj2.command.Command
 import org.littletonrobotics.junction.Logger
 import org.team4099.lib.controller.PIDController
 import org.team4099.lib.geometry.Pose2d
 import org.team4099.lib.geometry.Pose2dWPILIB
 import org.team4099.lib.hal.Clock
 import org.team4099.lib.kinematics.ChassisAccels
-import org.team4099.lib.units.Velocity
-import org.team4099.lib.units.base.Meter
-import org.team4099.lib.units.base.inMeters
-import org.team4099.lib.units.base.inSeconds
-import org.team4099.lib.units.base.inches
-import org.team4099.lib.units.base.meters
-import org.team4099.lib.units.base.seconds
-import org.team4099.lib.units.derived.Radian
-import org.team4099.lib.units.derived.cos
-import org.team4099.lib.units.derived.degrees
-import org.team4099.lib.units.derived.inDegreesPerSecondPerDegree
-import org.team4099.lib.units.derived.inDegreesPerSecondPerDegreePerSecond
-import org.team4099.lib.units.derived.inDegreesPerSecondPerDegreeSeconds
-import org.team4099.lib.units.derived.inMetersPerSecondPerMeter
-import org.team4099.lib.units.derived.inMetersPerSecondPerMeterSeconds
-import org.team4099.lib.units.derived.inMetersPerSecondPerMetersPerSecond
-import org.team4099.lib.units.derived.inRadians
-import org.team4099.lib.units.derived.metersPerSecondPerMetersPerSecond
-import org.team4099.lib.units.derived.perDegree
-import org.team4099.lib.units.derived.perDegreePerSecond
-import org.team4099.lib.units.derived.perDegreeSeconds
-import org.team4099.lib.units.derived.perMeter
-import org.team4099.lib.units.derived.perMeterSeconds
-import org.team4099.lib.units.derived.radians
-import org.team4099.lib.units.derived.sin
-import org.team4099.lib.units.inDegreesPerSecond
-import org.team4099.lib.units.inMetersPerSecond
-import org.team4099.lib.units.inMetersPerSecondPerSecond
-import org.team4099.lib.units.inRadiansPerSecond
-import org.team4099.lib.units.inRadiansPerSecondPerSecond
-import org.team4099.lib.units.perSecond
+import org.team4099.lib.units.*
+import org.team4099.lib.units.base.*
+import org.team4099.lib.units.derived.*
 import java.util.function.Supplier
 import kotlin.math.PI
 
@@ -65,7 +37,7 @@ class DrivePathCommand(
   val endPathOnceAtReference: Boolean = true,
   val leaveOutYAdjustment: Boolean = false,
   val endVelocity: Velocity2d = Velocity2d(),
-) : CommandBase() {
+) : Command() {
   private val xPID: PIDController<Meter, Velocity<Meter>>
   private val yPID: PIDController<Meter, Velocity<Meter>>
 
@@ -244,8 +216,7 @@ class DrivePathCommand(
       Pose2dWPILIB(desiredState.poseMeters.translation, desiredRotation.position)
     )
 
-    drivetrain.currentRequest =
-      Request.DrivetrainRequest.ClosedLoop(
+    drivetrain.currentRequest = DrivetrainRequest.ClosedLoop(
         nextDriveState,
         ChassisAccels(xAccel, yAccel, 0.0.radians.perSecond.perSecond).chassisAccelsWPILIB
       )
@@ -310,18 +281,16 @@ class DrivePathCommand(
     Logger.recordOutput("ActiveCommands/DrivePathCommand", false)
     if (interrupted) {
       // Stop where we are if interrupted
-      drivetrain.currentRequest =
-        Request.DrivetrainRequest.OpenLoop(
-          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.currentRequest =
-        Request.DrivetrainRequest.OpenLoop(
-          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)
+      )
     }
   }
 }
diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/GoToAngle.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/GoToAngle.kt
@@ -3,30 +3,20 @@ 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
+import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest
 import edu.wpi.first.wpilibj.RobotBase
-import edu.wpi.first.wpilibj2.command.CommandBase
+import edu.wpi.first.wpilibj2.command.Command
 import org.littletonrobotics.junction.Logger
 import org.team4099.lib.controller.ProfiledPIDController
 import org.team4099.lib.controller.TrapezoidProfile
 import org.team4099.lib.units.Velocity
 import org.team4099.lib.units.base.meters
-import org.team4099.lib.units.derived.Angle
-import org.team4099.lib.units.derived.Radian
-import org.team4099.lib.units.derived.degrees
-import org.team4099.lib.units.derived.inDegrees
-import org.team4099.lib.units.derived.inDegreesPerSecondPerDegree
-import org.team4099.lib.units.derived.inDegreesPerSecondPerDegreePerSecond
-import org.team4099.lib.units.derived.inDegreesPerSecondPerDegreeSeconds
-import org.team4099.lib.units.derived.perDegree
-import org.team4099.lib.units.derived.perDegreePerSecond
-import org.team4099.lib.units.derived.perDegreeSeconds
-import org.team4099.lib.units.derived.radians
+import org.team4099.lib.units.derived.*
 import org.team4099.lib.units.inDegreesPerSecond
 import org.team4099.lib.units.perSecond
 import kotlin.math.PI
 
-class GoToAngle(val drivetrain: Drivetrain) : CommandBase() {
+class GoToAngle(val drivetrain: Drivetrain) : Command() {
   private val thetaPID: ProfiledPIDController<Radian, Velocity<Radian>>
 
   val thetakP =
@@ -92,8 +82,7 @@ class GoToAngle(val drivetrain: Drivetrain) : CommandBase() {
 
     val thetaFeedback = thetaPID.calculate(drivetrain.odometryPose.rotation, desiredAngle)
 
-    drivetrain.currentRequest =
-      Request.DrivetrainRequest.OpenLoop(
+    drivetrain.currentRequest = DrivetrainRequest.OpenLoop(
         thetaFeedback, Pair(0.0.meters.perSecond, 0.0.meters.perSecond), fieldOriented = true
       )
 
@@ -109,9 +98,8 @@ class GoToAngle(val drivetrain: Drivetrain) : CommandBase() {
   }
 
   override fun end(interrupted: Boolean) {
-    drivetrain.currentRequest =
-      Request.DrivetrainRequest.OpenLoop(
-        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)
+    )
   }
 }