//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); }
/*! 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; }
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); }