roboproto.java

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

				 steps,refError, refAngleY, refAngleX, refD, 
				 1, refIndx, location, this);
	      errorcode = refError.in;
                  
              if ( errorcode != 1 )
                { 
                  for (int j = n; j > 0; 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);
		      errorcode = refError.in;
                      if ( errorcode == 1 ) 
			{
			  errorDo();
		          break;
			}  
                    }
                }
	      else errorDo();
            }
	  else errorDo();
	  CinematicCalculations.calculatePos(angles, pos, this);
		
	  double oldValueReminder ;
	  for (int j = 0; j < 6; j++)     
	    {    
	      oldValueReminder = angles[j].act;
	      angles[j].act = newValues[j]; // now the variable holds the
				            // new value 
	      if ((oldValueReminder != angles[j].act) || (!showTheMove))
	        updateManualLabel(j);
	    }
	  if (anglesLeftExtreme[0] || anglesLeftExtreme[1] ||
	      anglesLeftExtreme[2] || anglesLeftExtreme[3] ||
	      anglesLeftExtreme[4] || anglesLeftExtreme[5] ||
	      (angles[2].act > 0.4) || 
	      ((angles[0].act != 0) && (angles[2].act != 0)) ||
	      ((angles[0].act == 0) && (angles[1].act == 0) &&  
	       (angles[2].act == 0) && (angles[3].act == 0) && 
	       (angles[4].act == 0) && (angles[5].act == 0)) )
	    { // the robot is crooked and thus is going to be repaired
	      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:
	      makeRealRobotMove = false;//the real robot isn't supposed to move
	      refError = new RefInt(errorcode);
	      refAngleY = new RefDouble(angleY);
	      refAngleX = new RefDouble(angleX);
	      refD = new RefDouble(d);
	      refIndx = new RefInt(indx);
	      int stepss;
	      if ((angles[0].act == 0) && (angles[1].act == 0) &&  
	       (angles[2].act == 0) && (angles[3].act == 0) && 
	       (angles[4].act == 0) && (angles[5].act == 0))
		stepss = 80;
	      else stepss = 8;
              AllAboutMoving.MoveRobo (showTheMove, pl, line, angleOld, 
			angles,	stepss, refError, refAngleY, refAngleX, refD, 0, 
		        refIndx, location, this);
              errorcode = refError.in;
	      angleY = refAngleY.in;
	      angleX = refAngleX.in;
	      indx = refIndx.in;
	      d = refD.in;
	      makeRealRobotMove = true;
	      for (int i = 0; i < 6; i++)
	        {
		  updateManualLabel(i);
		  manualScrolls[i].setValue((int)
					(manualScrollScale * angles[i].act));
	 	}
	    } 
	  if (showTheMove)
	    { 
	      repaint();
	      // to unify the values of 'auto-' and 'manual control':
	      actualizeAutoPanel();// which are equal if the move isn't shown
	    }
	}
 
	private void manualChange(int no, double newValue)
	{
	  if (newValue >= angles[no].max)   // Test, if newValue has got an 
	    {                               // allowed value
	      newValue = angles[no].max;   
	      anglesLeftExtreme[no] = true; //it doesn't matter if left orright
	    }  
 	  if (newValue <= angles[no].min)
	    {
	      newValue = angles[no].min;
	      anglesLeftExtreme[no] = true;
	    }
	  if (newValue == angles[no].act)
	    return;
	  
	  reminder = 1;
	  for (int j = 0; j < 6; j++)
	    {
	      input[j] = 0;
              angleNew[j] = angles[j].act;
	    }
	  // input is the difference between the old and the new
	  // angle 
	  input[no] = newValue - angles[no].act;
	  angleNew[no] = angles[no].act + input[no];

	  if (!crashTest(angleNew))
	    {  
	      // 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;
	      manualScrolls[no].setValue((int)(manualScrollScale * 
						angles[no].act));
	      updateManualLabel(no);
	      crashIsActive = false;
	      return;// this change is forbidden
	    }
	    
	  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, steps,
				 refError, refAngleY, refAngleX, refD, 1, 
				 refIndx, location, this);
	      errorcode = refError.in;
                  
              if ( errorcode != 1 )
                { 
                  for (int j = n; j > 0; 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);
		      errorcode = refError.in;
                      if ( errorcode == 1 ) 
			{ 
			  errorDo();
		          break;
			}  
                    }
                }  
	      else errorDo();
	    }
	  else errorDo();
	  CinematicCalculations.calculatePos(angles, pos, this);
		
	  angles[no].act = newValue; // now the variable holds the
				     // new value 
	  updateManualLabel(no);
	  if (anglesLeftExtreme[no] || (angles[2].act > 0.4))
	    {// the robot is crooked and thus is going to be repaired*/
	      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:   
	      makeRealRobotMove = false;//the real robot isn't supposed to move
	      refError = new RefInt(errorcode);
	      refAngleY = new RefDouble(angleY);
	      refAngleX = new RefDouble(angleX);
	      refD = new RefDouble(d);
	      refIndx = new RefInt(indx);
              AllAboutMoving.MoveRobo (showTheMove, pl, line, angleOld, 
			angles,	8,refError, refAngleY, refAngleX, refD, 0, 
		        refIndx, location, this);
              errorcode = refError.in;
	      angleY = refAngleY.in;
	      angleX = refAngleX.in;
	      indx = refIndx.in;
	      d = refD.in;
	      if (errorcode == 1)
		errorDo();
	      makeRealRobotMove = true;
	      for (int i = 0; i < 6; i++)
	        {
		  updateManualLabel(i);
		  manualScrolls[i].setValue((int)
					(manualScrollScale * angles[i].act));
	 	}
	     }  
          if (showTheMove)
	    {
	      repaint();
	      // to unify the values of 'auto' and 'manual control':
	      actualizeAutoPanel();// which are equal if the move isn't shown
	    }
	}
  
	public void autoInput(double input[])
	{
	  autoChange(input);
	  actualizeAutoPanel();
	}

	private void autoChange(double input[])
	{
	  int i, j;
	  if ( input[0] != 0.0 || input[1] != 0.0 || input[2] != 0.0 )
            {
              line[0].x = input[0];
              line[0].y = input[1];
              line[0].z = input[2];


              CinematicCalculations.TO6(angles, linkLengths, T);

              line[1].x = linkLengths[3] * T[0][2] + T[0][3];
              line[1].y = linkLengths[3] * T[2][2] + T[2][3] + linkLengths[0];
              line[1].z = -(linkLengths[3] * T[1][2] + T[1][3]);
              p1.x = T[0][3];
              p1.y = T[1][3];
              p1.z = T[2][3];
              T[0][3] = input[0];
              T[1][3] = -input[2];
              T[2][3] = input[1] - linkLengths[0];
               
              T[0][3] = T[0][3] - T[0][2] * linkLengths[3];
              T[1][3] = T[1][3] - T[1][2] * linkLengths[3];
              T[2][3] = T[2][3] - T[2][2] * linkLengths[3];

	      if (fAutoLine == 1) 
                {
                  reminder = CinematicCalculations.inverseCinematic(T, input, 
			angles,	linkLengths, reminder, fAutoLine, this );

                  for ( i=0 ; i<6 ; i++ )
                    {
                      input[i] = input[i] - angles[i].act ;
                    }               
                  if ( reminder == 0 )
                    {
                      line[1].x = line[0].x;
                      line[1].y = line[0].y;
                      line[1].z = line[0].z;
                      line[0].y = -1.0;       
            
                      // Execution of the movement with acceleration
		      RefInt refN = new RefInt(n);
		      RefInt refSteps = new RefInt(steps);
                      AllAboutMoving.calculateAngle(input, wB, wE, refN, 
						    refSteps, this);
		      n = refN.in;
		      steps = refSteps.in;
                  
                      for ( i=1; i<n+1; i++ )
                        {
                          for ( j=0; j<6; j++ ) 
		            hw[j] = wB[j] * i;

			  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,hw,
			   angles,1, refError, refangleY, refangleX, refD,1,
			   refindx, location, this);
			  errorcode = refError.in;
			  angleX = refangleX.in;
			  angleY = refangleY.in;
			  d = refD.in;
			  indx = refindx.in;

                          if ( errorcode == 1 )
			    {
			      actualizeAutoPanel();
			      return;
			    }
                        }    
    
                      if ( errorcode != 1)
                        { 
			  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, wE,
				 angles,
				 steps, refError, refangleY, refangleX, refD,
			         1, refindx, location, this);
			  errorcode = refError.in;
			  angleX = refangleX.in;
			  angleY = refangleY.in;
			  d = refD.in;
			  indx = refindx.in;

                  
                          if (errorcode != 1 )
                            { 
                              for ( i=n; i>0; i-- )
                                {
                                  for ( j=0; j<6; j++ ) 
			            hw[j] = wB[j] * i;

  			          refError.in = errorcode;
			          refangleY.in = angleY;
			          refangleX.in = angleX;
			          refD.in = d;	
			          refindx.in = indx;
                     	          AllAboutMoving.MoveRobo(showTheMove, pl,
				    line, hw, angles,1, refError, refangleY,
				    refangleX,refD,1, refindx, 
				    location, this );
				  errorcode = refError.in;
				  angleY = refangleY.in;
				  angleX = refangleX.in;
				  d = refD.in;