/****************************************************************************/
/* 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
setThrusts( 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.PORT_VERT.CMD_THRUST",
              1, &Items[2], DM_INT16, 1, DM_ENDT);

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

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

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

    FOREVER
    {
        gets(input);
        if (strlen(input))
            thrust = atoi (input);
        else
            thrust = -thrust;

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