/* encfltcfg.c */

/* Copyright(c) 1991-2002 by Motion Engineering, Inc.  All rights reserved.
 *
 * This software  contains proprietary and  confidential information  of
 * Motion Engineering Inc., and its suppliers.  Except as may be set forth
 * in the license agreement under which  this software is supplied, use,
 * disclosure, or  reproduction is prohibited without the prior express
 * written consent of Motion Engineering, Inc.
 */

#if defined(MEI_RCS)
static const char MEIAppRCS[] =
    "$Header: /MainTree/XMPLib/XMP/app/encfltcfg.c 1     2/05/02 11:33a Erikb $";
#endif

#if defined(ARG_MAIN_RENAME)
#define main    limitsw1Main
argMainRENAME(main, limitsw1)
#endif

/*

:Configure encoder fault settings for a motor.

Encoder faults are set with an action, duration, and trigger.

  The action is one of the following:
  	
	MPIActionSTOP,
	MPIActionE_STOP,
	MPIActionE_STOP_ABORT,
	MPIActionABORT,

The duration is in seconds. Keep in mind that the resolution of this setting
  is one servo sample period (1/sample rate).

The trigger sets three switches.
	MPIMotorEncoderFaultMaskBW_DET,		Broken wire switch
	MPIMotorEncoderFaultMaskILL_DET,	Illegal state switch
	MPIMotorEncoderFaultMaskABS_ERR,	Absolute encoder error switch

Broken wires occur when the positive and negative halves of an encoder signal
  pull together. An example would be when A+ and A- are at the same voltage
  because one of the A wires is disconnected.

Illegal state occurs when the A and B channel change level within 40 nsec.

Absolute encoder errors occur when there is a communication error on an
  absolute encoder. The details of this will vary with different types of
  absolute encoders.

Events are configured using MPIMotorEventConfigGet/Set(...).

Warning!  This is a sample program to assist in the integration of the
 XMP motion controller with your application.  It may not contain all
 of the logic and safety features that your application requires.

*/

#include <stdlib.h>
#include <stdio.h>

#include "stdmpi.h"
#include "stdmei.h"

#include "apputil.h"


#define MOTOR           (0)
#define EVENT_DURATION  (0.010)     /* seconds */


/* Perform basic command line parsing. (-control -server -port -trace) */
void basicParsing(int                    argc,
                  char                  *argv[],
                  MPIControlType        *controlType,
                  MPIControlAddress     *controlAddress)
{
    long argIndex;

    /* Parse command line for Control type and address */
    argIndex = argControl(argc, argv, controlType, controlAddress);

    /* Check for unknown/invalid command line arguments */
    if (argIndex < argc) {
        fprintf(stderr,"usage: %s %s\n", argv[0], ArgUSAGE);
        exit(MPIMessageARG_INVALID);
    }
}


void encoderFaultConfigure(MPIMotor        motor,
						   MPIAction       action,
						   double          duration)
{
    MPIMotorEventConfig eventConfig;
    long returnValue;

    /* Get the current limit configuration */
    returnValue =
        mpiMotorEventConfigGet(motor,
                               MPIEventTypeENCODER_FAULT,
                               &eventConfig,
                               NULL);
    msgCHECK(returnValue);

      /* Wait duration seconds before triggering */
    eventConfig.duration = (float) duration;

    /* Configure motor to perform action upon limit */
    eventConfig.action = action;

	/* Turn on all encoder fault types */
	eventConfig.trigger.mask = 	MPIMotorEncoderFaultMaskBW_DET |
		MPIMotorEncoderFaultMaskILL_DET |
		MPIMotorEncoderFaultMaskABS_ERR;

    /* Set the new limit configuration */
    returnValue =
        mpiMotorEventConfigSet(motor,
                               MPIEventTypeENCODER_FAULT,
                               &eventConfig,
                               NULL);
    msgCHECK(returnValue);
}


/* Create and initialize MPI objects */
void programInit(MPIControl         *control,
                 MPIControlType      controlType,
                 MPIControlAddress  *controlAddress,
                 MPIMotor           *motor,
                 long                motorNumber)
{
    long returnValue;


    /* Obtain a Control handle */
    *control =
        mpiControlCreate(controlType,
                         controlAddress);
    msgCHECK(mpiControlValidate(*control));

    /* Initialize the controller */
    returnValue =
        mpiControlInit(*control);
    msgCHECK(returnValue);

    /* Obtain a Motor handle */
    *motor =
        mpiMotorCreate(*control,
                       motorNumber);
    msgCHECK(mpiMotorValidate(*motor));
}


/* Perform certain cleanup actions and delete MPI objects */
void programCleanup(MPIControl  *control,
                    MPIMotor    *motor)
{
    long returnValue;

    /* Delete motor object */
    returnValue =
        mpiMotorDelete(*motor);
    msgCHECK(returnValue);

    /* Delete control object */
    returnValue =
        mpiControlDelete(*control);
    msgCHECK(returnValue);
}


int main(int    argc,
         char   *argv[])
{
    MPIControl          control;
    MPIMotor            motor;
    MPIControlAddress   controlAddress;
    MPIControlType      controlType;


    /* Perform basic command line parsing. (-control -server -port -trace) */
    basicParsing(argc,
                 argv,
                 &controlType,
                 &controlAddress);

    /* Create and initialize MPI objects */
    programInit(&control,
                controlType,
                &controlAddress,
                &motor,
                MOTOR);

	/* Change the ENCODER_FAULT configuration */
    encoderFaultConfigure(motor,
						MPIActionABORT,             /* ABORT on limit  */
						EVENT_DURATION);

    /* Perform certain cleanup actions and delete MPI objects */
    programCleanup(&control,
                   &motor);

    return MPIMessageOK;
}