Final changes

frc-5160-the-chargers · Oct 15, 2024 · a4951fd · a4951fd
1 parent 5ab8dde
commit a4951fd
Show file tree

Hide file tree

Showing 13 changed files with 84 additions and 69 deletions.
diff --git a/simgui-ds.json b/simgui-ds.json
@@ -107,7 +107,10 @@
     {
       "guid": "Keyboard0"
     },
-    {},
+    {
+      "guid": "78696e70757401000000000000000000",
+      "useGamepad": true
+    },
     {
       "guid": "Keyboard2"
     }

diff --git a/simgui.json b/simgui.json
@@ -274,9 +274,21 @@
             "open": true
           }
         },
+        "GroundIntakeSerializer": {
+          "AngularVelocity": {
+            "open": true
+          },
+          "Voltage": {
+            "open": true
+          },
+          "open": true
+        },
         "Pivot": {
           "open": true
         },
+        "Shooter": {
+          "open": true
+        },
         "open": true
       },
       "RobotGeneral": {

diff --git a/src/main/deploy/pathplanner/autos/New Auto.auto b/src/main/deploy/pathplanner/autos/New Auto.auto
diff --git a/src/main/java/elastic/Elastic.java b/src/main/java/elastic/Elastic.java
@@ -8,6 +8,9 @@
 import edu.wpi.first.networktables.StringPublisher;
 import edu.wpi.first.networktables.StringTopic;
 
+/**
+ * Interface for showing notifications on the elastic dashboard.
+ */
 @SuppressWarnings({"unused", "CallToPrintStackTrace"})
 public final class Elastic {
     private static final StringTopic topic =

diff --git a/src/main/kotlin/frc/chargers/framework/HorseLog.kt b/src/main/kotlin/frc/chargers/framework/HorseLog.kt
@@ -3,11 +3,10 @@ package frc.chargers.framework
 import com.batterystaple.kmeasure.quantities.Quantity
 import dev.doglog.DogLog
 import dev.doglog.DogLogOptions
+import edu.wpi.first.networktables.NetworkTableInstance
+import edu.wpi.first.util.datalog.StringLogEntry
 import edu.wpi.first.util.struct.StructSerializable
-import edu.wpi.first.wpilibj.DriverStation
-import edu.wpi.first.wpilibj.PowerDistribution
-import edu.wpi.first.wpilibj.RobotBase
-import edu.wpi.first.wpilibj.Timer
+import edu.wpi.first.wpilibj.*
 import elastic.Elastic
 import elastic.Elastic.ElasticNotification.NotificationLevel
 
@@ -26,7 +25,7 @@ import elastic.Elastic.ElasticNotification.NotificationLevel
  * HorseLog.log("Angle", 30.degrees) // also valid
  */
 @Suppress("unused")
-object HorseLog {
+object HorseLog { // inheritance doesnt do anything other than allowing for logQueuer access, as static inheritance doesnt exist in kotlin
     /* Ported from DogLog */
     fun log(key: String, value: Long) = DogLog.log(key, value)
     fun log(key: String, value: Double) = DogLog.log(key, value)
@@ -44,7 +43,6 @@ object HorseLog {
     fun <T: StructSerializable> log(key: String, value: Array<T>) = DogLog.log(key, value)
 
     fun faultsLogged() = DogLog.faultsLogged()
-    fun timestamp(key: String) = DogLog.timestamp(key)
     fun getOptions(): DogLogOptions = DogLog.getOptions()
     fun setOptions(options: DogLogOptions) = DogLog.setOptions(options)
     fun setEnabled(newEnabled: Boolean) = DogLog.setEnabled(newEnabled)
@@ -73,6 +71,16 @@ object HorseLog {
         elasticAlert(NotificationLevel.INFO, title, description.toString())
     }
 
+    /** Logs to the metadata tab of advantagescope. */
+    fun logMetadata(title: String, description: Any) {
+        NetworkTableInstance.getDefault()
+            .getStringTopic("/Metadata/$title")
+            .publish()
+            .set(description.toString())
+        StringLogEntry(DataLogManager.getLog(), "/Metadata/$title")
+            .append(description.toString())
+    }
+
     private val cache = mutableMapOf<String, String>() // ensures that no spamming occurs
     private val internalNotif = Elastic.ElasticNotification(NotificationLevel.INFO, "", "")
     private fun elasticAlert(level: NotificationLevel, title: String, description: String) {
@@ -118,7 +126,7 @@ object HorseLog {
             DogLog.log("$key/isPresent", false)
         } else {
             DogLog.log("$key/isPresent", true)
-            logRegular("$key/$value", value)
+            logRegular("$key/value", value)
         }
     }
     private val doubleArrayStore = mutableMapOf<String, DoubleArray>()
@@ -131,7 +139,6 @@ object HorseLog {
             DogLog.log(key, prevValue)
         }
     }
-
     @Deprecated("Use logError and logWarning instead.") fun logFault(faultName: String) = DogLog.logFault(faultName)
     @Deprecated("Use logError and logWarning instead.") fun logFault(faultName: Enum<*>) = DogLog.logFault(faultName)
 }
diff --git a/src/main/kotlin/frc/chargers/hardware/motorcontrol/ChargerSpark.kt b/src/main/kotlin/frc/chargers/hardware/motorcontrol/ChargerSpark.kt
@@ -160,6 +160,7 @@ open class ChargerSpark<BaseMotorType: CANSparkBase>(
         val errors = mutableListOf<REVLibError>()
         fun REVLibError.bind() { if (this != REVLibError.kOk) errors.add(this) }
         for (i in 1..4) {
+            errors.clear()
             if (inverted != null) base.inverted = inverted
             when (brakeWhenIdle) {
                 true -> base.setIdleMode(CANSparkBase.IdleMode.kBrake).bind()
@@ -240,7 +241,6 @@ open class ChargerSpark<BaseMotorType: CANSparkBase>(
             }
             base.burnFlash().bind()
             if (errors.isEmpty()) return this
-            errors.clear()
         }
         HorseLog.logError(
             "${faultLogName ?: "ChargerSpark($deviceID)"} could not configure.",

diff --git a/src/main/kotlin/frc/chargers/hardware/motorcontrol/ChargerTalonFX.kt b/src/main/kotlin/frc/chargers/hardware/motorcontrol/ChargerTalonFX.kt
@@ -135,8 +135,9 @@ class ChargerTalonFX(
         continuousInput: Boolean?
     ): ChargerTalonFX {
         val errors = mutableListOf<StatusCode>()
-        fun StatusCode.bind(){ if (this != StatusCode.OK) errors.add(this) }
+        fun StatusCode.bind() { if (this != StatusCode.OK) errors.add(this) }
         for (i in 1..4) {
+            errors.clear()
             config.MotorOutput.apply {
                 when (brakeWhenIdle) {
                     true -> NeutralMode = NeutralModeValue.Brake
@@ -220,7 +221,6 @@ class ChargerTalonFX(
                 }
             }
             if (errors.isEmpty()) return this
-            errors.clear()
         }
         HorseLog.logError(
             "${faultLogName ?: "ChargerTalonFX($deviceID)"} failed to configure.",

diff --git a/src/main/kotlin/frc/chargers/hardware/sensors/encoders/ChargerCANcoder.kt b/src/main/kotlin/frc/chargers/hardware/sensors/encoders/ChargerCANcoder.kt
@@ -33,7 +33,7 @@ class ChargerCANcoder(
         if (absoluteSensorRange != null) config.MagnetSensor.AbsoluteSensorRange = absoluteSensorRange
         if (magnetOffset != null) config.MagnetSensor.MagnetOffset = magnetOffset.inUnit(rotations)
         for (i in 1..4) {
-            val status = base.configurator.apply(config)
+            val status = base.configurator.apply(config, 0.1)
             if (status == StatusCode.OK) break
             if (i == 4) HorseLog.logError("CANcoder($deviceID) failed to configure", status)
         }

diff --git a/src/main/kotlin/frc/chargers/hardware/sensors/imu/ChargerNavX.kt b/src/main/kotlin/frc/chargers/hardware/sensors/imu/ChargerNavX.kt
@@ -7,6 +7,7 @@ import com.batterystaple.kmeasure.units.seconds
 import com.batterystaple.kmeasure.units.standardGravities
 import com.kauailabs.navx.frc.AHRS
 import edu.wpi.first.wpilibj.RobotBase.isReal
+import edu.wpi.first.wpilibj2.command.WaitCommand
 import frc.chargers.framework.logged
 
 
@@ -15,9 +16,14 @@ class ChargerNavX(
     private val useFusedHeading: Boolean = false,
     var simHeadingSource: () -> Angle = { Angle(0.0) }
 ): ZeroableHeadingProvider {
-    private var headingOffset by logged(0.degrees)
+    private var headingOffset = 0.degrees
 
-    init { zeroHeading() }
+    init {
+        WaitCommand(0.1)
+            .finallyDo(::zeroHeading)
+            .ignoringDisable(true)
+            .schedule()
+    }
 
     /**
      * Equivalent to the NavX [yaw].

diff --git a/src/main/kotlin/frc/robot/rigatoni/CompetitionRobot.kt b/src/main/kotlin/frc/robot/rigatoni/CompetitionRobot.kt
@@ -11,6 +11,7 @@ import edu.wpi.first.math.geometry.Pose2d
 import edu.wpi.first.net.PortForwarder
 import edu.wpi.first.wpilibj.DriverStation
 import edu.wpi.first.wpilibj.PowerDistribution
+import edu.wpi.first.wpilibj.RobotBase
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard
 import edu.wpi.first.wpilibj2.command.Command
@@ -43,9 +44,9 @@ import kotlin.math.max
 class CompetitionRobot: ChargerRobot() {
     private val gyro = ChargerNavX()
     private val drivetrain = getDrivetrain(gyro)
-    private val pivot = Pivot(disable = true)
-    private val groundIntake = GroundIntakeSerializer(disable = true)
-    private val shooter = Shooter(disable = true)
+    private val pivot = Pivot()
+    private val groundIntake = GroundIntakeSerializer()
+    private val shooter = Shooter()
     private val climber = Climber()
     private val noteObserver = NoteObserver()
 
@@ -199,7 +200,8 @@ class CompetitionRobot: ChargerRobot() {
         }
 
         pivot.defaultCommand = RunCommand(pivot){
-            var speed = applyDeadband(operatorController.rightY, PIVOT_DEADBAND).squareMagnitude()
+            val invert = if (RobotBase.isReal()) -1.0 else 1.0
+            var speed = applyDeadband(operatorController.rightY * invert, PIVOT_DEADBAND).squareMagnitude()
             speed *= PIVOT_SPEED_MULTIPLIER
             HorseLog.log("OperatorController/PivotSpeed", speed)
             pivot.setSpeed(speed)
@@ -344,7 +346,7 @@ class CompetitionRobot: ChargerRobot() {
         buildCommand("ShootInSpeaker") {
             require(shooter, groundIntake, pivot)
 
-            parallel { // since there are 2 deadlines, the loop {} block will stop when both deadlines end
+            parallel { // since there are 2 deadlines, the parallel block will stop when both deadlines end
                 wait(shooterSpinUpTime.inUnit(seconds)).asDeadline()
                 waitUntil { !movePivot || pivot.atTarget }.asDeadline()
                 loop{
@@ -366,13 +368,13 @@ class CompetitionRobot: ChargerRobot() {
 
     private fun getAutoCommands() =
         listOf(
-            buildCommand("1 Piece Amp + Taxi"){
+            buildCommand("1 Piece Amp + Taxi", log = true){
                 +ampAutoStartup()
                 +pivotAngleCommand(PivotAngle.STOWED)
                 +AutoBuilder.followPath(PathPlannerPath.fromPathFile("AmpSideTaxiShort"))
             },
 
-            buildCommand("2-3 Piece Amp"){
+            buildCommand("2-3 Piece Amp", log = true){
                 +ampAutoStartup()
                 +driveAndIntake(
                     PathPlannerPath.fromPathFile("AmpGrabG1"),
@@ -391,7 +393,7 @@ class CompetitionRobot: ChargerRobot() {
 
             speakerAutoStartup(AutoStartingPose::getSpeakerLeft).withName("1 Piece Speaker"),
 
-            buildCommand("1 Piece Speaker + Taxi"){
+            buildCommand("1 Piece Speaker + Taxi", log = true){
                 +speakerAutoStartup(AutoStartingPose::getSpeakerLeft)
 
                 loopForDuration(5){
@@ -401,7 +403,7 @@ class CompetitionRobot: ChargerRobot() {
                 onEnd{ drivetrain.stop() }
             },
 
-            buildCommand("4.5 Piece Speaker"){
+            buildCommand("4.5 Piece Speaker", log = true){
                 +speakerAutoStartup(AutoStartingPose::getSpeakerCenter)
                 repeat(3) { i ->
                     +driveAndIntake(PathPlannerPath.fromChoreoTrajectory("5pAutoCenter.${2 * i + 1}"))
@@ -410,16 +412,16 @@ class CompetitionRobot: ChargerRobot() {
                 +driveAndIntake(PathPlannerPath.fromChoreoTrajectory("5pAutoCenter.7"))
             },
 
-            buildCommand("4.5 Piece Speaker(Continuous Shooter Running)") {
+            buildCommand("4.5 Piece Speaker(Continuous Shooter Running)", log = true) {
                 +speakerAutoStartup(AutoStartingPose::getSpeakerCenter)
-                parallelRace {
+                parallel {
                     runSequence {
                         repeat(3) { i ->
                             +driveAndIntake(PathPlannerPath.fromChoreoTrajectory("5pAutoCenter.${2 * i + 1}"))
                             +AutoBuilder.followPath(PathPlannerPath.fromChoreoTrajectory("5pAutoCenter.${2 * i + 2}"))
                             +runNoteThroughShooter()
                         }
-                    }
+                    }.asDeadline()
 
                     loop { // will never finish; simply runs in background
                         shooter.outtakeAtSpeakerSpeed()

diff --git a/src/main/kotlin/frc/robot/rigatoni/DriverController.kt b/src/main/kotlin/frc/robot/rigatoni/DriverController.kt
@@ -1,6 +1,7 @@
 package frc.robot.rigatoni
 
 import edu.wpi.first.math.MathUtil.applyDeadband
+import edu.wpi.first.wpilibj.RobotBase
 import edu.wpi.first.wpilibj2.command.button.CommandPS5Controller
 import frc.chargers.framework.logged
 import frc.chargers.framework.tunable
@@ -42,6 +43,7 @@ class DriverController(port: Int, name: String): CommandPS5Controller(port) {
         rotation = rotationEquation(rotation) * if (invertRotation) -1 else 1
 
         scalar = if (DRIVER_RIGHT_HANDED) r2Axis else l2Axis
+        if (RobotBase.isReal()) scalar = (scalar + 1) / 2 // corrects for weird readings from ps5 controller
         scalar = 1 / (2 * scalar + 1)
 
         chassisPowers.xPower = forward * scalar

diff --git a/src/main/kotlin/frc/robot/rigatoni/subsystems/Drivetrain.kt b/src/main/kotlin/frc/robot/rigatoni/subsystems/Drivetrain.kt
@@ -48,7 +48,7 @@ private val SWERVE_CONSTANTS = SwerveConstants(
     moduleType = ModuleType.Mk4iL2,
     trackWidth = 27.inches,
     wheelBase = 27.inches,
-    azimuthPID = PIDConstants(7.0,0.0,0.0),
+    azimuthPID = PIDConstants(2.0,0.0,0.0),
     azimuthPIDTolerance = 1.degrees,
     velocityPID = PIDConstants(0.05,0.0,0.0),
     velocityFF = AngularMotorFeedforward(0.0,0.13),
@@ -83,9 +83,9 @@ private val DRIVE_MOTORS = listOf(
         brakeWhenIdle = true,
         optimizeUpdateRate = true
     ).limitSupplyCurrent(
-        50.amps,
+        45.amps,
         highLimit = 70.amps,
-        highLimitAllowedFor = 0.1.seconds
+        highLimitAllowedFor = 0.2.seconds
     )
 }
 private val TURN_MOTOR_MOI = 0.004.kilo.grams * (meters * meters)