Slipping detection

commons/src/main/scala/com/lynbrookrobotics/potassium/commons/position/Position.scala → commons/src/main/scala/com/lynbrookrobotics/potassium/commons/cartesianPosition/Position.scala

@@ -1,9 +1,9 @@
-package com.lynbrookrobotics.potassium.commons.position
+package com.lynbrookrobotics.potassium.commons.cartesianPosition
 
-import com.lynbrookrobotics.potassium.{Component, Signal}
-import com.lynbrookrobotics.potassium.streams.Stream
 import com.lynbrookrobotics.potassium.control.{PIDF, PIDFConfig}
+import com.lynbrookrobotics.potassium.streams.Stream
 import com.lynbrookrobotics.potassium.tasks.{ContinuousTask, FiniteTask}
+import com.lynbrookrobotics.potassium.{Component, Signal}
 import squants.Quantity
 import squants.time.{TimeDerivative, TimeIntegral}
 

commons/src/main/scala/com/lynbrookrobotics/potassium/commons/position/Slipping.scala

@@ -0,0 +1,40 @@
+package com.lynbrookrobotics.potassium.commons.position
+
+import com.lynbrookrobotics.potassium.streams._
+import squants.Velocity
+import squants.motion.{Acceleration, AngularVelocity}
+import squants.space.{Feet, Length}
+
+object Slipping {
+  def slippingDetection (angularVelocity: Stream[AngularVelocity],
+                         linearAcceleration: Stream[Acceleration],
+                         leftEncoderVelocity: Stream[Velocity],
+                         rightEncoderVelocity: Stream[Velocity],
+                         distanceFromAccelerometerLeft: Length,
+                         distanceFromAccelerometerRight: Length,
+                         allowedAccelerationDeviation: Acceleration): Stream[(Boolean, Boolean)] = {
+    val radius = linearAcceleration.zip(angularVelocity.derivative).map{ case(linearAcc, angularAcc) =>
+      Feet(linearAcc.toFeetPerSecondSquared / angularAcc.toRadiansPerSecondSquared)
+    }
+
+    val leftRadius = radius.map(_ - distanceFromAccelerometerLeft)
+    val rightRadius = radius.map(_ + distanceFromAccelerometerRight)
+
+    val leftCalculatedAcceleration: Stream[Acceleration] = leftRadius.zip(angularVelocity.derivative).map{ case(radius, angularAcc) =>
+      angularAcc onRadius radius
+    }
+    val rightCalculatedAcceleration = rightRadius.zip(angularVelocity.derivative).map{ case(radius, angularAcc) =>
+      angularAcc onRadius radius
+    }
+
+    val leftIsSlipping = leftCalculatedAcceleration.zip(leftEncoderVelocity.derivative).map{ case(calculatedAcc, actualAcc) =>
+      (calculatedAcc - actualAcc).abs > allowedAccelerationDeviation
+    }
+
+    val rightIsSlipping = rightCalculatedAcceleration.zip(rightEncoderVelocity.derivative).map{ case(calculatedAcc, actualAcc) =>
+      (calculatedAcc - actualAcc).abs > allowedAccelerationDeviation
+    }
+
+    leftIsSlipping.zip(rightIsSlipping)
+  }
+}

commons/src/test/scala/com/lynbrookrobotics/potassium/commons/drivetrain/SlippingTest.scala

@@ -0,0 +1,123 @@
+package com.lynbrookrobotics.potassium.commons.drivetrain
+
+import com.lynbrookrobotics.potassium.ClockMocking
+import com.lynbrookrobotics.potassium.streams.Stream
+import com.lynbrookrobotics.potassium.commons.position.Slipping
+import org.scalatest.FunSuite
+import squants.Velocity
+import squants.motion._
+import squants.space.{Feet, Length}
+import squants.time.Milliseconds
+
+class SlippingTest extends FunSuite {
+
+  implicit val (clock, triggerClock) = ClockMocking.mockedClockTicker
+  val period = Milliseconds(5)
+
+  test("left and right slipping") {
+    val angularVelocity: Stream[AngularVelocity] = Stream.periodic(period)(RadiansPerSecondSquared(5)).integral
+    val linearAcceleration: Stream[Acceleration] = Stream.periodic(period)(FeetPerSecondSquared(5))
+    val leftEncoderVelocity: Stream[Velocity] = Stream.periodic(period)(FeetPerSecondSquared(2)).integral
+    val rightEncoderVelocity: Stream[Velocity] = Stream.periodic(period)(FeetPerSecondSquared(9.5)).integral
+    val distanceFromAccelerometerLeft: Length = Feet(0.5) // calc = 2.5
+    val distanceFromAccelerometerRight: Length = Feet(1) // calc = 10
+    val allowedAccelerationDeviation: Acceleration = MetersPerSecondSquared(1)
+
+    val isSlipping = Slipping.slippingDetection(angularVelocity,
+      linearAcceleration,
+      leftEncoderVelocity,
+      rightEncoderVelocity,
+      distanceFromAccelerometerLeft,
+      distanceFromAccelerometerRight,
+      allowedAccelerationDeviation)
+    isSlipping.foreach(x => assert(!x._1 && !x._2))
+
+    triggerClock.apply(period)
+    triggerClock.apply(period)
+    triggerClock.apply(period)
+    triggerClock.apply(period)
+    triggerClock.apply(period)
+    triggerClock.apply(period)
+  }
+
+  test("left and right not slipping") {
+    val angularVelocity: Stream[AngularVelocity] = Stream.periodic(period)(RadiansPerSecondSquared(5)).integral
+    val linearAcceleration: Stream[Acceleration] = Stream.periodic(period)(FeetPerSecondSquared(5))
+    val leftEncoderVelocity: Stream[Velocity] = Stream.periodic(period)(FeetPerSecondSquared(2)).integral
+    val rightEncoderVelocity: Stream[Velocity] = Stream.periodic(period)(FeetPerSecondSquared(9.5)).integral
+    val distanceFromAccelerometerLeft: Length = Feet(0.5) // calc = 2.5
+    val distanceFromAccelerometerRight: Length = Feet(1) // calc = 10
+    val allowedAccelerationDeviation: Acceleration = MetersPerSecondSquared(0.1)
+
+    val isSlipping = Slipping.slippingDetection(angularVelocity,
+      linearAcceleration,
+      leftEncoderVelocity,
+      rightEncoderVelocity,
+      distanceFromAccelerometerLeft,
+      distanceFromAccelerometerRight,
+      allowedAccelerationDeviation)
+
+    isSlipping.foreach(x => assert(x._1 && x._2))
+
+    triggerClock.apply(period)
+    triggerClock.apply(period)
+    triggerClock.apply(period)
+    triggerClock.apply(period)
+    triggerClock.apply(period)
+    triggerClock.apply(period)
+  }
+
+  test("right slipping only") {
+    val angularVelocity: Stream[AngularVelocity] = Stream.periodic(period)(RadiansPerSecondSquared(5)).integral
+    val linearAcceleration: Stream[Acceleration] = Stream.periodic(period)(FeetPerSecondSquared(5))
+    val leftEncoderVelocity: Stream[Velocity] = Stream.periodic(period)(FeetPerSecondSquared(2)).integral
+    val rightEncoderVelocity: Stream[Velocity] = Stream.periodic(period)(FeetPerSecondSquared(30)).integral
+    val distanceFromAccelerometerLeft: Length = Feet(0.5) // calc = 2.5
+    val distanceFromAccelerometerRight: Length = Feet(1) // calc = 10
+    val allowedAccelerationDeviation: Acceleration = MetersPerSecondSquared(1)
+
+    val isSlipping = Slipping.slippingDetection(angularVelocity,
+      linearAcceleration,
+      leftEncoderVelocity,
+      rightEncoderVelocity,
+      distanceFromAccelerometerLeft,
+      distanceFromAccelerometerRight,
+      allowedAccelerationDeviation)
+
+    isSlipping.foreach(x => assert(!x._1 && x._2))
+
+    triggerClock.apply(period)
+    triggerClock.apply(period)
+    triggerClock.apply(period)
+    triggerClock.apply(period)
+    triggerClock.apply(period)
+    triggerClock.apply(period)
+  }
+
+  test("left slipping only") {
+    val angularVelocity: Stream[AngularVelocity] = Stream.periodic(period)(RadiansPerSecondSquared(5)).integral
+    val linearAcceleration: Stream[Acceleration] = Stream.periodic(period)(FeetPerSecondSquared(5))
+    val leftEncoderVelocity: Stream[Velocity] = Stream.periodic(period)(FeetPerSecondSquared(30)).integral
+    val rightEncoderVelocity: Stream[Velocity] = Stream.periodic(period)(FeetPerSecondSquared(9.5)).integral
+    val distanceFromAccelerometerLeft: Length = Feet(0.5) // calc = 2.5
+    val distanceFromAccelerometerRight: Length = Feet(1) // calc = 10
+    val allowedAccelerationDeviation: Acceleration = MetersPerSecondSquared(1)
+
+    val isSlipping = Slipping.slippingDetection(angularVelocity,
+      linearAcceleration,
+      leftEncoderVelocity,
+      rightEncoderVelocity,
+      distanceFromAccelerometerLeft,
+      distanceFromAccelerometerRight,
+      allowedAccelerationDeviation)
+
+    isSlipping.foreach(x => assert(x._1 && !x._2))
+
+    triggerClock.apply(period)
+    triggerClock.apply(period)
+    triggerClock.apply(period)
+    triggerClock.apply(period)
+    triggerClock.apply(period)
+    triggerClock.apply(period)
+  }
+}

core/shared/src/main/scala/com/lynbrookrobotics/potassium/streams/Stream.scala

@@ -23,11 +23,7 @@ abstract class Stream[+T] { self =>
 
   protected def publishValue(value: T @uncheckedVariance): Unit = {
     listeners.foreach { l =>
-      try {
-        l.apply(value)
-      } catch {
-        case e: Throwable => e.printStackTrace()
-      }
+      l.apply(value)
     }
     // TODO: more stuff maybe
   }