Tutorial 1-7: Reading the status and executing motion

The motion functions that are available in the WMX3 library are discussed in detail in the other sections. This part of the tutorial adds a simple motion command that moves axis 0 by 10000 user units using a Trapezoidal motion profile.

As the discussion at SetServoOn states, there may be a few cycles delay after this function is called before the axis servo turns on. To prevent the motion command from returning an error, the status should be polled to first verify that the axis is in the servo on state.

The CoreMotion module contains a GetStatus function which obtains the status of the system and of the axes. The Servo On status can be read to verify that the axis servo is on.

The following adds the simple motion command and status polling to the example code.

using namespace wmx3Api;
WMX3Api wmxlib;
CoreMotion wmxlib_cm(&wmxlib);
int err;
char errString[256];
CoreMotionStatus st;
int counter;
Motion::PosCommand pos;

printf("Starting application...\n");

err = wmxlib.CreateDevice(_T("C:\\Program Files\\SoftServo\\WMX3"), DeviceType::DeviceTypeNormal);
if (err != ErrorCode::None) {
    wmxlib.ErrorToString(err, errString, sizeof(errString));
    printf("Failed to create device. Error=%d (%s)\n", err, errString);
    goto exit;
}

err = wmxlib.StartCommunication(5000); //Wait up to 5 seconds for communication to start
if (err != ErrorCode::None) {
    wmxlib.ErrorToString(err, errString, sizeof(errString));
    printf("Failed to start communication. Error=%d (%s)\n", err, errString);
    goto exit;
}

err = wmxlib_cm.axisControl->SetServoOn(0, 1); //Set axis 0 servo on
if (err != ErrorCode::None) {
    wmxlib_cm.ErrorToString(err, errString, sizeof(errString));
    printf("Failed to set servo on. Error=%d (%s)\n", err, errString);
    goto exit;
}

//Wait for servo to turn on
counter = 1000; //Counter to prevent infinite loop
do {
    err = wmxlib_cm.GetStatus(&st);
    if (err != ErrorCode::None) {
        wmxlib_cm.ErrorToString(err, errString, sizeof(errString));
        printf("Failed to get status. Error=%d (%s)\n", err, errString);
        goto exit;
    }
    Sleep(10);
    counter--;
} while (counter > 0 && st.axesStatus[0].servoOn == 0);

//Execute position command
pos.axis = 0;
pos.profile.type = ProfileType::Trapezoidal;
pos.profile.velocity = 10000;
pos.profile.acc = 10000;
pos.profile.dec = 10000;
pos.target = 10000;
err = wmxlib_cm.motion->StartMov(&pos);
if (err != ErrorCode::None) {
    wmxlib_cm.ErrorToString(err, errString, sizeof(errString));
    printf("Failed to execute motion. Error=%d (%s)\n", err, errString);
    goto exit;
}

Sleep(5000); //Wait five seconds before stopping communication

err = wmxlib_cm.axisControl->SetServoOn(0, 0); //Set axis 0 servo off
if (err != ErrorCode::None) {
    wmxlib_cm.ErrorToString(err, errString, sizeof(errString));
    printf("Failed to set servo off. Error=%d (%s)\n", err, errString);
    goto exit;
}

exit:

err = wmxlib.StopCommunication();
if (err != ErrorCode::None) {
    wmxlib.ErrorToString(err, errString, sizeof(errString));
    printf("Failed to stop communication. Error=%d (%s)\n", err, errString);
}

err = wmxlib.CloseDevice();
if (err != ErrorCode::None) {
    wmxlib.ErrorToString(err, errString, sizeof(errString));
    printf("Failed to close device. Error=%d (%s)\n", err, errString);
}

printf("Press any key to exit.\n");
getchar();