Stall safety

build.sbt

@@ -35,9 +35,9 @@ lazy val robot = crossProject(JVMPlatform, NativePlatform)
     libraryDependencies += "org.scalatest" %% "scalatest" % "3.0.5" % Test
   )
   .nativeSettings(
-    libraryDependencies += "com.lynbrookrobotics" %%% "wpilib-scala-native" % "0.1.2",
-    libraryDependencies += "com.lynbrookrobotics" %%% "ntcore-scala-native" % "0.1.2",
-    libraryDependencies += "com.lynbrookrobotics" %%% "phoenix-scala-native" % "0.1.2",
+    libraryDependencies += "com.lynbrookrobotics" %%% "wpilib-scala-native" % "0.1.3",
+    libraryDependencies += "com.lynbrookrobotics" %%% "ntcore-scala-native" % "0.1.3",
+    libraryDependencies += "com.lynbrookrobotics" %%% "phoenix-scala-native" % "0.1.3",
     scalaVersion := "2.11.12",
     scalacOptions ++= Seq("-target:jvm-1.8")
   )

jvm/src/test/scala/com/lynbrookrobotics/eighteen/ConfigGenerator.scala

@@ -1,18 +1,18 @@
 package com.lynbrookrobotics.eighteen
 
+import argonaut.Argonaut._
 import com.lynbrookrobotics.eighteen.driver.DriverConfig
 import com.lynbrookrobotics.eighteen.drivetrain.{DrivetrainConfig, DrivetrainPorts, DrivetrainProperties}
+import com.lynbrookrobotics.eighteen.lift.{CubeLiftConfig, CubeLiftPorts, CubeLiftProperties}
 import com.lynbrookrobotics.potassium.control.PIDConfig
-import squants.{Each, Percent}
-import squants.motion.{DegreesPerSecond, FeetPerSecond, FeetPerSecondSquared}
-import squants.space.{Degrees, Feet, Inches, Turns}
-import squants.time.Seconds
 import com.lynbrookrobotics.potassium.units.GenericValue._
 import com.lynbrookrobotics.potassium.units._
-import com.lynbrookrobotics.eighteen.lift.{CubeLiftConfig, CubeLiftPorts, CubeLiftProperties}
-import squants.electro.{Amperes, Volts}
-import argonaut.Argonaut._
 import com.lynbrookrobotics.potassium.vision.VisionProperties
+import squants.electro.{Amperes, Volts}
+import squants.motion.{DegreesPerSecond, FeetPerSecond, FeetPerSecondSquared}
+import squants.space.{Degrees, Feet, Inches, Turns}
+import squants.time.Seconds
+import squants.{Each, Percent}
 
 object ConfigGenerator extends App {
   println(
@@ -37,7 +37,11 @@ object ConfigGenerator extends App {
             leftPort = 12,
             rightPort = 11,
             leftFollowerPort = 14,
-            rightFollowerPort = 13
+            rightFollowerPort = 13,
+            rightFollowerPdpPort = 3,
+            leftFollowerPdpPort = 1,
+            rightPdpPort = 2,
+            leftPdpPort = 0
           ),
           props = DrivetrainProperties(
             maxLeftVelocity = FeetPerSecond(18.8),
@@ -77,7 +81,12 @@ object ConfigGenerator extends App {
             wheelOverEncoderGears = Ratio(
               Turns(18),
               Turns(74)
-            )
+            ),
+            leftFudge = 1.0,
+            rightFudge = 1.0,
+            parallelMotorCurrentThreshold = Amperes(5),
+            deltaVelocityStallThreshold = FeetPerSecond(10),
+            stallTimeout = Seconds(3)
           )
         )
       ),
@@ -103,7 +112,9 @@ object ConfigGenerator extends App {
             maxMotorOutput = Percent(20),
             maxHeight = Inches(35),
             minHeight = Inches(10),
-            twistyTotalRange = Feet(1)
+            twistyTotalRange = Feet(1),
+            maxCurrentDraw = Amperes(20),
+            stallTimeout = Seconds(3)
           )
         )
       ),

practice-robot-config.json

@@ -93,6 +93,14 @@
         0,
         "Inches"
       ],
+      "maxCurrentDraw": [
+        20,
+        "Amperes"
+      ],
+      "stallTimeout": [
+        3,
+        "Seconds"
+      ],
       "twistyTotalRange": [
         0.75,
         "Feet"
@@ -209,6 +217,8 @@
           "Turns"
         ]
       },
+      "leftFudge": 1.08,
+      "rightFudge": 1.08,
       "rightVelocityGains": {
         "kd": {
           "den": [

robot-config.json

@@ -96,6 +96,14 @@
       "twistyTotalRange": [
         0.75,
         "Feet"
+      ],
+      "maxCurrentDraw": [
+        20,
+        "Amperes"
+      ],
+      "stallTimeout": [
+        3,
+        "Seconds"
       ]
     },
     "ports": {
@@ -114,7 +122,11 @@
       "rightFollowerPort": 13,
       "leftFollowerPort": 14,
       "rightPort": 11,
-      "leftPort": 12
+      "leftPort": 12,
+      "rightFollowerPdpPort": 3,
+      "leftFollowerPdpPort": 1,
+      "rightPdpPort": 2,
+      "leftPdpPort": 0
     },
     "props": {
       "maxLeftVelocity": [
@@ -169,6 +181,10 @@
         35,
         "Amperes"
       ],
+      "parallelMotorCurrentThreshold" : [
+        5,
+        "Amperes"
+      ],
       "maxRightVelocity": [
         13.3,
         "FeetPerSecond"
@@ -181,6 +197,8 @@
         6,
         "Inches"
       ],
+      "leftFudge": 1.08,
+      "rightFudge": 1.08,
       "wheelOverEncoderGears": {
         "num": [
           18,
@@ -319,7 +337,15 @@
             "Percent"
           ]
         }
-      }
+      },
+      "deltaVelocityStallThreshold": [
+        10,
+        "FeetPerSecond"
+      ],
+      "stallTimeout": [
+        3,
+        "Seconds"
+      ]
     }
   },
   "driver": {

shared/src/main/scala/com/lynbrookrobotics/eighteen/SingleOutputChecker.scala → shared/src/main/scala/com/lynbrookrobotics/eighteen/Checkers.scala

@@ -1,5 +1,9 @@
 package com.lynbrookrobotics.eighteen
 
+import com.lynbrookrobotics.potassium.streams._
+import squants.time.Seconds
+import squants.{Quantity, Time}
+
 class SingleOutputChecker[T](hardwareName: String, get: => T) {
   private var lastOutput: Option[T] = None
 
@@ -20,3 +24,14 @@ class SingleOutputChecker[T](hardwareName: String, get: => T) {
     lastOutput = Some(get)
   }
 }
+
+object StallChecker {
+  def timeAboveThreshold[Q <: Quantity[Q]](stream: Stream[Q], threshold: => Q): Stream[Time] =
+    stream.zipWithDt
+      .scanLeft(Seconds(0)) {
+        case (stallTime, (value, dt)) =>
+          if (value > threshold) {
+            stallTime + dt
+          } else Seconds(0)
+      }
+}

shared/src/main/scala/com/lynbrookrobotics/eighteen/CoreRobot.scala

@@ -150,37 +150,37 @@ class CoreRobot(configFileValue: Signal[String], updateConfigFile: String => Uni
     cameraHardware <- cameraHardware
   } {
     // Full 3 cube
-//    addAutonomousRoutine(1) {
-//      val switchScalePattern = DriverStation.getInstance().getGameSpecificMessage
-//      switchScalePattern match {
-//        case "LLL" | "LLR" =>
-//          generator.OppositeSideSwitchAndScale
-//            .scaleSwitch3CubeAuto(drivetrain, collectorRollers, collectorClamp, collectorPivot, cubeLiftComp)
-//            .toContinuous
-//        case "RLL" | "RLR" =>
-//          generator.SameSideSwitchOppositeScale
-//            .justSwitchAuto(drivetrain, collectorRollers, collectorClamp, collectorPivot, cubeLiftComp)
-//            .toContinuous // same op
-//        case "LRL" | "LRR" =>
-//          generator.OppositeSideSwitchSameSideScale
-//            .scaleSwitch3CubeAuto(drivetrain, collectorRollers, collectorClamp, collectorPivot, cubeLiftComp)
-//            .toContinuous // op same
-//        case "RRL" | "RRR" =>
-//          generator.SameSideSwitchAndScale
-//            .scaleSwitch3Cube(
-//              drivetrain,
-//              collectorRollers,
-//              collectorClamp,
-//              collectorPivot,
-//              cubeLiftComp,
-//              cameraHardware
-//            )
-//            .toContinuous // same same
-//        case _ =>
-//          println(s"Switch scale patter didn't match what was expected. Was $switchScalePattern")
-//          ContinuousTask.empty
-//      }
-//    }
+    //    addAutonomousRoutine(1) {
+    //      val switchScalePattern = DriverStation.getInstance().getGameSpecificMessage
+    //      switchScalePattern match {
+    //        case "LLL" | "LLR" =>
+    //          generator.OppositeSideSwitchAndScale
+    //            .scaleSwitch3CubeAuto(drivetrain, collectorRollers, collectorClamp, collectorPivot, cubeLiftComp)
+    //            .toContinuous
+    //        case "RLL" | "RLR" =>
+    //          generator.SameSideSwitchOppositeScale
+    //            .justSwitchAuto(drivetrain, collectorRollers, collectorClamp, collectorPivot, cubeLiftComp)
+    //            .toContinuous // same op
+    //        case "LRL" | "LRR" =>
+    //          generator.OppositeSideSwitchSameSideScale
+    //            .scaleSwitch3CubeAuto(drivetrain, collectorRollers, collectorClamp, collectorPivot, cubeLiftComp)
+    //            .toContinuous // op same
+    //        case "RRL" | "RRR" =>
+    //          generator.SameSideSwitchAndScale
+    //            .scaleSwitch3Cube(
+    //              drivetrain,
+    //              collectorRollers,
+    //              collectorClamp,
+    //              collectorPivot,
+    //              cubeLiftComp,
+    //              cameraHardware
+    //            )
+    //            .toContinuous // same same
+    //        case _ =>
+    //          println(s"Switch scale patter didn't match what was expected. Was $switchScalePattern")
+    //          ContinuousTask.empty
+    //      }
+    //    }
 
     // 3 cube when switch on our side, 1 cube in switch
     // when switch on other side
@@ -424,29 +424,25 @@ class CoreRobot(configFileValue: Signal[String], updateConfigFile: String => Uni
       board
         .datasetGroup("Drivetrain/Current")
         .addDataset(
-          coreTicks.map(_ => drivetrainHardware.left.t.getOutputCurrent).toTimeSeriesNumeric("Left master current")
+          drivetrainHardware.leftMasterCurrent.map(_.toAmperes).toTimeSeriesNumeric("Left master current")
         )
 
       board
         .datasetGroup("Drivetrain/Current")
         .addDataset(
-          coreTicks
-            .map(_ => drivetrainHardware.leftFollowerSRX.getOutputCurrent)
-            .toTimeSeriesNumeric("Left follower current")
+          drivetrainHardware.leftFollowerCurrent.map(_.toAmperes).toTimeSeriesNumeric("Left follower current")
         )
 
       board
         .datasetGroup("Drivetrain/Current")
         .addDataset(
-          coreTicks.map(_ => drivetrainHardware.right.t.getOutputCurrent).toTimeSeriesNumeric("Right master current")
+          drivetrainHardware.rightMasterCurrent.map(_.toAmperes).toTimeSeriesNumeric("Right master current")
         )
 
       board
         .datasetGroup("Drivetrain/Current")
         .addDataset(
-          coreTicks
-            .map(_ => drivetrainHardware.rightFollowerSRX.getOutputCurrent)
-            .toTimeSeriesNumeric("Right follower current")
+          drivetrainHardware.rightFollowerCurrent.map(_.toAmperes).toTimeSeriesNumeric("Right follower current")
         )
 
       board