1 package org
.usfirst
.frc
.team3501
.robot
.utils
;
3 import java
.util
.TimerTask
;
4 import edu
.wpi
.first
.wpilibj
.I2C
;
5 import edu
.wpi
.first
.wpilibj
.Timer
;
6 import edu
.wpi
.first
.wpilibj
.I2C
.Port
;
7 import edu
.wpi
.first
.wpilibj
.PIDSource
;
8 import edu
.wpi
.first
.wpilibj
.PIDSourceType
;
10 public class Lidar
implements PIDSource
{
12 private byte[] distance
;
13 private java
.util
.Timer updater
;
15 private final int LIDAR_ADDR
= 0x62;
16 private final int LIDAR_CONFIG_REGISTER
= 0x00;
17 private final int LIDAR_DISTANCE_REGISTER
= 0x8f;
19 public Lidar(Port port
) {
20 i2c
= new I2C(port
, LIDAR_ADDR
);
22 distance
= new byte[2];
24 updater
= new java
.util
.Timer();
28 public int getDistance() {
29 return (int) Integer
.toUnsignedLong(distance
[0] << 8)
30 + Byte
.toUnsignedInt(distance
[1]);
34 public double pidGet() {
40 updater
.scheduleAtFixedRate(new LIDARUpdater(), 0, 100);
43 // Start polling for period in milliseconds
44 public void start(int period
) {
45 updater
.scheduleAtFixedRate(new LIDARUpdater(), 0, period
);
50 updater
= new java
.util
.Timer();
53 // Update distance variable
54 public void update() {
55 i2c
.write(LIDAR_CONFIG_REGISTER
, 0x04); // Initiate measurement
56 Timer
.delay(0.04); // Delay for measurement to be taken
57 i2c
.read(LIDAR_DISTANCE_REGISTER
, 2, distance
); // Read in measurement
58 Timer
.delay(0.005); // Delay to prevent over polling
61 // Timer task to keep distance updated
62 private class LIDARUpdater
extends TimerTask
{
69 } catch (InterruptedException e
) {
77 public void setPIDSourceType(PIDSourceType pidSource
) {
78 // TODO Auto-generated method stub
83 public PIDSourceType
getPIDSourceType() {
84 // TODO Auto-generated method stub