diff --git a/breezyslam-cross.patch b/breezyslam-cross.patch index 7f7b599..2bfcc0c 100644 --- a/breezyslam-cross.patch +++ b/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 . + + # 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" diff --git a/build.sbt b/build.sbt index 5d97fab..71769f5 100644 --- a/build.sbt +++ b/build.sbt @@ -35,9 +35,9 @@ lazy val robot = crossProject(JVMPlatform, NativePlatform) libraryDependencies += "org.scalatest" %% "scalatest" % "3.0.5" % Test ) .nativeSettings( - libraryDependencies += "com.lynbrookrobotics" %%% "wpilib-scala-native" % "0.1.2", - libraryDependencies += "com.lynbrookrobotics" %%% "ntcore-scala-native" % "0.1.2", - libraryDependencies += "com.lynbrookrobotics" %%% "phoenix-scala-native" % "0.1.2", + libraryDependencies += "com.lynbrookrobotics" %%% "wpilib-scala-native" % "0.1.3+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") ) diff --git a/native/src/main/scala/com/lynbrookrobotics/eighteen/drivetrain/slam/BreezySLAM.scala b/native/src/main/scala/com/lynbrookrobotics/eighteen/drivetrain/slam/BreezySLAM.scala new file mode 100644 index 0000000..fde11cc --- /dev/null +++ b/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 { + +} \ 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 7f771cc..9da9540 100644 --- a/shared/src/main/scala/com/lynbrookrobotics/eighteen/CoreRobot.scala +++ b/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)") + ) + } } } diff --git a/shared/src/main/scala/com/lynbrookrobotics/eighteen/RobotHardware.scala b/shared/src/main/scala/com/lynbrookrobotics/eighteen/RobotHardware.scala index 48d937d..c3776d7 100644 --- a/shared/src/main/scala/com/lynbrookrobotics/eighteen/RobotHardware.scala +++ b/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)) ) } } diff --git a/shared/src/main/scala/com/lynbrookrobotics/eighteen/drivetrain/slam/XVLidar.scala b/shared/src/main/scala/com/lynbrookrobotics/eighteen/drivetrain/slam/XVLidar.scala new file mode 100644 index 0000000..3932348 --- /dev/null +++ b/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 +}