# **Distance Measuring Related Algorithms**
## **Current Status Overview**
###### tags: `FRC` `java`
在機器人前側高**88.74125 cm**處架設兩個測距儀，使用**encoder**輔助垂直轉盤方向就位。

水平轉盤桌方向就位使用位於左前角高**142.0175 or 138.0175 cm**的**馬達轉動測距儀**，測量與**左前方柱子**的最短距離。


使用**TTL**傳輸訊號，**rx和tx互接**。

### **測距儀的限制:**
1 可測範圍介於30cm至12m，不過桌子長邊到中間距離>30cm，所以前側測距儀不凸出則無礙
2 數值為unsigned int
3 與水平面角度不可偏超過10°

### **場地限制:**
1 右邊牆過低，且為透明、頂部為圓柱狀無法測距
2 左邊沒有適合測距之物

## **待測量**
- 轉盤旁柱與有框牆面角度差
- 柱寬


# ***Robot.java***
---

private LidarProxy Proxy;
@Override
public void teleopInit() 
{
    Proxy = new LidarProxy(SerialPort.Port.kUSB);
}
@Override
public void teleopPeriodic() 
{
   Turn(); 
}

## ***Left Front Pillar Targeting Algorithm O(1) (best)***
---

public void LFPTargeting()
{
    //Lidar從水平車子側邊方向轉出去
    double dist=1e9;
    double i=3.1;
    double x,y;
    double angle=0;
    while(SM.getAngle()<50)
    {
        i+=0.7;
        SM.setAngle(i);
        if(Proxy.get()<dist)
        {
            dist=Proxy.get();
            angle=SM.getAngle();
        }
    }
    x=dist*Math.sin(angle);
    y=dist*Math.cos(angle);//垂直frame
    x+=2.744364-pillar_dist_ to_wall;//240.4364: distance of wall to frame 34: half of 車寬
    y+=9.291574;
    Turn(Math.atan(x/y));//仍維持同一位置(左輪往後，右輪往前)
    motor(x,y);//z已知為248.92
}

# ***LidarProxy.java***
---

package frc.robot;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.SerialPort;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

public class LidarProxy1 {
    private int lastReadDistance;
    private LidarListener _listener;
    private Thread _thread;
    private boolean _initializedProperly;

    public LidarProxy(SerialPort.Port port) {
        setup(port);
    }

    public void setup(SerialPort.Port port) {
        try {
            _listener = new LidarListener(this, port);//this: represent current class
            _thread = new Thread(_listener);
            _thread.start();
            _initializedProperly = true;
        } catch (Exception e) {
            _initializedProperly = false;
            DriverStation.reportError("LidarProxy could not intialize properly. " + e.getStackTrace().toString(), false);
        }
        SmartDashboard.putBoolean("Lidar/initializedProperly", _initializedProperly);
    }
  
    public double get() {
        return lastReadDistance;
    }
    class LidarListener implements Runnable {
        private SerialPort _port;
        private LidarProxy _proxy;

        public LidarListener(LidarProxy proxy, SerialPort.Port port) {
            _proxy = proxy;
            _port = new SerialPort(115200, port);
        }
        public void run() {
            while (true) {
                try {
                    byte[] read = _port.read(9);
                    _proxy.lastReadDistance = read[2] & 0xFF;
                } catch (Exception e) {
                    DriverStation.reportError("LidarListener exception: " + e.toString(), false);
                }
            }
        }
    }
}