TouchProbe complete code

#include "RT_Project_01.h"
#include "TouchProbe.h"

VOID RecordPosition(int Index)
{
    RtPrintf("Use TouchProbe to record a position.\n\n");

    double PosRecord = 0;

    McProbeTrigger ProbeTrigger = { 0 };

    ProbeTrigger.TouchProbeId = TOUCH_PROBE_ID;     //Use TouchProbe Zero.
    ProbeTrigger.Software = TOUCH_PROBE_SOFTWARE;   //FALSE, use servo drive's touch probe.
    ProbeTrigger.Edge = TOUCH_PROBE_EDGE;           //TRUE, rising edge.
    ProbeTrigger.IndexPulse = TOUCH_PROBE_INDEXPULSE;   //TRUE, use a Z signal to trigger.
    ProbeTrigger.AxisSwitch = TOUCH_PROBE_AXIS; //FALSE, I/O module.
    ProbeTrigger.Index = TOUCH_PROBE_INDEX;     //Zero, the index of an axis or I/O module.
    ProbeTrigger.Offset = TOUCH_PROBE_OFFSET;   //Zero, the offset for an axis or I/O module.

    /*-----Record the position in the first rotation-----*/

    KsCommandStatus probe = SetAxisTouchProbe(Index, ProbeTrigger, FALSE, 0, 0, &PosRecord);
    if (probe.Error)
        RtPrintf("SetAxisTouchProbe failed: %d\n", probe.ErrorId);

    RtPrintf("Make a velocity move.\n\n");

    //Start a velocity move.
    KsCommandStatus move = WaitForCommand(10, FALSE, MoveAxisVelocity(Index, 1000, 10000,
        10000, 1000000, mcNegativeDirection, mcAborting));
    if (move.Error)
        RtPrintf("MoveAxisVelocity failed: %d\n", move.ErrorId);

    //Wait for the position to be recorded.
    probe = WaitForCommand(30, TRUE, probe);

    if (probe.Done)
        printf("Recorded position: %f\n\n", PosRecord);

    /*-----Record the position in the second rotation-----*/
    /*We add this part to demonstrate how to record the position in the second rotation.
      Feel free to remove this code block if you don't need it.*/

    //Abort the touch probe.
    RtPrintf("Abort the TouchProbe command.\n\n");
    AbortCommand(probe);

    RtPrintf("Use TouchProbe to record the position in the second rotation.\n\n");
    PosRecord = 0;

    probe = SetAxisTouchProbe(Index, ProbeTrigger, FALSE, 0, 0, &PosRecord);
    if (probe.Error)
        RtPrintf("SetAxisTouchProbe failed: %d\n", probe.ErrorId);

    RtPrintf("Make a velocity move.\n\n");

    //Start a velocity move.
    move = WaitForCommand(10, FALSE, MoveAxisVelocity(Index, 1000, 10000,
        10000, 1000000, mcNegativeDirection, mcAborting));
    if (move.Error)
        RtPrintf("MoveAxisVelocity failed: %d\n", move.ErrorId);

    //Wait for the position to be recorded.
    probe = WaitForCommand(30, TRUE, probe);

    if (probe.Done)
        printf("Recorded position: %f\n\n", PosRecord);

    /*-----End of touch probe-----*/

    //Halt the axis.
    KsCommandStatus halt = WaitForCommand(5, FALSE, HaltAxis(Index, 10000, 1000000,
        mcAborting));
    if (halt.Error)
        RtPrintf("HaltAxis failed: %d\n", halt.ErrorId);
}