//Constructor
//Initializes hardware and software variables. Redundant for ZStepperInit
zStepperDriver::zStepperDriver()
{
	int			dummy=0;
//	double		prevPos;

	port = 1;
	prevPort = 1;
	

	fullStepConvFactor = Num_Microns_Per_Full_Step;		// microns/step
	halfStepConvFactor = Num_Microns_Per_Half_Step;
	stepMode = Default_Step_Mode;
	bTorqueOn = 1;

	//Try to init motor.
	if (!initialized)
		dummy =  OpenPort();
	
	if (dummy)
	{	
		//MessageBox();

		return;
	}

	dummy = SetTorque(1);
	if (dummy)
		bTorqueOn = true;

	SetStepMode(stepMode);

	switch (stepMode)
	{
		case 0:
			currentStepConvFactor = Num_Microns_Per_Half_Step;
			break;
		case 1:
			currentStepConvFactor = Num_Microns_Per_Full_Step;
			break;
		default:;
	}

	ReadPos(currentStepConvFactor);
	
}
//Offset-------------------------------
//Move by value specified by stepSize field.
//Input: zStep = distance, in um, to move (relative to current position)
//Ouput: none
void zStepperDriver::Offset(double zStep, double convFactor)
{
	char Str[15];
	int numSteps;
	
	//Calculate wait time
	double waitTime = .015 * zStep;
	
	if(waitTime < 0)
		waitTime = -waitTime;
	
	if(zStep > 0)
		numSteps = (int) (zStep / convFactor + 0.5);
	else
		numSteps = (int) (zStep / convFactor - 0.5);
	
	//Output command to RS232
	sprintf(Str, "II%i\r", numSteps);
	ComWrt(port, Str, strlen(Str));
	Sleep(waitTime*1000);		//*1000 because original value was for Delay function in CVI 

	ReadPos(currentStepConvFactor);
}
Example #3
0
/*! process command
 *
 * \return A value : braitenberg end mode
 *    - 0 if success
 *    - <0 if any error
*/
int process_command ()
{
	char Buffer[MAXBUFFERSIZE];
 	int narg;
  
  int out=0;
	
 	string larg[MAX_ARG];
	char *bptr;


	// receive and interpret commands (wait)
  if ( readLine(Buffer) >0 ) 
  {
  	#ifdef DEBUG
  		printf("%c length %d |%s|",Buffer[0],strlen(Buffer),Buffer);
  	#endif
  	
  	if (strlen(Buffer)>2)
  	{
  		/* Process all the args */
			bptr = Buffer + 2;
			
			narg=getArgs(bptr,larg);
			
		}
  	
		switch(Buffer[0])
		{
			case 'A':
				out=braitenberg(narg,larg);
				break;
			case 'B':
				revisionOS(narg,larg);
				break;
			case 'C': 
				configureOS(narg,larg);	
				break;
			case 'D': 
				SetSpeed(narg,larg);	
				break;
			case 'E': 
				GetSpeed(narg,larg);	
				break;
			case 'F': 
				SetTargetProfile(narg,larg);	
				break;
			case 'G': 
				GetUS(narg,larg);	
				break;
			case 'H': 
				ConfigPID(narg,larg);	
				break;
			case 'I': 
				SetEncPosition(narg,larg);	
				break;
			case 'J': 
				ConfigSpeedProfile(narg,larg);	
				break;
			case 'K': 
				SetLED(narg,larg);	
				break;
			case 'L': 
				SetSpeedOpenLoop(narg,larg);	
				break;
			case 'M': 
				InitMot(narg,larg);	
				break;					
			case 'N': 
				ReadProxSensors(narg,larg);	
				break;
			case 'O': 
				ReadAmbSensors(narg,larg);	
				break;
			case 'P': 
				SetTargetPosition(narg,larg);	
				break;
			case 'R': 
				ReadPos(narg,larg);	
				break;							
			case 'V':
				batStatus(narg,larg);
				break;
			case 'X':
				BinaryRead(narg,larg);
				break;
			case 'Z':
				tstampRST(narg,larg);
				break;
			default:
				sprintf(Buffer,"%c\r\n",ERROR_CMD_CHAR);
				com_send(Buffer, strlen(Buffer));
				
		}
  }
      
	return out;
}
Example #4
0
void CAM_Render(CEntity *pen, CDrawPort *pdp)
{
  if( cam_bRecord) {
    if (!_bInitialized) {
      _bInitialized = TRUE;
      SetSpeed(1.0f);
      _fStartTime = _pTimer->CurrentTick();
    }
    FLOATmatrix3D m;
    MakeRotationMatrixFast(m, _cp.cp_aRot);
    FLOAT3D vX, vY, vZ;
    vX(1) = m(1,1); vX(2) = m(2,1); vX(3) = m(3,1);
    vY(1) = m(1,2); vY(2) = m(2,2); vY(3) = m(3,2);
    vZ(1) = m(1,3); vZ(2) = m(2,3); vZ(3) = m(3,3);

    _cp.cp_aRot(1)-=_pInput->GetAxisValue(MOUSE_X_AXIS)*0.5f;
    _cp.cp_aRot(2)-=_pInput->GetAxisValue(MOUSE_Y_AXIS)*0.5f;
    
    if( cam_bMoveForward)      { _cp.cp_vPos -= vZ *cam_fSpeed; };
    if( cam_bMoveBackward)     { _cp.cp_vPos += vZ *cam_fSpeed; };
    if( cam_bMoveLeft)         { _cp.cp_vPos -= vX *cam_fSpeed; };
    if( cam_bMoveRight)        { _cp.cp_vPos += vX *cam_fSpeed; };
    if( cam_bMoveUp)           { _cp.cp_vPos += vY *cam_fSpeed; };
    if( cam_bMoveDown)         { _cp.cp_vPos -= vY *cam_fSpeed; };
    if( cam_bTurnBankingLeft)  { _cp.cp_aRot(3) += 10.0f; };
    if( cam_bTurnBankingRight) { _cp.cp_aRot(3) -= 10.0f; };
    if( cam_bZoomIn)           { _cp.cp_aFOV -= 1.0f; };
    if( cam_bZoomOut)          { _cp.cp_aFOV += 1.0f; };
    if( cam_bZoomDefault)      { _cp.cp_aFOV  = 90.0f; };
    Clamp( _cp.cp_aFOV, 10.0f, 150.0f);

    if( cam_bResetToPlayer) {
      _cp.cp_vPos = pen->GetPlacement().pl_PositionVector;
      _cp.cp_aRot = pen->GetPlacement().pl_OrientationAngle;
    }

    if( cam_bSnapshot) {
      cam_bSnapshot = FALSE;
      WritePos(_cp);
    }

  } else {
    if (!_bInitialized) {
      _bInitialized = TRUE;
      ReadPos(_cp0);
      ReadPos(_cp1);
      SetSpeed(_cp0.cp_fSpeed);
      _fStartTime = _pTimer->CurrentTick();
    }
    TIME tmNow = _pTimer->GetLerpedCurrentTick()-_fStartTime;
    if (tmNow>_cp1.cp_tmTick) {
      _cp0 = _cp1;
      ReadPos(_cp1);
      SetSpeed(_cp0.cp_fSpeed);
    }
    FLOAT fRatio = (tmNow-_cp0.cp_tmTick)/(_cp1.cp_tmTick-_cp0.cp_tmTick);

    _cp.cp_vPos = Lerp(_cp0.cp_vPos, _cp1.cp_vPos, fRatio);
    _cp.cp_aRot = Lerp(_cp0.cp_aRot, _cp1.cp_aRot, fRatio);
    _cp.cp_aFOV = Lerp(_cp0.cp_aFOV, _cp1.cp_aFOV, fRatio);
  }

  CPlacement3D plCamera;
  plCamera.pl_PositionVector = _cp.cp_vPos;
  plCamera.pl_OrientationAngle = _cp.cp_aRot;

  // init projection parameters
  CPerspectiveProjection3D prPerspectiveProjection;
  prPerspectiveProjection.FOVL() = _cp.cp_aFOV;
  prPerspectiveProjection.ScreenBBoxL() = FLOATaabbox2D(
    FLOAT2D(0.0f, 0.0f), FLOAT2D((float)pdp->GetWidth(), (float)pdp->GetHeight())
  );
  prPerspectiveProjection.AspectRatioL() = 1.0f;
  prPerspectiveProjection.FrontClipDistanceL() = 0.3f;

  CAnyProjection3D prProjection;
  prProjection = prPerspectiveProjection;

  // set up viewer position
  prProjection->ViewerPlacementL() = plCamera;
  // render the view
  RenderView(*pen->en_pwoWorld, *(CEntity*)NULL, prProjection, *pdp);
}
//MoveTo-------------------------------
//Move to absolute position
//Input: desired z position in um, relative to origin, converstion factor
//Ouput: none
void zStepperDriver::MoveTo(double zPos, double convFactor)
{
	
	char Str [15];
	char out [15];
	char in [15];
	int retVal = 0;
	int termCharVal = 13;
	int numSteps;

	//Calculate wait time
	//double stepWait = 0.010 * (zPos - ReadPos(convFactor)) ;  //In seconds

	//if(stepWait < 0)
	//	stepWait = -stepWait;

	//double waitTime = stepWait + .1;
	
	if(zPos > 0)
	{
		numSteps = (int) (zPos / convFactor + 0.5);
		numSteps += 0;		//Fudge factor
	}
	else
	{	
		numSteps = (int) (zPos / convFactor - 0.5);
		numSteps -= 0;		//Fudge factor
	}
	//Output command to RS232
	sprintf(Str, "MI%i\r", numSteps);
	ComWrt(port, Str, strlen(Str));
	Sleep(0.1*1000);		//*1000 because original value was for Delay function in CVI
//	retVal = FlushInQ (port);
//	Sleep(.5*1000);		//*1000 because original value was for Delay function in CVI
	//Sleep(waitTime*1000);		//*1000 because original value was for Delay function in CVI

	//Wait here until z has reached position
	int delta;
	int test;
	int count = 0;
	sprintf(out, "RT\r");
	do
	{
		
		retVal=ComWrt(port, out, strlen(out));
		Sleep(0.1*1000);		//*1000 because original value was for Delay function in CVI
		//ComRd (port, in, 5);
		retVal=ComRdTerm (port, in, 7, termCharVal);
		if (retVal > 0)
			in[retVal]='\0';
		Sleep(0.1*1000);		//*1000 because original value was for Delay function in CVI
		//printf("%s \n",in);
		//retVal = FlushOutQ (port);
		//Sleep(.5*1000);		//*1000 because original value was for Delay function in CVI
		//retVal=ComRdTerm (port, in, 7, termCharVal);
		//Sleep(.5*1000);		//*1000 because original value was for Delay function in CVI
		delta = atoi(in);
		test = abs(delta);
		sprintf(in,"");
		count++;
	} while(test > 4);


	ReadPos(convFactor);


}