public void drive()
{
Console.Write("Driving .. ");
float[] startVector = new float[] { 0.2f, 0.0f };
float[] dir = new float[2];
float a = 0.0f;
while (com.isConnected() && false == bumper.value())
{
//rotate 360degrees in 5s
rotate(startVector, dir, a);
a = 360.0f * com.msecsElapsed() / 5000;
omniDrive.setVelocity(dir[0], dir[1], 0);
System.Threading.Thread.Sleep(100);
}
}
public void followWalls()
{
float[] ev = new float[] { 1.0f, 0.0f };
// escape vector for distance sensor
float[][] escapeVector = new float[][]
{
new float[] { 0.0f, 0.0f },
new float[] { 0.0f, 0.0f },
new float[] { 0.0f, 0.0f },
new float[] { 0.0f, 0.0f },
new float[] { 0.0f, 0.0f },
new float[] { 0.0f, 0.0f },
new float[] { 0.0f, 0.0f },
new float[] { 0.0f, 0.0f },
new float[] { 0.0f, 0.0f }
};
for (int i = 0; i < 9; i++)
{
rotate(ev, escapeVector[i], 40.0f * i);
}
const float ESCAPE_DISTANCE = 0.10f;
const float WALL_LOST_DISTANCE = 0.35f;
const float WALL_FOUND_DISTANCE = 0.30f;
const float WALL_FOLLOW_DISTANCE = 0.15f;
const float NEW_WALL_FOUND_DISTANCE = 0.12f;
float[] escape = new float[] { 0.0f, 0.0f };
int curWallSensor = -1;
float[] dir = new float[] { 1.0f, 0.0f };
float velocity = VELOCITY;
float rotVelocity = ANGULARVELOCITY;
while (_com.isConnected() && _bumper.Value != true)
{
velocity = VELOCITY;
int minIndex = 0;
float minDistance = 0.40f;
int numEscape = 0;
escape[0] = escape[1] = 0.0f;
StringBuilder values = new StringBuilder();
for (int i = 0; i < _distanceSensors.Count; ++i)
{
float v = (float)_distanceSensors[i].Distance;
values.Append(v.ToString() + " ");
if (v < minDistance)
{
minDistance = v;
minIndex = i;
}
if (v < ESCAPE_DISTANCE)
{
++numEscape;
addScaledVector(escape, escapeVector[i], v);
}
}
Console.WriteLine(values.ToString());
if (numEscape >= 2)
{
// close to walls with more than one sensor, try to escape
normalizeVector(escape);
rotate(escape, dir, 180);
velocity = SLOW_VELOCITY;
}
else
{
if (curWallSensor != -1 &&
_distanceSensors[curWallSensor].Distance > WALL_LOST_DISTANCE)
{
curWallSensor = -1;
rotateInPlace(dir, -20);
rotVelocity = -0.20f;
}
if (curWallSensor == -1)
{
// wall not found yet
if (minDistance < WALL_FOUND_DISTANCE)
{
curWallSensor = minIndex;
velocity = SLOW_VELOCITY;
}
}
if (curWallSensor != -1)
{
float wallDist = (float)_distanceSensors[curWallSensor].Distance;
// check for global new wall
if (minIndex != curWallSensor && minDistance < NEW_WALL_FOUND_DISTANCE)
{
// new wall found, drive along this wall
curWallSensor = minIndex;
wallDist = (float)_distanceSensors[curWallSensor].Distance;
velocity = SLOW_VELOCITY;
}
else
{
if (_distanceSensors[(curWallSensor + 1) % 9].Distance < wallDist)
{
// switch walls
curWallSensor = (curWallSensor + 1) % 9;
velocity = MEDIUM_VELOCITY;
}
// check for new wall in direction
for (int i = 0; i < 2; ++i)
{
int tmpId = (curWallSensor + 2 + i) % 9;
if (_distanceSensors[tmpId].Distance < WALL_FOUND_DISTANCE)
{
curWallSensor = tmpId;
wallDist = (float)_distanceSensors[tmpId].Distance;
velocity = SLOW_VELOCITY;
break;
}
}
}
// try to keep neighbor distance sensors in balance
float vr = (float)_distanceSensors[(curWallSensor + 1) % 9].Distance;
float vl = (float)_distanceSensors[(curWallSensor + 8) % 9].Distance;
rotVelocity = (vr - vl);
float followAngle = 95;
if (Math.Abs(rotVelocity) > 0.30)
{
velocity = SLOW_VELOCITY;
followAngle = rotVelocity >= 0.0 ? 140 : 80;
}
else if (Math.Abs(rotVelocity) > 0.40)
{
velocity = MEDIUM_VELOCITY;
followAngle = rotVelocity >= 0.0 ? 120 : 85;
}
rotVelocity *= 8 * ANGULARVELOCITY;
// follow the wall to the left
rotate(escapeVector[curWallSensor], dir, followAngle);
// keep distance to wall steady
float scale = wallDist - WALL_FOLLOW_DISTANCE;
scale *= 10f;
addScaledVector(dir, escapeVector[curWallSensor], scale);
normalizeVector(dir);
}
}
if (minDistance > 0.20f)
{
velocity = FAST_VELOCITY;
}
_omniDrive.setVelocity(velocity * (float)dir[0], velocity * (float)dir[1], rotVelocity);
System.Threading.Thread.Sleep(100);
}
}