laneser

Day15 的地圖要自己慢慢畫, 但是機器人都是自己 DIY 的啊...

所以, 我們有必要再改良一點點.

其實我們只要在畫完地圖後, 再通知自己進行計算, 然後走下一步, 這樣就是自動畫地圖囉.

所以我們只要在 LRFDriveTypes.cs 當中再加一條 DSSP 宣告 (記得把它放進 LRFDriveOperations 的宣告當中)

public class CalculateNextScan : Update<CalculateNextScanMsg, PortSet<DefaultSubmitResponseType, Fault>> { }

然後, Start() 最後加上

Activate(Arbiter.Receive(false, TimeoutPort(1000), time => 
_mainPort.Post(new ScanMapCommand() { Body = new ScanMapData() { ForwardScan = 0, RotateScan = 0 } })));

表示我們在一開始過了一秒後, 開始 ScanMap.
在 ScanMapCommandHandler 當中, Scan 完畢後就通知自己計算下一步, 如下:

if ((smd.Body.ForwardScan == 0) && (smd.Body.RotateScan == 0))
{
    LRFMapDrawer.DrawMap(_state.LrfState, _state.Map, _state.CurrntPosition);
    UpdateMap();
    UpdatePosInfo();

    _mainPort.Post(new CalculateNextScan());
}

最後, 就是 CalculateNextScanHandler 囉:

private bool Pass(int[] data, int begin, int end, int min)
{
    for (int i = begin; i < end; i++)
    {
        if (data[i] < min)
            return false;
    }
    return true;                
}

[ServiceHandler(ServiceHandlerBehavior.Exclusive)]
public IEnumerator<ITask> CalculateNextScanHandler(CalculateNextScan scan)
{
    if (_state.LrfState == null)
        yield break;

    List<ScanMapData> CanDoList = new List<ScanMapData>();

    int mid = _state.LrfState.DistanceMeasurements.Length / 2;

    if (Pass(_state.LrfState.DistanceMeasurements, mid - 15, mid + 15, 2000))
        CanDoList.Add(new ScanMapData() { ForwardScan = 1, RotateScan = 0 });

    if (Pass(_state.LrfState.DistanceMeasurements, _state.LrfState.DistanceMeasurements.Length - 16, _state.LrfState.DistanceMeasurements.Length - 1, 2000))
        CanDoList.Add(new ScanMapData() { ForwardScan = 0, RotateScan = 90 });

    if (Pass(_state.LrfState.DistanceMeasurements, 1, 15, 2000))
        CanDoList.Add(new ScanMapData() { ForwardScan = 0, RotateScan = 270 });

    if (CanDoList.Count == 0)
        CanDoList.Add(new ScanMapData() { ForwardScan = 0, RotateScan = 180 });

    _mainPort.Post(new ScanMapCommand() { Body = CanDoList[0] });

    scan.ResponsePort.Post(DefaultSubmitResponseType.Instance);

    yield break;
}

 

我採用超級簡單的策略, 因為這樣就可以漫遊了.
如果能夠加上 Map 當中的資料, 這樣機器人就會變得聰明許多.
但這些就留給有心人啦.

最後, 自動畫出來的地圖像是這樣:

你可以發現, 會有點重疊, 這是因為定位不準確導致.

我們可以考慮利用 Map 當中的資料來做自動修正, 依照我的經驗, 系統的 RotateDegree 似乎不是很準確, 會有點誤差, 所以才會在旋轉過後, 地圖就有點誤差.
這些就留待日後修正.

最後, 整個 LRFDrive 的原始碼可以在這裡下載.

LRFDrive.zip