#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;
}