public void moveFromNest(KitStand ks, Part[] p, Nest[] n, int[] i, int base) {
if (ks.getKit() != null) {
isMoving = true;
nest = n;
this.base = base;
// store parts to pick up
for (int k = 0; k < p.length; k++) holdObj[k] = p[k];
s2 = ks;
// store destinations
for (int m = 0; m < p.length; m++) {
if (p[m] != null) {
xdes1[m] = n[m].getPositionX() - 20;
ydes1[m] = n[m].getPositionY();
}
}
// find total parts
totalParts = findTotalParts(p);
// System.out.println(totalParts);
// set kit indexes parts go to
for (int j = 0; j < i.length; j++) kitIndex[j] = i[j];
// find each final destination based on where in the kit it needs to go
for (int l = 0; l < kitIndex.length; l++) {
if (kitIndex[l] > 3) ydes2[l] = ks.getKit().getPositionY() + 25;
else ydes2[l] = ks.getKit().getPositionY() + 5;
if (kitIndex[l] == 0 || kitIndex[l] == 4) xdes2[l] = ks.getKit().getPositionX() + 10;
else if (kitIndex[l] == 1 || kitIndex[l] == 5) xdes2[l] = ks.getKit().getPositionX() + 30;
else if (kitIndex[l] == 2 || kitIndex[l] == 6) xdes2[l] = ks.getKit().getPositionX() + 50;
else if (kitIndex[l] == 3 || kitIndex[l] == 7) xdes2[l] = ks.getKit().getPositionX() + 70;
}
}
}
public void move() {
csCount = us.getCount() - base;
// System.out.println(csCount);
// move to first nest
if (csCount < 20) {
x2 += (xdes1[0] - x1) / 20;
y2 += (ydes1[0] - y1) / 20;
g.setPosition((int) x2, (int) y2);
// add part to gripper
if (csCount == 19) {
g.addPart(holdObj[0]);
us.lm.removePart(nest[0].index);
}
g.updateParts();
csCount++;
}
// move to second nest
else if (csCount >= 20 && csCount < 40) {
if (totalParts >= 2) {
x2 += (xdes1[1] - xdes1[0]) / 20;
y2 += (ydes1[1] - ydes1[0]) / 20;
g.setPosition((int) x2, (int) y2);
if (csCount == 39) {
g.addPart(holdObj[1]);
us.lm.removePart(nest[1].index);
}
g.updateParts();
csCount++;
}
// if no more parts to pick up go back to part robot center
else csCount = 80;
} else if (csCount >= 40 && csCount < 60) {
// move to third nest
if (totalParts >= 3) {
x2 += (xdes1[2] - xdes1[1]) / 20;
y2 += (ydes1[2] - ydes1[1]) / 20;
g.setPosition((int) x2, (int) y2);
if (csCount == 59) {
g.addPart(holdObj[2]);
us.lm.removePart(nest[2].index);
}
g.updateParts();
csCount++;
}
// if no more parts to pick up go back to part robot center
else csCount = 80;
} else if (csCount >= 60 && csCount < 80) {
// move to fourth nest
if (totalParts >= 4) {
x2 += (xdes1[3] - xdes1[2]) / 20;
y2 += (ydes1[3] - ydes1[2]) / 20;
g.setPosition((int) x2, (int) y2);
if (csCount == 79) {
g.addPart(holdObj[3]);
us.lm.removePart(nest[3].index);
}
g.updateParts();
csCount++;
}
// if no more parts to pick up go back to part robot center
else csCount = 80;
}
// go back to part robot center
else if (csCount >= 80 && csCount < 100) {
x2 += (x1 - xdes1[totalParts - 1]) / 20;
y2 += (y1 - ydes1[totalParts - 1]) / 20;
g.setPosition((int) x2, (int) y2);
g.updateParts();
csCount++;
}
// go to kit index
else if (csCount >= 100 && csCount < 120) {
x2 += (xdes2[0] - x1) / 20;
y2 += (ydes2[0] - y1) / 20;
g.setPosition((int) x2, (int) y2);
g.updateParts();
csCount++;
// m.repaint();
}
// go back to robot center
else if (csCount >= 120 && csCount < 140) {
if (csCount == 120) {
for (int j = 0; j < totalParts; j++) {
s2.getKit().addPart(holdObj[j], kitIndex[j]);
g.removePart(j).setPosition((int) xdes2[j], (int) ydes2[j]);
}
}
// s2.getKit().setIsComplete(true);
x2 += (x1 - xdes2[0]) / 20;
y2 += (y1 - ydes2[0]) / 20;
g.setPosition((int) x2, (int) y2);
g.updateParts();
csCount++;
// m.repaint();
}
// reset everything
else if (csCount == 140) {
csCount = 0;
isMoving = false;
}
}