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