Merge remote-tracking branch 'origin/development' into development

build.gradle

@@ -3,6 +3,7 @@ buildscript {
     ext.gradleRioVersion = '2018.03.06'
     ext.sertainVersion = '1.2.0'
     ext.ktlintVersion = '0.19.0'
+    ext.gsonVersion = '2.7'
 
     repositories {
         jcenter()
@@ -37,6 +38,7 @@ dependencies {
     compile "org.jetbrains.kotlin:kotlin-stdlib-jdk8:$kotlinVersion"
 
     compile "org.sert2521.sertain:core:$sertainVersion"
+    compile "com.google.code.gson:gson:$gsonVersion"
 
     compile wpilib()
     compile navx()

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

@@ -7,6 +7,7 @@ import org.sert2521.powerup.drivetrain.Drivetrain
 import org.sert2521.powerup.elevator.Elevator
 import org.sert2521.powerup.intake.Intake
 import org.sert2521.powerup.util.Modes
+import org.sert2521.powerup.util.UDPServer
 import org.sertain.Robot
 
 class Poe : Robot() {
@@ -18,6 +19,7 @@ class Poe : Robot() {
         Auto
         Modes
 
+        UDPServer.start()
         CameraServer.getInstance().startAutomaticCapture()
     }
 }

src/main/java/org/sert2521/powerup/autonomous/Autonomous.kt

@@ -15,6 +15,7 @@ import org.sert2521.powerup.intake.commands.IntakeBlock
 import org.sert2521.powerup.util.AutoMode
 import org.sert2521.powerup.util.ENCODER_TICKS_PER_REVOLUTION
 import org.sert2521.powerup.util.MAX_VELOCITY
+import org.sert2521.powerup.util.UDPServer
 import org.sert2521.powerup.util.WHEEL_DIAMETER
 import org.sert2521.powerup.util.autoMode
 import org.sertain.RobotLifecycle
@@ -54,6 +55,8 @@ object Auto : RobotLifecycle {
         RightToLeftScalePath
         LeftSwitchToRearPath
         RightSwitchToRearPath
+
+        UDPServer
     }
 
     override fun onAutoStart() {

src/main/java/org/sert2521/powerup/drivetrain/commands/DriveToCube.kt

@@ -1,11 +1,10 @@
 package org.sert2521.powerup.drivetrain.commands
 
-import edu.wpi.first.networktables.NetworkTable
-import edu.wpi.first.networktables.NetworkTableInstance
 import org.sert2521.powerup.drivetrain.Drivetrain
 import org.sert2521.powerup.elevator.Elevator
 import org.sert2521.powerup.intake.Intake
 import org.sert2521.powerup.util.DEGREES_PER_PIXEL
+import org.sert2521.powerup.util.VisionData
 
 class DriveToCube : AngleDriver(0.01, 0.0, 0.022) {
     init {
@@ -15,9 +14,17 @@ class DriveToCube : AngleDriver(0.01, 0.0, 0.022) {
     override fun onCreate() = updateSetpoint(0.0)
 
     override fun execute(output: Double): Boolean {
-        Drivetrain.drive(BASE_SPEED + output, BASE_SPEED - output)
-        updateSetpoint(table.getEntry("cube_offset_x").getDouble(0.0) * DEGREES_PER_PIXEL)
-        return Intake.hasCube && Elevator.atBottom
+        println("Found Cube: ${VisionData.foundCube}, " +
+                        "X Offset: ${VisionData.xOffset}, " +
+                        "Y Offset: ${VisionData.yOffset}")
+
+        if (VisionData.foundCube) {
+            Drivetrain.drive(BASE_SPEED + output, BASE_SPEED - output)
+            updateSetpoint(VisionData.xOffset!! * DEGREES_PER_PIXEL)
+            return Intake.hasCube && Elevator.atBottom
+        }
+
+        return true
     }
 
     private fun updateSetpoint(offset: Double) {
@@ -26,6 +33,5 @@ class DriveToCube : AngleDriver(0.01, 0.0, 0.022) {
 
     private companion object {
         const val BASE_SPEED = 0.3
-        val table: NetworkTable = NetworkTableInstance.getDefault().getTable("Vision")
     }
 }

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

@@ -38,5 +38,6 @@ const val MAX_VELOCITY = 0.6
 const val MAX_ACCELERATION = 0.1
 const val MAX_JERK = 60.0
 
-// Camera
+// 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/UDPServer.kt

@@ -0,0 +1,35 @@
+package org.sert2521.powerup.util
+
+import com.google.gson.Gson
+import java.net.DatagramPacket
+import java.net.DatagramSocket
+
+object UDPServer : Thread() {
+    private val PACKET_SIZE = 256
+
+    private val socket = DatagramSocket(UDP_PORT)
+    private val gson = Gson()
+
+    override fun run() {
+        val buf = ByteArray(PACKET_SIZE)
+        val packet = DatagramPacket(buf, buf.size)
+
+        socket.receive(packet)
+        val msg = String(packet.data).trim { it <= ' ' }
+
+        if (!msg.contains("alive")) {
+            gson.fromJson(msg, VisionData.javaClass).also {
+                VisionData.apply {
+                    foundCube = it.foundCube
+                    time = it.time
+                    xOffset = it.xOffset
+                    yOffset = it.yOffset
+                }
+            }
+
+            println("Message from ${packet.address.hostAddress}: $msg")
+        } else {
+            println("Heartbeat from ${packet.address.hostAddress}")
+        }
+    }
+}

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

@@ -0,0 +1,8 @@
+package org.sert2521.powerup.util
+
+object VisionData {
+    var foundCube = false
+    var time = 0
+    var xOffset: Int? = null
+    var yOffset: Int? = null
+}