diff --git a/projects/bin/build.gradle.kts b/projects/bin/build.gradle.kts index 7c642b4..5edfe84 100644 --- a/projects/bin/build.gradle.kts +++ b/projects/bin/build.gradle.kts @@ -13,6 +13,7 @@ dependencies { compile("org.jetbrains.kotlin:kotlin-stdlib-jre8") compile(project(":core")) compile(project(":gps")) + compile(project(":imu")) compile(project(":microcontrollers")) compile("com.fazecast:jSerialComm:1.3.11") compile("org.tinylog:tinylog:1.1") diff --git a/projects/bin/src/main/kotlin/bin/Main.kt b/projects/bin/src/main/kotlin/bin/Main.kt index 7c25d2a..0885d7c 100644 --- a/projects/bin/src/main/kotlin/bin/Main.kt +++ b/projects/bin/src/main/kotlin/bin/Main.kt @@ -2,86 +2,26 @@ package bin -import core.Boat -import core.PropulsionSystem -import core.broadcast.* -import core.values.GpsValue -import core.values.ValueSerializer -import gps.Gps -import gps.PMTK -import microcontrollers.PropulsionMicrocontroller - -import java.net.DatagramSocket -import java.net.InetAddress -import java.util.concurrent.CountDownLatch +import com.pi4j.io.i2c.I2CFactory +import io.L3GD20 +import io.LSM303DLHC import java.util.concurrent.TimeUnit - -import rx.Observable -import rx.broadcast.BasicOrder -import rx.broadcast.InMemoryBroadcast -import rx.broadcast.UdpBroadcast -import rx.schedulers.Schedulers - -const val ENVIRONMENT_VARIABLE_PREFIX = "LM_" -const val STARTUP_MESSAGE = "Lake Maps NL boat control software started" - -fun panic(message: String): Nothing = throw RuntimeException(message) -fun panic(message: String, cause: Throwable): Nothing = throw RuntimeException(message, cause) - -fun envPrefixed(name: String) = ENVIRONMENT_VARIABLE_PREFIX + name -fun env(name: String): String = (envPrefixed(name)).let { System.getenv(it) ?: panic("$it must be set") } -fun env(name: String, default: String): String = System.getenv().getOrDefault(envPrefixed(name), default) +import java.util.concurrent.atomic.AtomicBoolean fun main(args: Array) { - println("Lake Maps NL boat control software starting...") - - val propulsionMicrocontroller = SerialPort(env("PROPULSION_SERIAL_DEVICE"), baudRate = 57600).let { - serialPort -> PropulsionMicrocontroller(serialPort::recvByte, serialPort::send) - } - - val broadcastAddress = InetAddress.getByName(env("BROADCAST_ADDRESS")) - val broadcastPort = env("BROADCAST_PORT").toInt() - val broadcastSocket = DatagramSocket(broadcastPort) - val broadcast = UdpBroadcast(broadcastSocket, broadcastAddress, broadcastPort, ValueSerializer(), BasicOrder()) - - val memoryBroadcast = InMemoryBroadcast() - - val gpsEnabled = !env("DISABLE_GPS", "false").toBoolean() - if (gpsEnabled) { - with(SerialPort(env("GPS_SERIAL_DEVICE"), baudRate = 9600)) { - val gps = Gps({ recvChar() }, { send(it) }, { _ -> }) - gps.setNmeaBaudRate(PMTK.BaudRate.BAUD_RATE_57600) - disconnect() - } - - val gps = SerialPort(env("GPS_SERIAL_DEVICE"), baudRate = 57600).let { - serialPort -> Gps(serialPort::recvChar, serialPort::send, memoryBroadcast::sendAsync) - } - gps.setNmeaUpdateRate(PMTK.UpdateRate(100)) - gps.poll() - - Observable.interval(100, TimeUnit.MILLISECONDS) - .observeOn(Schedulers.io()) - .subscribe({ - // TODO: issue #67 - gps.poll() - }, { - e -> panic("Poll GPS timer failed", e) - }) + val i2CBus = I2CFactory.getInstance(1) + val accelerometerMagnetometer = LSM303DLHC(i2CBus.getDevice(0b0011001), i2CBus.getDevice(0b0011110)) + val gyroscope = L3GD20(i2CBus.getDevice(0b1101011)) + + // If this runs and print out three values we good + val b = AtomicBoolean(true) + Runtime.getRuntime().addShutdownHook(Thread({ + b.set(false) + })) + while (b.get()) { + println(gyroscope.rateOfRotation()) +// println(accelerometerMagnetometer.acceleration()) +// println(accelerometerMagnetometer.magneticFluxDensity()) + TimeUnit.SECONDS.sleep(1) } - - val boat = Boat(memoryBroadcast, PropulsionSystem(propulsionMicrocontroller)) - Runtime.getRuntime().addShutdownHook(Thread(boat::shutdown)) - boat.start(io = Schedulers.io(), clock = Schedulers.computation()) - - // GPS values should be sent across the wire - memoryBroadcast.valuesOfType() - .observeOn(Schedulers.io()) - .subscribe(broadcast::sendAsync, { e -> panic("Rebroadcast GPS value failed", e) }) - - // Anything sent across the wire should be re-broadcasted locally - broadcast.valuesOfType().subscribe(memoryBroadcast::sendAsync, { e -> panic("Local rebroadcast failed", e) }) - - println(STARTUP_MESSAGE) - CountDownLatch(1).await() } diff --git a/projects/imu/build.gradle.kts b/projects/imu/build.gradle.kts new file mode 100644 index 0000000..de40527 --- /dev/null +++ b/projects/imu/build.gradle.kts @@ -0,0 +1,31 @@ +import org.gradle.api.tasks.testing.logging.TestLogging +import org.jetbrains.kotlin.gradle.tasks.KotlinCompile + +repositories { + jcenter() +} + +plugins { + id("org.jetbrains.kotlin.jvm") version "1.2.31" +} + +dependencies { + compile(project(":units")) + compile("com.pi4j:pi4j-core:1.1") + compile("org.jetbrains.kotlin:kotlin-stdlib-jre8") + testCompile("junit:junit:4.12") + testCompile("org.jetbrains.kotlin:kotlin-test-junit") +} + +tasks.withType { + testLogging(closureOf { + showStandardStreams = true + events("passed", "skipped", "failed") + }) +} + +tasks.withType { + kotlinOptions { + jvmTarget = "1.8" + } +} diff --git a/projects/imu/src/main/kotlin/io/Accelerometer.kt b/projects/imu/src/main/kotlin/io/Accelerometer.kt new file mode 100644 index 0000000..862d1da --- /dev/null +++ b/projects/imu/src/main/kotlin/io/Accelerometer.kt @@ -0,0 +1,5 @@ +package io + +interface Accelerometer { + fun acceleration(): XYZ +} diff --git a/projects/imu/src/main/kotlin/io/ByteArrayExtensions.kt b/projects/imu/src/main/kotlin/io/ByteArrayExtensions.kt new file mode 100644 index 0000000..8926ab3 --- /dev/null +++ b/projects/imu/src/main/kotlin/io/ByteArrayExtensions.kt @@ -0,0 +1,3 @@ +package microcontrollers + +internal fun ByteArray.hex() = this.joinToString(separator = "") { it.hex() } diff --git a/projects/imu/src/main/kotlin/io/ByteExtensions.kt b/projects/imu/src/main/kotlin/io/ByteExtensions.kt new file mode 100644 index 0000000..758911a --- /dev/null +++ b/projects/imu/src/main/kotlin/io/ByteExtensions.kt @@ -0,0 +1,3 @@ +package microcontrollers + +internal fun Byte.hex() = Integer.toHexString(this.toInt()).takeLast(2).padStart(2, '0') diff --git a/projects/imu/src/main/kotlin/io/Gyroscope.kt b/projects/imu/src/main/kotlin/io/Gyroscope.kt new file mode 100644 index 0000000..23ee4c6 --- /dev/null +++ b/projects/imu/src/main/kotlin/io/Gyroscope.kt @@ -0,0 +1,5 @@ +package io + +interface Gyroscope { + fun rateOfRotation(): XYZ +} diff --git a/projects/imu/src/main/kotlin/io/I2CBus.kt b/projects/imu/src/main/kotlin/io/I2CBus.kt new file mode 100644 index 0000000..c0e8a45 --- /dev/null +++ b/projects/imu/src/main/kotlin/io/I2CBus.kt @@ -0,0 +1,5 @@ +package io + +interface I2CBus { + fun writeBytes(deviceAddress: Int, address: Int, bytes: ByteArray) +} diff --git a/projects/imu/src/main/kotlin/io/L3GD20.kt b/projects/imu/src/main/kotlin/io/L3GD20.kt new file mode 100644 index 0000000..6596101 --- /dev/null +++ b/projects/imu/src/main/kotlin/io/L3GD20.kt @@ -0,0 +1,33 @@ +package io + +import com.pi4j.io.i2c.I2CDevice +import java.nio.ByteBuffer + +private const val CTRL_REG1 = 0x20 +private const val OUT_X_L = 0x28 + +class L3GD20(private val i2c: I2CDevice): Gyroscope { + init { + i2c.write(CTRL_REG1, 0x00) + i2c.write(CTRL_REG1, 0x0F) + } + + override fun rateOfRotation(): XYZ { + val readSize = 6 + val buffer = ByteArray(readSize) + i2c.read(OUT_X_L or 0x80, buffer, 0, readSize) + + val bytes = buffer.foldRight(ByteBuffer.allocate(readSize)) { byte, acc -> + acc.put(byte) + acc + } + + bytes.flip() + return with(bytes.asShortBuffer()) { + val z = get(0) + val y = get(1) + val x = get(2) + XYZ(x.toDouble(), y.toDouble(), z.toDouble()) + } + } +} diff --git a/projects/imu/src/main/kotlin/io/LSM303DLHC.kt b/projects/imu/src/main/kotlin/io/LSM303DLHC.kt new file mode 100644 index 0000000..d84ed77 --- /dev/null +++ b/projects/imu/src/main/kotlin/io/LSM303DLHC.kt @@ -0,0 +1,54 @@ +package io + +import com.pi4j.io.i2c.I2CDevice +import java.nio.ByteBuffer + +private const val CTRL_REG1_A = 0x20 +private const val MR_REG_M = 0x02 +private const val OUT_X_L_A = 0x28 +private const val OUT_X_L_M = 0x03 + +class LSM303DLHC(private val accelerometerDevice: I2CDevice, private val magDevice: I2CDevice): Accelerometer, Magnetometer { + init { + accelerometerDevice.write(CTRL_REG1_A, 0x27) + magDevice.write(MR_REG_M, 0x00) + } + + override fun acceleration(): XYZ { + val readSize = 6 + val buffer = ByteArray(readSize) + accelerometerDevice.read(OUT_X_L_A or 0x80, buffer, 0, readSize) + + val bytes = buffer.foldRight(ByteBuffer.allocate(readSize)) { byte, acc -> + acc.put(byte) + acc + } + + bytes.flip() + return with(bytes.asShortBuffer()) { + val z = get(0) + val y = get(1) + val x = get(2) + XYZ(x.toDouble(), y.toDouble(), z.toDouble()) + } + } + + override fun magneticFluxDensity(): XYZ { + val readSize = 6 + val buffer = ByteArray(readSize) + magDevice.read(OUT_X_L_M, buffer, 0, readSize) + + val bytes = buffer.fold(ByteBuffer.allocate(readSize)) { acc, byte -> + acc.put(byte) + acc + } + + bytes.flip() + return with(bytes.asShortBuffer()) { + val x = get(0) + val z = get(1) + val y = get(2) + XYZ(x.toDouble(), y.toDouble(), z.toDouble()) + } + } +} diff --git a/projects/imu/src/main/kotlin/io/Magnetometer.kt b/projects/imu/src/main/kotlin/io/Magnetometer.kt new file mode 100644 index 0000000..b104e83 --- /dev/null +++ b/projects/imu/src/main/kotlin/io/Magnetometer.kt @@ -0,0 +1,5 @@ +package io + +interface Magnetometer { + fun magneticFluxDensity(): XYZ +} diff --git a/projects/imu/src/main/kotlin/io/XYZ.kt b/projects/imu/src/main/kotlin/io/XYZ.kt new file mode 100644 index 0000000..595b4b0 --- /dev/null +++ b/projects/imu/src/main/kotlin/io/XYZ.kt @@ -0,0 +1,3 @@ +package io + +data class XYZ(val x: Double, val y: Double, val z: Double) diff --git a/settings.gradle b/settings.gradle index 43fd9f4..4ffab10 100644 --- a/settings.gradle +++ b/settings.gradle @@ -1,6 +1,7 @@ include "bin" include "core" include "gps" +include "imu" include "kv" include "microcontrollers" include "units"