[WIP] Synchronize time with Jetson using UDP

src/main/java/org/sert2521/powerup/Poe.kt

@@ -8,6 +8,7 @@ import org.sert2521.powerup.elevator.Elevator
 import org.sert2521.powerup.intake.Intake
 import org.sert2521.powerup.util.Lights
 import org.sert2521.powerup.util.Modes
+import org.sert2521.powerup.util.TimeSync
 import org.sert2521.powerup.util.UDPServer
 import org.sert2521.powerup.util.initPreferences
 import org.sert2521.powerup.util.logTelemetry
@@ -24,6 +25,7 @@ class Poe : Robot() {
         Lights
 
         UDPServer.start()
+        TimeSync.start()
         CameraServer.getInstance().startAutomaticCapture()
 
         initPreferences()

src/main/java/org/sert2521/powerup/util/Constants.kt

@@ -42,6 +42,10 @@ const val MAX_VELOCITY = 0.6
 const val MAX_ACCELERATION = 0.1
 const val MAX_JERK = 60.0
 
+// Time synchronization
+const val JETSON_PORT = 5801 // UDP port that the Jetson is listening on
+const val BROADCAST_IP = "10.25.21.255" // Broadcast address for robot network
+
 // Other
 const val DEGREES_PER_PIXEL = 53.4 / 680 // Logitech Webcam C270 FOV in degrees / pixel width
 const val UDP_PORT = 5800

src/main/java/org/sert2521/powerup/util/TimeSync.kt

@@ -0,0 +1,40 @@
+package org.sert2521.powerup.util
+
+import java.net.DatagramPacket
+import java.net.DatagramSocket
+import java.net.InetAddress
+import java.util.Date
+
+object TimeSync : Thread() {
+    // Time (in secs) to wait between syncs
+    private const val WAIT_PERIOD: Long = 5000
+
+    // UDP socket
+    private val socket = DatagramSocket().apply { broadcast = true } // Broadcast is true by default, but it helps to specify
+
+    override fun run() {
+        socket.connect(InetAddress.getByName(BROADCAST_IP), JETSON_PORT)
+
+        while (true) {
+            val epoch = Date().toInstant().toEpochMilli()
+            val msg = "${epochSecs(epoch)}-${epochMillis(epoch)}".toByteArray()
+
+            socket.send(DatagramPacket(msg, msg.size))
+            sleep(WAIT_PERIOD)
+        }
+    }
+
+    // Extract seconds from an epoch number
+    private fun epochSecs(epoch: Long): Long {
+        return epoch.toString().let {
+            it.substring(0, it.length - 3).toLong()
+        }
+    }
+
+    // Extract milliseconds from the epoch number
+    private fun epochMillis(epoch: Long): Long {
+        return epoch.toString().let {
+            it.substring(it.length - 3).toLong()
+        }
+    }
+}