private void thereAndBack(int displacement_mm, int no_of_measurements_there, robotOdometry odometry, int offset_mm, int offset_x_mm, int clip)
{
float x, y = 0, y2 = 0, pan = 0, tot = 0;
float length_mm = displacement_mm * no_of_measurements_there;
for (int i = 0; i < (no_of_measurements_there * 2) - clip; i++)
{
x = 0;
if (tot < length_mm)
{
y = (i * displacement_mm) + offset_mm;
y2 = y;
pan = 0;
}
else
{
y2 -= displacement_mm;
y = y2 + displacement_mm - offset_mm;
pan = (float)Math.PI;
}
odometry.AddPosition(x + offset_x_mm, y, pan);
tot += displacement_mm;
}
}
/// <summary>
/// update the odometry positions
/// </summary>
/// <param name="odometry"></param>
public void update(robotOdometry odometry)
{
for (int v = 0; v < viewpoints.Count; v++)
{
viewpoint view = (viewpoint)viewpoints[v];
view.odometry_position = odometry.getPosition(v);
}
}
// produces an X track pattern in the odometry data
private void standardXTrack(int length_mm, int displacement_mm, robotOdometry odometry, String trackType)
{
float x, y, z, pan;
if (trackType == "M")
{
for (int i = 0; i < odometry.no_of_measurements; i++)
{
x = 0;
y = (i * length_mm) / odometry.no_of_measurements;
z = 0;
pan = 0;
odometry.setPosition(i, x, y, z, pan);
}
}
else
{
float half_length = length_mm / 2;
float hyp = (float)Math.Sqrt((displacement_mm * displacement_mm) + (half_length * half_length));
float angle = (float)Math.Asin(half_length / hyp);
if (displacement_mm < 0)
{
angle = -angle + (float)Math.PI;
}
for (int i = 0; i < odometry.no_of_measurements; i++)
{
float forward = (i * length_mm) / odometry.no_of_measurements;
x = (forward - half_length) * (float)Math.Cos(angle);
y = (forward - half_length) * (float)Math.Sin(angle);
z = 0;
pan = angle;
odometry.setPosition(i, x, y, z, pan);
}
}
}
/// <summary>
/// update the odometry positions
/// </summary>
/// <param name="odometry"></param>
public void update(robotOdometry odometry)
{
for (int v = 0; v < viewpoints.Count; v++)
{
viewpoint view = (viewpoint)viewpoints[v];
view.odometry_position = odometry.getPosition(v);
}
}
private robotPath getPath(robot rob, String path, String track, String index, int starting_index, String TrackName, int trackType)
{
robotPath rpath = new robotPath();
robotOdometry odometry = new robotOdometry();
odometry.readPathFileSentience(path + track + "\\" + TrackName + ".path", index, rob);
switch (trackType)
{
case 1:
{
// mapping X track
standardXTrack(2000, 500, odometry, index);
break;
}
case 2:
{
//localisation X track
standardXTrack(2000, -500, odometry, index);
break;
}
case 3:
{
//there and back track
odometry.Clear();
thereAndBack(100, 46, odometry, 110, 0, 0);
break;
}
case 4:
{
//square
odometry.Clear();
squareAndBack(100, 10, 6, odometry, 110);
break;
}
case 5:
{
//special
odometry.Clear();
specialTrack(100, 16, 21, 3, odometry, 110);
break;
}
}
if (odometry.no_of_measurements == 0)
{
errorMessage = path + track + "\\" + TrackName + ".path not found or contains no data. ";
errorMessage += "Check that the path file is included as 'Content' within the project folder";
}
for (int distance_index = 0; distance_index < odometry.no_of_measurements; distance_index++)
{
pos3D pos = odometry.getPosition(distance_index);
rob.x = pos.x;
rob.y = pos.y;
rob.z = pos.z;
rob.pan = pos.pan;
rob.tilt = pos.tilt;
rob.roll = pos.roll;
loadGlimpseSentience(rob, path + track + "\\", TrackName, index, distance_index + starting_index, odometry.no_of_measurements, index);
rob.updatePath(rpath);
}
return (rpath);
}
private void thereAndBack_x(int displacement_mm, int no_of_measurements_there, robotOdometry odometry, int offset_mm, int offset_x_mm, int offset_y_mm)
{
float x = 0, y = 0, x2 = 0, pan = 0, tot = 0;
float length_mm = displacement_mm * no_of_measurements_there;
for (int i = 0; i < no_of_measurements_there * 2; i++)
{
y = 0;
if (tot < length_mm)
{
x = (i * displacement_mm) + offset_mm;
x2 = x;
pan = (float)Math.PI / 2;
//pan = (float)Math.PI*3/2;
}
else
{
x2 -= displacement_mm;
x = x2 + displacement_mm - offset_mm;
pan = (float)Math.PI * 3 / 2;
//pan = (float)Math.PI/2;
}
odometry.AddPosition(-x + offset_x_mm, y + offset_y_mm, pan);
tot += displacement_mm;
}
}
private void thereAndBack(int displacement_mm, int no_of_measurements_there, robotOdometry odometry, int offset_mm, int offset_x_mm, int clip)
{
float x, y = 0, y2 = 0, pan=0, tot=0;
float length_mm = displacement_mm * no_of_measurements_there;
for (int i = 0; i < (no_of_measurements_there*2)-clip; i++)
{
x = 0;
if (tot < length_mm)
{
y = (i * displacement_mm) + offset_mm;
y2 = y;
pan = 0;
}
else
{
y2 -= displacement_mm;
y = y2 + displacement_mm - offset_mm;
pan = (float)Math.PI;
}
odometry.AddPosition(x + offset_x_mm, y, pan);
tot += displacement_mm;
}
}
private void specialTrack(int displacement_mm, int no_of_measurements_x, int no_of_measurements_y, int lines, robotOdometry odometry, int offset_mm)
{
int clip = 0;
for (int l = 0; l < lines; l++)
{
if (l == lines - 1) clip = 2;
thereAndBack(displacement_mm, no_of_measurements_y, odometry, offset_mm, -l * 500, clip);
}
for (int l = 0; l < lines; l++)
{
thereAndBack_x(displacement_mm, no_of_measurements_x, odometry, offset_mm, 0, (no_of_measurements_y * displacement_mm) - (l * 500));
}
}
private void squareAndBack(int displacement_mm, int no_of_measurements_forward, int no_of_measurements_right, robotOdometry odometry, int offset_mm)
{
squareTrack(displacement_mm, no_of_measurements_forward, no_of_measurements_right, odometry, offset_mm);
float x, y, pan;
int i;
int side1 = no_of_measurements_forward * displacement_mm;
int side2 = no_of_measurements_right * displacement_mm;
x = -(side2 / 2);
y = -(side1 / 2);
for (int side = 0; side < 4; side++)
{
switch (side)
{
case 0:
{
pan = (float)Math.PI * 3 / 2;
for (i = 0; i < no_of_measurements_right; i++)
{
x += displacement_mm;
odometry.AddPosition(x + offset_mm, y, pan);
}
break;
}
case 1:
{
pan = 0;
for (i = 0; i < no_of_measurements_forward; i++)
{
y += displacement_mm;
odometry.AddPosition(x, y + offset_mm, pan);
}
break;
}
case 2:
{
pan = (float)Math.PI / 2;
for (i = 0; i < no_of_measurements_right; i++)
{
x -= displacement_mm;
odometry.AddPosition(x - offset_mm, y, pan);
}
break;
}
case 3:
{
pan = (float)Math.PI;
for (i = 0; i < no_of_measurements_forward; i++)
{
y -= displacement_mm;
odometry.AddPosition(x, y - offset_mm, pan);
}
break;
}
}
}
}
// produces an X track pattern in the odometry data
private void standardXTrack(int length_mm, int displacement_mm, robotOdometry odometry, String trackType)
{
float x, y, z, pan;
if (trackType == "M")
{
for (int i = 0; i < odometry.no_of_measurements; i++)
{
x = 0;
y = (i * length_mm) / odometry.no_of_measurements;
z = 0;
pan = 0;
odometry.setPosition(i, x, y, z, pan);
}
}
else
{
float half_length = length_mm/2;
float hyp = (float)Math.Sqrt((displacement_mm*displacement_mm)+(half_length*half_length));
float angle = (float)Math.Asin(half_length / hyp);
if (displacement_mm < 0) angle = -angle+(float)Math.PI;
for (int i = 0; i < odometry.no_of_measurements; i++)
{
float forward = (i * length_mm) / odometry.no_of_measurements;
x = (forward - half_length) * (float)Math.Cos(angle);
y = (forward - half_length) * (float)Math.Sin(angle);
z = 0;
pan = angle;
odometry.setPosition(i, x, y, z, pan);
}
}
}
private robotPath getPath(robot rob, String path, String track, String index, int starting_index, String TrackName, int trackType)
{
robotPath rpath = new robotPath();
robotOdometry odometry = new robotOdometry();
odometry.readPathFileSentience(path + track + "\\" + TrackName + ".path", index, rob);
switch (trackType)
{
case 1:
{
// mapping X track
standardXTrack(2000, 500, odometry, index);
break;
}
case 2:
{
//localisation X track
standardXTrack(2000, -500, odometry, index);
break;
}
case 3:
{
//there and back track
odometry.Clear();
thereAndBack(100, 46, odometry, 110, 0, 0);
break;
}
case 4:
{
//square
odometry.Clear();
squareAndBack(100, 10, 6, odometry, 110);
break;
}
case 5:
{
//special
odometry.Clear();
specialTrack(100, 16, 21, 3, odometry, 110);
break;
}
}
if (odometry.no_of_measurements == 0)
{
errorMessage = path + track + "\\" + TrackName + ".path not found or contains no data. ";
errorMessage += "Check that the path file is included as 'Content' within the project folder";
}
for (int distance_index = 0; distance_index < odometry.no_of_measurements; distance_index++)
{
pos3D pos = odometry.getPosition(distance_index);
rob.x = pos.x;
rob.y = pos.y;
rob.z = pos.z;
rob.pan = pos.pan;
rob.tilt = pos.tilt;
rob.roll = pos.roll;
loadGlimpseSentience(rob, path + track + "\\", TrackName, index, distance_index + starting_index, odometry.no_of_measurements, index);
rob.updatePath(rpath);
}
return(rpath);
}
private void thereAndBack_x(int displacement_mm, int no_of_measurements_there, robotOdometry odometry, int offset_mm, int offset_x_mm, int offset_y_mm)
{
float x = 0, y = 0, x2 = 0, pan = 0, tot = 0;
float length_mm = displacement_mm * no_of_measurements_there;
for (int i = 0; i < no_of_measurements_there * 2; i++)
{
y = 0;
if (tot < length_mm)
{
x = (i * displacement_mm) + offset_mm;
x2 = x;
pan = (float)Math.PI / 2;
//pan = (float)Math.PI*3/2;
}
else
{
x2 -= displacement_mm;
x = x2 + displacement_mm - offset_mm;
pan = (float)Math.PI * 3 / 2;
//pan = (float)Math.PI/2;
}
odometry.AddPosition(-x + offset_x_mm, y + offset_y_mm, pan);
tot += displacement_mm;
}
}
private void specialTrack(int displacement_mm, int no_of_measurements_x, int no_of_measurements_y, int lines, robotOdometry odometry, int offset_mm)
{
int clip = 0;
for (int l = 0; l < lines; l++)
{
if (l == lines - 1)
{
clip = 2;
}
thereAndBack(displacement_mm, no_of_measurements_y, odometry, offset_mm, -l * 500, clip);
}
for (int l = 0; l < lines; l++)
{
thereAndBack_x(displacement_mm, no_of_measurements_x, odometry, offset_mm, 0, (no_of_measurements_y * displacement_mm) - (l * 500));
}
}
private void squareAndBack(int displacement_mm, int no_of_measurements_forward, int no_of_measurements_right, robotOdometry odometry, int offset_mm)
{
squareTrack(displacement_mm, no_of_measurements_forward, no_of_measurements_right, odometry, offset_mm);
float x, y, pan;
int i;
int side1 = no_of_measurements_forward * displacement_mm;
int side2 = no_of_measurements_right * displacement_mm;
x = -(side2 / 2);
y = -(side1 / 2);
for (int side = 0; side < 4; side++)
{
switch (side)
{
case 0:
{
pan = (float)Math.PI * 3 / 2;
for (i = 0; i < no_of_measurements_right; i++)
{
x += displacement_mm;
odometry.AddPosition(x + offset_mm, y, pan);
}
break;
}
case 1:
{
pan = 0;
for (i = 0; i < no_of_measurements_forward; i++)
{
y += displacement_mm;
odometry.AddPosition(x, y + offset_mm, pan);
}
break;
}
case 2:
{
pan = (float)Math.PI / 2;
for (i = 0; i < no_of_measurements_right; i++)
{
x -= displacement_mm;
odometry.AddPosition(x - offset_mm, y, pan);
}
break;
}
case 3:
{
pan = (float)Math.PI;
for (i = 0; i < no_of_measurements_forward; i++)
{
y -= displacement_mm;
odometry.AddPosition(x, y - offset_mm, pan);
}
break;
}
}
}
}
