public override async Task<bool> HandleUltraSonicDataChange(RobotStateArgs args)
{
await base.HandleUltraSonicDataChange(args);
if (args.Context.Data.UltrasonicSensorData <= PREVENT_COLLISTION_DISTANCE)
{
await args.Context.Controller.DriveStop();
switch(_mode)
{
case Mode.NextTurnRight:
{
await args.Context.NextState(new TurnRightState(1));
break;
}
case Mode.NextTurnLeft:
{
await args.Context.NextState(new TurnLeftState(1));
break;
}
}
}
return true;
}
public override async Task<bool> InitState(RobotStateArgs args)
{
await base.InitState(args);
_startAngle = args.Context.Data.Angle;
await args.Context.Controller.TurnLeftStart();
return true;
}
public override async Task<bool> HandleUltraSonicDataChange(RobotStateArgs args)
{
await base.HandleUltraSonicDataChange(args);
if (args.Context.Data.UltrasonicSensorData <= PREVENT_COLLISTION_DISTANCE)
{
await args.Context.Controller.DriveStop();
await args.Context.NextState(new ReadyState());
}
return true;
}
public override async Task<bool> InitState(RobotStateArgs args)
{
await base.InitState(args);
await args.Context.Controller.DriveForwardXSteps(STEPS);
switch(_mode)
{
case Mode.NextTurnRight:
{
await args.Context.NextState(new TurnRightState(2));
break;
}
case Mode.NextTurnLeft:
{
await args.Context.NextState(new TurnLeftState(2));
break;
}
}
return true;
}
public override async Task<bool> HandleAngleChange(RobotStateArgs args)
{
await base.HandleAngleChange(args);
int currentAngle = args.Context.Data.Angle;
int angleDiff = Math.Abs(currentAngle - _startAngle);
//Debug.WriteLine("angleDiff: " + angleDiff);
if (angleDiff >= (_targetAngle - ANGLE_CORRECTION))
{
await args.Context.Controller.DriveStop();
if (_turnNumber == 1)
{
await args.Context.NextState(new DriveForwardDefinedWayState(DriveForwardDefinedWayState.Mode.NextTurnLeft));
}
else if (_turnNumber == 2)
{
await args.Context.NextState(new DriveForwardState( DriveForwardState.Mode.NextTurnRight));
}
}
return true;
}
public override async Task<bool> InitState(RobotStateArgs args)
{
return await args.Context.Controller.DriveForwardStart();
}
