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