/*
* Jenga.java
* Jeroen Kools & Kris Bende
* 28 juni 2005
*
* Class for playing/demonstrating a game of Robot Jenga
*
*/
import tio.*;
import java.io.*;
public class Jenga {
public static void main(String[] args) {
printbanner();
System.out.println("\nInitializing...");
JengaTower toren = new JengaTower();
System.out.println("New tower has been built");
playgame(toren);
}
public static void playgame(JengaTower toren){
System.out.print(toren);
while(!toren.invalid()) {
System.out.println("...tower still standing");
int[] pcmove = getComputerMove(toren);
// System.out.print(toren);
System.out.println("\n------\n\nComputer takes block "+pcmove[1]+ " from level "+pcmove[0]);
toren.getBlock(pcmove[0], pcmove[1]);
// roep PP aan
PP.PP(pcmove[0], pcmove[1], toren);
toren.placeBlock();
if(toren.invalid()) {
System.out.println(toren);
System.out.println("Whoops!");
break;
}
System.out.println(toren);
System.out.print(
"------\n\nWhich block do you take?\nPlease enter the level number or 0 to quit...: ");
int level = tio.Console.in.readInt();
if (level == 0) {
System.out.println("Goodbye!");
System.exit(0);}
System.out.print("... and block number: ");
int block = tio.Console.in.readInt();
toren.getBlock(level,block);
toren.placeBlock();
System.out.println(toren);
}
System.out.println("I guess the tower came down, didn't it?");
}
public static void printbanner() {
System.out.println( "\n\n\n\nJenga v0.6 - 30/06/05\n\n"+
" ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~\n" +
" ~@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@~\n" +
" ~@~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~@~\n" +
" ~@~ ~@~\n" +
" ~@~ Welkom bij Jenga ~@~\n" +
" ~@~ door J.Kools en K.Bende ~@~\n" +
" ~@~ Juni 2005 ~@~\n" +
" ~@~ Zoeken,Sturen & Bewegen ~@~\n" +
" ~@~ ~@~\n" +
" ~@~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~@~\n" +
" ~@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@~\n" +
" ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~\n\n" +
" PRESS ENTER TO CONTINUE ");
String boeiend = tio.Console.in.readLine();
}
public static int[] getComputerMove(JengaTower t) {
int[] heur = JengaTower.heuristic(t);
int[] pcmove = new int[2];
int ver1, ver2, ver3, hor1, hor2, hor3, vlow, hlow;
// Bereken heuristische waarde van alle 6 blokposities
hor1 = heur[0] + heur[1] + heur[2];
hor2 = heur[3] + heur[4] + heur[5];
hor3 = heur[6] + heur[7] + heur[8];
hlow = 1;
// if (hor2 < hlow) { hlow = hor2; } // Straks misschien
if (hor3 >= hor1) { hlow = 3; }
ver1 = heur[0] + heur[3] + heur[6];
ver2 = heur[1] + heur[4] + heur[7];
ver3 = heur[2] + heur[5] + heur[8];
vlow = 1;
// if (ver2 > ver1) { vlow = 2; } // De blokken in het midden laten we nog even voor wat ze zijn
if (ver3 >= ver1) { vlow = 3; }
System.out.println("\n vlow = "+vlow+"\n hlow = "+hlow);
// zoek eerst naar blok om te pakken
// op oneven verdiepingen onder 10, beginnend bij 10
if(t.toren.size() % 2 == 1) {
for (int i = 9; i > 0;i = i - 2) {
Verdieping current = (Verdieping)t.toren.elementAt(i-1);
if(!current.get(vlow)) {
pcmove[0] = i;
pcmove[1] = vlow;
return pcmove;
}
}
// dan even verdiepingen onder 10
for (int i = 10; i > 0;i = i - 2) {
Verdieping current = (Verdieping)t.toren.elementAt(i-1);
if(!current.get(hlow)) {
pcmove[0] = i;
pcmove[1] = hlow;
return pcmove;
}
}
}
// dan oneven verdiepingen boven 10
if(t.toren.size() % 2 == 0) {
for (int i = 11; i <= t.toren.size() ;i = i + 2) {
Verdieping current = (Verdieping)t.toren.elementAt(i-1);
if(!current.get(vlow)) {
pcmove[0] = i;
pcmove[1] = vlow;
return pcmove;
}
}
for (int i = 12; i > t.toren.size();i = i + 2) {
Verdieping current = (Verdieping)t.toren.elementAt(i-1);
if(!current.get(vlow)) {
pcmove[0] = i;
pcmove[1] = vlow;
return pcmove;
}
}
}
// dan even verdiepingen boven 10
//pcmove[0] = 4;
//pcmove[1] = 3;
System.out.println("Did not find a move");
return pcmove;
}
}
/*
* Jenga.java
* Jeroen Kools & Kris Bende
* 28 juni 2005
*
* Class for playing/demonstrating a game of Robot Jenga
*
*/
import tio.*;
import java.io.*;
public class GiveMove {
public static void main(String[] args) {
printbanner();
System.out.println("\nInitializing...");
JengaTower toren = new JengaTower();
System.out.println("New tower has been built");
playgame(toren);
}
public static void playgame(JengaTower toren){
System.out.print(toren);
while(!toren.invalid()) {
System.out.println("...tower still standing");
System.out.print(
"------\n\nWhich block do you take?\nPlease enter the level number or 0 to quit...: ");
int level = tio.Console.in.readInt();
if (level == 0) {
System.out.println("Goodbye!");
System.exit(0);}
System.out.print("... and block number: ");
int block = tio.Console.in.readInt();
int[] pcmove = new int[2];
pcmove[0] = level;
pcmove[1] = block;
// System.out.print(toren);
// roep PP aan
PP.PP(pcmove[0], pcmove[1], toren);
//if(pcmove.invalid()) {
// System.out.println("Illegal move!!");
// break;
//}
toren.placeBlock();
if(toren.invalid()) {
System.out.println(toren);
System.out.println("Whoops!");
break;
}
//System.out.println(toren);
toren.getBlock(level,block);
System.out.println(toren);
}
System.out.println("I guess the tower came down, didn't it?");
}
public static void printbanner() {
System.out.println( "\n\n\n\nJenga v0.6b - 30/06/05\n\n"+
" ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~\n" +
" ~@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@~\n" +
" ~@~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~@~\n" +
" ~@~ ~@~\n" +
" ~@~ Welkom bij Jenga ~@~\n" +
" ~@~ door J.Kools en K.Bende ~@~\n" +
" ~@~ Juni 2005 ~@~\n" +
" ~@~ Zoeken,Sturen & Bewegen ~@~\n" +
" ~@~ ~@~\n" +
" ~@~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~@~\n" +
" ~@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@~\n" +
" ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~\n\n" +
" PRESS ENTER TO CONTINUE ");
String boeiend = tio.Console.in.readLine();
}
public static int[] getComputerMove(JengaTower t) {
int[] heur = JengaTower.heuristic(t);
int[] pcmove = new int[2];
int ver1, ver2, ver3, hor1, hor2, hor3, vlow, hlow;
// Bereken heuristische waarde van alle 6 blokposities
hor1 = heur[0] + heur[1] + heur[2];
hor2 = heur[3] + heur[4] + heur[5];
hor3 = heur[6] + heur[7] + heur[8];
hlow = 1;
// if (hor2 < hlow) { hlow = hor2; } // Straks misschien
if (hor3 >= hor1) { hlow = 3; }
ver1 = heur[0] + heur[3] + heur[6];
ver2 = heur[1] + heur[4] + heur[7];
ver3 = heur[2] + heur[5] + heur[8];
vlow = 1;
// if (ver2 > ver1) { vlow = 2; } // De blokken in het midden laten we nog even voor wat ze zijn
if (ver3 >= ver1) { vlow = 3; }
System.out.println("\n vlow = "+vlow+"\n hlow = "+hlow);
// zoek eerst naar blok om te pakken
// op oneven verdiepingen onder 10, beginnend bij 10
if(t.toren.size() % 2 == 1) {
for (int i = 9; i > 0;i = i - 2) {
Verdieping current = (Verdieping)t.toren.elementAt(i-1);
if(!current.get(vlow)) {
pcmove[0] = i;
pcmove[1] = vlow;
return pcmove;
}
}
// dan even verdiepingen onder 10
for (int i = 10; i > 0;i = i - 2) {
Verdieping current = (Verdieping)t.toren.elementAt(i-1);
if(!current.get(hlow)) {
pcmove[0] = i;
pcmove[1] = hlow;
return pcmove;
}
}
}
// dan oneven verdiepingen boven 10
if(t.toren.size() % 2 == 0) {
for (int i = 11; i <= t.toren.size() ;i = i + 2) {
Verdieping current = (Verdieping)t.toren.elementAt(i-1);
if(!current.get(vlow)) {
pcmove[0] = i;
pcmove[1] = vlow;
return pcmove;
}
}
for (int i = 12; i > t.toren.size();i = i + 2) {
Verdieping current = (Verdieping)t.toren.elementAt(i-1);
if(!current.get(vlow)) {
pcmove[0] = i;
pcmove[1] = vlow;
return pcmove;
}
}
}
// dan even verdiepingen boven 10
//pcmove[0] = 4;
//pcmove[1] = 3;
System.out.println("Did not find a move");
return pcmove;
}
}
JengaTower.java is een klasse die een Jenga-toren representeert. Hij maakt gebruik van de klasse Verdieping en wordt gebruik door de programma's Jenga en GiveMove.
/*
* JengaTower.java
* Jeroen Kools & Kris Bende
* 28 juni 2005
*
* Class for representing the jenga world
*
*/
import java.io.*;
import java.lang.*;
import java.util.Vector;
public class JengaTower {
public static Vector toren = new Vector();
// block specifications
final double PIECE_LENGTH = 75.0;
final double PIECE_WIDTH = 25.0;
final double PIECE_HEIGHT = 15.0; // tussen 14.6 en 15.2, eigenlijk
// tower position
double TOWER_X = 150.0; // (1;1) ligt op (0.0, 200.0)
double TOWER_Y = 300.0;
// Constructors
// initialize tower
public JengaTower() {
build(toren);
}
// Accessors
public boolean taken(int verdieping, int plek) {
Verdieping v = (Verdieping)toren.elementAt(verdieping-1);
return v.get(plek);
}
// convert block position to Cartesian coordinates
public Point toCartesian(int verdieping, int plek) {
Point coords = new Point();
double x, y, z;
if (verdieping % 2 == 1) {
coords.x = TOWER_X + (PIECE_WIDTH) * (plek-2);
coords.y = TOWER_Y;
}
else if (verdieping % 2 == 0) {
coords.x = TOWER_X;
coords.y = TOWER_Y - (PIECE_WIDTH) * (plek-2);
}
coords.z = PIECE_HEIGHT * (verdieping - 1) + (0.5 * PIECE_HEIGHT);
return coords;
}
// make a nice printable representation of the tower
public String toString() {
String text = "\n------\n\nThe tower currently looks like this:\n\n";
for (int i = toren.size()-1; i >= 0; i--) {
text = text+(i+1)+ " ";
if ((i+1) < 10) {text = text + " ";}
text = text + toren.elementAt(i).toString();
text = text + "\n";
}
return text;
}
// check tower stability
public boolean invalid() {
for(int i=0; i
up
Verdieping.java is eigenlijk vooral een wrapper voor drie booleans, met een bonus toString en een handige accessor.
public class Verdieping {
boolean een ;
boolean twee;
boolean drie;
// Constructor
// je kunt zowel een verdieping vol met blokken
// als een verdieping zonder blokken aanmaken
public Verdieping(boolean boole) {
een = boole;
twee = boole;
drie = boole;
}
// toString
public String toString() {
String x,y,z;
if (een) {x = "T";}
else {x = "F";}
if (twee) {y = "T";}
else {y = "F";}
if (drie) {z = "T";}
else {z = "F";}
return (x+y+z);
}
// enige accessor
public boolean get(int i) {
if (i == 1) {return een;}
if (i == 2) {return twee;}
if (i == 3) {return drie;}
else {return false;}
}
}
up
PP.java converteert een zet in een serie coordinaten voor de robotarm. Een blok uit een bepaalde verdieping wordt bovenop de toren geplaatst.
/*
* PP.java
* Dit is het Path Planner voor de Jenga.
*
*/
import java.io.*;
import java.lang.*;
import java.util.Vector;
public class PP {
private static final double SAFE_HEIGHT = 500;
private static final double SAFE_DISTANCE = 100;
private static final double CLOSE_DISTANCE = 10;
private static final double OPEN_GRIP = 90;
private static final double CLOSED_GRIP = 40;
public static Vector PP(int verdieping, int plek, JengaTower t) {
Vector p = new Vector();
/* plan a path for the move */
try { path(verdieping, plek, t, p);
}
catch (Exception e) {
System.out.println(e);
System.exit(1);
}
GripperPosition.write(p); // after done write the gripper positions
System.out.println(p);
return p;
}
private static void path(int verdieping, int plek, JengaTower t, Vector p) {
System.out.println("**** In path");
Point oldCoord = new Point();
Point newCoord = new Point();
Point phase1 = new Point();
Point phase2 = new Point();
Point phase3 = new Point();
Point phase4 = new Point();
Point phase5 = new Point();
Point phase6 = new Point();
Point phase7 = new Point();
Point phase8 = new Point();
Point phase9 = new Point();
Point phase10 = new Point();
Point phase11 = new Point();
Point phase12 = new Point();
int angle1 = 0, angle2 = 0, minplus = 0, plusmin = 0, newplek = 0, newverdieping = 0;
double grip1, grip2, grip3, grip4, grip5, grip6,
grip7, grip8, grip9, grip10, grip11, grip12;
Verdieping ver = (Verdieping)t.toren.elementAt((t.toren.size() - 1));
newverdieping = t.toren.size()-1;
for(int i = 3; i > 1; i--) {
if( ver.get(i) ) {
newplek = i;
}
else {
newplek = 0;
}
}
if( newplek == 0) {
newverdieping++;
newplek = 1;
}
oldCoord = t.toCartesian(verdieping, plek);
newCoord = t.toCartesian(newverdieping, newplek);
if( (newverdieping % 2) == 1) {
angle2 = 0;
}
else {
angle2 = 90;
}
if( (verdieping % 2) == 0) {
if( oldCoord.x < 500) {
minplus = -1;
plusmin = 0;
angle1 = 0;
}
else {
minplus = 1;
plusmin = 0;
angle1 = 90;
}
}
else {
if( oldCoord.y < 0) {
minplus = 0;
plusmin = -1;
angle1 = 90;
}
else {
minplus = 0;
plusmin = 1;
angle1 = -90;
}
}
System.out.println("van: " + oldCoord + " " + "naar: " + newCoord);
try{
double x2 = newCoord.x;
double y2 = newCoord.y;
if ((verdieping % 2) == 1) {
oldCoord.x = oldCoord.x + 157;
oldCoord.y = oldCoord.y + 157;
}
// phase 1
phase1.x = oldCoord.x + (plusmin * SAFE_DISTANCE);
phase1.y = oldCoord.y + (minplus * SAFE_DISTANCE);
phase1.z = SAFE_HEIGHT;
grip1 = OPEN_GRIP;
// phase 2
phase2.x = oldCoord.x + (plusmin * SAFE_DISTANCE);
phase2.y = oldCoord.y + (minplus * SAFE_DISTANCE);
phase2.z = oldCoord.z;
grip2 = OPEN_GRIP;
// phase 3
phase3.x = oldCoord.x;
phase3.y = oldCoord.y;
phase3.z = oldCoord.z;
grip3 = OPEN_GRIP;
// phase 4
phase4.x = oldCoord.x;
phase4.y = oldCoord.y;
phase4.z = oldCoord.z;
grip4 = CLOSED_GRIP;
// phase 5
phase5.x = oldCoord.x + (plusmin * CLOSE_DISTANCE);
phase5.y = oldCoord.y + (minplus * CLOSE_DISTANCE);
phase5.z = oldCoord.z;
grip5 = CLOSED_GRIP;
// phase 6
phase6.x = oldCoord.x + (plusmin * SAFE_DISTANCE);
phase6.y = oldCoord.y + (minplus * SAFE_DISTANCE);
phase6.z = oldCoord.z;
grip6 = CLOSED_GRIP;
// phase 7
phase7.x = oldCoord.x + (plusmin * SAFE_DISTANCE);
phase7.y = oldCoord.y + (minplus * SAFE_DISTANCE);
phase7.z = SAFE_HEIGHT;
grip7 = CLOSED_GRIP;
newCoord.x = newCoord.x -25.0;
newCoord.y = newCoord.y +25.0;
// phase 8
phase8.x = newCoord.x;
phase8.y = newCoord.y;
phase8.z = SAFE_HEIGHT;
grip8 = CLOSED_GRIP;
// dingen voor draaien ;)
if (((verdieping % 2) == (newverdieping % 2))) {
x2 = x2 - 157.0;
x2 = x2 - 100; // waarom is dit een cruciale regel?!?
y2 = y2 + 132.0;
}
// phase 9
phase9.x = x2;
phase9.y = y2;
phase9.z = SAFE_HEIGHT;
grip9 = CLOSED_GRIP;
// phase 10
phase10.x = x2;
phase10.y = y2;
phase10.z = newCoord.z + CLOSE_DISTANCE + 10;
grip10 = CLOSED_GRIP;
// phase 11
phase11.x = x2;
phase11.y = y2;
phase11.z = newCoord.z + CLOSE_DISTANCE + 10;
grip11 = OPEN_GRIP;
// phase 12
phase12.x = x2;
phase12.y = y2;
phase12.z = SAFE_HEIGHT;
grip12 = OPEN_GRIP;
//construct vector p
GripperPosition gp1 = new GripperPosition( phase1, angle1, grip1);
GripperPosition gp2 = new GripperPosition( phase2, angle1, grip2);
GripperPosition gp3 = new GripperPosition( phase3, angle1, grip3);
GripperPosition gp4 = new GripperPosition( phase4, angle1, grip4);
GripperPosition gp5 = new GripperPosition( phase5, angle1, grip5);
GripperPosition gp6 = new GripperPosition( phase6, angle1, grip6);
GripperPosition gp7 = new GripperPosition( phase7, angle1, grip7);
GripperPosition gp8 = new GripperPosition( phase8, angle1, grip8);
GripperPosition gp9 = new GripperPosition( phase9, angle2, grip9);
GripperPosition gp10 = new GripperPosition( phase10, angle2, grip10);
GripperPosition gp11 = new GripperPosition( phase11, angle2, grip11);
GripperPosition gp12 = new GripperPosition( phase12, angle2, grip12);
p.add(gp1); p.add(gp2); p.add(gp3); p.add(gp4); p.add(gp5); p.add(gp6);
p.add(gp7); p.add(gp8); p.add(gp9); p.add(gp10); p.add(gp11); p.add(gp12);
}
catch (Exception e) {
System.out.println(e);
System.exit(1);
}
}
}
up
IK.java zorgt voor de inverse kinematica, het zet de coordinaten die PP.java geeft om in hoeken voor de gewrichten van de arm.
/*
* IK.java
* Assignment for the Inverse Kinematics part of the ZSB lab course.
*
* Nikos Massios, Matthijs Spaan
* $Id: Week6.java,v 1.3 2004/05/31 19:49:35 mtjspaan Exp $
*/
import java.lang.*;
import java.util.Vector;
public class IK {
// Class containing the Denavit-Hartenberg representation of the robot arm
private static RobotJoints robotJoints;
// Deze twee variabelen geven aan of het pad door twee cruciale gebieden op de tafel voert
// het zouden ook booleans kunnen zijn
private static int visitA8 = 0;
private static int visitH8 = 0;
/* Calculate roll, pitch, and yaw for the gripper of the robot arm plus
* the value of the gripper itself.
*/
private static void handJointCalculation(GripperPosition pos,
JointValues j){
// De eerste twee zijn constant omdat ze niet gebruikt worden op een schaakbord dat
// plat ligt. Yaw wordt verderop nog aangepast.
j.roll = 0;
j.pitch = 0;
j.yaw = pos.theta;
j.grip = pos.grip;
}
/* Calculate the wrist coordinates from the hand coordinates.
* If the robot's last link has some length and the tip of the robot grabs
* the piece. At what height is the start of the robot's last link?
*/
private static Point wristCoordinatesCalculation(GripperPosition pos){
double a1 = 0, b1 = 0;
if( pos.theta == 90) {
a1 = -32.0;
} else if( pos.theta == -90) {
a1 = 32.0;
} else if( pos.theta == 0) {
b1 = -32.0;
} else if( pos.theta == 110) {
b1 = 32.0;
}
System.out.println("a = " + a1 + " en b = " + b1);
Point c = new Point();
Point hand = pos.coords;
c.x = (hand.x - a1);
c.y = (hand.y + b1);
c.z = hand.z - 154.5;
System.out.println("voor = " + hand);
System.out.println("na = " + c + "\n\n");
return(c);
}
/* Calculate the arm joints from the (x,y,z) coordinates of the wrist (the
* last link).
*
* Inverse kinematics stuff here :-)
*/
private static void armJointCalculation(Point wristCoords, JointValues j){
double bovenarm = 0;
double onderarm = 0;
double s2 = 0;
j.zed = wristCoords.z + 80 + 95;
double c2 = (Math.pow(wristCoords.x, 2) + Math.pow(wristCoords.y, 2) - Math.pow( 253.5 , 2) - Math.pow( 253.5, 2)) /
(2 * 253.5 * 253.5 );
/* In de meeste gevallen is de robot linkshandig
* Als hij in de buurt komt van het rechter probleemgebied maar niet
* bij het andere, dann is hij rechts,
* en voor het speciale geval waarin hij door beide regio's komt
* is de methode analyze (zie onder)
*/
s2 = 1 * Math.sqrt( 1 - Math.pow( c2, 2 )); // rechtshandig
// if((visitH8 == 1) && (visitA8 == 1) ) {
// s2 = -1 * Math.sqrt( 1 - Math.pow( c2, 2 ));} // linkshandig
// de constante 253.5 is de lengte van de boven- en onderarm
j.shoulder = Math.toDegrees(Math.atan2(wristCoords.x, wristCoords.y) -
Math.atan2(253.5 * s2 , 253.5 + 253.5 * c2 ));
j.elbow = Math.toDegrees(Math.atan2(s2, c2));
j.yaw += -0.5 * j.elbow - j.shoulder;
}
// Calculate the appropriate values for all joints for position pos.
private static JointValues jointCalculation(GripperPosition pos){
JointValues j = new JointValues();
Point wristCoords;
Point pospoint = (Point)pos.coords;
handJointCalculation(pos,j);
wristCoords = wristCoordinatesCalculation(pos);
armJointCalculation(wristCoords, j);
return(j);
}
private static void inverseKinematics(Vector p, Vector j, int enterCorner) {
// initialize the Denavit-Hartenberg representation
robotJoints = new RobotJoints();
// Haal stuk voor stuk alle gripperpositions uit vector p en controleer
// of de probleemgebieden in de hoeken worden bezocht
// zoja, maak de bijbehorende variabelen 1
for (int i =0; i < p.size(); i++){
GripperPosition pos = (GripperPosition) p.elementAt(i);
if ( i == enterCorner + 1 && !(enterCorner == 0)) {
if (visitH8 == 1) {
visitH8 = 0;}
else { visitH8 = 1;}
if (visitA8 == 1) {
visitA8 = 0;}
else { visitA8 = 1;}
}
j.addElement(jointCalculation(pos));
}
}
public static int analyze(Vector p) { // onze hyperintelligente analysemethode B-)
/*
Analyze is bedoeld om te bepalen of en wanneer de robotarm moet 'flippen' van links- naar rechtshandig
Deze informatie haalt de methode uit de vector met GripperPositions
Door het manipuleren van deze vector, en de klassevariabelen visitA8 en visitH8, worden de flips juist
uitgevoerd
crossLeft- en crossRightLine houden bij op welke punten het pad in cruciaal gebied is voor de robotarm
twee tellers houden bij hoeveel fases er in dat gebied zijn,
en enterCorner is de fase waarbij de flip moet worden uitgevoerd
*/
int[] crossLeftLine = new int[6];
int[] crossRightLine = new int[6];
int leftcount = 0;
int rightcount = 0;
int enterCorner = 0;
// Check voor alle punten op het pad of ze in een van de twee cruciale gebieden liggen
// Deze gebieden zijn bepaald als gebieden op de tafel
for (int i = 0; i < p.size(); i++) {
GripperPosition gp = (GripperPosition)p.elementAt(i);
Point vakje = gp.coords;
double theta = gp.theta;
if( vakje.x > 3000 && vakje.z == 500) {
visitH8 = 1;
crossRightLine[rightcount] = i;
rightcount++;
}
if( vakje.x < -7000 && vakje.z == 500) {
visitA8 = 1;
crossLeftLine[leftcount] = i;
leftcount++;
}
}
// Als de zet via BEIDE cruciale gebieden gaat, moet er een flip worden gedaan
// deze code bepaalt waar, en voegt twee extra GripperPositions toe,
// zodat de flip hoog boven het bord wordt uitgevoerd
if( visitA8 == 1 && visitH8 == 1) {
//Afhankelijk van welk cruciaal gebied eerst wordt bezocht...
if( crossLeftLine[0] < crossRightLine[0]) {
visitH8 = 0;
enterCorner = crossRightLine[0];
}
if( crossLeftLine[0] > crossRightLine[0]) {
visitA8 = 0;
enterCorner = crossLeftLine[0];
}
/* ... worden twee punten toegevoegd:
* Als je bent aangekomen op de rand van het probleemgebied ga je omhoog (1)
* en daarna blijf je hoog en ga je het probleemgebied in (2)
* Nu wordt de flip hoog uitgevoerd.
* Na deze twee punten gaat hij weer naar beneden en verder als normaal
*/
GripperPosition voorHoek = (GripperPosition)p.elementAt(enterCorner - 1);
GripperPosition inHoek = (GripperPosition)p.elementAt(enterCorner);
Point omhoogP = new Point();
Point hoekInP = new Point();
omhoogP.x = voorHoek.coords.x;
omhoogP.y = voorHoek.coords.y;
omhoogP.z = voorHoek.coords.z + 200;
hoekInP.x = inHoek.coords.x;
hoekInP.y = inHoek.coords.y;
hoekInP.z = inHoek.coords.z + 200;
GripperPosition omhoog = new GripperPosition(omhoogP, 0, 0);
GripperPosition hoekIn = new GripperPosition(hoekInP, 0, 0);
// voeg de zojuist berekende GripperPositions toe aan het pad
p.insertElementAt( omhoog , enterCorner );
p.insertElementAt( hoekIn , enterCorner + 1);
}
return enterCorner;
}
public static void main(String[] args){
// Vector p is de gripperpositions en in vector j zitten de overeenkomende jointvalues
Vector p = new Vector();
Vector j = new Vector();
System.out.println ("**** THIS IS KRIS & JEROEN 's INVERSE KINEMATICS MODULE IN JAVA\n");
// read the gripper positions as produced by PP.java
GripperPosition.read(p);
// bekijk met welke hand de robot moet beginnen en of hij een flip moet maken.
int enterCorner = analyze(p);
// correct for errors in the arm
// *** Als we dit gebruikten kregen juist verkeerde coordinaten,
// *** dus dit hebben we uitgezet
// RobotJoints.correctCartesian(p);
inverseKinematics(p, j, enterCorner);
for (int i = 0; i < j.size(); i++) {
System.out.println("JointValue "+ i + " is " + (JointValues) j.get(i));
GripperPosition gp = (GripperPosition)p.elementAt(i);
Point vakje = gp.coords;
//System.out.println(vakje.x + ";" + vakje.y + " hoogte "+ vakje.z);} // nu onnodige feedback
}
JointValues.write(j);
}
}
up