From d5bfc97a9ac64ae8bae88398120ac39e2ce76558 Mon Sep 17 00:00:00 2001 From: TAO Cheng Date: Mon, 1 Jul 2013 16:23:27 +0800 Subject: [PATCH] =?UTF-8?q?=E5=B0=86=E5=8F=98=E5=80=8D=E6=96=87=E4=BB=B6?= =?UTF-8?q?=E7=BB=9F=E4=B8=80?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../Interfac/Msi/Hsi/SevenOcean/CAutoZoom.cpp | 426 ++++++++++++++++++ .../Interfac/Msi/Hsi/SevenOcean/CAutoZoom.h | 45 ++ 2 files changed, 471 insertions(+) create mode 100644 PcDmis/Base/Interfac/Msi/Hsi/SevenOcean/CAutoZoom.cpp create mode 100644 PcDmis/Base/Interfac/Msi/Hsi/SevenOcean/CAutoZoom.h diff --git a/PcDmis/Base/Interfac/Msi/Hsi/SevenOcean/CAutoZoom.cpp b/PcDmis/Base/Interfac/Msi/Hsi/SevenOcean/CAutoZoom.cpp new file mode 100644 index 0000000..b3a1312 --- /dev/null +++ b/PcDmis/Base/Interfac/Msi/Hsi/SevenOcean/CAutoZoom.cpp @@ -0,0 +1,426 @@ +#include "StdAfx.h" +#include "CAutoZoom.h" + +#define PI 3.1415926535897932384626433 +#define MAX_BUFF_SIZE 0x200 + +CPSerial* m_pSO7_Serial=NULL; +CAutoZoom::CAutoZoom(void) +{ + PreZoomRatio=0; + ZoomOrgPos=360; + ZoomStartPos=-10; + ZoomEndPos=210; + ZoomCurPos=0; + ZoomMode=0; + ZoomDeadband=0.1; + ZoomScale=25.13473606496862; + iRetryCntFrom=0; + iRetryCntReadV=0; + SelectedMultiple=0; + iReadingInterval=400; + MotorSpeedFast=4000; + MotorSpeedSlow=1000; + SerialComPort=1; + bInterrupt=FALSE; + _bReading=FALSE; + _bZoomHoming=FALSE; + _bZoomMoving=FALSE; + IsMotionFinished=0; + +}; +CAutoZoom::~CAutoZoom(void) +{ + delete m_pSO7_Serial; + m_pSO7_Serial=NULL; +} + +BOOL CAutoZoom::Initialization(bool _bInitConfig) +{ + BOOL status(FALSE); + if (!m_pSO7_Serial) + { + m_pSO7_Serial = new CPSerial(); + } + if (_bInitConfig) + { + LoadConfig(); + } + if (m_pSO7_Serial) + { + m_pSO7_Serial->SetPort(SerialComPort); + if(m_pSO7_Serial->Open()) + { + bEnComm=TRUE; + MaxReadTimes=static_cast(20000/MotorSpeedFast);//每一段倍率最多读取10次2000HZ时 + double dGrade=(ZoomStartPos-ZoomEndPos)/11; + for (INT i=0;i<12;i++) + { + dZoomMultiple[i]=dGrade*i+ZoomEndPos; + } + status=TRUE; + if (_bInitConfig) + { + status=CalibrateEncoder(); + } + } + else + { + status=FALSE; + bEnComm=FALSE; + delete m_pSO7_Serial; + m_pSO7_Serial=NULL; + } + } + return status; +} +//=========================== +void CAutoZoom::LoadConfig() +{ + FILE *hConfigFile = NULL; + char szLine[MAX_BUFF_SIZE]; + char *token = NULL; + char seps[] = " =,\t\n"; + char cTemp[20]={0}; + CString csSO7ConfigFile =GetAppPath()+("\\so7_config.ini"); + _wfopen_s(&hConfigFile,csSO7ConfigFile,_T("rt"));//Unicode + //hConfigFile=fopen(csSO7ConfigFile,_T("rt"));//MultiBytes + if(hConfigFile) + { + while (!feof(hConfigFile)) + { + fgets(szLine,MAX_BUFF_SIZE,hConfigFile);//read a line + if((szLine[0]!=';')&&(strlen(szLine)>2)) + { + token = strtok(szLine,seps); + if(!_stricmp(token,"ZOOM_COM_PORT")) + { + token = strtok( NULL, seps); + if (token) + { + strcpy(cTemp,token); + SerialComPort=atoi(cTemp); + } + } + else if(!_stricmp(token,"ZOOM_START_DEG")) + { + token = strtok( NULL, seps); + if (token) + { + strcpy(cTemp,token); + ZoomStartPos=atof(cTemp); + } + } + else if (!_stricmp(token,"ZOOM_END_DEG")) + { + token = strtok( NULL, seps); + if (token) + { + strcpy(cTemp,token); + ZoomEndPos=atof(cTemp); + } + } + else if (!_stricmp(token,"ZOOM_ORG_DEG")) + { + token = strtok( NULL, seps); + if (token) + { + strcpy(cTemp,token); + ZoomOrgPos=atof(cTemp); + } + } + else if (!_stricmp(token,"ZOOM_DEADBAND_DEG")) + { + token = strtok( NULL, seps); + if (token) + { + strcpy(cTemp,token); + ZoomDeadband=atof(cTemp); + } + } + else if (!_stricmp(token,"ZOOM_PULSE_PER_DEG")) + { + token = strtok( NULL, seps); + if (token) + { + strcpy(cTemp,token); + ZoomScale=atof(cTemp); + } + } + else if (!_stricmp(token,"ZOOM_READING_INTERVAL_TIME")) + { + token = strtok( NULL, seps); + if (token) + { + strcpy(cTemp,token); + iReadingInterval=atoi(cTemp); + } + } + else if (!_stricmp(token,"ZOOM_MOTOR_SPEED_FAST")) + { + token = strtok( NULL, seps); + if (token) + { + strcpy(cTemp,token); + MotorSpeedFast=static_cast(atoi(cTemp)); + } + } + else if (!_stricmp(token,"ZOOM_MOTOR_SPEED_SLOW")) + { + token = strtok( NULL, seps); + if (token) + { + strcpy(cTemp,token); + MotorSpeedSlow=static_cast(atoi(cTemp)); + } + } + + } + } + fclose(hConfigFile); + } +} +double CAutoZoom::ReadZoomAngle() +{ + double dZoomAngle(-1); + if(bEnComm) + { + if (!_bReading) + { + _bReading=TRUE; + char sendData[6]={'A','D',':','0',13,10}; + DWORD iWriteByte=m_pSO7_Serial->Send(sendData,6); + + INT iRetrys(0); + while(!m_pSO7_Serial->m_iRecvState && iRetrys<20 && iWriteByte==6) + { + iRetrys++; + Sleep(10); + } + if (m_pSO7_Serial->m_iRecvState) + { + if(m_pSO7_Serial->m_RecvData[0]==0) + Sleep(40);//200 + + char *token = NULL; + char cTemp[20]={0}; + + char seps[] = "$AD:"; + + token = strtok(m_pSO7_Serial->m_RecvData,seps); + if (token) + { + memcpy(cTemp,token,8); + dZoomAngle=atof(cTemp); + dZoomAngle=dZoomAngle*180/PI; + if (dZoomAngle>ZoomOrgPos) + { + dZoomAngle=dZoomAngle-360; + } + }; + token=NULL; + m_pSO7_Serial->m_iRecvState=FALSE; + _bReading=FALSE; + } + else + { + _bReading=FALSE; + dZoomAngle=-2; + } + } + else + { + dZoomAngle=-2; + } + } + return dZoomAngle; +} + +double CAutoZoom::GetZoomCurPos() +{ + if(bEnComm) + { + //====================AutoZoom============================== + double dZoomPos(0),dZoomAng(0),dZoomPreAng(0); + INT iCnt2(0); + INT iRetryCount(0); + dZoomAng=ReadZoomAngle(); + + while(iCnt2<2 && iRetryCount<10) + { + Sleep(iReadingInterval);//400 + dZoomPreAng=dZoomAng; + dZoomAng=ReadZoomAngle(); + INT iRetrys(0); + while ((dZoomAng==-1 || dZoomAng==-2 || dZoomAng==0 )&&iRetrys<2) + { + iRetrys++; + Sleep(iReadingInterval);//400 + dZoomAng=ReadZoomAngle(); + } + if(dZoomAng!=-1 && dZoomAng!=-2 && dZoomAng!=0) + { + if ((dZoomAng-dZoomPreAng)<1 && (dZoomPreAng-dZoomAng)<1) + { + dZoomPos+=dZoomAng; + iCnt2++; + } + } + iRetryCount++; + + } + if (iCnt2==0) + { + dZoomPos=-2; + } + else + { + dZoomPos=dZoomPos/(iCnt2); + } + + + return dZoomPos; + } + else + { + return 0; + } + +} +BOOL CAutoZoom::CalibrateEncoder() +{ + if (bEnComm) + { + char sendDataI[12]={'U','P',':','1','1','1','1','1','1','1',13,10}; + char sendDataII[6]={'S','C',':','1',13,10}; + char sendDataIII[6]={'R','F',':','1',13,10}; + CString csRightRecv; + csRightRecv.Format(_T("@OK\r\n")); + CStringA m_csRecv(""); + INT iRetrys(0); + DWORD iWriteByte(0); + m_pSO7_Serial->m_iRecvState=FALSE; + + //Phase I + iWriteByte=m_pSO7_Serial->Send(sendDataI,12); + while(!m_pSO7_Serial->m_iRecvState && iRetrys<20 && iWriteByte==12) + { + iRetrys++; + Sleep(50); + } + + if (m_pSO7_Serial->m_iRecvState) + { + if(m_pSO7_Serial->m_RecvData[0]==0) + { + m_pSO7_Serial->m_iRecvState=FALSE; + iRetrys=0; + while(!m_pSO7_Serial->m_iRecvState && iRetrys<30) + { + iRetrys++; + Sleep(50); + } + } + + m_csRecv.Format(("%s"),m_pSO7_Serial->m_RecvData); + if((CString)m_csRecv==csRightRecv) + { + m_pSO7_Serial->m_iRecvState=FALSE; + iRetrys=0; + iWriteByte=0; + //Phase II + + iWriteByte=m_pSO7_Serial->Send(sendDataII,6); + while(!m_pSO7_Serial->m_iRecvState && iRetrys<20 && iWriteByte==6) + { + iRetrys++; + Sleep(50); + } + if(m_pSO7_Serial->m_iRecvState) + { + if(m_pSO7_Serial->m_RecvData[0]==0) + { + m_pSO7_Serial->m_iRecvState=FALSE; + iRetrys=0; + while(!m_pSO7_Serial->m_iRecvState && iRetrys<30) + { + iRetrys++; + Sleep(50); + } + } + + m_csRecv.Format(("%s"),m_pSO7_Serial->m_RecvData); + if((CString)m_csRecv==csRightRecv) + { + m_pSO7_Serial->m_iRecvState=FALSE; + iRetrys=0; + iWriteByte=0; + //Phase III + + iWriteByte=m_pSO7_Serial->Send(sendDataIII,6); + while(!m_pSO7_Serial->m_iRecvState && iRetrys<20 && iWriteByte==6) + { + iRetrys++; + Sleep(50); + } + if(m_pSO7_Serial->m_iRecvState) + { + if(m_pSO7_Serial->m_RecvData[0]==0) + { + m_pSO7_Serial->m_iRecvState=FALSE; + iRetrys=0; + while(!m_pSO7_Serial->m_iRecvState && iRetrys<30) + { + iRetrys++; + Sleep(50); + } + } + + m_csRecv.Format(("%s"),m_pSO7_Serial->m_RecvData); + if((CString)m_csRecv==csRightRecv) + { + m_pSO7_Serial->m_iRecvState=FALSE; + iRetrys=0; + iWriteByte=0; + return TRUE; + } + else + { + m_pSO7_Serial->m_iRecvState=FALSE; + return FALSE; + } + } + else + { + return FALSE; + } + } + else + { + m_pSO7_Serial->m_iRecvState=FALSE; + return FALSE; + } + } + else + { + return FALSE; + } + + } + else + { + m_pSO7_Serial->m_iRecvState=FALSE; + return FALSE; + } + + } + else + { + return FALSE; + } + } + else + { + return FALSE; + } +} \ No newline at end of file diff --git a/PcDmis/Base/Interfac/Msi/Hsi/SevenOcean/CAutoZoom.h b/PcDmis/Base/Interfac/Msi/Hsi/SevenOcean/CAutoZoom.h new file mode 100644 index 0000000..8910876 --- /dev/null +++ b/PcDmis/Base/Interfac/Msi/Hsi/SevenOcean/CAutoZoom.h @@ -0,0 +1,45 @@ +#ifndef CAUTOZOOM_H_ +#define CAUTOZOOM_H_ + +#include "CMMIO_SERIAL.H" + +class CAutoZoom +{ +public: + CAutoZoom(void); + ~CAutoZoom(void); + BOOL Initialization(bool _bInitConfig=true); + void LoadConfig(); + double ReadZoomAngle(); + double GetZoomCurPos(); + BOOL CalibrateEncoder(); + double ZoomOrgPos; + double ZoomStartPos; + double ZoomEndPos; + double ZoomCurPos; + double ZoomDeadband; + double ZoomScale; + int iRetryCntFrom; + int iRetryCntReadV; + int iReadingInterval; + char PreZoomRatio; + BOOL bInterrupt; + BOOL bEnComm; + + BOOL _bZoomHoming; + BOOL _bZoomMoving; + char ZoomMode; + double dZoomMultiple[12]; + char SelectedMultiple; + short MotorSpeedFast;//fast + short MotorSpeedSlow;//slow + int SerialComPort; + int MaxReadTimes; + char IsMotionFinished; +protected: + BOOL _bReading; + +}; + + +#endif \ No newline at end of file