You cannot select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
487 lines
14 KiB
C++
487 lines
14 KiB
C++
#include "StdAfx.h"
|
|
#include "GratingRuler.h"
|
|
#include "Propertie.h"
|
|
#include "PropertieMgr.h"
|
|
#include "CommPortMgr.h"
|
|
#include "LogMgr.h"
|
|
#include "BitOperation.h"
|
|
#include "AuthorityMgr.h"
|
|
|
|
|
|
#define WE6800_RETURN_BTYE_CNT 17//WE6800 数显表串口协议返回的字节数
|
|
|
|
CGratingRuler *gGratingRuler = new CGratingRuler;
|
|
CGratingRuler::CGratingRuler(void)
|
|
{
|
|
m_bIni = false;//光栅尺是否初始化
|
|
m_bUseGratingRuler = false;//是否使用光栅尺
|
|
m_CommPort = -1;//串口编号
|
|
m_OriginCalibrationError = 0.02;//原点校准误差(mm)
|
|
m_MaxCalibrationTimes = 10;//最大校准次数，避免死循环
|
|
m_MeasureErrorGap = 1;//校准平台误差的间隔(mm)(越小分的越细)
|
|
m_MaxOriginError = 10;//回原点时平台原点和光栅尺原点最大允许的误差(mm)
|
|
}
|
|
CGratingRuler::~CGratingRuler(void)
|
|
{
|
|
}
|
|
#if 1
|
|
CMFCPropertyGridProperty *CGratingRuler::CreatGridProperty()
|
|
{
|
|
CString PropertyName;//属性名称
|
|
CString Description;//描述
|
|
CString Path = _T("GratingRuler");;//存储路径
|
|
CString Name;
|
|
//-------------------------------------------------------------------------------//
|
|
PropertyName = _T("光栅尺");
|
|
CMFCPropertyGridProperty* pGroup = new CMFCPropertyGridProperty(PropertyName);
|
|
//-------------------------------------------------------------------------------//
|
|
if(gAuthorityMgr->CheckAuthority(_FACTORY))
|
|
{
|
|
//添加属性变量映射
|
|
Name = _T("m_CommPort");//变量名字
|
|
CPropertie *pPropertie = new CPropertie;
|
|
pPropertie->SetpVal((void*)&m_CommPort);
|
|
pPropertie->SetType(_PROP_TYPE_INT);
|
|
pPropertie->SetpModule(this);
|
|
pPropertie->SetPath(Path);
|
|
pPropertie->SetName(Name);
|
|
pPropertie->WriteRead(true);//读取保存的属性
|
|
|
|
//添加属性显示
|
|
PropertyName = _T("串口号");
|
|
Description = _T("光栅尺和PC 通讯的串口号(0~16)");
|
|
CMFCPropertyGridProperty* p1 = new CMFCPropertyGridProperty(PropertyName, (_variant_t)m_CommPort, Description);
|
|
pGroup->AddSubItem(p1);
|
|
gDevicePropertieMgr.Insert(p1, pPropertie);
|
|
}
|
|
if(gAuthorityMgr->CheckAuthority(_FACTORY))
|
|
{
|
|
//添加属性变量映射
|
|
Name = _T("m_bUseGratingRuler");//变量名字
|
|
CPropertie *pPropertie = new CPropertie;
|
|
pPropertie->SetpVal((void*)&m_bUseGratingRuler);
|
|
pPropertie->SetType(_PROP_TYPE_BOOL);
|
|
pPropertie->SetpModule(this);
|
|
pPropertie->SetPath(Path);
|
|
pPropertie->SetName(Name);
|
|
pPropertie->WriteRead(true);//读取保存的属性
|
|
|
|
//添加属性显示
|
|
PropertyName = _T("光栅尺");
|
|
Description = _T("光栅尺校准开关");
|
|
CMFCPropertyGridProperty* p1 = new CMFCPropertyGridProperty(PropertyName, (_variant_t)m_bUseGratingRuler, Description);
|
|
pGroup->AddSubItem(p1);
|
|
gDevicePropertieMgr.Insert(p1, pPropertie);
|
|
}
|
|
if(gAuthorityMgr->CheckAuthority(_FACTORY))
|
|
{
|
|
//添加属性变量映射
|
|
Name = _T("m_OriginCalibrationError");//变量名字
|
|
CPropertie *pPropertie = new CPropertie;
|
|
pPropertie->SetpVal((void*)&m_OriginCalibrationError);
|
|
pPropertie->SetType(_PROP_TYPE_DOUBLE);
|
|
pPropertie->SetpModule(this);
|
|
pPropertie->SetPath(Path);
|
|
pPropertie->SetName(Name);
|
|
pPropertie->WriteRead(true);//读取保存的属性
|
|
|
|
//添加属性显示
|
|
PropertyName = _T("原点校准误差");
|
|
Description = _T("平台回原点时误差大于这个值需要补偿脉冲(单位: mm)");
|
|
CMFCPropertyGridProperty* p1 = new CMFCPropertyGridProperty(PropertyName, (_variant_t)m_OriginCalibrationError, Description);
|
|
pGroup->AddSubItem(p1);
|
|
gDevicePropertieMgr.Insert(p1, pPropertie);
|
|
}
|
|
if(gAuthorityMgr->CheckAuthority(_FACTORY))
|
|
{
|
|
//添加属性变量映射
|
|
Name = _T("m_MaxCalibrationTimes");//变量名字
|
|
CPropertie *pPropertie = new CPropertie;
|
|
pPropertie->SetpVal((void*)&m_MaxCalibrationTimes);
|
|
pPropertie->SetType(_PROP_TYPE_INT);
|
|
pPropertie->SetpModule(this);
|
|
pPropertie->SetPath(Path);
|
|
pPropertie->SetName(Name);
|
|
pPropertie->WriteRead(true);//读取保存的属性
|
|
|
|
//添加属性显示
|
|
PropertyName = _T("最大校准次数");
|
|
Description = _T("平台回原点时误差最大校准次数,避免死循环");
|
|
CMFCPropertyGridProperty* p1 = new CMFCPropertyGridProperty(PropertyName, (_variant_t)m_MaxCalibrationTimes, Description);
|
|
pGroup->AddSubItem(p1);
|
|
gDevicePropertieMgr.Insert(p1, pPropertie);
|
|
}
|
|
if(gAuthorityMgr->CheckAuthority(_FACTORY))
|
|
{
|
|
//添加属性变量映射
|
|
Name = _T("m_MeasureErrorGap");//变量名字
|
|
CPropertie *pPropertie = new CPropertie;
|
|
pPropertie->SetpVal((void*)&m_MeasureErrorGap);
|
|
pPropertie->SetType(_PROP_TYPE_DOUBLE);
|
|
pPropertie->SetpModule(this);
|
|
pPropertie->SetPath(Path);
|
|
pPropertie->SetName(Name);
|
|
pPropertie->WriteRead(true);//读取保存的属性
|
|
|
|
//添加属性显示
|
|
PropertyName = _T("测量间隔");
|
|
Description = _T("校准平台误差的间隔(单位: mm)");
|
|
CMFCPropertyGridProperty* p1 = new CMFCPropertyGridProperty(PropertyName, (_variant_t)m_MeasureErrorGap, Description);
|
|
pGroup->AddSubItem(p1);
|
|
gDevicePropertieMgr.Insert(p1, pPropertie);
|
|
}
|
|
//-------------------------------------------------------------------------------//
|
|
return pGroup;
|
|
}
|
|
void CGratingRuler::ExportPar(ofstream *pFile)
|
|
{
|
|
(*pFile)<<"[模块] [CGratingRuler]------------------------------------------------"<<endl;
|
|
(*pFile)<<"[光栅尺][m_bUseGratingRuler] = "<<m_bUseGratingRuler<<endl;
|
|
(*pFile)<<"[串口号][m_CommPort] = "<<m_CommPort<<endl;
|
|
(*pFile)<<"[原点校准误差][m_OriginCalibrationError] = "<<m_OriginCalibrationError<<endl;
|
|
(*pFile)<<"[最大校准次数][m_MaxCalibrationTimes] = "<<m_MaxCalibrationTimes<<endl;
|
|
(*pFile)<<"[测量间隔][m_MeasureErrorGap] = "<<m_MeasureErrorGap<<endl;
|
|
(*pFile)<<"[原点最大允许误差][m_MaxOriginError] = "<<m_MaxOriginError<<endl;
|
|
}
|
|
void CGratingRuler::ExportErrorTable(ofstream *pFile,vector<TypePosError> &ErrorTable)
|
|
{
|
|
|
|
}
|
|
#endif
|
|
|
|
#if 1//读取光栅尺
|
|
//读取光栅尺
|
|
bool CGratingRuler::ReadGratingRuler(Dbxy &pt)
|
|
{
|
|
CCommPortMgr com;
|
|
//打开串口
|
|
if(com.Open(m_CommPort)==false)
|
|
{
|
|
return false;
|
|
}
|
|
int DelayTime = 100;
|
|
|
|
CString str1 = "R";
|
|
CString str = str1+char(13)+char(10);
|
|
com.Write(str);
|
|
Sleep(DelayTime);
|
|
|
|
//读取串口反馈
|
|
char Buffer[WE6800_RETURN_BTYE_CNT];
|
|
int len = com.Read(Buffer,WE6800_RETURN_BTYE_CNT);
|
|
if(len<WE6800_RETURN_BTYE_CNT)
|
|
{
|
|
com.Close();
|
|
return false;
|
|
}
|
|
for(int i=0;i<WE6800_RETURN_BTYE_CNT;i++)
|
|
{
|
|
}
|
|
//解析坐标值
|
|
if(AnalysisRulerRet(Buffer,pt)==false)
|
|
{
|
|
com.Close();
|
|
return false;
|
|
}
|
|
|
|
com.Close();
|
|
return true;
|
|
}
|
|
//解析光栅尺反馈结果
|
|
bool CGratingRuler::AnalysisRulerRet(char *Buffer,Dbxy &pt)
|
|
{
|
|
bool bIsNegativeX = false;
|
|
bool bIsNegativeY = false;
|
|
|
|
//解析符号位-------------------------------------------
|
|
if(AnalysisSymbol(Buffer[1],bIsNegativeX,bIsNegativeY)==false)
|
|
{
|
|
return false;
|
|
}
|
|
//解析XY 轴的状态
|
|
if(AnalysisXYState(Buffer[2])==false)
|
|
{
|
|
return false;
|
|
}
|
|
//解析X 轴坐标值
|
|
pt.x = AnalysisCoord(Buffer[3],Buffer[4],Buffer[5],Buffer[6]);
|
|
//解析Y 轴坐标值
|
|
pt.y = AnalysisCoord(Buffer[7],Buffer[8],Buffer[9],Buffer[10]);
|
|
|
|
//处理符号
|
|
if(bIsNegativeX)
|
|
{
|
|
pt.x *= -1;
|
|
}
|
|
if(bIsNegativeY)
|
|
{
|
|
pt.y *= -1;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
//解析符号位
|
|
bool CGratingRuler::AnalysisSymbol(BYTE byte,bool &bIsNegativeX,bool &bIsNegativeY)
|
|
{
|
|
if(IsBitOn(byte,4))//判断单位0 是毫米1 是英尺
|
|
{
|
|
CString str = "[error] [数显表单位设置为英尺]";
|
|
gLogMgr->WriteDebugLog(str);
|
|
return false;
|
|
}
|
|
if(IsBitOn(byte,0))//判断x 轴的符号
|
|
{
|
|
bIsNegativeX = true;
|
|
}
|
|
if(IsBitOn(byte,1))//判断Y 轴的符号
|
|
{
|
|
bIsNegativeY = true;
|
|
}
|
|
return true;
|
|
}
|
|
//解析XY 轴的状态
|
|
bool CGratingRuler::AnalysisXYState(BYTE byte)
|
|
{
|
|
if(IsBitOn(byte,0))
|
|
{
|
|
CString str = "[error] [X 轴光栅尺异常]";
|
|
gLogMgr->WriteDebugLog(str);
|
|
return false;
|
|
}
|
|
if(IsBitOn(byte,1))
|
|
{
|
|
CString str = "[error] [Y 轴光栅尺异常]";
|
|
gLogMgr->WriteDebugLog(str);
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
//解析坐标值(压缩BCD 码格式)
|
|
double CGratingRuler::AnalysisCoord(BYTE byte1,BYTE byte2,BYTE byte3,BYTE byte4)
|
|
{
|
|
double coord = 0;
|
|
double coefficient = 0.0001;//这个系数是根据光栅尺的精度来取的
|
|
double Increment = 10;//每次增加的倍数
|
|
//千分位,百分位
|
|
{
|
|
double high = 0;
|
|
double low = 0;
|
|
GetTwoNum(byte1,high,low);
|
|
coord += low*coefficient;
|
|
coefficient *= Increment;
|
|
coord += high*coefficient;
|
|
coefficient *= Increment;
|
|
}
|
|
//十分位,个位
|
|
{
|
|
double high = 0;
|
|
double low = 0;
|
|
GetTwoNum(byte2,high,low);
|
|
coord += low*coefficient;
|
|
coefficient *= Increment;
|
|
coord += high*coefficient;
|
|
coefficient *= Increment;
|
|
}
|
|
//十位，百位
|
|
{
|
|
double high = 0;
|
|
double low = 0;
|
|
GetTwoNum(byte3,high,low);
|
|
coord += low*coefficient;
|
|
coefficient *= Increment;
|
|
coord += high*coefficient;
|
|
coefficient *= Increment;
|
|
}
|
|
//千位，万位
|
|
{
|
|
double high = 0;
|
|
double low = 0;
|
|
GetTwoNum(byte4,high,low);
|
|
coord += low*coefficient;
|
|
coefficient *= Increment;
|
|
coord += high*coefficient;
|
|
}
|
|
return coord;
|
|
}
|
|
//获取byte 低四位和高四位的数值
|
|
void CGratingRuler::GetTwoNum(BYTE byte,double &high,double &low)
|
|
{
|
|
//低四位
|
|
{
|
|
BYTE byte1 = byte<<4;//左边清0
|
|
int tmp = (int)(byte1>>4);//先转化为整型
|
|
low = tmp;
|
|
}
|
|
//高四位
|
|
{
|
|
BYTE byte1 = byte>>4;//右边清0
|
|
int tmp = (int)(byte1);//先转化为整型
|
|
high = tmp;
|
|
}
|
|
}
|
|
#endif
|
|
#if 1//坐标相关
|
|
//设置当前位置为光栅尺的原点
|
|
bool CGratingRuler::SetOriginCoord()
|
|
{
|
|
Dbxy pt;
|
|
if(ReadGratingRuler(pt)==false)//读取当前值
|
|
{
|
|
return false;
|
|
}
|
|
m_bIni = true;//初始化成功
|
|
m_OriginCoord = pt;//记录当前的读数
|
|
return true;
|
|
}
|
|
//获取光栅尺当前的坐标
|
|
bool CGratingRuler::GetCurCoord(Dbxy &pt)
|
|
{
|
|
if(!m_bIni)
|
|
{
|
|
return false;
|
|
}
|
|
Dbxy ReadPt;
|
|
if(ReadGratingRuler(ReadPt)==false)
|
|
{
|
|
return false;
|
|
}
|
|
//转换为相对坐标
|
|
pt.x = ReadPt.x - m_OriginCoord.x;
|
|
pt.y = ReadPt.y - m_OriginCoord.y;
|
|
|
|
return true;
|
|
}
|
|
#endif
|
|
#if 1//平台操作
|
|
//平台原点校准(平台此时理论坐标已经回原点，再通过光栅尺来校准其位置)
|
|
bool CGratingRuler::PlatformOriginCalibration(CPlatformXY &PlatformXY)
|
|
{
|
|
if(IsUsed()==false)//没有使用光栅尺，则认为已经校准
|
|
{
|
|
return true;
|
|
}
|
|
if(!m_bIni)
|
|
{
|
|
return false;
|
|
}
|
|
int i=0;
|
|
for(;i<m_MaxCalibrationTimes;i++)
|
|
{
|
|
//获取光栅尺当前的坐标
|
|
Dbxy pt;
|
|
if(GetCurCoord(pt)==false)
|
|
{
|
|
return false;
|
|
}
|
|
if(pt.x>m_MaxOriginError || pt.y>m_MaxOriginError)
|
|
{
|
|
//异常情况，误差大于正常值
|
|
return false;
|
|
}
|
|
//XY 轴的误差都小于m_OriginCalibrationError 时，结束校准
|
|
if(abs(pt.x)<m_OriginCalibrationError && abs(pt.y)<m_OriginCalibrationError)
|
|
{
|
|
break;
|
|
}
|
|
//否则补偿(脉冲变化不计入理论坐标值,这个时候理论坐标是零)
|
|
if(abs(pt.x)>m_OriginCalibrationError)
|
|
{
|
|
if(pt.x>0)
|
|
{
|
|
PlatformXY.MoveDistance(pt.x,_DIR_L,false);
|
|
}
|
|
else
|
|
{
|
|
PlatformXY.MoveDistance(pt.x,_DIR_R,false);
|
|
}
|
|
}
|
|
if(abs(pt.y)>m_OriginCalibrationError)
|
|
{
|
|
if(pt.y>0)
|
|
{
|
|
PlatformXY.MoveDistance(pt.x,_DIR_D,false);
|
|
}
|
|
else
|
|
{
|
|
PlatformXY.MoveDistance(pt.x,_DIR_U,false);
|
|
}
|
|
}
|
|
}
|
|
if(i==m_MaxCalibrationTimes)
|
|
{
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
//测量平台误差
|
|
//rect 是需要校准的范围(相对于平台理论坐标)
|
|
//PlatformXY 是要校准的平台
|
|
bool CGratingRuler::MeasurePlatfromError(DbRect rect,CPlatformXY &PlatformXY)
|
|
{
|
|
if(IsUsed()==false)
|
|
return true;
|
|
if(!m_bIni)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
//x 轴
|
|
if(MeasureMotorError(PlatformXY,_X,rect.L,rect.R,m_ErrorTableX)==false)
|
|
{
|
|
return false;
|
|
}
|
|
//y 轴
|
|
if(MeasureMotorError(PlatformXY,_Y,rect.B,rect.T,m_ErrorTableY)==false)
|
|
{
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
//测量马达的误差(Start,End 是测量范围)
|
|
bool CGratingRuler::MeasureMotorError(CPlatformXY &PlatformXY,X_OR_Y xy,double Start,double End,vector<TypePosError> &ErrorTable)
|
|
{
|
|
ErrorTable.clear();
|
|
|
|
Dbxy CurPt = PlatformXY.GetCoord();//当前平台坐标,应该是0,0
|
|
Dbxy OriginPt;//原点
|
|
|
|
double CurPos = Start;
|
|
while(CurPos <= End)
|
|
{
|
|
Dbxy MovePt;//当前测量点(平台理论坐标)
|
|
if(xy == _X)
|
|
{
|
|
MovePt.x = CurPos;
|
|
MovePt.y = CurPt.y;
|
|
}
|
|
else
|
|
{
|
|
MovePt.x = CurPt.x;
|
|
MovePt.y = CurPos;
|
|
}
|
|
PlatformXY.Move(false,MovePt,OriginPt);//移动
|
|
//读取光栅尺当前坐标
|
|
Dbxy RulerCoord;//光栅尺实际坐标
|
|
if(GetCurCoord(RulerCoord)==false)
|
|
{
|
|
return false;
|
|
}
|
|
//记录当前误差(实际走位的是否理论值减掉这个误差即可)
|
|
double Error;
|
|
if(xy == _X)
|
|
{
|
|
Error = RulerCoord.x - CurPos;
|
|
}
|
|
else
|
|
{
|
|
Error = RulerCoord.y - CurPos;
|
|
}
|
|
|
|
TypePosError PosError;
|
|
PosError.first = CurPos;
|
|
PosError.second = Error;
|
|
ErrorTable.push_back(PosError);
|
|
}
|
|
return true;
|
|
}
|
|
#endif |