#include <stdio.h>
#include "../h/rci.h"

extern startupFxn();
extern zerogravFxn();
extern releaseFxn();

#if VXWORKS_CFG
#define main zerogDemo_
#endif

main(argc, argv)
int argc;
char **argv;
{
        RCI_DESC *td;
	RCI_RBT *rbt;
	int *enable;

        if (argc != 2)
         { fprintf (stderr, "USAGE: zerograv <robotName>\n");
           exit (-1);
         }
        argc--; argv++;

	rciSetOptions (EXIT_ON_ERROR);

        td = rciCreate ("zerograv", 0x1);
        rciScheduling (td, ON_CLOCK, 0);
        rciRate (td, 30, 0);
        rciControlFxns (td, startupFxn, zerogravFxn, releaseFxn, 0);

	enable = (int*)allocMem ("enable", sizeof(int), 0);

        if (rciAttachRobot (td, *argv, 0) < 0)
         { printErrors();
           exit (-1);
         }

        rciStart(~0);
	rciBlock(td);

	rbt = TASKROBOT(td, 0);
	if (!rbt->how->calibrated)
	 { fprintf (stderr, "ERROR: robot is not calibrated\n");
	   rciRelease(~0);
	   exit (-1);
	 }

	SET_POWER_ON (rbt);
	rciBlock(td);
	*enable = 1;
	
        printf ("Hit <CR> to exit:\n");
        while (getchar() != '\n')
           ;
        rciRelease(~0);
}
