/*=-=-=-=-=-=-=-=-=-=-=-  RCI/RCCL Notice of Copyright -=-=-=-=-=-=-=-=-=-=-*
 *									    *
 *	Copyright (C) 1987 by John E. Lloyd. Permission to use, distribute  *
 *	and copy is granted for non-commercial and research purposes only,  *
 *	providing the authors are acknowledged and this copyright notice is *
 *	displayed. The authors provide this software in hope that it will   *
 *	useful, but do not assume responsibility for its use.		    *
 *									    *
 *	This software was developed using the facilities of the McGill 	    *
 *	University Research Center for Intelligent Machines, Montreal,	    *
 *	Canada, for a project undertaken and supported by the RCA Advanced  *
 *	Technology Laboratories, Moorestown, New Jersey.		    *
 *									    *
 *=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-*/

/*=~=~=~=~ McGill-Computer-Vision-and-Robotics-Laboratory--C-Module ~=~=~=~=
 *		
 * NAME:	gravity
 *
 * USAGE:	gravity
 *
 * DESCRIPTION: A demonstration program which puts the PUMA arm into
 *		zero gravity mode, allows the user to ``change'' the
 *		value of gravity, and also allows the user to eliminate the
 *		joint frictions.
 *
 *		The program is Ctree matcher driven.
 * 
 * ARGUMENTS:   None yet.
 *
 * FILES:
 *
 * AUTHOR/DATE: John Lloyd, September, 1986
 *
 * UPDATES:
 *
 *~=~=~=~=~=~= McGill-Computer-Vision-and-Robotics-Laboratory ~=~=~=~=~=~=*/

#include <stdio.h>
#include <math.h>

#include <rci.h>
#include <puma_kynvar.h>
#include "gravity.h"

#define	COMMAND(s)	(strcmp (key_buf, s) == 0)
#define	COMMANDS(s)	(strncmp (key_buf, s, strlen(s)) == 0)
char key_buf [256];

#define GRAVITY_MAX   2.5
#define GRAVITY_MIN  -1.0

#define MFREE	1
#define MLOCK	2
#define MSLIP	3

#define FIRST_3_JOINTS 0x7

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

#if VXWORKS_CFG
#define main gravityDemo_
#endif

main()
{
        RCI_DESC *td;
	RCI_RBT *rbt;
	COMBLK *cbk;

	char *keytree, *tree_match_parse(), *tree_match_err_at();

	int i, j;
	int jointSelect;
	int gravity;
	int jointMode[6];
	int firstThreeJointsLocked;

	keytree = tree_match_parse("					\
(	exit								\
	stop								\
	display								\
	set-gravity							\
	free-joints							\
	lock-joints							\
	slip-joints							\
	help								\
)");

	if (keytree == 0)
	 { printf ("ERROR/Parse error at %.30s\n", tree_match_err_at());
	   exit (-1);
	 }

	rciSetOptions (EXIT_ON_ERROR);

        td = rciCreate ("zerograv", 0x3);
	cbk = (COMBLK*)allocMem ("COMBLK", sizeof(COMBLK), 0);

        rciScheduling (td, ON_CLOCK, 0);
        rciRate (td, 30, 0);
        rciControlFxns (td, startupFxn, zerogravFxn, releaseFxn, (int)cbk);

        if (rciAttachRobot (td, "goldenChild", 0) < 0)
         { printErrors();
           exit (-1);
         }

	rbt = TASKROBOT(td, 0);

	SET_CHECKING (rbt, CHK_REQTOR | CHK_MAXTOR);

        rciStart(~0);
	rciBlock(td);

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

	SET_POWER_ON (rbt);
	rciBlock(td);

	for (i=0; i<PUMAJ; i++)
	 { cbk->current[i] = 0;
	   cbk->current_mode[i] = C_ZEROG;
	   jointMode[i] = MFREE;
	 }
	cbk->block = NO;

	firstThreeJointsLocked = 0;

	rciPrintToTerminal(1);

	do
 	 { tree_match (keytree, "GRAVITY> ", key_buf);

	   if (COMMAND("free-joints") || COMMAND("lock-joints") || 
	       COMMAND("slip-joints"))
	    { jointSelect = get_joint_specification ();
	      if (jointSelect == -1)
	       { continue;
	       }
	    }

	   if (COMMAND ("free-joints"))
	    { 
	      if (jointSelect & firstThreeJointsLocked)
	       { printf ("Type <CR> to confirm\n");
	         while (getchar() != '\n')
		    ;
	       }
	      firstThreeJointsLocked &= ~(jointSelect);

	      for (j=0; j<6; j++)
	       { if ((jointSelect >> j) & 0x01)
		  { cbk->current_mode[j] = C_ZEROG;
		    jointMode[j] = MFREE;
		  }
	       }
	    }
	   else if (COMMAND ("lock-joints"))
	    { for (j=0; j<6; j++)
	       { if ((jointSelect >> j) & 0x01)
		  { cbk->current_mode[j] = NO;
		    jointMode[j] = MLOCK;
		  }
	       }
	      firstThreeJointsLocked |= (jointSelect & FIRST_3_JOINTS);
	    }
	   else if (COMMAND ("slip-joints"))
	    { 
	      if (jointSelect & firstThreeJointsLocked)
	       { printf ("Type <CR> to confirm\n");
	         while (getchar() != '\n')
		    ;
	       }
	      firstThreeJointsLocked &= ~(jointSelect);
	     
	      for (j=0; j<6; j++)
	       { if ((jointSelect >> j) & 0x01)
		  { cbk->current_mode[j] = C_TORQUE;
		    cbk->torque[j] = 0.0;
		    jointMode[j] = MSLIP;
		  }
	       }
	    }
	   else if (COMMAND ("display"))
	    { printf ("GRAVITY = %g\n", 1.0 - cbk->scale);
	      printf ("\n");
	      printf ("JOINT  MODE\n");
	      for (j=0; j<6; j++)
	       { printf ("  %d:   %s  %8.3f\n", j+1, 
		         (jointMode[j] == MFREE ? "FREE" :
			  (jointMode[j] == MLOCK ? "LOCK" : "SLIP")),
			 cbk->torque[j]);
	       }
	      printf ("\n");
	    }
	   else if (COMMAND ("set-gravity"))
	    { float value;

	      printf ("Enter gravity (0 = weightless; 1 = normal) > ");
	      if (scanf ("%f", &value) != 1)
	       { printf ("ERROR -- cannot scan value\n");
		 continue;
	       }
	      if (value < GRAVITY_MIN || value > GRAVITY_MAX)
	       { printf ("ERROR -- gravity value must be between %g and %g\n",
			 GRAVITY_MIN, GRAVITY_MAX);
	         continue;
	       }

	      while (getchar() != '\n')
		 ;
	      printf ("Type <CR> to confirm\n");
	      while (getchar() != '\n')
		 ;

	      cbk->scale = 1.0 - value;

	    }
	   else if (COMMAND ("stop"))
	    { for (j=0; j<6; j++)
	       { cbk->current_mode[j] = NO;
		 jointMode[j] = MLOCK;
	       }
	    }
	   else if (COMMAND ("help"))
	    { int status;

	      printf (" Accessing help file ...\n");
	      status = system (
		 "/local/bin/less /u1/robot/rccl/demo/gravity.hlp");
	      if (status == 127)
	       { printf ("ERROR -- cannot execute /local/bin/less\n");
	       }
	    }
	 }
	while (!(COMMAND("exit")));

	rciRelease(1);
}

#include <ctype.h>

char inputBuf[256];
char stringBuf[256];

get_joint_specification ()
{
	char *sp;
	int i, j;
	char selected[6];
	int length, returnCode;
	

	for (j=0; j<6; j++)
	 { selected[j] = 0;
	 }

	printf ("joints [1-6, all] > ");
	
	gets (inputBuf);

	length = strlen (inputBuf);
	sp = inputBuf;
	for (i=0; i<length; )
	 { if (isspace (inputBuf[i]))
	    { i++;
	    }
	   else
	    { sscanf (&inputBuf[i], "%255s", stringBuf);
	      i += strlen (stringBuf);

	      if ((strlen(stringBuf) == 1) && stringBuf[0] >= '1' &&
		   stringBuf[0] <= '6')
	       { j = stringBuf[0] - '1';
		 if (selected[j])
		  { printf ("ERROR/ joint %d specified twice\n", j+1);
		    return (-1);
		  }
		 else
		  { selected[j] = 1;
		  }
	       }
	      else if ((strcmp (stringBuf, "all") == 0) || 
		       (strcmp (stringBuf, "ALL") == 0))
	       { for (j=0; j<6; j++)
		  { if (selected[j])
		     { printf ("ERROR/ joint %d specified twice\n", j+1);
		       return (-1);
		     }
		    else
		     { selected[j] = 1;
		     }
		  }
	       }
	      else
	       { printf ("ERROR/ joint specifier %s not recognized\n", 
			 stringBuf);
		 return (-1);
	       }
	    }
	 }

	returnCode = 0;
	for (j=0; j<6; j++)
	 { if (selected[j])
	    { returnCode |= (1 << j);
	    }
	 }
	return (returnCode);
}
	
	
