diff --git a/build.sbt b/build.sbt index 5d97fab..b679282 100644 --- 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.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") ) diff --git a/jvm/src/test/scala/com/lynbrookrobotics/eighteen/ConfigGenerator.scala b/jvm/src/test/scala/com/lynbrookrobotics/eighteen/ConfigGenerator.scala index ab988df..85be7c8 100644 --- 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), @@ -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) ) ) ), diff --git a/practice-robot-config.json b/practice-robot-config.json index 82b8021..4f12a36 100644 --- a/practice-robot-config.json +++ b/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": [ diff --git a/robot-config.json b/robot-config.json index ad64bd2..8d9f1d0 100644 --- a/robot-config.json +++ b/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": { diff --git a/shared/src/main/scala/com/lynbrookrobotics/eighteen/SingleOutputChecker.scala b/shared/src/main/scala/com/lynbrookrobotics/eighteen/Checkers.scala similarity index 55% rename from shared/src/main/scala/com/lynbrookrobotics/eighteen/SingleOutputChecker.scala rename to shared/src/main/scala/com/lynbrookrobotics/eighteen/Checkers.scala index ddc4b87..6392c63 100644 --- a/shared/src/main/scala/com/lynbrookrobotics/eighteen/SingleOutputChecker.scala +++ b/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) + } +} diff --git a/shared/src/main/scala/com/lynbrookrobotics/eighteen/CoreRobot.scala b/shared/src/main/scala/com/lynbrookrobotics/eighteen/CoreRobot.scala index 7f771cc..e5f844f 100644 --- a/shared/src/main/scala/com/lynbrookrobotics/eighteen/CoreRobot.scala +++ b/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 diff --git a/shared/src/main/scala/com/lynbrookrobotics/eighteen/DefaultConfig.scala b/shared/src/main/scala/com/lynbrookrobotics/eighteen/DefaultConfig.scala index 9d0b700..054a2a6 100644 --- 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,16 +89,24 @@ object DefaultConfig { | ] | }, | "maxHeight": [ - | 73, + | 74, | "Inches" | ], | "minHeight": [ - | 0.25, + | 0, | "Inches" | ], | "twistyTotalRange": [ - | 1.5, + | 0.75, | "Feet" + | ], + | "maxCurrentDraw": [ + | 20, + | "Amperes" + | ], + | "stallTimeout": [ + | 3, + | "Seconds" | ] | }, | "ports": { @@ -117,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": { @@ -172,8 +184,12 @@ object DefaultConfig { | 35, | "Amperes" | ], + | "parallelMotorCurrentThreshold" : [ + | 5, + | "Amperes" + | ], | "maxRightVelocity": [ - | 17.7, + | 13.3, | "FeetPerSecond" | ], | "track": [ @@ -184,6 +200,8 @@ object DefaultConfig { | 6, | "Inches" | ], + | "leftFudge": 1.08, + | "rightFudge": 1.08, | "wheelOverEncoderGears": { | "num": [ | 18, @@ -253,7 +271,7 @@ object DefaultConfig { | "Degrees" | ], | "num": [ - | 50, + | 80, | "Percent" | ] | } @@ -322,7 +340,15 @@ object DefaultConfig { | "Percent" | ] | } - | } + | }, + | "deltaVelocityStallThreshold": [ + | 10, + | "FeetPerSecond" + | ], + | "stallTimeout": [ + | 3, + | "Seconds" + | ] | } | }, | "driver": { @@ -348,8 +374,8 @@ object DefaultConfig { | }, | "collectorRollers": { | "ports": { - | "rollerLeftPort": 0, - | "rollerRightPort": 1 + | "rollerLeftPort": 1, + | "rollerRightPort": 0 | }, | "props": { | "collectSpeed": [ @@ -362,7 +388,19 @@ object DefaultConfig { | ] | } | }, - | "climberWinch": null, + | "climberWinch": { + | "ports": { + | "leftMotorPort": 5, + | "middleMotorPort": 6, + | "rightMotorPort": 7 + | }, + | "props": { + | "climbingSpeed": [ + | 85, + | "Percent" + | ] + | } + | }, | "limelight": { | "cameraAngleRelativeToFront": [ | 0, @@ -374,5 +412,6 @@ object DefaultConfig { | ] | }, | "led": null - |}""".stripMargin + |} + |""".stripMargin } \ No newline at end of file diff --git a/shared/src/main/scala/com/lynbrookrobotics/eighteen/RobotHardware.scala b/shared/src/main/scala/com/lynbrookrobotics/eighteen/RobotHardware.scala index 48d937d..8f00ee9 100644 --- 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/collector/rollers/CollectorRollers.scala b/shared/src/main/scala/com/lynbrookrobotics/eighteen/collector/rollers/CollectorRollers.scala index 30d1d5b..4d2e1d5 100644 --- a/shared/src/main/scala/com/lynbrookrobotics/eighteen/collector/rollers/CollectorRollers.scala +++ b/shared/src/main/scala/com/lynbrookrobotics/eighteen/collector/rollers/CollectorRollers.scala @@ -3,7 +3,9 @@ package com.lynbrookrobotics.eighteen.collector.rollers import com.lynbrookrobotics.eighteen.SingleOutputChecker import com.lynbrookrobotics.eighteen.driver.DriverHardware import com.lynbrookrobotics.potassium.Component +import com.lynbrookrobotics.potassium.commons.electronics.CurrentLimiting import com.lynbrookrobotics.potassium.streams.Stream +import squants.time.Seconds import squants.{Dimensionless, Each, Percent} class CollectorRollers(val coreTicks: Stream[Unit])( @@ -23,6 +25,22 @@ class CollectorRollers(val coreTicks: Stream[Unit])( (hardware.rollerLeft.get, hardware.rollerRight.get) ) + override def setController(controller: Stream[(Dimensionless, Dimensionless)]): Unit = { + val l = CurrentLimiting.slewRate( + Each(hardware.rollerLeft.get), + controller.map(_._1), + Percent(100) / Seconds(0.3) + ) + + val r = CurrentLimiting.slewRate( + Each(hardware.rollerRight.get), + controller.map(_._2), + Percent(100) / Seconds(0.3) + ) + + super.setController(l.zip(r)) + } + override def applySignal(signal: (Dimensionless, Dimensionless)): Unit = check.assertSingleOutput { hardware.rollerLeft.set(-signal._1.toEach) hardware.rollerRight.set(signal._2.toEach) diff --git a/shared/src/main/scala/com/lynbrookrobotics/eighteen/drivetrain/DrivetrainComponent.scala b/shared/src/main/scala/com/lynbrookrobotics/eighteen/drivetrain/DrivetrainComponent.scala index 7a206ab..9b31539 100644 --- a/shared/src/main/scala/com/lynbrookrobotics/eighteen/drivetrain/DrivetrainComponent.scala +++ b/shared/src/main/scala/com/lynbrookrobotics/eighteen/drivetrain/DrivetrainComponent.scala @@ -1,12 +1,15 @@ package com.lynbrookrobotics.eighteen.drivetrain -import com.lynbrookrobotics.eighteen.SingleOutputChecker +import com.lynbrookrobotics.eighteen.{SingleOutputChecker, StallChecker} import com.lynbrookrobotics.potassium.clock.Clock import com.lynbrookrobotics.potassium.commons.drivetrain.twoSided.TwoSided import com.lynbrookrobotics.potassium.commons.drivetrain.unicycle.UnicycleSignal import com.lynbrookrobotics.potassium.control.offload.OffloadedSignal import com.lynbrookrobotics.potassium.streams.Stream +import com.lynbrookrobotics.potassium.tasks.Task import com.lynbrookrobotics.potassium.{Component, Signal} +import edu.wpi.first.wpilibj.RobotState +import squants.time.Seconds import squants.{Each, Percent} class DrivetrainComponent(coreTicks: Stream[Unit])( @@ -48,6 +51,54 @@ class DrivetrainComponent(coreTicks: Stream[Unit])( (hardware.left.getLastCommand, hardware.right.getLastCommand) ) + StallChecker + .timeAboveThreshold( + hardware.leftVelocity.zipAsync(hardware.leftDutyCycle).map { + case (currVelocity, dutyCycle) => (props.get.maxLeftVelocity * dutyCycle.toEach) - currVelocity + }.map(_.abs), + props.get.deltaVelocityStallThreshold + ) + .filter(_ => RobotState.isAutonomous) + .filter(_ > props.get.stallTimeout) + .foreach { time => + println(s"[ERROR] LEFT SIDE OF DRIVETRAIN STALLED FOR $time. ABORTING TASK.") + Task.abortCurrentTask() + } + + StallChecker + .timeAboveThreshold( + hardware.rightVelocity.zipAsync(hardware.rightDutyCycle).map { + case (currVelocity, dutyCycle) => (props.get.maxRightVelocity * dutyCycle.toEach) - currVelocity + }.map(_.abs), + props.get.deltaVelocityStallThreshold + ) + .filter(_ => RobotState.isAutonomous) + .filter(_ > props.get.stallTimeout) + .foreach { time => + println(s"[ERROR] RIGHT SIDE OF DRIVETRAIN STALLED FOR $time. ABORTING TASK.") + Task.abortCurrentTask() + } + + StallChecker + .timeAboveThreshold( + hardware.rightMasterCurrent.zipAsync(hardware.rightFollowerCurrent).map { case (m, f) => (m - f).abs }, + props.get.parallelMotorCurrentThreshold + ) + .filter(_ > Seconds(0)) + .foreach { time => + println(s"[WARNING] RIGHT MASTER AND RIGHT FOLLOWER HAVE DIFFERENT CURRENT DRAWS. CHECK FUNKY DASHBOARD.") + } + + StallChecker + .timeAboveThreshold( + hardware.leftMasterCurrent.zipAsync(hardware.leftFollowerCurrent).map { case (m, f) => (m - f).abs }, + props.get.parallelMotorCurrentThreshold + ) + .filter(_ > Seconds(0)) + .foreach { time => + println(s"[WARNING] LEFT MASTER AND LEFT FOLLOWER HAVE DIFFERENT CURRENT DRAWS. CHECK FUNKY DASHBOARD.") + } + override def applySignal(signal: TwoSided[OffloadedSignal]): Unit = check.assertSingleOutput { hardware.left.applyCommand(signal.left) hardware.right.applyCommand(signal.right) diff --git a/shared/src/main/scala/com/lynbrookrobotics/eighteen/drivetrain/DrivetrainConfig.scala b/shared/src/main/scala/com/lynbrookrobotics/eighteen/drivetrain/DrivetrainConfig.scala index f923985..71f1b2c 100644 --- a/shared/src/main/scala/com/lynbrookrobotics/eighteen/drivetrain/DrivetrainConfig.scala +++ b/shared/src/main/scala/com/lynbrookrobotics/eighteen/drivetrain/DrivetrainConfig.scala @@ -5,7 +5,7 @@ import com.lynbrookrobotics.potassium.commons.drivetrain.offloaded.OffloadedDriv import com.lynbrookrobotics.potassium.control.offload.EscConfig import com.lynbrookrobotics.potassium.units._ import squants.electro.ElectricCurrent -import squants.{Acceleration, Angle, Dimensionless, Each, Length, Velocity} +import squants.{Acceleration, Angle, Dimensionless, Each, Length, Time, Velocity} import squants.motion.RadiansPerSecond import squants.space.Turns import squants.time.Seconds @@ -27,7 +27,12 @@ final case class DrivetrainProperties( blendExponent: Double, track: Length, wheelDiameter: Length, - wheelOverEncoderGears: Ratio[Angle, Angle] + wheelOverEncoderGears: Ratio[Angle, Angle], + leftFudge: Double, + rightFudge: Double, + parallelMotorCurrentThreshold: ElectricCurrent, + deltaVelocityStallThreshold: Velocity, + stallTimeout: Time ) extends OffloadedDriveProperties { override val encoderAngleOverTicks: Ratio[Angle, Dimensionless] = Ratio(Turns(1), Each(4096)) override val escConfig: EscConfig[Length] = EscConfig( @@ -44,5 +49,9 @@ final case class DrivetrainPorts( leftPort: Int, rightPort: Int, leftFollowerPort: Int, - rightFollowerPort: Int + rightFollowerPort: Int, + leftPdpPort: Int, + rightPdpPort: Int, + leftFollowerPdpPort: Int, + rightFollowerPdpPort: Int ) diff --git a/shared/src/main/scala/com/lynbrookrobotics/eighteen/drivetrain/DrivetrainHardware.scala b/shared/src/main/scala/com/lynbrookrobotics/eighteen/drivetrain/DrivetrainHardware.scala index 547da3a..bf8e729 100644 --- a/shared/src/main/scala/com/lynbrookrobotics/eighteen/drivetrain/DrivetrainHardware.scala +++ b/shared/src/main/scala/com/lynbrookrobotics/eighteen/drivetrain/DrivetrainHardware.scala @@ -10,10 +10,11 @@ import com.lynbrookrobotics.potassium.frc.{LazyTalon, TalonEncoder} import com.lynbrookrobotics.potassium.sensors.imu.{ADIS16448, DigitalGyro} import com.lynbrookrobotics.potassium.streams._ import com.lynbrookrobotics.potassium.units.{Ratio, Value3D} -import edu.wpi.first.wpilibj.SPI +import edu.wpi.first.wpilibj.{PowerDistributionPanel, SPI} +import squants.electro.{Amperes, ElectricCurrent} import squants.motion.AngularVelocity import squants.time.{Milliseconds, Seconds} -import squants.{Angle, Length, Velocity} +import squants.{Angle, Dimensionless, Each, Length, Velocity} final case class DrivetrainData( leftEncoderVelocity: AngularVelocity, @@ -27,11 +28,13 @@ final case class DrivetrainHardware( coreTicks: Stream[Unit], leftSRX: TalonSRX, rightSRX: TalonSRX, - leftFollowerSRX: BaseMotorController, - rightFollowerSRX: BaseMotorController, + leftFollower: BaseMotorController, + rightFollower: BaseMotorController, gyro: DigitalGyro, driverHardware: DriverHardware, - props: DrivetrainProperties + props: DrivetrainProperties, + ports: DrivetrainPorts, + pdp: PowerDistributionPanel ) extends TwoSidedDriveHardware { override val track: Length = props.track @@ -43,11 +46,11 @@ final case class DrivetrainHardware( val right /*Back*/ = new LazyTalon(rightSRX) - leftFollowerSRX.follow(left.t) - rightFollowerSRX.follow(right.t) + leftFollower.follow(left.t) + rightFollower.follow(right.t) right.t.setInverted(true) - rightFollowerSRX.setInverted(true) + rightFollower.setInverted(true) right.t.setSensorPhase(false) import props._ @@ -58,7 +61,7 @@ final case class DrivetrainHardware( left.t.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder, escIdx, escTout) right.t.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder, escIdx, escTout) - Set(leftSRX, rightSRX, leftFollowerSRX, rightFollowerSRX) + Set(leftSRX, rightSRX, leftFollower, rightFollower) .foreach(it => TalonManager.configSlave(it)) Set(left, right).foreach { it => @@ -94,20 +97,35 @@ final case class DrivetrainHardware( } override val leftPosition: Stream[Length] = rootDataStream.map(_.leftEncoderRotation).map { ar => - (wheelOverEncoderGears * ar) onRadius (wheelDiameter / 2) + (wheelOverEncoderGears * ar) onRadius (wheelDiameter / 2) * leftFudge } override val rightPosition: Stream[Length] = rootDataStream.map(_.rightEncoderRotation).map { ar => - (wheelOverEncoderGears * ar) onRadius (wheelDiameter / 2) + (wheelOverEncoderGears * ar) onRadius (wheelDiameter / 2) * rightFudge } + val leftDutyCycle: Stream[Dimensionless] = coreTicks.map(_ => Each(left.t.getMotorOutputPercent)) + val rightDutyCycle: Stream[Dimensionless] = coreTicks.map(_ => Each(right.t.getMotorOutputPercent)) + + val leftMasterCurrent: Stream[ElectricCurrent] = coreTicks.map(_ => Amperes(pdp.getCurrent(ports.leftPdpPort))) + val rightMasterCurrent: Stream[ElectricCurrent] = coreTicks.map(_ => Amperes(pdp.getCurrent(ports.rightPdpPort))) + val leftFollowerCurrent: Stream[ElectricCurrent] = + coreTicks.map(_ => Amperes(pdp.getCurrent(ports.leftFollowerPdpPort))) + val rightFollowerCurrent: Stream[ElectricCurrent] = + coreTicks.map(_ => Amperes(pdp.getCurrent(ports.rightFollowerPdpPort))) + override lazy val turnVelocity: Stream[AngularVelocity] = rootDataStream.map(_.gyroVelocities).map(_.z) override lazy val turnPosition: Stream[Angle] = turnVelocity.integral turnPosition.foreach(_ => {}) } object DrivetrainHardware { - def apply(config: DrivetrainConfig, coreTicks: Stream[Unit], driverHardware: DriverHardware): DrivetrainHardware = { + def apply( + config: DrivetrainConfig, + coreTicks: Stream[Unit], + driverHardware: DriverHardware, + pdp: PowerDistributionPanel + ): DrivetrainHardware = { new DrivetrainHardware( coreTicks, { println(s"[DEBUG] Creating driver left master talon on port ${config.ports.leftPort}") @@ -131,7 +149,9 @@ object DrivetrainHardware { new ADIS16448(new SPI(SPI.Port.kMXP), null) }, driverHardware, - config.props + config.props, + config.ports, + pdp ) } } diff --git a/shared/src/main/scala/com/lynbrookrobotics/eighteen/lift/CubeLiftComp.scala b/shared/src/main/scala/com/lynbrookrobotics/eighteen/lift/CubeLiftComp.scala index 036c530..396d82f 100644 --- a/shared/src/main/scala/com/lynbrookrobotics/eighteen/lift/CubeLiftComp.scala +++ b/shared/src/main/scala/com/lynbrookrobotics/eighteen/lift/CubeLiftComp.scala @@ -1,14 +1,16 @@ package com.lynbrookrobotics.eighteen.lift -import com.lynbrookrobotics.eighteen.SingleOutputChecker -import com.lynbrookrobotics.potassium.Component +import com.lynbrookrobotics.eighteen.{SingleOutputChecker, StallChecker} import com.lynbrookrobotics.potassium.control.offload.OffloadedSignal import com.lynbrookrobotics.potassium.control.offload.OffloadedSignal.OpenLoop import com.lynbrookrobotics.potassium.streams.Stream +import com.lynbrookrobotics.potassium.tasks.Task +import com.lynbrookrobotics.potassium.{Component, Signal} +import edu.wpi.first.wpilibj.RobotState import squants.electro.Volts import squants.{Each, Percent} -class CubeLiftComp(val coreTicks: Stream[Unit])(implicit hardware: CubeLiftHardware) +class CubeLiftComp(val coreTicks: Stream[Unit])(implicit hardware: CubeLiftHardware, props: Signal[CubeLiftProperties]) extends Component[OffloadedSignal] { override def defaultController: Stream[OffloadedSignal] = coreTicks.mapToConstant(OpenLoop(Each(0))) @@ -17,6 +19,18 @@ class CubeLiftComp(val coreTicks: Stream[Unit])(implicit hardware: CubeLiftHardw hardware.talon.getLastCommand ) + StallChecker + .timeAboveThreshold( + hardware.currentDraw, + props.get.maxCurrentDraw + ) + .filter(_ => RobotState.isAutonomous) + .filter(_ > props.get.stallTimeout) + .foreach { stallTime => + println(s"[ERROR] CUBE LIFT STALLED FOR $stallTime. ABORTING TASK.") + Task.abortCurrentTask() + } + override def applySignal(signal: OffloadedSignal): Unit = check.assertSingleOutput { signal match { case OpenLoop(s) => diff --git a/shared/src/main/scala/com/lynbrookrobotics/eighteen/lift/CubeLiftHardware.scala b/shared/src/main/scala/com/lynbrookrobotics/eighteen/lift/CubeLiftHardware.scala index c228b17..5718962 100644 --- a/shared/src/main/scala/com/lynbrookrobotics/eighteen/lift/CubeLiftHardware.scala +++ b/shared/src/main/scala/com/lynbrookrobotics/eighteen/lift/CubeLiftHardware.scala @@ -6,7 +6,7 @@ import com.lynbrookrobotics.eighteen.TalonManager import com.lynbrookrobotics.potassium.commons.lift._ import com.lynbrookrobotics.potassium.frc.LazyTalon import com.lynbrookrobotics.potassium.streams._ -import squants.electro.ElectricPotential +import squants.electro.{Amperes, ElectricCurrent, ElectricPotential} import squants.space.Length import squants.{Dimensionless, Each} @@ -40,6 +40,8 @@ final case class CubeLiftHardware(talon: LazyTalon)(implicit coreTicks: Stream[U def readPotentiometerVoltage: ElectricPotential = props.talonOverVoltage.recip * Each(sensors.getAnalogInRaw) val potentiometerVoltage: Stream[ElectricPotential] = coreTicks.map(_ => readPotentiometerVoltage) + + val currentDraw: Stream[ElectricCurrent] = coreTicks.map(_ => Amperes(talon.t.getOutputCurrent)) } object CubeLiftHardware { diff --git a/shared/src/main/scala/com/lynbrookrobotics/eighteen/lift/CubeLiftProperties.scala b/shared/src/main/scala/com/lynbrookrobotics/eighteen/lift/CubeLiftProperties.scala index eb2fd43..92cebdc 100644 --- a/shared/src/main/scala/com/lynbrookrobotics/eighteen/lift/CubeLiftProperties.scala +++ b/shared/src/main/scala/com/lynbrookrobotics/eighteen/lift/CubeLiftProperties.scala @@ -4,8 +4,8 @@ import com.lynbrookrobotics.potassium.commons.lift.offloaded.OffloadedLiftProper import com.lynbrookrobotics.potassium.control.PIDConfig import com.lynbrookrobotics.potassium.control.offload.EscConfig import com.lynbrookrobotics.potassium.units._ -import squants.Dimensionless -import squants.electro.ElectricPotential +import squants.{Dimensionless, Time, Velocity} +import squants.electro.{ElectricCurrent, ElectricPotential} import squants.motion.Velocity import squants.space.Length @@ -23,7 +23,9 @@ final case class CubeLiftProperties( maxMotorOutput: Dimensionless, maxHeight: Length, minHeight: Length, - twistyTotalRange: Length + twistyTotalRange: Length, + maxCurrentDraw: ElectricCurrent, + stallTimeout: Time ) extends OffloadedLiftProperties { override def positionGains: PIDConfig[ Length,