260 lines
8.9 KiB
C++
260 lines
8.9 KiB
C++
#include "rsensortozero.h"
|
|
|
|
RSensorToZero::RSensorToZero()
|
|
{
|
|
|
|
}
|
|
|
|
|
|
void RSensorToZero::setZAAxisSafePos(double value)
|
|
{
|
|
ZAAxisSafePos = value;
|
|
}
|
|
|
|
void RSensorToZero::setZAAxisToSafePosVel(double value)
|
|
{
|
|
ZAAxisToSafePosVel = value;
|
|
}
|
|
void RSensorToZero::setZAAxisToZero1Vel(double value)
|
|
{
|
|
ZAAxisToZero1Vel = value;
|
|
}
|
|
void RSensorToZero::setZAAxisToZero2Vel(double value)
|
|
{
|
|
ZAAxisToZero2Vel = value;
|
|
}
|
|
void RSensorToZero::setZAAxisToZero3Vel(double value)
|
|
{
|
|
ZAAxisToZero3Vel = value;
|
|
}
|
|
|
|
|
|
//QString RSensorToZero::getCode()
|
|
//{
|
|
// QString code;
|
|
// code+=
|
|
// QString("VEL(Z) = 2\n")+
|
|
// QString("ACC(Z) = 20\n")+
|
|
// QString("DEC(Z) = 20\n")+
|
|
// QString("JERK(Z) = 200\n")+
|
|
// QString("KDEC(Z) = 200\n")+
|
|
// QString("PTP/e Z, 0\n")+
|
|
// "IF RangeV<-1.5\n"+
|
|
// QString("VEL(ZA) = %1\n").arg(ZAAxisToSafePosVel)+
|
|
// QString("ACC(ZA) = %1\n").arg(ZAAxisToSafePosVel*10)+
|
|
// QString("DEC(ZA) = %1\n").arg(ZAAxisToSafePosVel*10)+
|
|
// QString("JERK(ZA) = %1\n").arg(ZAAxisToSafePosVel*100)+
|
|
// QString("KDEC(ZA) = %1\n").arg(ZAAxisToSafePosVel*100)+
|
|
// QString("PTP/e ZA, %1\n").arg(ZAAxisSafePos)+
|
|
// QString("VEL(ZA) = %1\n").arg(ZAAxisToZero1Vel)+
|
|
// QString("ACC(ZA) = %1\n").arg(ZAAxisToZero1Vel*10)+
|
|
// QString("DEC(ZA) = %1\n").arg(ZAAxisToZero1Vel*10)+
|
|
// QString("JERK(ZA) = %1\n").arg(ZAAxisToZero1Vel*100)+
|
|
// QString("KDEC(ZA) = %1\n").arg(ZAAxisToZero1Vel*100)+
|
|
// "PTP ZA, -30\n"
|
|
// "TILL ABS(RangeV) < 0.1\n"
|
|
// "HALT ZA\n"
|
|
// "WAIT 1000\n"+
|
|
// QString("VEL(ZA) = %1\n").arg(ZAAxisToZero2Vel)+
|
|
// QString("ACC(ZA) = %1\n").arg(ZAAxisToZero2Vel*10)+
|
|
// QString("DEC(ZA) = %1\n").arg(ZAAxisToZero2Vel*10)+
|
|
// QString("JERK(ZA) = %1\n").arg(ZAAxisToZero2Vel*100)+
|
|
// QString("KDEC(ZA) = %1\n").arg(ZAAxisToZero2Vel*100)+
|
|
// "PTP/er ZA, RangeV\n"
|
|
// "WAIT 1000\n"
|
|
// "ELSE\n"+
|
|
// QString("VEL(ZA) = %1\n").arg(ZAAxisToZero1Vel)+
|
|
// QString("ACC(ZA) = %1\n").arg(ZAAxisToZero1Vel*10)+
|
|
// QString("DEC(ZA) = %1\n").arg(ZAAxisToZero1Vel*10)+
|
|
// QString("JERK(ZA) = %1\n").arg(ZAAxisToZero1Vel*100)+
|
|
// QString("KDEC(ZA) = %1\n").arg(ZAAxisToZero1Vel*100)+
|
|
// "PTP/ER ZA,RangeV\n"
|
|
// "WAIT 1800\n"+
|
|
// QString("VEL(ZA) = %1\n").arg(ZAAxisToZero2Vel)+
|
|
// QString("ACC(ZA) = %1\n").arg(ZAAxisToZero2Vel*10)+
|
|
// QString("DEC(ZA) = %1\n").arg(ZAAxisToZero2Vel*10)+
|
|
// QString("JERK(ZA) = %1\n").arg(ZAAxisToZero2Vel*100)+
|
|
// QString("KDEC(ZA) = %1\n").arg(ZAAxisToZero2Vel*100)+
|
|
// "PTP/ER ZA,RangeV\n"
|
|
// "WAIT 600\n"
|
|
// "END\n"
|
|
// "IF ABS(RangeV) >= 0.0003\n"+
|
|
// QString("VEL(ZA) = %1\n").arg(ZAAxisToZero3Vel)+
|
|
// QString("ACC(ZA) = %1\n").arg(ZAAxisToZero3Vel*10)+
|
|
// QString("DEC(ZA) = %1\n").arg(ZAAxisToZero3Vel*10)+
|
|
// QString("JERK(ZA) = %1\n").arg(ZAAxisToZero3Vel*100)+
|
|
// QString("KDEC(ZA) = %1\n").arg(ZAAxisToZero3Vel*100)+
|
|
// "PTP/er ZA, RangeV\n"
|
|
// "END\n"
|
|
// "runF = 1\n"
|
|
// "STOP";
|
|
// return code;
|
|
//}
|
|
|
|
|
|
|
|
|
|
QString RSensorToZero::getCode()
|
|
{
|
|
QString strDoOpen,strDiSts1,strDoClose,strDiSts0;
|
|
if (CuCeGaoDiNum < 8)
|
|
strDiSts1 = QString("DI1.%1=1\n").arg(CuCeGaoDiNum);
|
|
else
|
|
strDiSts1 = QString("DI2.%1=1\n").arg(CuCeGaoDiNum-8);
|
|
if (CuCeGaoDiNum < 8)
|
|
strDiSts0 = QString("DI1.%1=0\n").arg(CuCeGaoDiNum);
|
|
else
|
|
strDiSts0 = QString("DI2.%1=0\n").arg(CuCeGaoDiNum-8);
|
|
|
|
if (CuCeGaoDoNum < 8)
|
|
strDoOpen = QString("DO1.%1=1\n").arg(CuCeGaoDoNum);
|
|
else
|
|
strDoOpen = QString("DO2.%1=1\n").arg(CuCeGaoDoNum-8);
|
|
if (CuCeGaoDoNum < 8)
|
|
strDoClose = QString("DO1.%1=0\n").arg(CuCeGaoDoNum);
|
|
else
|
|
strDoClose = QString("DO2.%1=0\n").arg(CuCeGaoDoNum-8);
|
|
|
|
QString code;
|
|
code+=
|
|
QString("GLOBAL INT DuiLingF=-1\n")+ //粗测高标志位
|
|
QString("VEL(Z) = 2\n")+
|
|
QString("ACC(Z) = 20\n")+
|
|
QString("DEC(Z) = 20\n")+
|
|
QString("JERK(Z) = 200\n")+
|
|
QString("KDEC(Z) = 200\n")+
|
|
QString("PTP/e Z, 0\n")+
|
|
QString("VEL(Z0) = 5\n")+
|
|
QString("ACC(Z0) = 50\n")+
|
|
QString("DEC(Z0) = 50\n")+
|
|
QString("JERK(Z0) = 500\n")+
|
|
QString("KDEC(Z0) = 500\n")+
|
|
QString("PTP/e Z0, 0\n")+
|
|
strDoClose+
|
|
QString("TILL %1\n").arg(strDiSts1)+
|
|
"IF RangeV<-1.5\n"+
|
|
QString("VEL(ZA) = %1\n").arg(ZAAxisToSafePosVel)+
|
|
QString("ACC(ZA) = %1\n").arg(ZAAxisToSafePosVel*10)+
|
|
QString("DEC(ZA) = %1\n").arg(ZAAxisToSafePosVel*10)+
|
|
QString("JERK(ZA) = %1\n").arg(ZAAxisToSafePosVel*100)+
|
|
QString("KDEC(ZA) = %1\n").arg(ZAAxisToSafePosVel*100)+
|
|
QString("PTP/e ZA, %1\n").arg(ZAAxisSafePos+1)+
|
|
QString("VEL(ZA) = %1\n").arg(ZAAxisToZero1Vel)+
|
|
QString("ACC(ZA) = %1\n").arg(ZAAxisToZero1Vel*10)+
|
|
QString("DEC(ZA) = %1\n").arg(ZAAxisToZero1Vel*10)+
|
|
QString("JERK(ZA) = %1\n").arg(ZAAxisToZero1Vel*100)+
|
|
QString("KDEC(ZA) = %1\n").arg(ZAAxisToZero1Vel*100)+
|
|
"PTP ZA, -30\n"
|
|
"TILL ABS(RangeV) < 0.5 | "+
|
|
strDiSts0+
|
|
"HALT ZA\n"
|
|
"WAIT 1000\n"
|
|
"if RangeV < -10\n"
|
|
"DuiLingF=0\n"
|
|
"goto duiling\n"
|
|
"end\n"+
|
|
QString("VEL(ZA) = %1\n").arg(ZAAxisToZero2Vel)+
|
|
QString("ACC(ZA) = %1\n").arg(ZAAxisToZero2Vel*10)+
|
|
QString("DEC(ZA) = %1\n").arg(ZAAxisToZero2Vel*10)+
|
|
QString("JERK(ZA) = %1\n").arg(ZAAxisToZero2Vel*100)+
|
|
QString("KDEC(ZA) = %1\n").arg(ZAAxisToZero2Vel*100)+
|
|
"PTP/er ZA, RangeV\n"
|
|
"WAIT 1000\n"
|
|
"ELSE\n"+
|
|
QString("VEL(ZA) = %1\n").arg(ZAAxisToZero1Vel)+
|
|
QString("ACC(ZA) = %1\n").arg(ZAAxisToZero1Vel*10)+
|
|
QString("DEC(ZA) = %1\n").arg(ZAAxisToZero1Vel*10)+
|
|
QString("JERK(ZA) = %1\n").arg(ZAAxisToZero1Vel*100)+
|
|
QString("KDEC(ZA) = %1\n").arg(ZAAxisToZero1Vel*100)+
|
|
"PTP/ER ZA,RangeV\n"
|
|
"WAIT 1800\n"+
|
|
QString("VEL(ZA) = %1\n").arg(ZAAxisToZero2Vel)+
|
|
QString("ACC(ZA) = %1\n").arg(ZAAxisToZero2Vel*10)+
|
|
QString("DEC(ZA) = %1\n").arg(ZAAxisToZero2Vel*10)+
|
|
QString("JERK(ZA) = %1\n").arg(ZAAxisToZero2Vel*100)+
|
|
QString("KDEC(ZA) = %1\n").arg(ZAAxisToZero2Vel*100)+
|
|
"PTP/ER ZA,RangeV\n"
|
|
"WAIT 600\n"
|
|
"END\n"
|
|
"IF ABS(RangeV) >= 0.0003\n"+
|
|
QString("VEL(ZA) = %1\n").arg(ZAAxisToZero3Vel)+
|
|
QString("ACC(ZA) = %1\n").arg(ZAAxisToZero3Vel*10)+
|
|
QString("DEC(ZA) = %1\n").arg(ZAAxisToZero3Vel*10)+
|
|
QString("JERK(ZA) = %1\n").arg(ZAAxisToZero3Vel*100)+
|
|
QString("KDEC(ZA) = %1\n").arg(ZAAxisToZero3Vel*100)+
|
|
"PTP/er ZA, RangeV\n"
|
|
"END\n"
|
|
"DuiLingF=1\n"
|
|
"duiling:\n"+
|
|
strDoOpen+
|
|
"runF = 1\n"
|
|
"STOP";
|
|
return code;
|
|
}
|
|
QString RSensorToZero::getCode_Z0()
|
|
{
|
|
/*
|
|
"ECIN(RangeSAddr_Z0, RangeVInt_Z0)\n"
|
|
"RangeV_Z0=RangeVInt_Z0/10000\n"
|
|
*/
|
|
QString code;
|
|
code+=
|
|
QString("VEL(Z0) = 2\n")+
|
|
QString("ACC(Z0) = 20\n")+
|
|
QString("DEC(Z0) = 20\n")+
|
|
QString("JERK(Z0) = 200\n")+
|
|
QString("KDEC(Z0) = 200\n")+
|
|
QString("PTP/e Z0, 0\n")+
|
|
"WAIT 4000\n"
|
|
"IF (RangeV_Z0-Z0_value)<-1.5\n"+
|
|
QString("VEL(Z0) = %1\n").arg(ZAAxisToSafePosVel)+
|
|
QString("ACC(Z0) = %1\n").arg(ZAAxisToSafePosVel*10)+
|
|
QString("DEC(Z0) = %1\n").arg(ZAAxisToSafePosVel*10)+
|
|
QString("JERK(Z0) = %1\n").arg(ZAAxisToSafePosVel*100)+
|
|
QString("KDEC(Z0) = %1\n").arg(ZAAxisToSafePosVel*100)+
|
|
QString("PTP/e Z0, %1\n").arg(ZAAxisSafePos)+
|
|
QString("VEL(Z0) = %1\n").arg(ZAAxisToZero1Vel)+
|
|
QString("ACC(Z0) = %1\n").arg(ZAAxisToZero1Vel*10)+
|
|
QString("DEC(Z0) = %1\n").arg(ZAAxisToZero1Vel*10)+
|
|
QString("JERK(Z0) = %1\n").arg(ZAAxisToZero1Vel*100)+
|
|
QString("KDEC(Z0) = %1\n").arg(ZAAxisToZero1Vel*100)+
|
|
"PTP Z0, -30\n"
|
|
"TILL ABS(RangeV_Z0-Z0_value) < 0.1\n"
|
|
"HALT Z0\n"
|
|
"WAIT 4000\n"+
|
|
QString("VEL(Z0) = %1\n").arg(ZAAxisToZero2Vel)+
|
|
QString("ACC(Z0) = %1\n").arg(ZAAxisToZero2Vel*10)+
|
|
QString("DEC(Z0) = %1\n").arg(ZAAxisToZero2Vel*10)+
|
|
QString("JERK(Z0) = %1\n").arg(ZAAxisToZero2Vel*100)+
|
|
QString("KDEC(Z0) = %1\n").arg(ZAAxisToZero2Vel*100)+
|
|
"PTP/er Z0, RangeV_Z0-Z0_value\n"
|
|
"WAIT 4000\n"
|
|
"ELSE\n"+
|
|
QString("VEL(Z0) = %1\n").arg(ZAAxisToZero1Vel)+
|
|
QString("ACC(Z0) = %1\n").arg(ZAAxisToZero1Vel*10)+
|
|
QString("DEC(Z0) = %1\n").arg(ZAAxisToZero1Vel*10)+
|
|
QString("JERK(Z0) = %1\n").arg(ZAAxisToZero1Vel*100)+
|
|
QString("KDEC(Z0) = %1\n").arg(ZAAxisToZero1Vel*100)+
|
|
"PTP/ER Z0,RangeV_Z0-Z0_value\n"
|
|
"WAIT 4000\n"+
|
|
QString("VEL(Z0) = %1\n").arg(ZAAxisToZero2Vel)+
|
|
QString("ACC(Z0) = %1\n").arg(ZAAxisToZero2Vel*10)+
|
|
QString("DEC(Z0) = %1\n").arg(ZAAxisToZero2Vel*10)+
|
|
QString("JERK(Z0) = %1\n").arg(ZAAxisToZero2Vel*100)+
|
|
QString("KDEC(Z0) = %1\n").arg(ZAAxisToZero2Vel*100)+
|
|
"PTP/ER Z0,RangeV_Z0-Z0_value\n"
|
|
"WAIT 4000\n"
|
|
"END\n"
|
|
"IF ABS(RangeV_Z0-Z0_value) >= 0.0003\n"+
|
|
QString("VEL(Z0) = %1\n").arg(ZAAxisToZero3Vel)+
|
|
QString("ACC(Z0) = %1\n").arg(ZAAxisToZero3Vel*10)+
|
|
QString("DEC(Z0) = %1\n").arg(ZAAxisToZero3Vel*10)+
|
|
QString("JERK(Z0) = %1\n").arg(ZAAxisToZero3Vel*100)+
|
|
QString("KDEC(Z0) = %1\n").arg(ZAAxisToZero3Vel*100)+
|
|
"PTP/er Z0, RangeV_Z0-Z0_value\n"
|
|
"END\n"
|
|
"runF = 1\n"
|
|
"STOP";
|
|
return code;
|
|
}
|