public void Track(Marker targetMarker)
{
for (; ; CheckCancelAndImageProcess())
{
// Tracking
Stopwatch failStopwatch = new Stopwatch();
// 이동 멈춤 대기
SpinCheck(() => _deviceService.PanRunning() == false);
CheckCancelAndImageProcess();
for (; ; CheckCancelAndImageProcess())
{
var targetRecognized = from m in _result.Markers
where m.Color == targetMarker.Color
select m;
if (targetRecognized.Count() == 0)
{
Console.WriteLine("실패");
// 인식 실패
if (failStopwatch.IsRunning == false)
{
failStopwatch.Start();
}
// 추적 포기
if (failStopwatch.ElapsedMilliseconds > 3500)
{
return;
}
}
else
{
Console.WriteLine("성공");
// 추적 중
failStopwatch.Reset();
var target = targetRecognized.First();
bool
xOk = false,
yOk = false;
var x = target.X;
var y = target.Y;
var xa = Math.Abs(x);
var ya = Math.Abs(y);
target.TransX *= 1000;
target.TransY *= 1000;
target.TransZ *= 1000;
/*
* var angleX = GeometryTools.GetAngleBetweenVectors(
* new IntPoint(0, 0), new IntPoint((int)target.TransZ, (int)target.TransX), new IntPoint((int)target.TransZ, 0));
*
* var angleY = GeometryTools.GetAngleBetweenVectors(
* new IntPoint(0, 0), new IntPoint((int)target.TransZ, (int)target.TransY), new IntPoint((int)target.TransZ, 0));
*
* if (Math.Abs(angleX) > 5)
* {
* if (target.TransX < 0)
* {
* angleX *= -1;
* }
* var add = _physicsService.PanAngleToStep(_physicsService.DegreeToRadian(angleX));
* _deviceService.PanAdd(add, 0);
* }
* else
* {
* xOk = true;
* }
*/
if (xa > 50)
{
_deviceService.PanAdd((short)(x > 0 ? 40 : -40), 0);
}
else if (xa > 15)
{
_deviceService.PanAdd((short)(x > 0 ? 5 : -5), 0);
}
else
{
xOk = true;
}
if (ya > 50)
{
_deviceService.TiltCameraAdd((short)(y > 0 ? 6 : -6));
}
else if (ya > 15)
{
_deviceService.TiltCameraAdd((short)(y > 0 ? 2 : -2));
}
else
{
yOk = true;
}
// 추적 성공
if (xOk && yOk)
{
CheckCancel();
// Watering
var result = _physicsService.Measure(
target,
_deviceService.TiltCameraGet(),
_deviceService.PanGet(), false);
_deviceService.TiltNozzleSet(result.EstimatedNozzleWidth);
_deviceService.PanSet(result.EstimatedPanStep, 0);
double timeFactor = 1.0;
switch (target.Rotate)
{
case MarkerRotate.Quad1: break;
case MarkerRotate.Quad2: timeFactor *= 2; break;
case MarkerRotate.Quad3: timeFactor *= 0.5; break;
case MarkerRotate.Quad4: timeFactor *= 0; break;
}
var plant = (from p in _plantService.GetModel()
where p.MarkerColor == target.Color
select p).First();
TimeSpan plantWaterTime = new TimeSpan((long)(plant.WaterTime.Ticks * timeFactor));
// 살수 안함
if (plantWaterTime.Ticks == 0)
{
plant.WateredNow();
}
else
{
// 이동 멈춤 대기
SpinCheck(() => _deviceService.PanRunning() == false);
// 살수 개시
_deviceService.WaterOn();
// 지연
SpinCheckSleep((long)(plantWaterTime.TotalMilliseconds));
// 살수 중단
_deviceService.WaterOff();
plant.WateredNow();
Thread.Sleep(1000);
}
return;
}
else
{
// 추적 실패
// 멈춤 대기
SpinCheck(() => _deviceService.PanRunning() == false);
}
}
}
}
}
