public void run()
{
// prev and new reading store short range sensor distances
double prevReading = 0;
double newReading = 0;
// State of the agent
EState state = EState.NONE;
// Stopwatch to measure the amount of time spent in each mode
Stopwatch stopwatch = new Stopwatch();
long randomTime = 0;
long wallTime = 0;
// Agent loop
while (true)
{
// find what to do
/*if (wallTime > randomTime)
* state = EState.RANDOM;
* else*/
state = EState.WALLFOLLOWING;
//state = EState.RANDOM;
// do it
stopwatch.Start();
if (state == EState.RANDOM)
{
while (bumperSensor.collide())
{
// spin left
movement.spinLeft(45, random.Next(0, 360));
cleaner.clean();
}
while (!bumperSensor.collide())
{
// move straight at 10cm/s
movement.straight(10);
cleaner.clean();
}
}
else if (state == EState.WALLFOLLOWING)
{
// while against a wall turn left
while (bumperSensor.collide())
{
movement.spinLeft(45, 1);
cleaner.clean();
}
// start following the wall adjusting the angle
prevReading = ShortRangeSensor.getDistance();
movement.spinLeft(45, 1);
while (!bumperSensor.collide())
{
double alpha;
movement.straight(10);
cleaner.clean();
newReading = shortRangeSensor.getDistance();
// if no previous reading do not modify the angle
if (prevReading == -1)
{
alpha = 0;
}
// if we are not against a wall anymore do a 90 degree turn
// to look for the wall
else if (newReading == -1)
{
for (int i = 0; i < 10; i++)
{
if (bumperSensor.collide())
{
break;
}
movement.straight(10);
cleaner.clean();
}
alpha = 90;
}
// if we got the previous and current reading calculate the inverse tangent
// of the angle that would put the agent in the right angle
else
{
alpha = Math.Atan((newReading - prevReading) / 1) * 180 / Math.PI;
}
movement.spinRight(45, alpha);
cleaner.clean();
prevReading = newReading;
}
}
stopwatch.Stop();
// allocate the time spent
if (state == EState.WALLFOLLOWING)
{
wallTime += stopwatch.ElapsedTicks;
}
else
{
randomTime += stopwatch.ElapsedTicks;
}
}
}
public void run()
{
double prevReading = 0;
double newReading = 0;
// Wall following
while (!gohome)
{
// Go to the wall
while (!longRangeSensor.againstWall())
{
movement.straight(1);
distance += 1;
System.Threading.Thread.Sleep(5);
}
nodes.Insert(distance, angle, 5);
distance = 0;
// While the agent is touching a wall
while (longRangeSensor.againstWall())
{
movement.spinLeft(1, 1);
angle = (angle - 1) % 360;
System.Threading.Thread.Sleep(5);
}
// start following the wall adjusting the angle
prevReading = ShortRangeSensor.getDistance();
while (!longRangeSensor.againstWall())
{
movement.straight(1);
distance += 1;
newReading = shortRangeSensor.getDistance();
double alpha;
if (prevReading == -1)
{
alpha = 0;
}
else if (newReading == -1)
{
alpha = 90;
}
else
{
alpha = Math.Atan((newReading - prevReading) / 1) * 180 / Math.PI;
if (Math.Abs(alpha) < 5)
{
alpha = 0;
}
}
if (alpha != 0)
{
//no more wall
nodes.Insert(distance, angle, 5);
distance = 0;
}
movement.spinRight(1, alpha);
angle = (angle + alpha) % 360;
prevReading = newReading;
System.Threading.Thread.Sleep(5);
}
}
nodes.Insert(distance, angle, 5);
distance = 0;
while (nodes.CurrentPosition(distance, angle).Distance(startPoint) > 5)
{
// Go home
// point in direction to the start
Point A = new Point(0, 0);
Point C = nodes.CurrentPosition(distance, angle);
Point Btemp = C + new Point(0, 100);
Point B = new Segment(C, Btemp).Rotate(angle).End;
double a_sq = Math.Pow(100, 2);
double b_sq = A.SquareDistance(C);
double c_sq = A.SquareDistance(B);
double gamma;
gamma = Math.Acos((c_sq - a_sq - b_sq) / (-2 * Math.Sqrt(a_sq) * Math.Sqrt(b_sq))) * 180 / Math.PI;
if (b_sq > c_sq)
{
gamma = 90 - gamma;
}
if (angle < 0)
{
angle = (360 + angle);
}
if (angle < 180)
{
movement.spinRight(1, gamma);
angle = (angle + gamma) % 360;
}
else
{
movement.spinLeft(1, gamma);
angle = (angle - gamma) % 360;
}
// no wall let's move straight
while (longRangeSensor.getDistance() == -1)
{
movement.straight(1);
distance++;
System.Threading.Thread.Sleep(5);
}
// A wall is in range
Point pos = nodes.CurrentPosition(distance, angle);
Segment sensor = new Segment(pos, new Point(longRangeSensor.getDistance(), 0)).Rotate(angle);
BDNode near = nodes.Nearest(sensor.End, 1)[0];
if (near.Next != null && new Segment(near.Coord, near.Next.Coord).Intersects(sensor) != null)
{
;//MessageBox.Show("intersect");
}
else if (near.Previous != null && new Segment(near.Coord, near.Previous.Coord).Intersects(sensor) != null)
{
;//MessageBox.Show("intersect");
}
else
{
;// MessageBox.Show("unknown wall");
}
// move straight until hit the wall
while (!longRangeSensor.againstWall() && nodes.CurrentPosition(distance, angle).Distance(startPoint) > 5)
{
movement.straight(1);
distance++;
System.Threading.Thread.Sleep(5);
}
nodes.Insert(distance, angle, 5);
distance = 0;
// While the agent is touching a wall
while (longRangeSensor.inRange())
{
movement.spinLeft(1, 1);
angle = (angle - 1) % 360;
System.Threading.Thread.Sleep(5);
}
while (shortRangeSensor.inRange())
{
movement.straight(1);
distance++;
System.Threading.Thread.Sleep(5);
}
nodes.Insert(distance, angle, 5);
distance = 0;
}
}