measure current using PDP instead of Victors & add fudging to drivetr…

…ain config run scalafmt tmp
Team846 · Mar 24, 2018 · bd46eac · bd46eac
1 parent 430208a
commit bd46eac
Show file tree

Hide file tree

Showing 10 changed files with 100 additions and 64 deletions.
diff --git a/build.sbt b/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.2+2-f4444a67",
+    libraryDependencies += "com.lynbrookrobotics" %%% "ntcore-scala-native" % "0.1.2+2-f4444a67",
+    libraryDependencies += "com.lynbrookrobotics" %%% "phoenix-scala-native" % "0.1.2+2-f4444a67",
     scalaVersion := "2.11.12",
     scalacOptions ++= Seq("-target:jvm-1.8")
   )

diff --git a/jvm/src/test/scala/com/lynbrookrobotics/eighteen/ConfigGenerator.scala b/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),

diff --git a/practice-robot-config.json b/practice-robot-config.json
@@ -207,6 +207,8 @@
           "Turns"
         ]
       },
+      "leftFudge": 1.08,
+      "rightFudge": 1.08,
       "rightVelocityGains": {
         "kd": {
           "den": [

diff --git a/robot-config.json b/robot-config.json
@@ -197,6 +197,8 @@
         6,
         "Inches"
       ],
+      "leftFudge": 1.08,
+      "rightFudge": 1.08,
       "wheelOverEncoderGears": {
         "num": [
           18,

diff --git a/shared/src/main/scala/com/lynbrookrobotics/eighteen/ButtonMappings.scala b/shared/src/main/scala/com/lynbrookrobotics/eighteen/ButtonMappings.scala
@@ -27,7 +27,7 @@ object ButtonMappings {
     } {
       driverHardware.joystickStream.eventWhen { _ =>
         driverHardware.driverJoystick.getRawButton(Trigger) &&
-          driverHardware.driverJoystick.getRawButton(TriggerBottom)
+        driverHardware.driverJoystick.getRawButton(TriggerBottom)
       }.foreach(
         new WhileAtPosition(
           coreTicks.map(_ => cubeLiftProps.get.collectHeight),
@@ -43,7 +43,7 @@ object ButtonMappings {
     } {
       driverHardware.joystickStream.eventWhen { _ =>
         driverHardware.driverJoystick.getRawButton(Trigger) &&
-          !driverHardware.driverJoystick.getRawButton(TriggerBottom)
+        !driverHardware.driverJoystick.getRawButton(TriggerBottom)
       }.foreach(
         new WhileAtPosition(
           coreTicks.map(_ => cubeLiftProps.get.collectHeight),
@@ -58,7 +58,7 @@ object ButtonMappings {
     } {
       driverHardware.joystickStream.eventWhen { _ =>
         driverHardware.driverJoystick.getRawButton(TriggerBottom) &&
-          !driverHardware.driverJoystick.getRawButton(Trigger)
+        !driverHardware.driverJoystick.getRawButton(Trigger)
       }.foreach(
         new OpenCollector(clamp) and new PivotDown(pivot)
       )
@@ -271,7 +271,7 @@ object ButtonMappings {
     } {
       driverHardware.joystickStream.eventWhen { _ =>
         driverHardware.operatorJoystick.getRawButton(LeftFive) &&
-          !driverHardware.operatorJoystick.getRawButton(LeftTwo)
+        !driverHardware.operatorJoystick.getRawButton(LeftTwo)
       }.foreach(
         new LiftManualControl(
           driverHardware.joystickStream.map(_.operator.y).syncTo(lift.coreTicks)
@@ -285,7 +285,7 @@ object ButtonMappings {
     } {
       driverHardware.joystickStream.eventWhen { _ =>
         driverHardware.operatorJoystick.getRawButton(LeftTwo) &&
-          driverHardware.operatorJoystick.getRawButton(LeftFive)
+        driverHardware.operatorJoystick.getRawButton(LeftFive)
       }.foreach(
         new PivotDown(pivot) and new LiftManualControl(
           driverHardware.joystickStream.map(_.operator.y).syncTo(lift.coreTicks)
@@ -320,10 +320,10 @@ object ButtonMappings {
     } {
       driverHardware.joystickStream.eventWhen { _ =>
         driverHardware.operatorJoystick.getRawButton(LeftTwo) &&
-          !driverHardware.operatorJoystick.getRawButton(LeftFive)
+        !driverHardware.operatorJoystick.getRawButton(LeftFive)
       }.foreach(
         new PivotDown(pivot)
       )
     }
   }
-}
+}
diff --git a/shared/src/main/scala/com/lynbrookrobotics/eighteen/CoreRobot.scala b/shared/src/main/scala/com/lynbrookrobotics/eighteen/CoreRobot.scala
@@ -83,7 +83,7 @@ class CoreRobot(configFileValue: Signal[String], updateConfigFile: String => Uni
         new LEDController(
           coreTicks,
           Signal.constant(DriverStation.Alliance.Red)
-        )
+      )
     )
 
   lazy val components: Seq[Component[_]] = Seq(
@@ -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.leftFollower.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.rightFollower.getOutputCurrent)
-            .toTimeSeriesNumeric("Right follower current")
+          drivetrainHardware.rightFollowerCurrent.map(_.toAmperes).toTimeSeriesNumeric("Right follower current")
         )
 
       board
@@ -525,4 +521,4 @@ object CoreRobot {
     }
   }
 
-}
+}
diff --git a/shared/src/main/scala/com/lynbrookrobotics/eighteen/DefaultConfig.scala b/shared/src/main/scala/com/lynbrookrobotics/eighteen/DefaultConfig.scala
@@ -5,11 +5,11 @@ object DefaultConfig {
                |  "cubeLift": {
                |    "props": {
                |      "lowScaleHeight": [
-               |        66,
+               |        64,
                |        "Inches"
                |      ],
                |      "highScaleHeight": [
-               |        68,
+               |        66,
                |        "Inches"
                |      ],
                |      "pidConfig": {
@@ -45,11 +45,11 @@ object DefaultConfig {
                |        }
                |      },
                |      "collectHeight": [
-               |        0.5,
+               |        0,
                |        "Inches"
                |      ],
                |      "voltageAtBottom": [
-               |        0.316,
+               |        0.277,
                |        "Volts"
                |      ],
                |      "liftPositionTolerance": [
@@ -70,7 +70,7 @@ object DefaultConfig {
                |          "Inches"
                |        ],
                |        "num": [
-               |          2.5,
+               |          1.983,
                |          "Volts"
                |        ]
                |      },
@@ -89,24 +89,24 @@ object DefaultConfig {
                |        ]
                |      },
                |      "maxHeight": [
-               |        73,
+               |        74,
                |        "Inches"
                |      ],
                |      "minHeight": [
-               |        0.25,
+               |        0,
                |        "Inches"
                |      ],
+               |      "twistyTotalRange": [
+               |        0.75,
+               |        "Feet"
+               |      ],
                |      "maxCurrentDraw": [
                |        20,
                |        "Amperes"
                |      ],
                |      "stallTimeout": [
                |        3,
                |        "Seconds"
-               |      ],
-               |      "twistyTotalRange": [
-               |        -1.5,
-               |        "Feet"
                |      ]
                |    },
                |    "ports": {
@@ -125,11 +125,15 @@ object DefaultConfig {
                |      "rightFollowerPort": 13,
                |      "leftFollowerPort": 14,
                |      "rightPort": 11,
-               |      "leftPort": 12
+               |      "leftPort": 12,
+               |      "rightFollowerPdpPort": 3,
+               |      "leftFollowerPdpPort": 1,
+               |      "rightPdpPort": 2,
+               |      "leftPdpPort": 0
                |    },
                |    "props": {
                |      "maxLeftVelocity": [
-               |        17.7,
+               |        13,
                |        "FeetPerSecond"
                |      ],
                |      "turnVelocityGains": {
@@ -185,7 +189,7 @@ object DefaultConfig {
                |        "Amperes"
                |      ],
                |      "maxRightVelocity": [
-               |        17.7,
+               |        13.3,
                |        "FeetPerSecond"
                |      ],
                |      "track": [
@@ -196,6 +200,8 @@ object DefaultConfig {
                |        6,
                |        "Inches"
                |      ],
+               |      "leftFudge": 1.08,
+               |      "rightFudge": 1.08,
                |      "wheelOverEncoderGears": {
                |        "num": [
                |          18,
@@ -265,7 +271,7 @@ object DefaultConfig {
                |            "Degrees"
                |          ],
                |          "num": [
-               |            50,
+               |            80,
                |            "Percent"
                |          ]
                |        }
@@ -368,8 +374,8 @@ object DefaultConfig {
                |  },
                |  "collectorRollers": {
                |    "ports": {
-               |      "rollerLeftPort": 0,
-               |      "rollerRightPort": 1
+               |      "rollerLeftPort": 1,
+               |      "rollerRightPort": 0
                |    },
                |    "props": {
                |      "collectSpeed": [
@@ -382,7 +388,19 @@ object DefaultConfig {
                |      ]
                |    }
                |  },
-               |  "climberWinch": null,
+               |  "climberWinch": {
+               |    "ports": {
+               |      "leftMotorPort": 5,
+               |      "middleMotorPort": 6,
+               |      "rightMotorPort": 7
+               |    },
+               |    "props": {
+               |      "climbingSpeed": [
+               |        85,
+               |        "Percent"
+               |      ]
+               |    }
+               |  },
                |  "limelight": {
                |    "cameraAngleRelativeToFront": [
                |      0,
@@ -394,5 +412,6 @@ object DefaultConfig {
                |    ]
                |  },
                |  "led": null
-               |}""".stripMargin
+               |}
+               |""".stripMargin
 }
diff --git a/shared/src/main/scala/com/lynbrookrobotics/eighteen/RobotHardware.scala b/shared/src/main/scala/com/lynbrookrobotics/eighteen/RobotHardware.scala
@@ -13,6 +13,7 @@ import com.lynbrookrobotics.potassium.Signal
 import com.lynbrookrobotics.potassium.frc.{LEDControllerHardware, WPIClock}
 import com.lynbrookrobotics.potassium.streams.Stream
 import com.lynbrookrobotics.potassium.vision.limelight.LimeLightHardware
+import edu.wpi.first.wpilibj.PowerDistributionPanel
 
 final case class RobotHardware(
   climberDeployment: Option[DeploymentHardware],
@@ -33,6 +34,7 @@ object RobotHardware {
     import robotConfig._
 
     val driverHardware = DriverHardware(robotConfig.driver.get) // drivetrain depends on this
+    val pdp = new PowerDistributionPanel(0)
 
     RobotHardware(
       climberDeployment = climberDeployment.map(DeploymentHardware.apply),
@@ -41,7 +43,7 @@ object RobotHardware {
       collectorPivot = collectorPivot.map(CollectorPivotHardware.apply),
       collectorRollers = collectorRollers.map(CollectorRollersHardware.apply),
       driver = driverHardware,
-      drivetrain = robotConfig.drivetrain.map(DrivetrainHardware.apply(_, coreTicks, driverHardware)),
+      drivetrain = robotConfig.drivetrain.map(DrivetrainHardware.apply(_, coreTicks, driverHardware, pdp)),
       forklift = robotConfig.forklift.map(ForkliftHardware.apply),
       cubeLift = robotConfig.cubeLift.map(CubeLiftHardware.apply(_, coreTicks)),
       camera = robotConfig.limelight.map { l =>

diff --git a/shared/src/main/scala/com/lynbrookrobotics/eighteen/drivetrain/DrivetrainConfig.scala b/shared/src/main/scala/com/lynbrookrobotics/eighteen/drivetrain/DrivetrainConfig.scala
@@ -28,6 +28,8 @@ final case class DrivetrainProperties(
   track: Length,
   wheelDiameter: Length,
   wheelOverEncoderGears: Ratio[Angle, Angle],
+  leftFudge: Double,
+  rightFudge: Double,
   parallelMotorCurrentThreshold: ElectricCurrent,
   deltaVelocityStallThreshold: Velocity,
   stallTimeout: Time