Add XVLidar parsing implementation and start integration with BreezySLAM

breezyslam-cross.patch

@@ -30,6 +30,109 @@ index dee4f6e..16bae1b 100644
 +CFLAGS = -fPIC
 +LIBEXT = so
 
+-ARCH = $(shell uname -m)
++ARCH = "armv7l"
+
+ # Set SIMD compile params based on architecture
+ ifeq ("$(ARCH)","armv7l")
+@@ -44,36 +47,41 @@ all: libbreezyslam.$(LIBEXT)
+ test: breezytest
+ 	./breezytest
+
++breezyslam.a: algorithms.o  Scan.o Map.o WheeledRobot.o \
++                         coreslam.o coreslam_$(ARCH).o random.o ziggurat.o
++	arm-frc-linux-gnueabi-ar rvs breezyslam.a algorithms.o Scan.o Map.o WheeledRobot.o \
++                        coreslam.o coreslam_$(ARCH).o random.o ziggurat.o
++
+ libbreezyslam.$(LIBEXT): algorithms.o  Scan.o Map.o WheeledRobot.o \
+                          coreslam.o coreslam_$(ARCH).o random.o ziggurat.o
+-	g++ -O3 -shared algorithms.o Scan.o Map.o WheeledRobot.o \
++	arm-frc-linux-gnueabi-g++ -O3 -shared algorithms.o Scan.o Map.o WheeledRobot.o \
+                         coreslam.o coreslam_$(ARCH).o random.o ziggurat.o \
+           -o libbreezyslam.$(LIBEXT) -lm
+
+ algorithms.o: algorithms.cpp algorithms.hpp Laser.hpp Position.hpp Map.hpp Scan.hpp Velocities.hpp \
+                WheeledRobot.hpp ../c/coreslam.h 
+-	g++ -O3 -I../c -c -Wall $(CFLAGS) algorithms.cpp
++	arm-frc-linux-gnueabi-g++ -O3 -I../c -c -Wall $(CFLAGS) algorithms.cpp
+
+ Scan.o: Scan.cpp Scan.hpp Velocities.hpp Laser.hpp ../c/coreslam.h
+-	g++ -O3 -I../c -c -Wall $(CFLAGS) Scan.cpp
++	arm-frc-linux-gnueabi-g++ -O3 -I../c -c -Wall $(CFLAGS) Scan.cpp
+
+ Map.o: Map.cpp Map.hpp Position.hpp Scan.hpp ../c/coreslam.h
+-	g++ -O3 -I../c -c -Wall $(CFLAGS) Map.cpp
++	arm-frc-linux-gnueabi-g++ -O3 -I../c -c -Wall $(CFLAGS) Map.cpp
+
+ WheeledRobot.o: WheeledRobot.cpp WheeledRobot.hpp 
+-	g++ -O3 -I../c -c -Wall $(CFLAGS) WheeledRobot.cpp
++	arm-frc-linux-gnueabi-g++ -O3 -I../c -c -Wall $(CFLAGS) WheeledRobot.cpp
+
+ coreslam.o: ../c/coreslam.c ../c/coreslam.h
+-	gcc -O3 -c -Wall $(CFLAGS) ../c/coreslam.c
++	arm-frc-linux-gnueabi-gcc -O3 -c -Wall $(CFLAGS) ../c/coreslam.c
+
+ coreslam_$(ARCH).o: ../c/coreslam_$(ARCH).c ../c/coreslam.h
+-	gcc -O3 -c -Wall $(CFLAGS) $(SIMD_FLAGS) ../c/coreslam_$(ARCH).c
++	arm-frc-linux-gnueabi-gcc -O3 -c -Wall $(CFLAGS) $(SIMD_FLAGS) ../c/coreslam_$(ARCH).c
+
+ random.o: ../c/random.c
+-	gcc -O3 -c -Wall $(CFLAGS) ../c/random.c
++	arm-frc-linux-gnueabi-gcc -O3 -c -Wall $(CFLAGS) ../c/random.c
+
+ ziggurat.o: ../c/ziggurat.c
+-	gcc -O3 -c -Wall $(CFLAGS) ../c/ziggurat.c
++	arm-frc-linux-gnueabi-gcc -O3 -c -Wall $(CFLAGS) ../c/ziggurat.c
+
+ install: libbreezyslam.$(LIBEXT)
+ 	cp libbreezyslam.$(LIBEXT) $(LIBDIR)
+diff --git a/c/coreslam.c b/c/coreslam.c
+index bcb2d7d..35b52f9 100644
+--- a/c/coreslam.c
++++ b/c/coreslam.c
+@@ -378,6 +378,10 @@ void
+     }
+ }
+
++scan_t * scan_alloc() {
++    return (scan_t *)malloc(sizeof(scan_t));
++}
++
+ void scan_init(
+     scan_t * scan, 
+     int span,
+diff --git a/cpp/Makefile b/cpp/Makefile
+index dee4f6e..16bae1b 100644
+--- a/cpp/Makefile
++++ b/cpp/Makefile
+@@ -16,19 +16,22 @@
+ #  along with this code.  If not, see <http:#www.gnu.org/licenses/>.
+
+ # Where you want to put the library
+-LIBDIR = /usr/local/lib
++LIBDIR = /usr/local/arm-frc-linux-gnueabi/lib
+
+ # Set library extension based on OS
+-ifeq ("$(shell uname)","Darwin")
+-  LIBEXT = dylib
+-else ifeq ("$(shell uname)","Linux")
+-  CFLAGS = -fPIC
+-  LIBEXT = so
+-else
+-  LIBEXT = dll
+-endif
++# ifeq ("$(shell uname)","Darwin")
++#   LIBEXT = dylib
++# else ifeq ("$(shell uname)","Linux")
++#   CFLAGS = -fPIC
++#   LIBEXT = so
++# else
++#   LIBEXT = dll
++# endif
++
++CFLAGS = -fPIC
++LIBEXT = so
+
 -ARCH = $(shell uname -m)
 +ARCH = "armv7l"
 

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+20180327-1841",
+    libraryDependencies += "com.lynbrookrobotics" %%% "ntcore-scala-native" % "0.1.3+20180327-1841",
+    libraryDependencies += "com.lynbrookrobotics" %%% "phoenix-scala-native" % "0.1.3+20180327-1841",
     scalaVersion := "2.11.12",
     scalacOptions ++= Seq("-target:jvm-1.8")
   )

native/src/main/scala/com/lynbrookrobotics/eighteen/drivetrain/slam/BreezySLAM.scala

@@ -0,0 +1,21 @@
+package com.lynbrookrobotics.eighteen.drivetrain.slam
+
+import scala.scalanative.native
+import scala.scalanative.native.{CDouble, Ptr}
+
+@native.extern
+object BreezySLAM {
+  def scan_alloc(): Ptr[Byte] = native.extern
+  def scan_init(scan: Ptr[Byte],
+                span: Int,
+                size: Int,
+                scanRateHz: CDouble,
+                detectionAngleDegrees: CDouble,
+                distanceNoDetectionMM: CDouble,
+                detectionMargin: Int,
+                offsetMM: Double): Unit = native.extern
+}
+
+object RunSLAM {
+
+}

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

@@ -509,6 +509,14 @@ class CoreRobot(configFileValue: Signal[String], updateConfigFile: String => Uni
           collectorClampHardware.proximitySensorReading.map(_.toVolts).toTimeSeriesNumeric("Proximity Sensor Voltage")
         )
     }
+
+    hardware.lidar.foreach { xv =>
+      board
+        .datasetGroup("LIDAR")
+        .addDataset(
+          xv.frames.map(_(180).toFeet).toTimeSeriesNumeric("Distance at 180 deg (feet)")
+        )
+    }
   }
 }
 

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

@@ -7,12 +7,14 @@ import com.lynbrookrobotics.eighteen.collector.pivot.CollectorPivotHardware
 import com.lynbrookrobotics.eighteen.collector.rollers.CollectorRollersHardware
 import com.lynbrookrobotics.eighteen.driver.DriverHardware
 import com.lynbrookrobotics.eighteen.drivetrain.DrivetrainHardware
+import com.lynbrookrobotics.eighteen.drivetrain.slam.XVLidar
 import com.lynbrookrobotics.eighteen.forklift.ForkliftHardware
 import com.lynbrookrobotics.eighteen.lift.CubeLiftHardware
 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.SerialPort
 
 final case class RobotHardware(
   climberDeployment: Option[DeploymentHardware],
@@ -25,7 +27,8 @@ final case class RobotHardware(
   forklift: Option[ForkliftHardware],
   cubeLift: Option[CubeLiftHardware],
   camera: Option[LimeLightHardware],
-  ledHardware: Option[LEDControllerHardware]
+  ledHardware: Option[LEDControllerHardware],
+  lidar: Option[XVLidar]
 )
 
 object RobotHardware {
@@ -47,7 +50,8 @@ object RobotHardware {
       camera = robotConfig.limelight.map { l =>
         new LimeLightHardware(true)(WPIClock, Signal.constant(l))
       },
-      ledHardware = robotConfig.led.map(l => LEDControllerHardware(l))
+      ledHardware = robotConfig.led.map(l => LEDControllerHardware(l)),
+      lidar = Some(new XVLidar(SerialPort.Port.kUSB))
     )
   }
 }

shared/src/main/scala/com/lynbrookrobotics/eighteen/drivetrain/slam/XVLidar.scala

@@ -0,0 +1,77 @@
+package com.lynbrookrobotics.eighteen.drivetrain.slam
+
+import edu.wpi.first.wpilibj.SerialPort
+import com.lynbrookrobotics.potassium.streams.Stream
+import edu.wpi.first.wpilibj.hal.SerialPortJNI
+import squants.space.{Length, Millimeters}
+
+class XVLidar(port: SerialPort.Port) {
+  val serial = new SerialPort(115200, port)
+
+  val startBytesBuffer = new Array[Byte](1)
+  def readSingleByte: Byte = {
+    SerialPortJNI.serialRead(port.value.toByte, startBytesBuffer, 1)
+    startBytesBuffer(0)
+  }
+  private def readStartBytes(): Unit = {
+    while((readSingleByte & 0xFF) != 0xFA) {}
+    while ((readSingleByte & 0xFF) != 0xA0) {}
+  }
+
+  def timed[T](f: String)(thunk: => T): T = {
+    val start = System.currentTimeMillis()
+    val ret = thunk
+    println(s"$f: took ${System.currentTimeMillis() - start}")
+    ret
+  }
+
+  serial.writeString("MotorOn\r\n")
+  serial.writeString("SetRPM 300\r\n")
+
+  private val (stream, pub) = Stream.manual[Seq[Length]]
+  new Thread(new Runnable {
+    override def run(): Unit = {
+      val buffer = new Array[Byte](1980 - 2)
+      val retRanges = new Array[Length](360)
+      while (true) {
+        try {
+          readStartBytes()
+          val remainingBytes = {
+            // use the same buffer unlike wpilib
+            SerialPortJNI.serialRead(port.value.toByte, buffer, 1980 - 2)
+            buffer
+          } // remaining data, we already read the first two bytes
+
+          // total of ((2 /* crc */ + 2 /* rpm */ + 16 /* 4 readings of 4 bytes */ + 2 /* random data? */) = 22) * 90 = 1980
+          def getNthByte(i: Int): Int = {
+            if (i == 0) 0xFA
+            else if (i == 1) 0xA0
+            else remainingBytes(i - 2) & 0xFF
+          }
+
+          (0 until 90).foreach { setI =>
+            val startI = setI * 22
+            val rpms = ((getNthByte(startI + 3) << 8) | getNthByte(startI + 2)).toDouble / 64
+            (1 to 4).foreach { distI =>
+              val byte0 = getNthByte(startI + (distI * 4))
+              val byte1 = getNthByte(startI + (distI * 4) + 1)
+              val byte2 = getNthByte(startI + (distI * 4) + 2)
+              val byte3 = getNthByte(startI + (distI * 4) + 3)
+
+              val range = ((byte1 & 0x3F) << 8) + byte0
+              val intensity = (byte3 << 8) + byte2
+
+              retRanges((setI * 4) + (distI - 1)) = Millimeters(range)
+            }
+          }
+
+          pub(retRanges)
+        } catch {
+          case t: Throwable => t.printStackTrace()
+        }
+      }
+    }
+  }).start()
+
+  val frames = stream
+}