roboproto.java

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

	   String result = new String(String.valueOf(number));
	   int length = result.length();
	
	   switch(length)
	     {
		case 4: return result + ")   ";
		case 3: return result + ")    ";
		case 2: return result + ")     ";
		case 1: return result + ")      ";
		default: return result + ")  ";
	     }
	}

	private void updateManualLabel(int no)
	{
	  if ((no >= 0) && (no < 6))
	    {
	      String blank = null; // behind the first number, a blank is
		    // necessary for angle no. 4 and 5 (they're too short)
	      if (no == 4 || no == 5)
	        blank = "  ";
	      else
	   	blank = "";
  	      manualLabels[no].setText(String.valueOf(
		        Math.round(angles[no].min * 180 / Math.PI)) + blank  
			+ "   Angle" + String.valueOf(no + 1) + " (" +
			formatTo5(Math.round(angles[no].act * 180 / Math.PI)) 
			+ "        " + String.valueOf(Math.round(
			angles[no].max * 180 / Math.PI)));
	   }
	} 

	private void updateLinkLabel(int no)
	{
	  if ((no >= 0) && (no < 3))
	    linkLabels[no].setText(String.valueOf(
			roundTo2(linkMin[no])) +
			"   Linklength" + String.valueOf(no + 1) + " (" +
			formatTo5(roundTo2(linkLengths[no])) + "      " +
			String.valueOf(roundTo2(linkMax[no])));
	}

	private void updateOldLinkLabel(int no, double actValue)
	// this method is called, if the linkLenth is changed, but the change
	// is not yet executed
	{
	  if ((no >= 0) && (no < 3))
	    linkLabels[no].setText(String.valueOf(
			roundTo2(linkMin[no])) +
			"   Linklength" + String.valueOf(no + 1) + " (" +
			formatTo5(roundTo2(actValue)) + "      " +
			String.valueOf(roundTo2(linkMax[no])));
	}

	private void updateAutoLabel(int no)
	{
	  if ((no >= 0) && (no < 3))
	    {
	      String[] labelHead = { new String("X-Value"), 
			new String("Y-Value"), new String("Z-Value") };
	      autoLabels[no].setText(String.valueOf(-autoScrollRange) + 
			"       " + labelHead[no] + " (" + 
			formatTo5(roundTo2(target[no]))
			+ "           " + String.valueOf(autoScrollRange));
	    }
	}

	private void updateScrolledAutoLabel(int no)
	// this method is called, if an auto scrollbar has been used, but the
	// robot hasn't yet moved to the new position, because the execute
	// button has to be pushed to make him do that
	{
	  if ((no >= 0) && (no < 3))
	    {
	      String[] labelHead = { new String("X-Value"), 
			new String("Y-Value"), new String("Z-Value") };
	      autoLabels[no].setText(String.valueOf(-autoScrollRange) + 
			"       " + labelHead[no] + " (" + 
			formatTo5(roundTo2(input[no]))
			+ "           " + String.valueOf(autoScrollRange));
	    }
	}

	public void writeError(String text)
	{
	    errWin = new ErrorWindow(text, this);
	}

	private void actualizeAutoPanel()
	{
	  CinematicCalculations.calculatePos(angles, pos, this);
	  for (int kl = 0; kl < 3; kl++)
	    {
	      target[kl] = pos[kl];
	      autoScrolls[kl].setValue(autoScrollScale * (int) target[kl]);
	      updateAutoLabel(kl);
	    }
	}

	public void manualInput(double newAngles[])
	// the read angle dates are interpreted
	{
	   if (robotIsReal)
	     {
	       newAngles[3] = 0.0;
	       newAngles[5] = 0.0;
	     }
	   if (! ((newAngles[0] == angles[0].act) &&
		  (newAngles[1] == angles[1].act) && 
		  (newAngles[2] == angles[2].act) &&
		  (newAngles[3] == angles[3].act) && 
		  (newAngles[4] == angles[4].act) &&
		  (newAngles[5] == angles[5].act)))
	     {
	       manualChange(newAngles);
	       for (int i = 0; i < 6; i++)
	         manualScrolls[i].setValue(manualScrollScale * 
						(int) angles[i].act); 
	     }
	      
	}

	public void linkLengthInput(double newLength[])
	{
	   if (robotIsReal)
	     return;
	   linkLengthChange(newLength);
	   for (int j = 0; j < 3; j++)
	     { 
	       linkScrolls[j].setValue(linkScrollScale * (int) linkLengths[j]);
	       updateLinkLabel(j);
	     }	
	}

	private void linkLengthChange(double newLength[])
	{
	   int i;
	   double[] remindNewLengths = { newLength[0], newLength[1],
					 newLength[2] };
	   boolean link1NotChanged = false;

	   if ( newLength[0] < linkMin[0] || newLength[1] < linkMin[1] ||
                newLength[2] < linkMin[2] || newLength[0] > linkMax[0] ||
                newLength[1] > linkMax[1] || newLength[2] > linkMax[2] ||
		((newLength[0] == linkLengths[0]) &&  
		 (newLength[1] == linkLengths[1]) &&
		 (newLength[2] == linkLengths[2]))     )
	     return;  // at least one of the new lengths is wrong

	   if (newLength[0] == linkLengths[0])
	     link1NotChanged = true;
	   if ( fFile == 0 )
             {    // Test, if location of view has to be adjusted 
	       locationNew[0] = 0.0;
               locationNew[1] = 0.0;
               locationNew[2] = d;
               x = -angleX;
               y = -angleY;
               AllAboutMoving.calculateLocation(locationNew,y,x);
                  
                   
               x = location[1];
               y = newLength[0] + newLength[1] + newLength[2] + linkLengths[3] 
		   + 2.0;
               if ( x < y )
                 {
                   x = 0.0;
                   y = newLength[0] + newLength[1] + newLength[2] + 
	               linkLengths[3] + 2.0;
                   z = 0.0;
                    
                   x = Math.sqrt(locationNew[0] * locationNew[0] + 
		    	         locationNew[2] * locationNew[2]);
                   y = newLength[1] + newLength[2] + linkLengths[3] + 2.0;

                   s_wx = Math.sin(angleX);
                   z = s_wx * y ;

                   if ( z >= ( d-2.0) || x < y) 
                     {
		       writeError("This change isn't executable.");
		       for (int kl = 0; kl < 3; kl++)
			 {
			   linkScrolls[kl].setValue((int) linkLengths[kl]
							  * linkScrollScale);
			   updateLinkLabel(kl);
			 }
                       return;      
                     }
                 }


             }  // endif f_file == 0 
                  
            // the arms get changed:
           for ( i=0 ; i<6 ; i++ )  
	     {
               angleOld[i] = angles[i].act;
               angles[i].act = 0.0;
             }
	   for (i = 0; i < 3; i++)
	     {
               h = linkLengths[i];
               linkLengths[i] = newLength[i]; 
               newLength[i] = h - newLength[i];
	     }
	   for ( i=48 ; i<78 ; i++ )
	     PL[i].y = PL[i].y - newLength[0];
           for ( i=78 ; i<108 ; i++ ) 
	     {
	       PL[i].y = PL[i].y - newLength[0];
               PL[i].x = PL[i].x - newLength[1];
             }
           for ( i=108 ; i<numberOfPoints ; i++ ) 
	     {
               PL[i].y = PL[i].y - newLength[0] + newLength[2];
               PL[i].x = PL[i].x - newLength[1];
             }
           AllAboutMoving.copyPointList(PL, pl, this);    
           angleNew[0] = 0.0;
           angleNew[1] = -Math.PI/2;
           angleNew[2] = angleNew[1];
           angleNew[3] = 0.0;
           angleNew[4] = 0.0;
           angleNew[5] = 0.0;
           AllAboutMoving.PosRobo(pl, angleNew, angles, this);// angles get
							// their new values
	   angleOld[1] = angleOld[1] + (Math.PI/2);
           angleOld[2] = angleOld[2] + (Math.PI/2);
	   // Creation of variables of primitive data type needed as 
	   // call-by-reference variables:
	   RefInt refError = new RefInt(errorcode);
	   RefDouble refAngleY = new RefDouble(angleY);
	   RefDouble refAngleX = new RefDouble(angleX);
	   RefDouble refD = new RefDouble(d);
	   RefInt refIndx = new RefInt(indx);
           AllAboutMoving.MoveRobo (showTheMove, pl, line, angleOld, angles, 
			45,refError, refAngleY, refAngleX, refD,0, refIndx,
			location, this);
           errorcode = refError.in;
	   angleY = refAngleY.in;
	   angleX = refAngleX.in;
	   indx = refIndx.in;
	   d = refD.in;

	   if (showTheMove)
	     repaint();
	   for (int k = 0; k < 3; k++)
	     linkLengths[k] = remindNewLengths[k];
	
	   if (!link1NotChanged)
	     {
	       manualChange(0, angles[0].act + 0.1);
	       manualChange(0, angles[0].act - 0.1);
	       // The first change of 'linkLength1' is only
	       // executed, if angle 0 gets changed (I don't know why).
	       // Thus I make this angle change every time, 'linkLength1'
	       // gets changed.
	     }
	   for (int kl = 0; kl < 6; kl++)
	     {
	       updateManualLabel(kl);
	       manualScrolls[kl].setValue((int)angles[kl].act
					   * manualScrollScale);
	     }
	}
    
      private void errorDo()
        {
	   if (!crashIsActive)
	     writeError("This angle change would cause a crash.");
	   crashIsActive = true;
	   for (int i = 0; i < 6; i++)
	     {
	       updateManualLabel(i);
	       manualScrolls[i].setValue((int) (angles[i].act * 
						manualScrollScale));
	     }  
	}

      private boolean crashTest(double dummies[])
	{ // Tests, if a manual change would cause a crash with the floor

	  double[] position = new double[3];
	  AngleDates[] helper = new AngleDates[6];

	  for (int i = 0; i < 6; i++)
	    helper[i] = new AngleDates(angles[i].max, angles[i].min,
					 dummies[i]);
	  CinematicCalculations.calculatePos(helper, pos, this);
	  if (pos[1] <= 0)
	    {
	      if (!crashIsActive)
	        writeError("This angle change would cause a crash.");
	      crashIsActive = true;
	      return false;
	    }
	  return true;
	}
	  
	private void manualChange(double newValues[])
	{
	  reminder = 1;
	  for (int j = 0; j < 6; j++)
	    {
	      input[j] = 0;
              angleNew[j] = angles[j].act;
	    }
	  for (int j = 0; j < 6; j++)
	    {
	      if (newValues[j] >= angles[j].max)// Test,if newValue has got an
		{ 
	          newValues[j] = angles[j].max;   // allowed value
	          anglesLeftExtreme[j] = true;
	        }
	      else
 	        if (newValues[j] <= angles[j].min)
	          {
		    newValues[j] = angles[j].min;
		    anglesLeftExtreme[j] = true;
		  }
	    } 
	  if (!crashTest(newValues))
	    {
	       // Reparation of the robot:
	      for (int i=0 ; i<6 ; i++ )  
	        {
                  angleOld[i] = angles[i].act;
                  angles[i].act = 0.0;
                }
	      AllAboutMoving.copyPointList(PL, pl, this);    
              angleNew[0] = 0.0;
              angleNew[1] = -Math.PI/2;
              angleNew[2] = angleNew[1];
              angleNew[3] = 0.0;
              angleNew[4] = 0.0;
              angleNew[5] = 0.0;
              AllAboutMoving.PosRobo(pl, angleNew, angles, this);// angles get
							// their new values
	      angleOld[1] = angleOld[1] + (Math.PI/2);
              angleOld[2] = angleOld[2] + (Math.PI/2);
	      // Creation of variables of primitive data type needed as 
	      // call-by-reference variables:
	      RefInt refError = new RefInt(errorcode);
	      RefDouble refAngleY = new RefDouble(angleY);
	      RefDouble refAngleX = new RefDouble(angleX);
	      RefDouble refD = new RefDouble(d);
	      RefInt refIndx = new RefInt(indx);
              AllAboutMoving.MoveRobo (showTheMove, pl, line, angleOld, 
			angles,	15,refError, refAngleY, refAngleX, refD, 0, 
		        refIndx, location, this);
              errorcode = refError.in;
	      angleY = refAngleY.in;
	      angleX = refAngleX.in;
	      indx = refIndx.in;
	      d = refD.in;
	      for (int i = 0; i < 6; i++)
	        manualScrolls[i].setValue((int)(manualScrollScale * 
						angles[i].act));
	      crashIsActive = false;
	      return; // this change would cause a bad crash
	    }
	  // input is the difference between the old and the new
	  // angle 
	  for (int j = 0; j < 6; j++)
	    {
	      input[j] = newValues[j] - angles[j].act;
	      angleNew[j] = angles[j].act + input[j];
	    }
	  RefInt refN = new RefInt(n);        // copies of the variables
	  RefInt refSteps = new RefInt(steps);// which are objects
	  AllAboutMoving.calculateAngle(input, wB, wE, refN, 
				refSteps, this);
	  n = refN.in;
	  steps = refSteps.in;
	  // reference variables for the following MoveRobo-
	  // methods exactly representing the primitive data-
	  // type variables:
	  RefInt refError = new RefInt(errorcode);
	  RefDouble refAngleY = new RefDouble(angleY);
	  RefDouble refAngleX = new RefDouble(angleX);
	  RefDouble refD = new RefDouble(d);
	  RefInt refIndx = new RefInt(indx);
	  for (int j = 1; j < n + 1; j++)
	    {
	      for (int k = 0; k < 6; k++)
	      hw[k] = wB[k] * j;
	      AllAboutMoving.MoveRobo(showTheMove, pl, line, hw, angles, 1,
				refError, refAngleY, refAngleX, refD, 1, 
				refIndx, location, this);
			  
	      if (refError.in == 1)
		{
		  errorDo();
	          break;
		}  
	      errorcode = refError.in;
	      angleY = refAngleY.in;
	      angleX = refAngleX.in;
	      d = refD.in;
	      indx = refIndx.in;
	    }	

	  if ( errorcode != 1)
            {
              AllAboutMoving.MoveRobo (showTheMove, pl, line, wE, angles,