From 2db494ceac81886400c546027a27330de1286bfe Mon Sep 17 00:00:00 2001
From: EvanYap <evanyap.14@gmail.com>
Date: Sat, 6 Feb 2016 15:40:02 -0800
Subject: [PATCH] Import a lidar class

---
 src/org/usfirst/frc/team3501/robot/Lidar.java | 88 +++++++++++++++++++
 1 file changed, 88 insertions(+)
 create mode 100644 src/org/usfirst/frc/team3501/robot/Lidar.java

diff --git a/src/org/usfirst/frc/team3501/robot/Lidar.java b/src/org/usfirst/frc/team3501/robot/Lidar.java
new file mode 100644
index 00000000..1db2d809
--- /dev/null
+++ b/src/org/usfirst/frc/team3501/robot/Lidar.java
@@ -0,0 +1,88 @@
+package org.usfirst.frc.team3501.robot;
+
+import java.util.TimerTask;
+
+import edu.wpi.first.wpilibj.I2C;
+import edu.wpi.first.wpilibj.I2C.Port;
+import edu.wpi.first.wpilibj.PIDSource;
+import edu.wpi.first.wpilibj.PIDSourceType;
+import edu.wpi.first.wpilibj.Timer;
+
+public class Lidar implements PIDSource {
+  private I2C i2c;
+  private byte[] distance;
+  private java.util.Timer updater;
+
+  private final int LIDAR_ADDR = 0x62;
+  private final int LIDAR_CONFIG_REGISTER = 0x00;
+  private final int LIDAR_DISTANCE_REGISTER = 0x8f;
+
+  public Lidar(Port port) {
+    i2c = new I2C(port, LIDAR_ADDR);
+
+    distance = new byte[2];
+
+    updater = new java.util.Timer();
+  }
+
+  // Distance in cm
+  public int getDistance() {
+    return (int) Integer.toUnsignedLong(distance[0] << 8)
+        + Byte.toUnsignedInt(distance[1]);
+  }
+
+  @Override
+  public double pidGet() {
+    return getDistance();
+  }
+
+  // Start 10Hz polling
+  public void start() {
+    updater.scheduleAtFixedRate(new LIDARUpdater(), 0, 100);
+  }
+
+  // Start polling for period in milliseconds
+  public void start(int period) {
+    updater.scheduleAtFixedRate(new LIDARUpdater(), 0, period);
+  }
+
+  public void stop() {
+    updater.cancel();
+    updater = new java.util.Timer();
+  }
+
+  // Update distance variable
+  public void update() {
+    i2c.write(LIDAR_CONFIG_REGISTER, 0x04); // Initiate measurement
+    Timer.delay(0.04); // Delay for measurement to be taken
+    i2c.read(LIDAR_DISTANCE_REGISTER, 2, distance); // Read in measurement
+    Timer.delay(0.005); // Delay to prevent over polling
+  }
+
+  // Timer task to keep distance updated
+  private class LIDARUpdater extends TimerTask {
+    @Override
+    public void run() {
+      while (true) {
+        update();
+        try {
+          Thread.sleep(10);
+        } catch (InterruptedException e) {
+          e.printStackTrace();
+        }
+      }
+    }
+  }
+
+  @Override
+  public void setPIDSourceType(PIDSourceType pidSource) {
+    // TODO Auto-generated method stub
+
+  }
+
+  @Override
+  public PIDSourceType getPIDSourceType() {
+    // TODO Auto-generated method stub
+    return null;
+  }
+}
-- 
2.30.2