/****************************************************************************/
/* Copyright 1992 MBARI                                                     */
/****************************************************************************/
/* Summary  : Moog Thruster Motor Controller Interface Module for VxWorks   */
/* Filename : setThrusts.c                                                  */
/* Author   : Andrew Pearce                                                 */
/* Project  : New ROV                                                       */
/* Version  : Version 1.0                                                   */
/* Created  : 05/19/92                                                      */
/* Modified : 10/08/92                                                      */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/* $Header: autoEnable.c,v 1.1 93/07/01 14:39:06 pean Exp $
 * $Log:$
 */
/****************************************************************************/

#include <vxWorks.h>                /* vxWorks system declarations          */
#include <stdio.h>                  /* vxWorks stdio library                */
#include <lstLib.h>                 /* vxWorks linked list library          */
#include <strLib.h>                 /* vxWorks string library functions     */
#include <taskLib.h>                /* vxWorks task library functions       */
#include <wdLib.h>                  /* vxWorks watch dog timer functions    */
#include <semLib.h>                 /* vxWorks semaphore functions          */

#include <mbariTypes.h>             /* mbari style guide type declarations  */
#include <ModNum.h>                 /* mbari module numbers for error code  */
#include <usrTime.h>                /* mbari time and date functions        */
#include <datamgr.h>                /* Data Manager declarations            */
#include <dm_errno.h>               /* Data Manager error declarations      */

#include <thrusterDM.h>

    STATUS
regenTest( Int32 numThrusters )
{
    Int32 i;
    Int16 thrust;
    char input[100];
    DM_Item Items[6];

    dm_create("THRUSTER.PORT_HORIZ.CMD_THRUST",
              1, &Items[0], DM_INT16, 1, DM_ENDT);

    dm_create("THRUSTER.STBD_HORIZ.CMD_THRUST",
              1, &Items[1], DM_INT16, 1, DM_ENDT);

    dm_create("THRUSTER.FWD_LATERAL.CMD_THRUST",
              1, &Items[2], DM_INT16, 1, DM_ENDT);

    dm_create("THRUSTER.AFT_LATERAL.CMD_THRUST",
              1, &Items[3], DM_INT16, 1, DM_ENDT);

    dm_create("THRUSTER.PORT_VERT.CMD_THRUST",
              1, &Items[4], DM_INT16, 1, DM_ENDT);

    dm_create("THRUSTER.STBD_VERT.CMD_THRUST",
              1, &Items[5], DM_INT16, 1, DM_ENDT);

    for (i = 0; i < numThrusters; i++)
        dm_urgent_open(Items[i], DM_STATIC);

    printf ("Enter thrust level, return to change sign, q to exit\n");

    FOREVER
    {
        gets(input);

        if (strlen(input))
        {
            if (strchr(input, 'q') || strchr(input, 'Q'))
                break;

            thrust = atoi (input);
        } /* if */
        else
            thrust = -thrust;

        printf ("Thrust Level: %7d\n", thrust);

        for (i = 0; i < numThrusters; i++)
            dm_write(Items[i], (char *) &thrust, sizeof(thrust), 0);
    } /* forever */

    for (i = 0; i < numThrusters; i++)
        dm_urgent_close(Items[i]);

}  /* regenTest */