public BasicMetaCommandResult ProcessResponce(BasicResponce rsp)
{
if (_step == 0)
{
_step = 1;
var cmd = new StatusCommand(_degree, 1200);
return(new BasicMetaCommandResult(MetaCommandAction.Command, cmd));
}
if (rsp == null)
{
return(new BasicMetaCommandResult(MetaCommandAction.Idle));
}
var scanResponce = rsp as StatusResponce;
if (scanResponce != null)
{
_scanResult[scanResponce.Degree] = scanResponce.DistanceToObstacle;
}
if (_step == 1 && _degree < MaxDegree)
{
_degree += 1;
var cmd = new StatusCommand(_degree, 50);
return(new BasicMetaCommandResult(MetaCommandAction.Command, cmd));
}
return(new BasicMetaCommandResult(MetaCommandAction.Done));
}
public BasicMetaCommandResult ProcessResponce(BasicResponce rsp)
{
if (rsp == null)
{
Console.WriteLine("Sending status");
var cmd = new StatusCommand(Degree, 200);
return(new BasicMetaCommandResult(MetaCommandAction.Command, cmd));
}
Console.WriteLine("Received command");
var status = rsp as StatusResponce;
if (status == null)
{
Console.WriteLine("Idle");
return(new BasicMetaCommandResult(MetaCommandAction.Idle));
}
if (status.DistanceToObstacle < MinDistance && _direction == MoveDirection.Forward)
{
return(MoveBackward());
}
if (status.DistanceToObstacle >= MinDistance && _direction == MoveDirection.Backwards)
{
return(MoveForward());
}
if (_direction == MoveDirection.Stopped)
{
Console.WriteLine("Stopped.Moving forward");
return(MoveForward());
}
return(new BasicMetaCommandResult(MetaCommandAction.Idle));
}
/*
* 1. Двигаемся вперед пока не наткнемся на препятствие
* 2. Сканируем, поворачиваем на направление с максимальной дистанцией
* 2.1 Если повсюду препятствия, поворачиваемся на случайный угол(25-180). Переход к пункту 2.
* 3. Двигаемся по новому направлению. Переход к п.1.
*/
public BasicMetaCommandResult ProcessResponce(BasicResponce rsp)
{
switch (_status)
{
case Status.Start:
Console.WriteLine(_status.ToString());
var startCmd = new RangeScanCommand(MinScanDegree, MaxScanDegree, 200);
_status = Status.WaitingForScanResults;
return(new BasicMetaCommandResult(MetaCommandAction.Command, startCmd));
case Status.StartScanning:
var answer = rsp as AnswerResponce;
if (answer != null && answer.Code == Commands.Move)
{
Console.WriteLine(_status.ToString());
var cmd = new RangeScanCommand(MinScanDegree, MaxScanDegree, 200);
_status = Status.WaitingForScanResults;
return(new BasicMetaCommandResult(MetaCommandAction.Command, cmd));
}
break;
case Status.WaitingForScanResults:
var responce = rsp as RangeScanResponce;
if (responce != null)
{
Console.WriteLine(_status.ToString());
var degree = FindNewDirection(responce);
Console.WriteLine(" Selected degree: " + degree);
var rnd = new Random();
if (degree == null)
{
_turnDirection = (TurnDirection)rnd.Next((int)TurnDirection.Left, (int)TurnDirection.Right + 1);
_turnDegree = (byte)rnd.Next(90, 180);
_status = Status.StartTurning;
break;
}
if (degree == 0xFF)
{
_status = Status.StartMoving;
break;
}
if (degree > RangeScanCommand.CenterDegree)
{
_turnDirection = TurnDirection.Right;
_turnDegree = (byte)(degree - RangeScanCommand.CenterDegree);
}
else
{
_turnDirection = TurnDirection.Left;
_turnDegree = (byte)(RangeScanCommand.CenterDegree - degree);
}
_status = Status.StartTurning;
}
break;
case Status.StartMoving:
Console.WriteLine(_status.ToString());
_status = Status.Moving;
var moveCmd = new MoveCommand(MoveDirection.Forward, Speed, 500);
return(new BasicMetaCommandResult(MetaCommandAction.Command, moveCmd));
case Status.Moving:
if (rsp == null)
{
var statusCmd = new StatusCommand(RangeScanCommand.CenterDegree, 500);
return(new BasicMetaCommandResult(MetaCommandAction.Command, statusCmd));
}
Console.WriteLine(_status.ToString());
var status = rsp as StatusResponce;
if (status == null)
{
break;
}
if (status.DistanceToObstacle < MinDistance)
{
_status = Status.StartScanning;
var stopCmd = new MoveCommand(MoveDirection.Stopped, Speed, 500);
return(new BasicMetaCommandResult(MetaCommandAction.Command, stopCmd));
}
break;
case Status.StartTurning:
Console.WriteLine(_status.ToString());
_status = Status.StartScanning;
var turnCmd = new TurnCommand(_turnDirection, _turnDegree, 255, true, 500);
return(new BasicMetaCommandResult(MetaCommandAction.Command, turnCmd));
case Status.None:
break;
default:
Console.WriteLine("ERROR! Unknown status!");
break;
}
return(new BasicMetaCommandResult(MetaCommandAction.Idle));
}
