From 41671b298c1070add96790a68f9eb726cbfffb64 Mon Sep 17 00:00:00 2001 From: Kunal Sheth Date: Sat, 3 Mar 2018 18:38:44 -0800 Subject: [PATCH 01/27] remove outdated comments from Button Mappings --- .../lynbrookrobotics/eighteen/ButtonMappings.scala | 14 +++++++------- .../eighteen/drivetrain/DrivetrainComponent.scala | 6 ++++++ 2 files changed, 13 insertions(+), 7 deletions(-) diff --git a/shared/src/main/scala/com/lynbrookrobotics/eighteen/ButtonMappings.scala b/shared/src/main/scala/com/lynbrookrobotics/eighteen/ButtonMappings.scala index 89d3cab..7a48062 100644 --- 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) ) } } -} +} \ No newline at end of file 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..ef28319 100644 --- a/shared/src/main/scala/com/lynbrookrobotics/eighteen/drivetrain/DrivetrainComponent.scala +++ b/shared/src/main/scala/com/lynbrookrobotics/eighteen/drivetrain/DrivetrainComponent.scala @@ -7,6 +7,7 @@ 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.{Component, Signal} +import squants.motion.FeetPerSecond import squants.{Each, Percent} class DrivetrainComponent(coreTicks: Stream[Unit])( @@ -48,6 +49,11 @@ class DrivetrainComponent(coreTicks: Stream[Unit])( (hardware.left.getLastCommand, hardware.right.getLastCommand) ) + val notMoving = hardware.leftVelocity.zip(hardware.rightVelocity) + .map { t => (t._1 + t._2) / 2 } + .scanLeft(FeetPerSecond(0)) { (acc, v) => (acc + v) / 2 } + .eventWhen(it => it ~= FeetPerSecond(0)) + override def applySignal(signal: TwoSided[OffloadedSignal]): Unit = check.assertSingleOutput { hardware.left.applyCommand(signal.left) hardware.right.applyCommand(signal.right) From b3804e54091ac72f1ad304c43723da037d2bdd73 Mon Sep 17 00:00:00 2001 From: Kunal Sheth Date: Mon, 5 Mar 2018 04:16:16 -0800 Subject: [PATCH 02/27] add StallChecker --- .../lynbrookrobotics/eighteen/Checkers.scala | 39 +++++++++++++++++++ .../eighteen/SingleOutputChecker.scala | 22 ----------- .../drivetrain/DrivetrainComponent.scala | 9 ++--- .../drivetrain/DrivetrainConfig.scala | 2 +- .../drivetrain/DrivetrainHardware.scala | 5 ++- 5 files changed, 47 insertions(+), 30 deletions(-) create mode 100644 shared/src/main/scala/com/lynbrookrobotics/eighteen/Checkers.scala delete mode 100644 shared/src/main/scala/com/lynbrookrobotics/eighteen/SingleOutputChecker.scala diff --git a/shared/src/main/scala/com/lynbrookrobotics/eighteen/Checkers.scala b/shared/src/main/scala/com/lynbrookrobotics/eighteen/Checkers.scala new file mode 100644 index 0000000..7bc89b5 --- /dev/null +++ b/shared/src/main/scala/com/lynbrookrobotics/eighteen/Checkers.scala @@ -0,0 +1,39 @@ +package com.lynbrookrobotics.eighteen + +import com.lynbrookrobotics.potassium.streams._ +import squants.time.Seconds +import squants.{Dimensionless, Time, Velocity} + +class SingleOutputChecker[T](hardwareName: String, get: => T) { + private var lastOutput: Option[T] = None + + def assertSingleOutput(output: => Unit): Unit = { + val currentOutput = get + lastOutput + .filter(_ != currentOutput) + .foreach( + it => + println( + s"""[ERROR] $hardwareName has been set twice! + |[ERROR] -> expected $it, but set to $currentOutput""".stripMargin + ) + ) + + output + + lastOutput = Some(get) + } +} + +class StallChecker(deltaVelocityStallThreshold: => Velocity, maxVelocity: => Velocity) { + def checkStall(velocityAndDc: Stream[(Velocity, Dimensionless)]): Stream[Time] = + velocityAndDc.map { case (actual, dc) => (actual, maxVelocity * dc.toEach) }.map { + case (actual, expected) => expected - actual + }.zipWithDt + .scanLeft(Seconds(0)) { + case (stallTime, (velocityDiff, dt)) => + if (velocityDiff > deltaVelocityStallThreshold) { + stallTime + dt + } else Seconds(0) + } +} diff --git a/shared/src/main/scala/com/lynbrookrobotics/eighteen/SingleOutputChecker.scala b/shared/src/main/scala/com/lynbrookrobotics/eighteen/SingleOutputChecker.scala deleted file mode 100644 index ddc4b87..0000000 --- a/shared/src/main/scala/com/lynbrookrobotics/eighteen/SingleOutputChecker.scala +++ /dev/null @@ -1,22 +0,0 @@ -package com.lynbrookrobotics.eighteen - -class SingleOutputChecker[T](hardwareName: String, get: => T) { - private var lastOutput: Option[T] = None - - def assertSingleOutput(output: => Unit): Unit = { - val currentOutput = get - lastOutput - .filter(_ != currentOutput) - .foreach( - it => - println( - s"""[ERROR] $hardwareName has been set twice! - |[ERROR] -> expected $it, but set to $currentOutput""".stripMargin - ) - ) - - output - - lastOutput = Some(get) - } -} 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 ef28319..babfee6 100644 --- a/shared/src/main/scala/com/lynbrookrobotics/eighteen/drivetrain/DrivetrainComponent.scala +++ b/shared/src/main/scala/com/lynbrookrobotics/eighteen/drivetrain/DrivetrainComponent.scala @@ -6,9 +6,10 @@ 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 squants.motion.FeetPerSecond -import squants.{Each, Percent} +import squants.time.Seconds +import squants.{Each, Percent, Time, Velocity} class DrivetrainComponent(coreTicks: Stream[Unit])( implicit hardware: DrivetrainHardware, @@ -49,10 +50,6 @@ class DrivetrainComponent(coreTicks: Stream[Unit])( (hardware.left.getLastCommand, hardware.right.getLastCommand) ) - val notMoving = hardware.leftVelocity.zip(hardware.rightVelocity) - .map { t => (t._1 + t._2) / 2 } - .scanLeft(FeetPerSecond(0)) { (acc, v) => (acc + v) / 2 } - .eventWhen(it => it ~= FeetPerSecond(0)) override def applySignal(signal: TwoSided[OffloadedSignal]): Unit = check.assertSingleOutput { hardware.left.applyCommand(signal.left) 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..15f3a07 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 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..ffc02ca 100644 --- a/shared/src/main/scala/com/lynbrookrobotics/eighteen/drivetrain/DrivetrainHardware.scala +++ b/shared/src/main/scala/com/lynbrookrobotics/eighteen/drivetrain/DrivetrainHardware.scala @@ -13,7 +13,7 @@ import com.lynbrookrobotics.potassium.units.{Ratio, Value3D} import edu.wpi.first.wpilibj.SPI 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, @@ -101,6 +101,9 @@ final case class DrivetrainHardware( (wheelOverEncoderGears * ar) onRadius (wheelDiameter / 2) } + val leftDutyCycle: Stream[Dimensionless] = coreTicks.map(_ => Each(left.t.getMotorOutputPercent)) + val rightDutyCycle: Stream[Dimensionless] = coreTicks.map(_ => Each(right.t.getMotorOutputPercent)) + override lazy val turnVelocity: Stream[AngularVelocity] = rootDataStream.map(_.gyroVelocities).map(_.z) override lazy val turnPosition: Stream[Angle] = turnVelocity.integral turnPosition.foreach(_ => {}) From c6ccaf6cc4f296a1b9c0cde301d64b56c2bd6427 Mon Sep 17 00:00:00 2001 From: Kunal Sheth Date: Mon, 5 Mar 2018 04:19:04 -0800 Subject: [PATCH 03/27] add stall check params to drivetrain props --- .../eighteen/drivetrain/DrivetrainComponent.scala | 2 +- .../eighteen/drivetrain/DrivetrainConfig.scala | 4 +++- 2 files changed, 4 insertions(+), 2 deletions(-) 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 babfee6..07b0e86 100644 --- a/shared/src/main/scala/com/lynbrookrobotics/eighteen/drivetrain/DrivetrainComponent.scala +++ b/shared/src/main/scala/com/lynbrookrobotics/eighteen/drivetrain/DrivetrainComponent.scala @@ -1,6 +1,6 @@ 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 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 15f3a07..69edca8 100644 --- a/shared/src/main/scala/com/lynbrookrobotics/eighteen/drivetrain/DrivetrainConfig.scala +++ b/shared/src/main/scala/com/lynbrookrobotics/eighteen/drivetrain/DrivetrainConfig.scala @@ -27,7 +27,9 @@ final case class DrivetrainProperties( blendExponent: Double, track: Length, wheelDiameter: Length, - wheelOverEncoderGears: Ratio[Angle, Angle] + wheelOverEncoderGears: Ratio[Angle, Angle], + deltaVelocityStallThreshold: Velocity, + stallTimeout: Time ) extends OffloadedDriveProperties { override val encoderAngleOverTicks: Ratio[Angle, Dimensionless] = Ratio(Turns(1), Each(4096)) override val escConfig: EscConfig[Length] = EscConfig( From 3662e1521309b0e983b5e1dff435f4f6725d501c Mon Sep 17 00:00:00 2001 From: Kunal Sheth Date: Mon, 5 Mar 2018 04:23:10 -0800 Subject: [PATCH 04/27] add stall checks to `DrivetrainComponent` --- .../drivetrain/DrivetrainComponent.scala | 18 ++++++++++++++++-- 1 file changed, 16 insertions(+), 2 deletions(-) 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 07b0e86..08d6de1 100644 --- a/shared/src/main/scala/com/lynbrookrobotics/eighteen/drivetrain/DrivetrainComponent.scala +++ b/shared/src/main/scala/com/lynbrookrobotics/eighteen/drivetrain/DrivetrainComponent.scala @@ -8,8 +8,7 @@ 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 squants.time.Seconds -import squants.{Each, Percent, Time, Velocity} +import squants.{Each, Percent} class DrivetrainComponent(coreTicks: Stream[Unit])( implicit hardware: DrivetrainHardware, @@ -50,6 +49,21 @@ class DrivetrainComponent(coreTicks: Stream[Unit])( (hardware.left.getLastCommand, hardware.right.getLastCommand) ) + new StallChecker(props.get.deltaVelocityStallThreshold, props.get.maxLeftVelocity) + .checkStall(hardware.leftVelocity.zip(hardware.leftDutyCycle)) + .filter(_ > props.get.stallTimeout) + .foreach { time => + println(s"[ERROR] LEFT SIDE OF DRIVETRAIN STALLED FOR $time. ABORTING TASK.") + Task.abortCurrentTask() + } + + new StallChecker(props.get.deltaVelocityStallThreshold, props.get.maxRightVelocity) + .checkStall(hardware.rightVelocity.zip(hardware.rightDutyCycle)) + .filter(_ > props.get.stallTimeout) + .foreach { time => + println(s"[ERROR] RIGHT SIDE OF DRIVETRAIN STALLED FOR $time. ABORTING TASK.") + Task.abortCurrentTask() + } override def applySignal(signal: TwoSided[OffloadedSignal]): Unit = check.assertSingleOutput { hardware.left.applyCommand(signal.left) From 189b6f81af6c80e17015110fcb61920318b634fd Mon Sep 17 00:00:00 2001 From: Kunal Sheth Date: Mon, 5 Mar 2018 04:37:29 -0800 Subject: [PATCH 05/27] make stall checker generic --- .../com/lynbrookrobotics/eighteen/Checkers.scala | 14 ++++++-------- .../eighteen/lift/CubeLiftComp.scala | 5 +++-- .../eighteen/lift/CubeLiftHardware.scala | 4 +++- .../eighteen/lift/CubeLiftProperties.scala | 8 +++++--- 4 files changed, 17 insertions(+), 14 deletions(-) diff --git a/shared/src/main/scala/com/lynbrookrobotics/eighteen/Checkers.scala b/shared/src/main/scala/com/lynbrookrobotics/eighteen/Checkers.scala index 7bc89b5..6f9941f 100644 --- a/shared/src/main/scala/com/lynbrookrobotics/eighteen/Checkers.scala +++ b/shared/src/main/scala/com/lynbrookrobotics/eighteen/Checkers.scala @@ -2,7 +2,7 @@ package com.lynbrookrobotics.eighteen import com.lynbrookrobotics.potassium.streams._ import squants.time.Seconds -import squants.{Dimensionless, Time, Velocity} +import squants.{Dimensionless, Quantity, Time, Velocity} class SingleOutputChecker[T](hardwareName: String, get: => T) { private var lastOutput: Option[T] = None @@ -25,14 +25,12 @@ class SingleOutputChecker[T](hardwareName: String, get: => T) { } } -class StallChecker(deltaVelocityStallThreshold: => Velocity, maxVelocity: => Velocity) { - def checkStall(velocityAndDc: Stream[(Velocity, Dimensionless)]): Stream[Time] = - velocityAndDc.map { case (actual, dc) => (actual, maxVelocity * dc.toEach) }.map { - case (actual, expected) => expected - actual - }.zipWithDt +object StallChecker { + def timeAboveThreshold[Q<:Quantity[Q]](stream: Stream[Q], threshold: Q): Stream[Time] = stream + .zipWithDt .scanLeft(Seconds(0)) { - case (stallTime, (velocityDiff, dt)) => - if (velocityDiff > deltaVelocityStallThreshold) { + case (stallTime, (value, dt)) => + if (value > threshold) { stallTime + dt } else Seconds(0) } 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..d00a9be 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,15 @@ package com.lynbrookrobotics.eighteen.lift import com.lynbrookrobotics.eighteen.SingleOutputChecker -import com.lynbrookrobotics.potassium.Component 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 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))) 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..80bff0c 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: ElectricCurrent ) extends OffloadedLiftProperties { override def positionGains: PIDConfig[ Length, From 4b9a7ace7e6c1f8643895294303f6525db1d36fe Mon Sep 17 00:00:00 2001 From: Kunal Sheth Date: Mon, 5 Mar 2018 04:40:58 -0800 Subject: [PATCH 06/27] make drivetrain use generic stall checker --- .../lynbrookrobotics/eighteen/Checkers.scala | 4 ++-- .../drivetrain/DrivetrainComponent.scala | 18 ++++++++++++++---- 2 files changed, 16 insertions(+), 6 deletions(-) diff --git a/shared/src/main/scala/com/lynbrookrobotics/eighteen/Checkers.scala b/shared/src/main/scala/com/lynbrookrobotics/eighteen/Checkers.scala index 6f9941f..b3b80d3 100644 --- a/shared/src/main/scala/com/lynbrookrobotics/eighteen/Checkers.scala +++ b/shared/src/main/scala/com/lynbrookrobotics/eighteen/Checkers.scala @@ -26,8 +26,8 @@ class SingleOutputChecker[T](hardwareName: String, get: => T) { } object StallChecker { - def timeAboveThreshold[Q<:Quantity[Q]](stream: Stream[Q], threshold: Q): Stream[Time] = stream - .zipWithDt + def timeAboveThreshold[Q <: Quantity[Q]](stream: Stream[Q], threshold: => Q): Stream[Time] = + stream.zipWithDt .scanLeft(Seconds(0)) { case (stallTime, (value, dt)) => if (value > threshold) { 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 08d6de1..0993fb4 100644 --- a/shared/src/main/scala/com/lynbrookrobotics/eighteen/drivetrain/DrivetrainComponent.scala +++ b/shared/src/main/scala/com/lynbrookrobotics/eighteen/drivetrain/DrivetrainComponent.scala @@ -49,16 +49,26 @@ class DrivetrainComponent(coreTicks: Stream[Unit])( (hardware.left.getLastCommand, hardware.right.getLastCommand) ) - new StallChecker(props.get.deltaVelocityStallThreshold, props.get.maxLeftVelocity) - .checkStall(hardware.leftVelocity.zip(hardware.leftDutyCycle)) + StallChecker + .timeAboveThreshold( + hardware.leftVelocity.zip(hardware.leftDutyCycle).map { + case (currVelocity, dutyCycle) => (props.get.maxLeftVelocity * dutyCycle.toEach) - currVelocity + }, + props.get.deltaVelocityStallThreshold + ) .filter(_ > props.get.stallTimeout) .foreach { time => println(s"[ERROR] LEFT SIDE OF DRIVETRAIN STALLED FOR $time. ABORTING TASK.") Task.abortCurrentTask() } - new StallChecker(props.get.deltaVelocityStallThreshold, props.get.maxRightVelocity) - .checkStall(hardware.rightVelocity.zip(hardware.rightDutyCycle)) + StallChecker + .timeAboveThreshold( + hardware.rightVelocity.zip(hardware.rightDutyCycle).map { + case (currVelocity, dutyCycle) => (props.get.maxLeftVelocity * dutyCycle.toEach) - currVelocity + }, + props.get.deltaVelocityStallThreshold + ) .filter(_ > props.get.stallTimeout) .foreach { time => println(s"[ERROR] RIGHT SIDE OF DRIVETRAIN STALLED FOR $time. ABORTING TASK.") From 37d654e0e50bb06a3dd7bee66d831d33217cd8e7 Mon Sep 17 00:00:00 2001 From: Kunal Sheth Date: Mon, 5 Mar 2018 04:43:47 -0800 Subject: [PATCH 07/27] add stall check to cube lift --- .../eighteen/lift/CubeLiftComp.scala | 14 ++++++++++++-- .../eighteen/lift/CubeLiftProperties.scala | 2 +- 2 files changed, 13 insertions(+), 3 deletions(-) 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 d00a9be..263f346 100644 --- a/shared/src/main/scala/com/lynbrookrobotics/eighteen/lift/CubeLiftComp.scala +++ b/shared/src/main/scala/com/lynbrookrobotics/eighteen/lift/CubeLiftComp.scala @@ -1,6 +1,6 @@ package com.lynbrookrobotics.eighteen.lift -import com.lynbrookrobotics.eighteen.SingleOutputChecker +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 @@ -10,7 +10,7 @@ import squants.electro.Volts import squants.{Each, Percent} class CubeLiftComp(val coreTicks: Stream[Unit])(implicit hardware: CubeLiftHardware, props: Signal[CubeLiftProperties]) - extends Component[OffloadedSignal] { + extends Component[OffloadedSignal] { override def defaultController: Stream[OffloadedSignal] = coreTicks.mapToConstant(OpenLoop(Each(0))) private val check = new SingleOutputChecker( @@ -18,6 +18,16 @@ class CubeLiftComp(val coreTicks: Stream[Unit])(implicit hardware: CubeLiftHardw hardware.talon.getLastCommand ) + StallChecker.timeAboveThreshold( + hardware.currentDraw, + props.get.maxCurrentDraw + ) + .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/CubeLiftProperties.scala b/shared/src/main/scala/com/lynbrookrobotics/eighteen/lift/CubeLiftProperties.scala index 80bff0c..92cebdc 100644 --- a/shared/src/main/scala/com/lynbrookrobotics/eighteen/lift/CubeLiftProperties.scala +++ b/shared/src/main/scala/com/lynbrookrobotics/eighteen/lift/CubeLiftProperties.scala @@ -25,7 +25,7 @@ final case class CubeLiftProperties( minHeight: Length, twistyTotalRange: Length, maxCurrentDraw: ElectricCurrent, - stallTimeout: ElectricCurrent + stallTimeout: Time ) extends OffloadedLiftProperties { override def positionGains: PIDConfig[ Length, From c9c614a05e156659c40ecfc7809593a6829785b1 Mon Sep 17 00:00:00 2001 From: Kunal Sheth Date: Mon, 5 Mar 2018 04:45:05 -0800 Subject: [PATCH 08/27] run scalafmt --- .../eighteen/drivetrain/DrivetrainComponent.scala | 2 +- .../lynbrookrobotics/eighteen/lift/CubeLiftComp.scala | 11 ++++++----- 2 files changed, 7 insertions(+), 6 deletions(-) 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 0993fb4..4fafba7 100644 --- a/shared/src/main/scala/com/lynbrookrobotics/eighteen/drivetrain/DrivetrainComponent.scala +++ b/shared/src/main/scala/com/lynbrookrobotics/eighteen/drivetrain/DrivetrainComponent.scala @@ -65,7 +65,7 @@ class DrivetrainComponent(coreTicks: Stream[Unit])( StallChecker .timeAboveThreshold( hardware.rightVelocity.zip(hardware.rightDutyCycle).map { - case (currVelocity, dutyCycle) => (props.get.maxLeftVelocity * dutyCycle.toEach) - currVelocity + case (currVelocity, dutyCycle) => (props.get.maxRightVelocity * dutyCycle.toEach) - currVelocity }, props.get.deltaVelocityStallThreshold ) 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 263f346..760bfb1 100644 --- a/shared/src/main/scala/com/lynbrookrobotics/eighteen/lift/CubeLiftComp.scala +++ b/shared/src/main/scala/com/lynbrookrobotics/eighteen/lift/CubeLiftComp.scala @@ -10,7 +10,7 @@ import squants.electro.Volts import squants.{Each, Percent} class CubeLiftComp(val coreTicks: Stream[Unit])(implicit hardware: CubeLiftHardware, props: Signal[CubeLiftProperties]) - extends Component[OffloadedSignal] { + extends Component[OffloadedSignal] { override def defaultController: Stream[OffloadedSignal] = coreTicks.mapToConstant(OpenLoop(Each(0))) private val check = new SingleOutputChecker( @@ -18,10 +18,11 @@ class CubeLiftComp(val coreTicks: Stream[Unit])(implicit hardware: CubeLiftHardw hardware.talon.getLastCommand ) - StallChecker.timeAboveThreshold( - hardware.currentDraw, - props.get.maxCurrentDraw - ) + StallChecker + .timeAboveThreshold( + hardware.currentDraw, + props.get.maxCurrentDraw + ) .filter(_ > props.get.stallTimeout) .foreach { stallTime => println(s"[ERROR] CUBE LIFT STALLED FOR $stallTime. ABORTING TASK.") From 8feb871278ff39fbb1f412a4d6198b51e011ccba Mon Sep 17 00:00:00 2001 From: Kunal Sheth Date: Mon, 5 Mar 2018 04:49:40 -0800 Subject: [PATCH 09/27] add missing params to ConfigGenerator --- .../com/lynbrookrobotics/eighteen/ConfigGenerator.scala | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/jvm/src/test/scala/com/lynbrookrobotics/eighteen/ConfigGenerator.scala b/jvm/src/test/scala/com/lynbrookrobotics/eighteen/ConfigGenerator.scala index ab988df..7f49e77 100644 --- a/jvm/src/test/scala/com/lynbrookrobotics/eighteen/ConfigGenerator.scala +++ b/jvm/src/test/scala/com/lynbrookrobotics/eighteen/ConfigGenerator.scala @@ -77,7 +77,9 @@ object ConfigGenerator extends App { wheelOverEncoderGears = Ratio( Turns(18), Turns(74) - ) + ), + deltaVelocityStallThreshold = FeetPerSecond(10), + stallTimeout = Seconds(3) ) ) ), @@ -103,7 +105,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) ) ) ), From d1379be406a30094e320a1e1c0c9577f0509c24e Mon Sep 17 00:00:00 2001 From: Kunal Sheth Date: Mon, 5 Mar 2018 04:50:12 -0800 Subject: [PATCH 10/27] run scalafmt --- .../src/main/scala/com/lynbrookrobotics/eighteen/Checkers.scala | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/shared/src/main/scala/com/lynbrookrobotics/eighteen/Checkers.scala b/shared/src/main/scala/com/lynbrookrobotics/eighteen/Checkers.scala index b3b80d3..6392c63 100644 --- a/shared/src/main/scala/com/lynbrookrobotics/eighteen/Checkers.scala +++ b/shared/src/main/scala/com/lynbrookrobotics/eighteen/Checkers.scala @@ -2,7 +2,7 @@ package com.lynbrookrobotics.eighteen import com.lynbrookrobotics.potassium.streams._ import squants.time.Seconds -import squants.{Dimensionless, Quantity, Time, Velocity} +import squants.{Quantity, Time} class SingleOutputChecker[T](hardwareName: String, get: => T) { private var lastOutput: Option[T] = None From f51b6306748c78e9b1b06ebd8f25ddd8c7733df7 Mon Sep 17 00:00:00 2001 From: Kunal Sheth Date: Mon, 5 Mar 2018 04:56:17 -0800 Subject: [PATCH 11/27] add missing params to robot-config.json --- robot-config.json | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) diff --git a/robot-config.json b/robot-config.json index ad64bd2..fbb9aa9 100644 --- a/robot-config.json +++ b/robot-config.json @@ -96,6 +96,14 @@ "twistyTotalRange": [ 0.75, "Feet" + ], + "maxCurrentDraw": [ + 20, + "Amperes" + ], + "stallTimeout": [ + 3, + "Seconds" ] }, "ports": { @@ -319,7 +327,15 @@ "Percent" ] } - } + }, + "deltaVelocityStallThreshold": [ + 10, + "FeetPerSecond" + ], + "stallTimeout": [ + 3, + "Seconds" + ] } }, "driver": { From b0b6abfdf978dcc8e3a6807e34a55aff20eee733 Mon Sep 17 00:00:00 2001 From: Kunal Sheth Date: Mon, 5 Mar 2018 05:04:35 -0800 Subject: [PATCH 12/27] stream all motor currents from drivetrain hardware --- .../eighteen/drivetrain/DrivetrainHardware.scala | 6 ++++++ 1 file changed, 6 insertions(+) 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 ffc02ca..f18cb29 100644 --- a/shared/src/main/scala/com/lynbrookrobotics/eighteen/drivetrain/DrivetrainHardware.scala +++ b/shared/src/main/scala/com/lynbrookrobotics/eighteen/drivetrain/DrivetrainHardware.scala @@ -11,6 +11,7 @@ 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 squants.electro.{Amperes, ElectricCurrent} import squants.motion.AngularVelocity import squants.time.{Milliseconds, Seconds} import squants.{Angle, Dimensionless, Each, Length, Velocity} @@ -104,6 +105,11 @@ final case class DrivetrainHardware( 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(left.t.getOutputCurrent)) + val rightMasterCurrent: Stream[ElectricCurrent] = coreTicks.map(_ => Amperes(right.t.getOutputCurrent)) + val leftFollowerCurrent: Stream[ElectricCurrent] = coreTicks.map(_ => Amperes(leftFollowerSRX.getOutputCurrent)) + val rightFollowerCurrent: Stream[ElectricCurrent] = coreTicks.map(_ => Amperes(rightFollowerSRX.getOutputCurrent)) + override lazy val turnVelocity: Stream[AngularVelocity] = rootDataStream.map(_.gyroVelocities).map(_.z) override lazy val turnPosition: Stream[Angle] = turnVelocity.integral turnPosition.foreach(_ => {}) From 0ce9cba38574404fb55ffb58addc1e783b626e50 Mon Sep 17 00:00:00 2001 From: Kunal Sheth Date: Mon, 5 Mar 2018 05:09:21 -0800 Subject: [PATCH 13/27] log all drivetrain motors' current in funky dash --- .../lynbrookrobotics/eighteen/CoreRobot.scala | 66 +++++++++---------- 1 file changed, 33 insertions(+), 33 deletions(-) diff --git a/shared/src/main/scala/com/lynbrookrobotics/eighteen/CoreRobot.scala b/shared/src/main/scala/com/lynbrookrobotics/eighteen/CoreRobot.scala index 6b17663..6bbb5a5 100644 --- 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( @@ -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 @@ -525,4 +525,4 @@ object CoreRobot { } } -} +} \ No newline at end of file From d419219e76073619384cb932ba1ffc3fbf062172 Mon Sep 17 00:00:00 2001 From: Kunal Sheth Date: Mon, 5 Mar 2018 05:17:02 -0800 Subject: [PATCH 14/27] enable slew rate limiting on collector rollers --- .../collector/rollers/CollectorRollers.scala | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) 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..7073e87 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) From 6c20f06d35396a407bb3badccc840bf8f64e8ada Mon Sep 17 00:00:00 2001 From: Kunal Sheth Date: Mon, 5 Mar 2018 05:22:37 -0800 Subject: [PATCH 15/27] add parallel motor current draw alert to drivetrain --- .../drivetrain/DrivetrainComponent.scala | 23 +++++++++++++++++++ .../drivetrain/DrivetrainConfig.scala | 1 + 2 files changed, 24 insertions(+) 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 4fafba7..25f82a9 100644 --- a/shared/src/main/scala/com/lynbrookrobotics/eighteen/drivetrain/DrivetrainComponent.scala +++ b/shared/src/main/scala/com/lynbrookrobotics/eighteen/drivetrain/DrivetrainComponent.scala @@ -8,6 +8,7 @@ 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 squants.time.Seconds import squants.{Each, Percent} class DrivetrainComponent(coreTicks: Stream[Unit])( @@ -75,6 +76,28 @@ class DrivetrainComponent(coreTicks: Stream[Unit])( Task.abortCurrentTask() } + StallChecker + .timeAboveThreshold( + hardware.rightMasterCurrent.zip(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.zip(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 69edca8..011499a 100644 --- 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,7 @@ final case class DrivetrainProperties( track: Length, wheelDiameter: Length, wheelOverEncoderGears: Ratio[Angle, Angle], + parallelMotorCurrentThreshold: ElectricCurrent, deltaVelocityStallThreshold: Velocity, stallTimeout: Time ) extends OffloadedDriveProperties { From bfd9addfeb9247406c5cf8c3e4c8fced13979a8b Mon Sep 17 00:00:00 2001 From: Kunal Sheth Date: Mon, 5 Mar 2018 05:24:27 -0800 Subject: [PATCH 16/27] add missing params to robot-config.json and ConfigGenerator --- .../scala/com/lynbrookrobotics/eighteen/ConfigGenerator.scala | 1 + robot-config.json | 4 ++++ 2 files changed, 5 insertions(+) diff --git a/jvm/src/test/scala/com/lynbrookrobotics/eighteen/ConfigGenerator.scala b/jvm/src/test/scala/com/lynbrookrobotics/eighteen/ConfigGenerator.scala index 7f49e77..7dca8ca 100644 --- a/jvm/src/test/scala/com/lynbrookrobotics/eighteen/ConfigGenerator.scala +++ b/jvm/src/test/scala/com/lynbrookrobotics/eighteen/ConfigGenerator.scala @@ -78,6 +78,7 @@ object ConfigGenerator extends App { Turns(18), Turns(74) ), + parallelMotorCurrentThreshold = Amperes(5), deltaVelocityStallThreshold = FeetPerSecond(10), stallTimeout = Seconds(3) ) diff --git a/robot-config.json b/robot-config.json index fbb9aa9..edc0dd8 100644 --- a/robot-config.json +++ b/robot-config.json @@ -177,6 +177,10 @@ 35, "Amperes" ], + "parallelMotorCurrentThreshold" : [ + 5, + "Amperes" + ], "maxRightVelocity": [ 13.3, "FeetPerSecond" From a963723f50db7261d8506de87c99f3e72bc5d628 Mon Sep 17 00:00:00 2001 From: Kunal Sheth Date: Mon, 5 Mar 2018 05:25:09 -0800 Subject: [PATCH 17/27] run scalafmt --- .../eighteen/drivetrain/DrivetrainComponent.scala | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) 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 25f82a9..f5457ae 100644 --- a/shared/src/main/scala/com/lynbrookrobotics/eighteen/drivetrain/DrivetrainComponent.scala +++ b/shared/src/main/scala/com/lynbrookrobotics/eighteen/drivetrain/DrivetrainComponent.scala @@ -78,8 +78,7 @@ class DrivetrainComponent(coreTicks: Stream[Unit])( StallChecker .timeAboveThreshold( - hardware.rightMasterCurrent.zip(hardware.rightFollowerCurrent) - .map { case (m, f) => (m - f).abs }, + hardware.rightMasterCurrent.zip(hardware.rightFollowerCurrent).map { case (m, f) => (m - f).abs }, props.get.parallelMotorCurrentThreshold ) .filter(_ <= Seconds(0)) @@ -89,8 +88,7 @@ class DrivetrainComponent(coreTicks: Stream[Unit])( StallChecker .timeAboveThreshold( - hardware.leftMasterCurrent.zip(hardware.leftFollowerCurrent) - .map { case (m, f) => (m - f).abs }, + hardware.leftMasterCurrent.zip(hardware.leftFollowerCurrent).map { case (m, f) => (m - f).abs }, props.get.parallelMotorCurrentThreshold ) .filter(_ <= Seconds(0)) From 484f43ad02657c40b9adbfb2cb8db8f2cddf00a1 Mon Sep 17 00:00:00 2001 From: Kunal Sheth Date: Mon, 5 Mar 2018 05:30:21 -0800 Subject: [PATCH 18/27] remove parens from speed controller .get method call --- .../eighteen/collector/rollers/CollectorRollers.scala | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 7073e87..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 @@ -27,13 +27,13 @@ class CollectorRollers(val coreTicks: Stream[Unit])( override def setController(controller: Stream[(Dimensionless, Dimensionless)]): Unit = { val l = CurrentLimiting.slewRate( - Each(hardware.rollerLeft.get()), + Each(hardware.rollerLeft.get), controller.map(_._1), Percent(100) / Seconds(0.3) ) val r = CurrentLimiting.slewRate( - Each(hardware.rollerRight.get()), + Each(hardware.rollerRight.get), controller.map(_._2), Percent(100) / Seconds(0.3) ) From 89cb5263db06ac209119b8ad2e18a7bb8df1b50b Mon Sep 17 00:00:00 2001 From: Kunal Sheth Date: Tue, 6 Mar 2018 02:50:55 -0800 Subject: [PATCH 19/27] add missing params to robot configs --- practice-robot-config.json | 22 ++++++++++++++++- .../eighteen/DefaultConfig.scala | 24 +++++++++++++++++-- 2 files changed, 43 insertions(+), 3 deletions(-) diff --git a/practice-robot-config.json b/practice-robot-config.json index f1ab124..8a827f2 100644 --- a/practice-robot-config.json +++ b/practice-robot-config.json @@ -93,6 +93,14 @@ 0.25, "Inches" ], + "maxCurrentDraw": [ + 20, + "Amperes" + ], + "stallTimeout": [ + 3, + "Seconds" + ], "twistyTotalRange": [ -1.5, "Feet" @@ -169,6 +177,10 @@ 35, "Amperes" ], + "parallelMotorCurrentThreshold": [ + 5, + "Amperes" + ], "maxRightVelocity": [ 13.5, "FeetPerSecond" @@ -319,7 +331,15 @@ "Percent" ] } - } + }, + "deltaVelocityStallThreshold": [ + 10, + "FeetPerSecond" + ], + "stallTimeout": [ + 3, + "Seconds" + ] } }, "driver": { diff --git a/shared/src/main/scala/com/lynbrookrobotics/eighteen/DefaultConfig.scala b/shared/src/main/scala/com/lynbrookrobotics/eighteen/DefaultConfig.scala index 9d0b700..9e02cd3 100644 --- a/shared/src/main/scala/com/lynbrookrobotics/eighteen/DefaultConfig.scala +++ b/shared/src/main/scala/com/lynbrookrobotics/eighteen/DefaultConfig.scala @@ -96,8 +96,16 @@ object DefaultConfig { | 0.25, | "Inches" | ], + | "maxCurrentDraw": [ + | 20, + | "Amperes" + | ], + | "stallTimeout": [ + | 3, + | "Seconds" + | ], | "twistyTotalRange": [ - | 1.5, + | -1.5, | "Feet" | ] | }, @@ -172,6 +180,10 @@ object DefaultConfig { | 35, | "Amperes" | ], + | "parallelMotorCurrentThreshold" : [ + | 5, + | "Amperes" + | ], | "maxRightVelocity": [ | 17.7, | "FeetPerSecond" @@ -322,7 +334,15 @@ object DefaultConfig { | "Percent" | ] | } - | } + | }, + | "deltaVelocityStallThreshold": [ + | 10, + | "FeetPerSecond" + | ], + | "stallTimeout": [ + | 3, + | "Seconds" + | ] | } | }, | "driver": { From 749cf90789613d8000c284a2730ee7a0468fb4ba Mon Sep 17 00:00:00 2001 From: Kunal Sheth Date: Wed, 7 Mar 2018 20:21:21 -0600 Subject: [PATCH 20/27] only allow task abortion incase of stall during auto --- .../eighteen/drivetrain/DrivetrainComponent.scala | 3 +++ .../com/lynbrookrobotics/eighteen/lift/CubeLiftComp.scala | 2 ++ 2 files changed, 5 insertions(+) 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 f5457ae..29fe05b 100644 --- a/shared/src/main/scala/com/lynbrookrobotics/eighteen/drivetrain/DrivetrainComponent.scala +++ b/shared/src/main/scala/com/lynbrookrobotics/eighteen/drivetrain/DrivetrainComponent.scala @@ -8,6 +8,7 @@ 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} @@ -57,6 +58,7 @@ class DrivetrainComponent(coreTicks: Stream[Unit])( }, props.get.deltaVelocityStallThreshold ) + .filter(_ => RobotState.isAutonomous) .filter(_ > props.get.stallTimeout) .foreach { time => println(s"[ERROR] LEFT SIDE OF DRIVETRAIN STALLED FOR $time. ABORTING TASK.") @@ -70,6 +72,7 @@ class DrivetrainComponent(coreTicks: Stream[Unit])( }, props.get.deltaVelocityStallThreshold ) + .filter(_ => RobotState.isAutonomous) .filter(_ > props.get.stallTimeout) .foreach { time => println(s"[ERROR] RIGHT SIDE OF DRIVETRAIN STALLED FOR $time. ABORTING TASK.") 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 760bfb1..396d82f 100644 --- a/shared/src/main/scala/com/lynbrookrobotics/eighteen/lift/CubeLiftComp.scala +++ b/shared/src/main/scala/com/lynbrookrobotics/eighteen/lift/CubeLiftComp.scala @@ -6,6 +6,7 @@ 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} @@ -23,6 +24,7 @@ class CubeLiftComp(val coreTicks: Stream[Unit])(implicit hardware: CubeLiftHardw hardware.currentDraw, props.get.maxCurrentDraw ) + .filter(_ => RobotState.isAutonomous) .filter(_ > props.get.stallTimeout) .foreach { stallTime => println(s"[ERROR] CUBE LIFT STALLED FOR $stallTime. ABORTING TASK.") From 94e334d0cdc1fda118179c10af067482d31459b2 Mon Sep 17 00:00:00 2001 From: Kunal Sheth Date: Sat, 17 Mar 2018 17:27:33 -0700 Subject: [PATCH 21/27] fix logic error on Drivetrain Stall check --- .../eighteen/drivetrain/DrivetrainComponent.scala | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) 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 29fe05b..48a46d9 100644 --- a/shared/src/main/scala/com/lynbrookrobotics/eighteen/drivetrain/DrivetrainComponent.scala +++ b/shared/src/main/scala/com/lynbrookrobotics/eighteen/drivetrain/DrivetrainComponent.scala @@ -53,7 +53,7 @@ class DrivetrainComponent(coreTicks: Stream[Unit])( StallChecker .timeAboveThreshold( - hardware.leftVelocity.zip(hardware.leftDutyCycle).map { + hardware.leftVelocity.zipAsync(hardware.leftDutyCycle).map { case (currVelocity, dutyCycle) => (props.get.maxLeftVelocity * dutyCycle.toEach) - currVelocity }, props.get.deltaVelocityStallThreshold @@ -67,7 +67,7 @@ class DrivetrainComponent(coreTicks: Stream[Unit])( StallChecker .timeAboveThreshold( - hardware.rightVelocity.zip(hardware.rightDutyCycle).map { + hardware.rightVelocity.zipAsync(hardware.rightDutyCycle).map { case (currVelocity, dutyCycle) => (props.get.maxRightVelocity * dutyCycle.toEach) - currVelocity }, props.get.deltaVelocityStallThreshold @@ -81,20 +81,20 @@ class DrivetrainComponent(coreTicks: Stream[Unit])( StallChecker .timeAboveThreshold( - hardware.rightMasterCurrent.zip(hardware.rightFollowerCurrent).map { case (m, f) => (m - f).abs }, + hardware.rightMasterCurrent.zipAsync(hardware.rightFollowerCurrent).map { case (m, f) => (m - f).abs }, props.get.parallelMotorCurrentThreshold ) - .filter(_ <= Seconds(0)) + .filter(_ > Seconds(0)) .foreach { time => println(s"[WARNING] RIGHT MASTER AND RIGHT FOLLOWER HAVE DIFFERENT CURRENT DRAWS. CHECK FUNKY DASHBOARD.") } StallChecker .timeAboveThreshold( - hardware.leftMasterCurrent.zip(hardware.leftFollowerCurrent).map { case (m, f) => (m - f).abs }, + hardware.leftMasterCurrent.zipAsync(hardware.leftFollowerCurrent).map { case (m, f) => (m - f).abs }, props.get.parallelMotorCurrentThreshold ) - .filter(_ <= Seconds(0)) + .filter(_ > Seconds(0)) .foreach { time => println(s"[WARNING] LEFT MASTER AND LEFT FOLLOWER HAVE DIFFERENT CURRENT DRAWS. CHECK FUNKY DASHBOARD.") } From 430208a3bfd6b0ebbe55b18c4a474abc7c4a84bd Mon Sep 17 00:00:00 2001 From: Kunal Sheth Date: Sat, 17 Mar 2018 17:35:10 -0700 Subject: [PATCH 22/27] rename follower ESCs in DrivetrainHardware tmp --- practice-robot-config.json | 6 +++- robot-config.json | 6 +++- .../lynbrookrobotics/eighteen/CoreRobot.scala | 4 +-- .../drivetrain/DrivetrainConfig.scala | 6 +++- .../drivetrain/DrivetrainHardware.scala | 36 ++++++++++--------- 5 files changed, 36 insertions(+), 22 deletions(-) diff --git a/practice-robot-config.json b/practice-robot-config.json index 8a827f2..68afb49 100644 --- a/practice-robot-config.json +++ b/practice-robot-config.json @@ -122,7 +122,11 @@ "rightFollowerPort": 14, "leftFollowerPort": 12, "rightPort": 13, - "leftPort": 11 + "leftPort": 11, + "rightFollowerPdpPort": 3, + "leftFollowerPdpPort": 1, + "rightPdpPort": 2, + "leftPdpPort": 0 }, "props": { "maxLeftVelocity": [ diff --git a/robot-config.json b/robot-config.json index edc0dd8..feee9cf 100644 --- a/robot-config.json +++ b/robot-config.json @@ -122,7 +122,11 @@ "rightFollowerPort": 13, "leftFollowerPort": 14, "rightPort": 11, - "leftPort": 12 + "leftPort": 12, + "rightFollowerPdpPort": 3, + "leftFollowerPdpPort": 1, + "rightPdpPort": 2, + "leftPdpPort": 0 }, "props": { "maxLeftVelocity": [ diff --git a/shared/src/main/scala/com/lynbrookrobotics/eighteen/CoreRobot.scala b/shared/src/main/scala/com/lynbrookrobotics/eighteen/CoreRobot.scala index 6bbb5a5..a12fcfc 100644 --- a/shared/src/main/scala/com/lynbrookrobotics/eighteen/CoreRobot.scala +++ b/shared/src/main/scala/com/lynbrookrobotics/eighteen/CoreRobot.scala @@ -431,7 +431,7 @@ class CoreRobot(configFileValue: Signal[String], updateConfigFile: String => Uni .datasetGroup("Drivetrain/Current") .addDataset( coreTicks - .map(_ => drivetrainHardware.leftFollowerSRX.getOutputCurrent) + .map(_ => drivetrainHardware.leftFollower.getOutputCurrent) .toTimeSeriesNumeric("Left follower current") ) @@ -445,7 +445,7 @@ class CoreRobot(configFileValue: Signal[String], updateConfigFile: String => Uni .datasetGroup("Drivetrain/Current") .addDataset( coreTicks - .map(_ => drivetrainHardware.rightFollowerSRX.getOutputCurrent) + .map(_ => drivetrainHardware.rightFollower.getOutputCurrent) .toTimeSeriesNumeric("Right follower current") ) 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 011499a..3f8ae11 100644 --- a/shared/src/main/scala/com/lynbrookrobotics/eighteen/drivetrain/DrivetrainConfig.scala +++ b/shared/src/main/scala/com/lynbrookrobotics/eighteen/drivetrain/DrivetrainConfig.scala @@ -47,5 +47,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 f18cb29..173cccb 100644 --- a/shared/src/main/scala/com/lynbrookrobotics/eighteen/drivetrain/DrivetrainHardware.scala +++ b/shared/src/main/scala/com/lynbrookrobotics/eighteen/drivetrain/DrivetrainHardware.scala @@ -10,7 +10,7 @@ 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} @@ -25,14 +25,16 @@ final case class DrivetrainData( ) final case class DrivetrainHardware( - coreTicks: Stream[Unit], - leftSRX: TalonSRX, - rightSRX: TalonSRX, - leftFollowerSRX: BaseMotorController, - rightFollowerSRX: BaseMotorController, - gyro: DigitalGyro, - driverHardware: DriverHardware, - props: DrivetrainProperties + coreTicks: Stream[Unit], + leftSRX: TalonSRX, + rightSRX: TalonSRX, + leftFollower: BaseMotorController, + rightFollower: BaseMotorController, + gyro: DigitalGyro, + driverHardware: DriverHardware, + props: DrivetrainProperties, + ports: DrivetrainPorts, + pdp: PowerDistributionPanel ) extends TwoSidedDriveHardware { override val track: Length = props.track @@ -44,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._ @@ -59,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 => @@ -105,10 +107,10 @@ final case class DrivetrainHardware( 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(left.t.getOutputCurrent)) - val rightMasterCurrent: Stream[ElectricCurrent] = coreTicks.map(_ => Amperes(right.t.getOutputCurrent)) - val leftFollowerCurrent: Stream[ElectricCurrent] = coreTicks.map(_ => Amperes(leftFollowerSRX.getOutputCurrent)) - val rightFollowerCurrent: Stream[ElectricCurrent] = coreTicks.map(_ => Amperes(rightFollowerSRX.getOutputCurrent)) + 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.leftFollowerPdpPort))) override lazy val turnVelocity: Stream[AngularVelocity] = rootDataStream.map(_.gyroVelocities).map(_.z) override lazy val turnPosition: Stream[Angle] = turnVelocity.integral From bd46eac017596f03b0d8eb6e3e6a7c51fd8bb792 Mon Sep 17 00:00:00 2001 From: Kunal Sheth Date: Mon, 19 Mar 2018 18:56:20 -0700 Subject: [PATCH 23/27] measure current using PDP instead of Victors & add fudging to drivetrain config run scalafmt tmp --- build.sbt | 6 +- .../eighteen/ConfigGenerator.scala | 20 ++++--- practice-robot-config.json | 2 + robot-config.json | 2 + .../eighteen/ButtonMappings.scala | 14 ++--- .../lynbrookrobotics/eighteen/CoreRobot.scala | 16 ++---- .../eighteen/DefaultConfig.scala | 57 ++++++++++++------- .../eighteen/RobotHardware.scala | 4 +- .../drivetrain/DrivetrainConfig.scala | 2 + .../drivetrain/DrivetrainHardware.scala | 41 +++++++------ 10 files changed, 100 insertions(+), 64 deletions(-) diff --git a/build.sbt b/build.sbt index 468c39f..853adbf 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.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 index 7dca8ca..2546f5a 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), diff --git a/practice-robot-config.json b/practice-robot-config.json index 68afb49..7f0e366 100644 --- 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 index feee9cf..8d9f1d0 100644 --- 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 index 7a48062..89d3cab 100644 --- 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) ) } } -} \ No newline at end of file +} diff --git a/shared/src/main/scala/com/lynbrookrobotics/eighteen/CoreRobot.scala b/shared/src/main/scala/com/lynbrookrobotics/eighteen/CoreRobot.scala index a12fcfc..155a561 100644 --- 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 { } } -} \ No newline at end of file +} diff --git a/shared/src/main/scala/com/lynbrookrobotics/eighteen/DefaultConfig.scala b/shared/src/main/scala/com/lynbrookrobotics/eighteen/DefaultConfig.scala index 9e02cd3..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,13 +89,17 @@ object DefaultConfig { | ] | }, | "maxHeight": [ - | 73, + | 74, | "Inches" | ], | "minHeight": [ - | 0.25, + | 0, | "Inches" | ], + | "twistyTotalRange": [ + | 0.75, + | "Feet" + | ], | "maxCurrentDraw": [ | 20, | "Amperes" @@ -103,10 +107,6 @@ object DefaultConfig { | "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 } \ 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/drivetrain/DrivetrainConfig.scala b/shared/src/main/scala/com/lynbrookrobotics/eighteen/drivetrain/DrivetrainConfig.scala index 3f8ae11..71f1b2c 100644 --- 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 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 173cccb..bf8e729 100644 --- a/shared/src/main/scala/com/lynbrookrobotics/eighteen/drivetrain/DrivetrainHardware.scala +++ b/shared/src/main/scala/com/lynbrookrobotics/eighteen/drivetrain/DrivetrainHardware.scala @@ -25,16 +25,16 @@ final case class DrivetrainData( ) final case class DrivetrainHardware( - coreTicks: Stream[Unit], - leftSRX: TalonSRX, - rightSRX: TalonSRX, - leftFollower: BaseMotorController, - rightFollower: BaseMotorController, - gyro: DigitalGyro, - driverHardware: DriverHardware, - props: DrivetrainProperties, - ports: DrivetrainPorts, - pdp: PowerDistributionPanel + coreTicks: Stream[Unit], + leftSRX: TalonSRX, + rightSRX: TalonSRX, + leftFollower: BaseMotorController, + rightFollower: BaseMotorController, + gyro: DigitalGyro, + driverHardware: DriverHardware, + props: DrivetrainProperties, + ports: DrivetrainPorts, + pdp: PowerDistributionPanel ) extends TwoSidedDriveHardware { override val track: Length = props.track @@ -97,11 +97,11 @@ 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)) @@ -109,8 +109,10 @@ final case class DrivetrainHardware( 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.leftFollowerPdpPort))) + 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 @@ -118,7 +120,12 @@ final case class DrivetrainHardware( } 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}") @@ -142,7 +149,9 @@ object DrivetrainHardware { new ADIS16448(new SPI(SPI.Port.kMXP), null) }, driverHardware, - config.props + config.props, + config.ports, + pdp ) } } From 8ddca5aa2a38faa6d416e34a63347fa569322a59 Mon Sep 17 00:00:00 2001 From: Kunal Sheth Date: Tue, 3 Apr 2018 18:48:18 -0700 Subject: [PATCH 24/27] run scalafix and scalafmt --- build.sbt | 6 +++--- .../scala/com/lynbrookrobotics/eighteen/CoreRobot.scala | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/build.sbt b/build.sbt index 853adbf..731ac3b 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+2-f4444a67", - libraryDependencies += "com.lynbrookrobotics" %%% "ntcore-scala-native" % "0.1.2+2-f4444a67", - libraryDependencies += "com.lynbrookrobotics" %%% "phoenix-scala-native" % "0.1.2+2-f4444a67", + 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/shared/src/main/scala/com/lynbrookrobotics/eighteen/CoreRobot.scala b/shared/src/main/scala/com/lynbrookrobotics/eighteen/CoreRobot.scala index 155a561..cfa3fa2 100644 --- a/shared/src/main/scala/com/lynbrookrobotics/eighteen/CoreRobot.scala +++ b/shared/src/main/scala/com/lynbrookrobotics/eighteen/CoreRobot.scala @@ -430,7 +430,7 @@ class CoreRobot(configFileValue: Signal[String], updateConfigFile: String => Uni board .datasetGroup("Drivetrain/Current") .addDataset( - drivetrainHardware.leftFollowerCurrent.map(_.toAmperes).toTimeSeriesNumeric("Left follower current") + drivetrainHardware.leftFollowerCurrent.map(_.toAmperes).toTimeSeriesNumeric("Left follower current") ) board From 5d5cd3041199836c3301a4943a15676f3642c499 Mon Sep 17 00:00:00 2001 From: Kunal Sheth Date: Tue, 3 Apr 2018 18:59:04 -0700 Subject: [PATCH 25/27] add missing params to ConfigGenerator --- .../scala/com/lynbrookrobotics/eighteen/ConfigGenerator.scala | 1 + 1 file changed, 1 insertion(+) diff --git a/jvm/src/test/scala/com/lynbrookrobotics/eighteen/ConfigGenerator.scala b/jvm/src/test/scala/com/lynbrookrobotics/eighteen/ConfigGenerator.scala index 2546f5a..2f03069 100644 --- a/jvm/src/test/scala/com/lynbrookrobotics/eighteen/ConfigGenerator.scala +++ b/jvm/src/test/scala/com/lynbrookrobotics/eighteen/ConfigGenerator.scala @@ -82,6 +82,7 @@ object ConfigGenerator extends App { Turns(18), Turns(74) ), + leftFudge = 1.0, rightFudge = 1.0, parallelMotorCurrentThreshold = Amperes(5), deltaVelocityStallThreshold = FeetPerSecond(10), stallTimeout = Seconds(3) From 49e7213134ca5c625a53ce7847c900f9aee9baec Mon Sep 17 00:00:00 2001 From: Kunal Sheth Date: Thu, 5 Apr 2018 04:31:02 -0700 Subject: [PATCH 26/27] run scalafix/scalafmt --- .../scala/com/lynbrookrobotics/eighteen/ConfigGenerator.scala | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/jvm/src/test/scala/com/lynbrookrobotics/eighteen/ConfigGenerator.scala b/jvm/src/test/scala/com/lynbrookrobotics/eighteen/ConfigGenerator.scala index 2f03069..85be7c8 100644 --- a/jvm/src/test/scala/com/lynbrookrobotics/eighteen/ConfigGenerator.scala +++ b/jvm/src/test/scala/com/lynbrookrobotics/eighteen/ConfigGenerator.scala @@ -82,7 +82,8 @@ object ConfigGenerator extends App { Turns(18), Turns(74) ), - leftFudge = 1.0, rightFudge = 1.0, + leftFudge = 1.0, + rightFudge = 1.0, parallelMotorCurrentThreshold = Amperes(5), deltaVelocityStallThreshold = FeetPerSecond(10), stallTimeout = Seconds(3) From aef1aeeb29bdd00c30a5ef429aee2e31a94742c7 Mon Sep 17 00:00:00 2001 From: Kunal Sheth Date: Thu, 5 Apr 2018 04:51:07 -0700 Subject: [PATCH 27/27] fix negative dc/velocity stall case --- .../eighteen/drivetrain/DrivetrainComponent.scala | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 48a46d9..9b31539 100644 --- a/shared/src/main/scala/com/lynbrookrobotics/eighteen/drivetrain/DrivetrainComponent.scala +++ b/shared/src/main/scala/com/lynbrookrobotics/eighteen/drivetrain/DrivetrainComponent.scala @@ -55,7 +55,7 @@ class DrivetrainComponent(coreTicks: Stream[Unit])( .timeAboveThreshold( hardware.leftVelocity.zipAsync(hardware.leftDutyCycle).map { case (currVelocity, dutyCycle) => (props.get.maxLeftVelocity * dutyCycle.toEach) - currVelocity - }, + }.map(_.abs), props.get.deltaVelocityStallThreshold ) .filter(_ => RobotState.isAutonomous) @@ -69,7 +69,7 @@ class DrivetrainComponent(coreTicks: Stream[Unit])( .timeAboveThreshold( hardware.rightVelocity.zipAsync(hardware.rightDutyCycle).map { case (currVelocity, dutyCycle) => (props.get.maxRightVelocity * dutyCycle.toEach) - currVelocity - }, + }.map(_.abs), props.get.deltaVelocityStallThreshold ) .filter(_ => RobotState.isAutonomous)