public void Process()
{
//read position and direction
updateDirection();
rangeFinder.UpdateValue();
Vector curnetPosition = new Vector(Position[0], Position[1]);
//TODO: map update
//TODO: colision detection
Command toDo;
lock (commandQueue)
{
if (commandQueue.Count == 0)
{ //nothing to do
if (status == DriverStatus.Processing)
status = DriverStatus.Done;
return;
}
toDo = commandQueue[0];
}
status = DriverStatus.Processing;
Vector goal = new Vector(toDo.x, toDo.y);
Vector dV;
switch (toDo.type)
{
case Command.CType.LookAt:
goal.Sub(curnetPosition);
goal.Normalize();
dV = new Vector(goal);
dV.Sub(direction);
if (dV.Lenght() < rotationEpsilon || goal.Lenght() == 0.0)
{
SetEngineVelocity(0, 0);
lock (commandQueue)
commandQueue.RemoveAt(0);
return;
}
bool rotateRight = direction.IsRotateRightAGoodIdea(goal);
// set the engine velocity
double vel = maxVelocity * RotationSpeedMap(dV.Lenght());
if (rotateRight)
{
SetEngineVelocity(vel, -vel);
}
else
{
SetEngineVelocity(-vel, vel);
}
break;
case Command.CType.GoTo:
// obstacle detection
if (rangeFinder.value < obstacleDetectionRange)
{
CommandEmergencyStop();
status = DriverStatus.Obstacle;
return;
}
dV = new Vector(goal);
dV.Sub(curnetPosition);
if (dV.Lenght() < positionEpsilon)
{
SetEngineVelocity(0, 0);
lock (commandQueue)
commandQueue.RemoveAt(0);
return;
}
double leftVel, rightVel;
leftVel = rightVel = maxVelocity * MovementSpeedMap(dV.Lenght());
//correction direction
Vector dDirection = new Vector(dV);
dDirection.Normalize();
Vector dDv = new Vector(dDirection);
dDv.Sub(direction);
if (dDv.Lenght() > rotationEpsilon)
{
if (direction.IsRotateRightAGoodIdea(dDirection))
{
rightVel -= rightVel * dDv.Lenght(); //rotationEpsilon < dDv.Length <= 2.0, 2.0 mean 180 degree error
}
else
{
leftVel -= leftVel * dDv.Lenght();
}
}
SetEngineVelocity(leftVel, rightVel);
break;
}
}
private bool isInGoalPosition()
{
Vector currentPosition = new Vector(driver.Position[0], driver.Position[1]);
currentPosition.Sub(desierPosition);
return currentPosition.Lenght() < epsilon;
}