roboproto.java

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

				  indx = refindx.in;

                                  if (errorcode == 1 ) 
			            { 
				      actualizeAutoPanel();
				      return;
				    }
                                }
                            }  
                        }

                      if (errorcode == 1) 
		        { 
		          writeError("Crash.");
		        }
                    } 
                  else 
		    {  
		      if ( fFile == 0 )
		        {
			  writeError("Target is unreachable.");
			}
                    }

                }
              else /* if (fAutoline == 1).. */
                { 
                  p2.x = T[0][3];
                  p2.y = T[1][3];
                  p2.z = T[2][3];
                  distance = Math.sqrt( (p2.x - p1.x) * (p2.x - p1.x) + 
				        (p2.y - p1.y) * (p2.y - p1.y) +
					(p2.z - p1.z) * (p2.z - p1.z)  );
             
                  steps =(int)(distance / 0.5);
                  if ( steps == 0 ) 
		    steps = 1;
                  mdiff = 1.0 / steps;
                  m = mdiff;

                  while ( m <= 1.0 )
                    {
                      T[0][3] = p1.x + m * (p2.x - p1.x);
                      T[1][3] = p1.y + m * (p2.y - p1.y);
                      T[2][3] = p1.z + m * (p2.z - p1.z);
                      reminder = CinematicCalculations.inverseCinematic(T,
				input, angles, linkLengths, reminder,
				fAutoLine, this);
                   
                      if ( reminder == 0 )
                        {
                          for ( i=0 ; i<6 ; i++ )
                            {
                              input[i] = input[i] - angles[i].act;
			      if (Math.abs(input[i]) > 0.087266462 && m < 1.0)
			        reminder = 1;
                 	      if ( Math.abs(input[i]) > Math.PI ) 
			        { 
			          reminder = 2;
				  actualizeAutoPanel();
				  return;
                                } 
                            }
                          if ( reminder == 1 ) 
			    {
                              mdiff = mdiff / 2.0;
                              m = m - mdiff;
                            }
                          else 
                            {
			      if ( reminder == 0 )
                                {
                                  stepNumber = Math.abs((int)(input[0] / 
				          	(power[0]*(Math.PI/180))));
                 	          for ( i=1 ; i<6 ; i++ )
                 	            {
                  	              reminder = Math.abs((int)(input[i] / 
				            	(power[i]*(Math.PI/180))));
                     		      if (reminder >stepNumber)
				        stepNumber = reminder;
                  	            } 
                   	          line[0].y = -1.0; 

  			          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, input, 
				     angles,stepNumber, refError,refangleY,
			             refangleX, refD,1,refindx,location, this);
				  errorcode = refError.in;
				  angleY = refangleY.in;
				  angleX = refangleX.in;
				  d = refD.in;
				  indx = refindx.in;
				
                                  if ( errorcode == 1 ) 
			            {
				      if ( fFile == 0 )
					{
					 //writeError("Crash!!");
                                        }
                                      m = 2.0;
                                    }
                                  mdiff = 1.0 / steps;
                                  if ( m < 1.0 && (m + mdiff) > 1.0 ) 
				    m = 1.0;
                  	          else m = m + mdiff;
                                }
                              else 
			        {
                                  if ( fFile == 0 )
				    {
				      writeError("Target is unreachable.");
				      actualizeAutoPanel();
				    }
                                  // here sounds the bell in the original
                                  m = 2.0;
                                }    
                            } 
		 	}   
                      else 
		        {
		          if ( fFile == 0 )
                             {
			       writeError("Target is unreachable.");	
			       actualizeAutoPanel();
			     }
                           m = 2.0;
                        } 
                   }
               }
             CinematicCalculations.calculatePos(angles, pos, this);
	     for (int kl = 0; kl < 3; kl++)
	       {
		 target[kl] = pos[kl];
		 autoScrolls[kl].setValue(autoScrollScale * (int) target[kl]);
	       } 
	     for (int kl = 0; kl < 6; kl++)
	       { // to unify the values of 'auto control' and 'manual control'
		 manualScrolls[kl].setValue(manualScrollScale * 
						     (int) angles[kl].act);
		 updateManualLabel(kl);
	       } 	
             line[0].y = -2.0;
           }  
         else 
	   {
	     if ( fFile == 0 )
                {
		  writeError("Target is unreachable."); 
		  actualizeAutoPanel();
	        } 
           }  
	 actualizeAutoPanel();
         line[0].y = -1.0;  
         repaint();        
        } 
   
	public void weightsInput(int newValues[])
	{
	  for (int i = 0; i < 6; i++)
	    {
	      angleWeights[i] = newValues[i];
	      weightsText[i].setText(String.valueOf(angleWeights[i]));
	    }
	}      

	private void installRealRobot()
	{
	  manualLabels[3].hide();
	  manualScrolls[3].hide();
	  manualChange(3, 0);
	  manualLabels[5].hide();
	  manualScrolls[5].hide();
	  manualChange(5, 0);
	  linkHeader.hide();
	  linkExec.hide();
	  for (int i = 0; i < 3; i++)
	    {
	      linkLabels[i].hide();
	      linkScrolls[i].hide();
	      autoHeader.hide();
	      autoScrolls[i].hide();
	      autoLabels[i].hide();
	      lineButton.hide();
	      autoExecButton.hide();
	    }
	  double[] newLengths = { 4.5, 10.0, 10.0 };
	  linkLengthChange(newLengths); // these are the lengths of 
					// the robot
	  for (int i = 0; i < 3; i++)
	    {
	      linkScrolls[i].setValue((int)(linkScrollScale * 
					linkLengths[i]));
	      updateLinkLabel(i);
	    }
	  int actualSpeed = xControl;//to make the installation faster
	  speedInput(80);
	  manualChange(1, -1.57);// -90 degree
	  manualScrolls[1].setValue((int)(manualScrollScale * 
						angles[1].act));
	  manualChange(2, 0); //start position of the real robot
 	  manualScrolls[2].setValue((int)(manualScrollScale * 
						angles[2].act));
	  manualChange(0, 0); //start position of the real robot
	  manualScrolls[0].setValue((int)(manualScrollScale * 
						angles[0].act));
	  manualChange(4, 0); //start position of the real robot
	  manualScrolls[4].setValue((int)(manualScrollScale * 
						angles[4].act));
	  speedInput(actualSpeed);
	  savedAngles[0][1] = -1.57;//90 degree
	  exampleButton.disable();
	  robotIsReal = true;
	  RealRobot.RealRobotControl.openFile();// the file into which the 
			// commands for the real robot are written, is opened
	}

	private void tidyUpRealRobot()
	{
	  RealRobot.RealRobotControl.closeFile();// ...and closed
	  robotIsReal = false;
	  manualLabels[3].show();
	  manualScrolls[3].show();
	  manualLabels[5].show();
	  manualScrolls[5].show();
	  linkHeader.show();
	  linkExec.show();
	  for (int i = 0; i < 3; i++)
	    {
	      linkLabels[i].show();
	      linkScrolls[i].show();
	      autoHeader.show();
	      autoScrolls[i].show();
	      autoLabels[i].show();
	      lineButton.show();
	      autoExecButton.show();
	    }
 	  savedAngles[0][1] =  0;
	  double[] nullAngles = { 0, -Math.PI/2, 0, 0, 0, 0 };
	  int actualSpeed = xControl;//to make the installation faster
	  speedInput(80);// 80 is fast
	  manualChange(nullAngles);
	  double[] newLengths = { 13.5, 10.0, 10.0 };
	  linkLengthChange(newLengths); // these are the lengths of 
					// the robot
	  nullAngles[1] = 0;
	  manualChange(nullAngles);
	  for (int i = 0; i < 6; i++)
	    manualScrolls[i].setValue((int)(manualScrollScale * 
						angles[i].act));
	  for (int i = 0; i < 3; i++)
	    {
	      linkScrolls[i].setValue((int)(linkScrollScale * 
					linkLengths[i]));
	      updateLinkLabel(i);
	    }
	  speedInput(actualSpeed);
	  exampleButton.enable();
	}

	private void saveActualPosition(int where)
	{
	   for (int i = 0; i < 6; i++)
	     savedAngles[where][i] = angles[i].act;
	   savedAngles[where][6] = handValue;
	}

	private void loadPositionNo(int which)
	{
	  for (int i = 0; i < 6; i++)
	    if ( (angles[i].act != savedAngles[which][i]) && robotIsReal
		 && (i == 3 || i == 5) )
	      savedAngles[which][i] = 0;
	  manualChange(savedAngles[which]);
	  handInput(savedAngles[which][6]);
	  for (int kl = 0; kl < 6; kl++)
	     {
	       updateManualLabel(kl);
	       manualScrolls[kl].setValue((int)(angles[kl].act
					   * manualScrollScale));
	     } // it doesn't care, if too many labels are new printed
	   if (robotIsReal)
	     {
	        for (int kl = 0; kl < 3; kl++)
	          realRobotInterface(kl, angles[kl].act);
	        realRobotInterface(3, angles[4].act);
	     }
	}

	private void newSpeed()
	{ // calculation of the speeds corresponding to every angle
	  int trSpeed;

	  if (xControl > 60)
	    trSpeed = xControl * 20; // the turbo works
	  else
	    trSpeed = xControl / 3;
	  power[0] = power[1] = 1 + (trSpeed/2);
	  power[2] = 2 + (trSpeed/2);
	  power[3] = power[4] = power[5] = 3 + (trSpeed/2);
	}

	public void speedInput(int newSpeed)
	{
	  if (newSpeed < 1)
	    newSpeed = 1;
	  if (newSpeed > 82)
	    newSpeed = 82;
	  xControl = newSpeed;
	  speedScroll.setValue(xControl);
	  newSpeed();
	}

	public void zoomInput(float newValue)
	{
	  if (newValue == zoomValue)
	    return;
	  if (newValue < 0.1)
	    newValue = 0.1f;
	  if (newValue > 3.5)
	    newValue = 3.5f;
	  if (Math.abs(newValue - zoomValue) < 0.2)
	    {
	      zoomValue = newValue;
	      zoomScroll.setValue((int) (zoomScrollScale * zoomValue));
	      animation.setScale(zoomValue);
	      if (showTheMove)
	        repaint();
	    }
	  else
	    { // a smooth movement of the robot's size increase/decrease
	      if (newValue > zoomValue)
		{
		  while (zoomValue < (newValue - 0.1))
		    {
		      zoomValue += 0.1;
		      zoomScroll.setValue((int) (zoomScrollScale * zoomValue));
 		      animation.setScale(zoomValue);
 		      if (showTheMove)
	                repaint();
		    }		
	        }
	      else 
		{
	          while (zoomValue > (newValue + 0.1))
		    {
		      zoomValue -= 0.1;
		      zoomScroll.setValue((int) (zoomScrollScale * zoomValue));
 		      animation.setScale(zoomValue);
 		      if (showTheMove)
	                repaint();
		    }
	        }
	      zoomValue = newValue;
	      zoomScroll.setValue((int) (zoomScrollScale * zoomValue));
	      animation.setScale(zoomValue);
	      if (showTheMove)
	        repaint();
	    }
	
	}
	   
	public void handInput(double newValue)
	{
	  handChange(newValue);
	  handScroll.setValue((int) (handScrollScale * handValue));
	}

	private void handChange(double newValue)
	//to open or close the hand, it's sufficient to change the x-coordinate
	//value only
	{
	  if ((newValue == handValue) || (newValue > 0.5) || (newValue < 0.0))
	    return;
	  
	  showTheMove = false;
	  for (int i=0 ; i<6 ; i++ )  
	     {
               angleOld[i] = angles[i].act;
               angles[i].act = 0.0;
             }
	  double base9 = PL[134].x - handValue;//no.134 is a length 9 point
	  	 base9 += newValue;
	  double base11 = PL[136].x + handValue;//no.136 is a length 11 point
		 base11 -= newValue;
	  double base95 = base9 + 0.5;
	  double base105 = base11 - 0.5;
	  handValue = newValue;// now the actual hand value gets changed
	  for (int i = 134; i < 154; i++) // these points determine the hand
	    {
	      switch(i)
		{
		  case 134: case 135: case 139: case 140: case 142: 
		  case 143: PL[i].x = base9;
	     		    break;
		  case 136: case 137: case 138: case 141: case 148:
		  case 149: PL[i].x = base11;
			    break;
		  case 144: case 145: case 146: case 147:
			    PL[i].x = base95;
			    break;
	          case 150: case 151: case 153: case 152: