public void ExecuteInstruction(HardwareInstruction instruction)
{
switch (instruction.Operator)
{
case HardwareOperator.MoveForward:
this.ActMoveForward(instruction.Operand);
break;
case HardwareOperator.MoveBack:
this.ActMoveBack(instruction.Operand);
break;
case HardwareOperator.TurnLeft:
this.ActTurnLeft(instruction.Operand);
break;
case HardwareOperator.TurnRight:
this.ActTurnRight(instruction.Operand);
break;
case HardwareOperator.SetSpeed:
this.SetSpeed(instruction.Operand);
break;
}
}
static void Main(string[] args)
{
Console.OutputEncoding = Encoding.UTF8;
var navigation = new Navigator(/*ChangePortName is needed*/);
var logger = new Logger(navigation);
//var route1 = navigation.GetRoute(@"北京市鼓楼东大街46号", @"三里屯");
//var routing = new Routing();
var destination = @"独墅湖体育馆";
Double desLat = 31.265, desLgt = 120.7267;
if (args.Length == 1)
{
var pos = args[0].Split(',');
Double.TryParse(pos[0], out desLat);
Double.TryParse(pos[1], out desLgt);
}
else if (args.Length == 2)
{
Double.TryParse(args[0], out desLat);
Double.TryParse(args[1], out desLgt);
}
try
{
////var srcLat = desLat + 0.01;//navigation.getLatitude();
////var srcLgt = desLgt + 0.01;//navigation.getLongitude();
navigation.Connect();
var s = new Stopwatch();
s.Start();
while (s.Elapsed.Seconds < 5)
{
continue;
}
var currentLocation = navigation.GetCurrentLocation();
Console.WriteLine(@"Current Location is :", navigation.GetCurrentLocation());
var srcLat = navigation.getLatitude();
var srcLgt = navigation.getLongitude();
var srcCoordinate = new Coordinate(srcLat, srcLgt);
var desCoordinate = new Coordinate(desLat, desLgt);
////var controller = new HardwareInstructionController();
var op = HardwareOperator.Brake;
var instruction = new HardwareInstruction(op, 2 * numIntervalTurn);
////controller.ExecuteInstruction(instruction);
op = HardwareOperator.MoveForward;
instruction = new HardwareInstruction(op, 2 * numIntervalTurn);
////controller.ExecuteInstruction(instruction);
while (s.Elapsed < TimeSpan.FromSeconds(600))
{
if (s.Elapsed.Seconds % 10 == 5)
{
var lat = navigation.getLatitude();
var lgt = navigation.getLongitude();
var currentCoordinate = new Coordinate(lat, lgt);
var direction = Position2Direcion(srcCoordinate, desCoordinate, currentCoordinate);
Console.WriteLine(direction);
op = HardwareOperator.NoOp;
if (direction > 99)
{
op = HardwareOperator.Brake;
Console.WriteLine("st");
}
else if (direction > 0.5)
{
op = HardwareOperator.TurnRight;
Console.WriteLine("rt");
}
else if (direction < -0.5)
{
op = HardwareOperator.TurnLeft;
Console.WriteLine("lt");
}
else
{
Console.WriteLine("NOP");
continue; // do No Op
}
try
{
instruction = new HardwareInstruction(op, numIntervalTurn);
}
catch (Exception e)
{
Console.WriteLine("[Invalid Instruction: {0}]", e.Message);
}
}
}
}
catch (Exception e)
{
navigation.Disconnect();
Console.WriteLine("[Invalid Instruction: {0}]", e.Message);
}
finally
{
navigation.Disconnect();
}
}