1 package org
.usfirst
.frc
.team3501
.robot
;
3 import java
.util
.TimerTask
;
5 import edu
.wpi
.first
.wpilibj
.I2C
;
6 import edu
.wpi
.first
.wpilibj
.I2C
.Port
;
7 import edu
.wpi
.first
.wpilibj
.PIDSource
;
8 import edu
.wpi
.first
.wpilibj
.PIDSourceType
;
9 import edu
.wpi
.first
.wpilibj
.Timer
;
11 public class Lidar
implements PIDSource
{
13 private byte[] distance
;
14 private java
.util
.Timer updater
;
16 private final int LIDAR_ADDR
= 0x62;
17 private final int LIDAR_CONFIG_REGISTER
= 0x00;
18 private final int LIDAR_DISTANCE_REGISTER
= 0x8f;
20 public Lidar(Port port
) {
21 i2c
= new I2C(port
, LIDAR_ADDR
);
23 distance
= new byte[2];
25 updater
= new java
.util
.Timer();
29 public int getDistance() {
30 return (int) Integer
.toUnsignedLong(distance
[0] << 8)
31 + Byte
.toUnsignedInt(distance
[1]);
35 public double pidGet() {
41 updater
.scheduleAtFixedRate(new LIDARUpdater(), 0, 100);
44 // Start polling for period in milliseconds
45 public void start(int period
) {
46 updater
.scheduleAtFixedRate(new LIDARUpdater(), 0, period
);
51 updater
= new java
.util
.Timer();
54 // Update distance variable
55 public void update() {
56 i2c
.write(LIDAR_CONFIG_REGISTER
, 0x04); // Initiate measurement
57 Timer
.delay(0.04); // Delay for measurement to be taken
58 i2c
.read(LIDAR_DISTANCE_REGISTER
, 2, distance
); // Read in measurement
59 Timer
.delay(0.005); // Delay to prevent over polling
62 // Timer task to keep distance updated
63 private class LIDARUpdater
extends TimerTask
{
70 } catch (InterruptedException e
) {
78 public void setPIDSourceType(PIDSourceType pidSource
) {
82 public PIDSourceType
getPIDSourceType() {