// 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);
}
}
}
// 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);
}
}
}
