roboproto.java

一个由c转成java的3D robot 仿真平台

/************************************************************************/
/* File: ~/RoboPackII/RoboProto.java                                    */
/* This file consists of the main class which includes the user 	*/
/* interface and holds the specific values of the robot.		*/
/* The most important methods are also part of this file.		*/
/************************************************************************/

package RoboPackII;

import java.awt.*;
import java.io.RandomAccessFile;
import java.io.IOException;
import java.applet.Applet;
import java.net.MalformedURLException;
import java.net.URL;

public class RoboProto extends Frame
{ 
	// Constants:
	private final double[][] realRobRestr =   // realRobotRestrictions
	  { {  -0.88, 0.9  },     //angle1
	    {  -2.38, -0.82},     //angle2
	    { -0.686, 0.406},	  //angle3
	    {      0, 0    },     // useful dummy
	    { -0.698, 0.698} };	  //angle4
	private final double[] adjustFactors = { 2, -2.2, 2.5, -3, -8.6 };
		// adjusts the movement speed of the real robot to the 
		// virtual robot speed

	// Declaration of the graphics constants:
	public final static int numberOfPoints = 154;
	public final static int numberOfEdges = 211;
	public final static int numberOfAreas = 100;
		
	//Declaration of the variables specifying the robot:
	public AreasPointsList[] apl = new AreasPointsList[numberOfAreas];
	public Points light = null;
	public long[][] colorTable = new long[7][31];
	public double[] linkLengths = new double[4];
	public int[] angleWeights = new int[6]; 
	public int[] power = new int[6];
	public AngleDates[] angles = new AngleDates[6];
	public Points[] line = new Points[2];
	public Edges gkl = null;
	public int[] rpl = new int[14];
	public Points[] coorpl = new Points[4];
	public Edges coorc[] = new Edges[3];
	public Points[] pl = new Points[numberOfPoints];
	public Points[] PL = new Points[numberOfPoints];//vertices of the robot
	public Edges[] el = new Edges[numberOfEdges];   //edges of the robot
	public Areas[] al = new Areas[numberOfAreas];
	public boolean robotIsReal = false; // signifies whether the existing
					     // robot is simulated

	public double[] target = { 0, 0, 0 };// for the auto control

	// Some strange, but nevertheless needed variables:
	private int errorcode, reminder, steps, stepNumber;
	private double angleX, angleY, d, x, y, z, s_wx, h;
	private double[] input = new double[9];
	private double[] location = new double[3];
	private double[] pos = new double[3];
	private double[] angleNew = new double[6], angleOld = new double[6];
	private double[] hw = new double[6];
	private double[] wB = new double[6];
	private double[] wE = new double[6];
	private int n;
        public boolean crashIsActive = false;
	private int indx = -1;
	private double[] locationNew = new double[3];
	public Points[] BListe = new Points[361];
	public short f_Z_B = 0;
    	public short  f_Z_R = 0;
	public int akt = 0;
	public int errvariable = 0;
	public short fFile = 0;
	private double[][] T = new double[3][4];
	private Points p1 = new Points(), p2 = new Points();
	private int fAutoLine = 0;
	private double m, mdiff, distance;
	private boolean[] anglesLeftExtreme = { false, false, false, false,
					        false, false };
		// signifies whether the minimum of an angle had been reached

	// Variables for the saving and loading of positions:
	public double[][] savedAngles = new double[4][7];//no.6 is the hand
	private Button[] storeButtons = { new Button("Def.Init "),
					  new Button("Def.Pos.1"),
					  new Button("Def.Pos.2"), 
					  new Button("Def.Pos.3") };
	private Button[] loadButtons = { new Button("Go Init"),
					 new Button("Go Pos.1"),
					 new Button("Go Pos.2"),
					 new Button("Go Pos.3") }; 
	
	// Declaration of the user interface variables:
	
	private Font headerFont = new Font("TimesRoman", Font.BOLD, 14);

	public Canvas roboCanvas = new Canvas();
	public final int canvasSize = 500; // as in the original program
	public ThreeD animation = new ThreeD(roboCanvas, this);
	  // the animation has to be started after the roboCanvas gets resized
	
	public int xControl = 25; //controls the speed
	private Scrollbar speedScroll = new Scrollbar(Scrollbar.HORIZONTAL, 
					 		xControl, 10, 1, 82); 
	   // 'speedScroll' represents 1 to 82
	public boolean megaSpeed = false;// if 'megaSpeed' is true, the double
					 // buffering is omitted
       	public  float zoomValue = 1.5f; // should be between 0.1 and 3.5
	private final int zoomScrollScale = 100;
	private Scrollbar zoomScroll = new Scrollbar(Scrollbar.HORIZONTAL, 
			       (int)(zoomScrollScale * zoomValue), 10, 0, 350);
	  // 'zoomScrollScale' times the representing value 
         
 	private Panel belowP = new Panel();
	private Panel leftP = new Panel();
	private Panel rightP = new Panel();
	private Panel upP = new Panel();

	private Button exampleButton = new Button("Example");

	private Scrollbar[] manualScrolls = new Scrollbar[6]; 
	private Label[] manualLabels = new Label[6];
	private final int manualScrollScale = 30;
		// ... * 30, so that the control is more sensitive

	private Scrollbar[] linkScrolls = new Scrollbar[3];
	private Label[] linkLabels = new Label[3];
	private Label linkHeader = new Label("Linklengths:");
	private Button linkExec = new Button("e x e c u t e");
	private final double[] linkMax = { 15.0, 15.0, 15.0 };
	private final double[] linkMin = { 4.5, 2.5, 2.5 };
	private final int linkScrollScale = 30; 
	
	private Scrollbar[] autoScrolls = new Scrollbar[3];
	private Label[] autoLabels = new Label[3];
	private final int autoScrollScale = 20;
	private final int autoScrollRange = 50;
	private final String onLine = new String("Move on a straight line");
	private final String notOnLine = new String("Move on arbitrary way");
	private Choice lineButton = new Choice();
	private Label autoHeader = new Label("Inverse Kinematics:");
	private Button autoExecButton = new Button("e x e c u t e");

	private static ErrorWindow errWin = null;//the object instanciated in 
					  // 'writeError'
	private static Win helpWin = null; // the help / about text are shown
	private static Win aboutWin = null;// in these windows
	
	private Choice hand = new Choice();
	private Label handLabel = new Label("Hand");
	private Scrollbar handScroll = new Scrollbar(Scrollbar.HORIZONTAL, 
			                 0, 10, 0, 50);//represents 0 to 1
	private final int handScrollScale = 100;
	public  double handValue = 0.0;// must be between 0.0 and 0.5
				// 0.5 is fully closed and 0.0 open
	private boolean showTheMove = true; // necessary for the "hand move"
	public  boolean makeRealRobotMove = true;

	private CheckboxGroup whichRobotGroup = new CheckboxGroup();
	private Checkbox realRobot = new Checkbox("Real Robot", 
					whichRobotGroup, false);
	private Checkbox virtualRobot = new Checkbox("Virtual Robot", 
					whichRobotGroup, true);
	private Button fileButton = new Button("Open File");
	private final String teachStr = new String("Teach");
	private final String enterStr = new String("Enter");
	private Button teachButton = new Button(teachStr);
	private Button stopButton = new Button("Stop");
	private RandomAccessFile saveFile = null;
	private TextField[] weightsText = new TextField[6];
	private Button weightsExec = new Button("e x e c u t e");

	private Button helpButton = new Button("Help");
	private Button aboutButton = new Button("About");
	private Button quit = new Button("Quit");
       	 		
	RoboProto()    
	{
	   super("RoboProto");
	   this.resize(920, 690);
	   this.move(100, 100);
 	  

	   for (int i = 0; i < 6; i++)
	     manualLabels[i] = new Label("  ");
	   for (int i = 0; i < 3; i++)
	     {
	       linkLabels[i] = new Label("  ");
	       autoLabels[i] = new Label("  ");
	     }  // instanciation of the label-arrays
	   Label manualHeader = new Label("Forward Kinematics:");
	   manualHeader.setFont(headerFont);
	   linkHeader.setFont(headerFont);
	  
	   autoHeader.setFont(headerFont);	

	   RobotInit.now(this); // the robot specific variables get its values
	   setLayout(new BorderLayout());

	   // Definition of the 'roboCanvas':
	   roboCanvas.resize(canvasSize, canvasSize);
	   roboCanvas.setBackground(new Color(200, 200, 200));// a nice gray
 	   add("Center", roboCanvas);

	   // Defintion of the 'upP'anel:
	   
	   upP.setLayout(new FlowLayout()); 
	   upP.add(helpButton);
	   Label title = new Label("RoboSim - A Robot Manipulator Simulator");
	   Font titleFont = new Font("TimesRoman", Font.ITALIC, 30);
	   title.setFont(titleFont);
	   upP.add(title);
	   upP.add(aboutButton);
	   add("North", upP);
	   	   
	   // Definition of the 'leftP'anel:

	   leftP.setLayout(new GridLayout(33, 1, 10, 1));
	   leftP.add(manualHeader);
	   for (int i = 0; i < 6; i++)
	     {
		manualScrolls[i] = new Scrollbar(Scrollbar.HORIZONTAL, 
			   (int)(manualScrollScale  * angles[i].act),
			   10 /* width */, 
			   (int)(manualScrollScale * angles[i].min), 
			   (int)(manualScrollScale * angles[i].max));
		manualScrolls[i].resize(100, 10);
		updateManualLabel(i);
		leftP.add(manualLabels[i]);
		leftP.add(manualScrolls[i]);
	   	leftP.add(new Label(""));
	     }
	   leftP.add(new Label(" ")); // Distance to the next entries
	   leftP.add(new Label(" ")); 
	   Label speedHeader = new Label("Speed:");
	   speedHeader.setFont(headerFont);
	   leftP.add(speedHeader);
	   leftP.add(new Label("slow           -->                fast"));
	   leftP.add(speedScroll); 
	   leftP.add(new Label(" ")); // Distance to the next entries
	   Label zoomHeader = new Label("Zoom:");
	   zoomHeader.setFont(headerFont);
	   leftP.add(zoomHeader);
	   leftP.add(new Label("out            -->                   in"));
	   leftP.add(zoomScroll);
  	   leftP.add(new Label(" "));
	   handLabel.setFont(headerFont);
	   leftP.add(handLabel);
	   leftP.add(new Label("open          -->                closed"));
	   leftP.add(handScroll);

	   add("West", leftP);  

	   // Definition of the 'rightP'anel:

	   rightP.setLayout(new GridLayout(28, 1, 10, 1));
	
	   rightP.add(autoHeader);
  	 
	   for (int i = 0; i < 3; i++)
	     {
		autoScrolls[i] = new Scrollbar(Scrollbar.HORIZONTAL, 
			   autoScrollScale * (int)target[i], 10 /* width */, 
 		           autoScrollScale * (-autoScrollRange), 
			   autoScrollScale * autoScrollRange);
		autoScrolls[i].resize(100, 10);
		updateAutoLabel(i);				
		rightP.add(autoLabels[i]);
		rightP.add(autoScrolls[i]);
		if (i != 2) 
		  rightP.add(new Label(""));
	     }
	   actualizeAutoPanel(); // to initialize the default values
	   rightP.add(autoExecButton);
	   lineButton.addItem(notOnLine);
	   lineButton.addItem(onLine);
	   rightP.add(lineButton);  

	   rightP.add(new Label(""));
 	   rightP.add(linkHeader);
 	   for (int i = 0; i < 3; i++)
	     {
		linkScrolls[i] = new Scrollbar(Scrollbar.HORIZONTAL, 
			   (int)(linkScrollScale * linkLengths[i]), 
			   10 /* width */, 
			   (int)(linkScrollScale * linkMin[i]), 
			   (int)(linkScrollScale * linkMax[i]));
		linkScrolls[i].resize(100, 10);
		updateLinkLabel(i);				
		rightP.add(linkLabels[i]);
		rightP.add(linkScrolls[i]);
	        if (i != 2)
		  rightP.add(new Label(""));
	     }
	   rightP.add(linkExec);
	   rightP.add(new Label(" "));
	   rightP.add(fileButton); 
	   rightP.add(new Label(" "));
	   rightP.add(virtualRobot);
	   rightP.add(realRobot);  
	   rightP.add(quit);
	  
	 	  	 
	   add("East", rightP);	

	   // Definition of the 'belowP'anel:

	   belowP.setLayout(new GridLayout(2, 12));
	   Label weightHeader = new Label("Weights of:");
	   weightHeader.setFont(headerFont);
	   belowP.add(weightHeader); 
	   for (int i = 0; i < 6; i++)
	     belowP.add(new Label("Angle " + String.valueOf(i + 1)));
	   belowP.add(exampleButton);
	   for (int i = 1; i < 4; i++) // storeButton[0] should be irreversible
	     belowP.add(storeButtons[i]);	
	   belowP.add(teachButton);
	   belowP.add(weightsExec);
	   for (int i = 0; i < 6; i++)
	     {
	       weightsText[i] = new TextField(String.valueOf(angleWeights[i]));
	       belowP.add(weightsText[i]);
	     }
	   for (int i = 0; i < 4; i++)
	     belowP.add(loadButtons[i]);
	   stopButton.disable();
	   belowP.add(stopButton);
	   add("South", belowP);

	   this.show();

	   // First robot calculations and first painting of the robot:

	   AllAboutMoving.copyPointList(PL, pl, this);
	   RefDouble refAngleX = new RefDouble(angleX);// reference paramaters
	   RefDouble refAngleY = new RefDouble(angleY);// are needed
	   RefDouble refD = new RefDouble(d);
	   AllAboutMoving.angleCalculation(0.0, 10.0, 26.0, refAngleY, 
				refAngleX, refD);
	   angleX = refAngleX.in; // the real variables get the new values
	   angleY = refAngleY.in;
	   d = refD.in;
	   location[0] = 0.0; // I don't know, why these initialisations are
	   location[1] = 10.0;// not in the init part of this program
	   location[2] = 26.0;
	   AllAboutMoving.Nvector(pl, al, apl, 0, this);
	   animation.setScale(zoomValue);
	   animation.start(); // now the animation can be started, a new thread
			      // begins to work
	   CinematicCalculations.calculatePos(angles, pos, this);	 

	   repaint();
	   menuEnable();
	   repaint();
	
	   repaint();
	   repaint();// if java doesn't want to paint the robot
	   repaint();
	   repaint();
	} 

	public void stop()
	{
	  if (errWin != null)
	    errWin.dispose(); // tidies up the only window which can be open
	}
	

	public void repaint()
	{
	   animation.repaint();
	}

	public void menuEnable()
	{
	   // If another window, which has the panels disabled,is destroyed, 
	   // the panels should be enabled by using this method
	   if (!leftP.isEnabled()) 
	     leftP.enable();
 	   if (!rightP.isEnabled()) 
	     rightP.enable();
	   if (!belowP.isEnabled()) 
	     belowP.enable();
	   if (!upP.isEnabled()) 
	     upP.enable();
	   repaint();
	}

	public void menuDisable()
	{
	   upP.disable();
	   leftP.disable();
	   rightP.disable();
 	   belowP.disable();
	}

	private double roundTo2(double arg)
	{
	   return Math.floor(arg * 100) / 100;
	}

	private String formatTo5(double number)
	{