/// <summary>
/// which way rotate vector
/// </summary>
/// <param name="toVector">desire direction</param>
/// <returns>true if rotation right will be faster than left</returns>
public bool IsRotateRightAGoodIdea(Vector toVector)
{
bool rotateRight = true;
// finding which way the robot should be rotate
if (toVector.X != 0)
{
if (y < (toVector.Y / toVector.X) * x)
{
rotateRight = false;
}
if (toVector.X < 0.0)
rotateRight = !rotateRight;
}
else //toVector.X == 0.0
{
if (x > 0.0)
rotateRight = false;
if (toVector.Y < 0.0)
rotateRight = !rotateRight;
}
return rotateRight;
}
public void SetRobot(Robot r)
{
this.robot = r;
status = DriverStatus.Done;
rangeFinder = robot.GetSensorByName(rangeFinderName) as Robot.SensorRangeFinder;
leftWhell = robot.GetPartByName(leftWheelName);
rightWheel = robot.GetPartByName(rightWheelName);
direction = new Vector(0, 0);
updateDirection();
commandQueue = new List<Command>();
}
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;
}
}
public Vector(Vector copy)
{
x = copy.X;
y = copy.Y;
z = copy.Z;
}
public void Add(Vector v)
{
x += v.X;
y += v.Y;
}
public void Sub(Vector v)
{
x -= v.X;
y -= v.Y;
}
public void doWork()
{
if(state == SimpleAgentStatus.DONE){
return;
}
Thread.Sleep(200);
switch (driver.Status)
{
case DriverStatus.Processing:
return;
case DriverStatus.Done:
if(isInGoalPosition()){
mainForm.sLog.log("\tAgent: I'm done my work");
driver.CommandEmergencyStop();
state = SimpleAgentStatus.DONE;
return;
}else if(state == SimpleAgentStatus.GO){
mainForm.sLog.log("\tAgent: go straight to goal");
driver.CommandGoTo(desierPosition.X, desierPosition.Y);
}
else if (state == SimpleAgentStatus.FIND)
{
findValue[findIndex] = driver.SensorsValue[0];
mainForm.sLog.log("\tAgent: Finding <" + findIndex + "> direction:" + findDirection[findIndex].ToString() +" actual direction: " +driver.Direction.ToString()+" value = " + findValue[findIndex]);
findIndex++;
if (findIndex == findDirection.Length)
{
mainForm.sLog.log("\tAgent: must choose");
int maxIndex = 0;
double maxVal = -1;
for (int i = 0; i < findValue.Length; i++ )
{
if(findValue[i] >= maxVal){
maxIndex = i;
maxVal = findValue[i];
}
}
Vector toGo = findDirection[maxIndex];
Vector vGo = new Vector(toGo);
vGo.Sub(new Vector(driver.Position[0],driver.Position[1]));
vGo.Normalize();
double toX = vGo.X * 0.3 + driver.Position[0];
double toY = vGo.Y * 0.3 + driver.Position[1];
driver.CommandGoTo(toX , toY );
mainForm.sLog.log("\tAgent: choose ["+maxIndex+"] : going to [" + toX +" : " + toY + "]");
state = SimpleAgentStatus.GO;
}
else
{
driver.CommandLookAt(findDirection[findIndex].X, findDirection[findIndex].Y);
}
}
break;
case DriverStatus.Collision:
mainForm.sLog.log("\tAgent: Collision");
break;
case DriverStatus.Obstacle:
if (state == SimpleAgentStatus.GO)
{
mainForm.sLog.log("\tAgent: Obstacle, state:" + state);
mainForm.sLog.log("\tAgent: prepare finding table");
prepareFindTable();
state = SimpleAgentStatus.FIND;
driver.CommandLookAt(findDirection[findIndex].X, findDirection[findIndex].Y);
mainForm.sLog.log("\tAgent: look at");
}
break;
}
}
private void prepareFindTable()
{
double dx = driver.Direction.X;
double dy = driver.Direction.Y;
//finding first point on the left;
Vector lookAt = new Vector(findDirectionCount * dy, - findDirectionCount * dx);
lookAt.Add(new Vector(dx, dy));
lookAt.Add(new Vector(driver.Position[0],driver.Position[1]));
dx/=2.0;
dy/=2.0;
mainForm.sLog.log("\tAgent: position : "+ driver.Position[0] +" :" + driver.Position[1] + " direction :" + driver.Direction.ToString());
for (int i = 0; i < findDirectionCount * 4 +1;i++ )
{
findValue[i] = -1;
findDirection[i] = new Vector(lookAt);
mainForm.sLog.log("\tAgent: "+i+">>"+findDirection[i].ToString());
lookAt.Sub(new Vector(dy,-dx));
}
findIndex = 0;
}
private bool isInGoalPosition()
{
Vector currentPosition = new Vector(driver.Position[0], driver.Position[1]);
currentPosition.Sub(desierPosition);
return currentPosition.Lenght() < epsilon;
}
public void setGoalPosition(double x, double y)
{
desierPosition = new Vector(x, y);
}
