/* --- ROBOTUSR.C       Version 2.1
   --- This is an example user-code file for controlling the PUMA robot
   --- with the Trident Robotics and Research, Inc. interface boards.
   ---
   --- Copyright 1996-1998, Trident Robotics and Research, Inc.
   ---
   --- 01/20/96 RMV     Recoded to work with scheduler.
   --- 05/29/97 RMV     Added lines to help screen.
   --- 04/15/98 RMV     Added MOVEQ with list of points in TRIDENT.CFG
   --- 04/17/98 RMV     mqFlag has meaning inverted wrt PUMA100.
   ---
   --- */

#include <stdio.h>
#include <math.h>
#include <string.h>
#include <unistd.h>
#include "cfig.h"
#include "vtypes.h"
#include "robot.h"

#include "control.h"
#include "rtcontrol.h"

#define DEF_SPEED_PARAM         100.0
#define PUMA_NOMINAL_SPEED   (0.25) /* nominal speed (rads/sec) at DEF_SPEED_PARAM */

/* --- Global variables */
int             cntrlFlagG;
int             oscFlagG;


/* ******************************************************************** */
/*      Global variables                                                */
/* ******************************************************************** */

float   Q_cmdG[8];       /* first 6 are joint positions, checksum, type */
float   Q_deltaG[6];
int     mqStepnG;
float   mqStepSizeG;
float   moveqFreqG;
int     mqFlagG;
float   monSpeedG;

int print_menu()
{
  static int    line;

  switch (line){
    case 1:
      printf("Menu options ->\n");
      break;
    case 2:
      printf("\ta\tRead an A/D voltage \n");
      break;
    case 3:
      printf("\tA\tRead all A/D voltages \n");
      break;
    case 4:
      if (cntrlFlagG){
        printf("\tc\tController toggle off \n");
      } else {
        printf("\tc\tController toggle on \n");
      } /* endif */
      break;
    case 5:
      printf("\tC\tCalibrate all joints\n");
      break;
    case 6:
      printf("\td\tDisable arm power \n");
      break;
    case 7:
      printf("\tD\tDiscrete output \n");
      break;
    case 8:
      printf("\te\tEnable arm power \n");
      break;
    case 9:
      printf("\tg\tDisplay gains \n");
      break;
    case 10:
      printf("\tG\tGenerate pot table for one joint \n");
      break;
    case 11:
      printf("\th\thand \n");
      break;
    case 12:
      printf("\tH\tHelp \n");
      break;
    case 13:
      printf("\tI\tIndex display \n");
      break;
    case 14:
      printf("\ti\tinput from any port \n");
      break;
    case 15:
      printf("\tJ\tJog any joint \n");
      break;
    case 16:
      printf("\tO\tOscillation \n");
      break;
    case 17:
      printf("\to\toutput to any port \n");
      break;
    case 18:
      printf("\tp\tPosition display \n");
      break;
    case 19:
      printf("\tP\tPosition reference display \n");
      break;
    case 20:
      printf("\tq\tQuit \n");
      break;
    case 21:
      printf("\tr\tReference position \n");
      break;
    case 22:
      printf("\ts\tStatus read \n");
      break;
    case 23:
      printf("\tT\tMove to next Trajectory point if defined \n");
      break;
    case 24:
      printf("\tw\tWrite a DAC voltage \n");
      break;
    case 25:
      printf("\tW\tWrite a DAC value in hex \n");
      break;
    case 26:
      printf("\tZ\tZero encoder counters \n");
      break;
    case 27:
      printf("\t?\tprint this list \n");
      break;
    default:
      line = 1;
      return -1;
  } /* endswitch */
  line++;
  return I_OK;
}

void print_help()
{
  printf("\nTo test proper wiring, DO NOT RUN THE CONTROLLER!\n");
  printf("Use the brake release and move the robot by hand to verify operation\n");
  printf("of the encoders, pots, and index registers.\n\n");
  printf("Connect a remote Emergency Power Off switch (EPO) before running controller.\n\n");
  printf("To run the controller:\n");
  printf("\tManually move the arm to the straight up position for safety.\n");
  printf("\tZ - to zero the encoders\n");
  printf("\te - to enable arm power\n");
  printf("\tc - to start the controller (check for green LED on TRC004)\n");
  printf("\tStay out of the workspace and keep the EPO in hand.\n");
  printf("\tPress the ARM POWER ON button. Arm should not move.\n");
  printf("\tPress O to make a joint oscillate. (O again halts joint)\n");
  printf("\tPress J to jog a joint. (<number> or <shift>-<number> moves +,-)\n");
  printf("\tPress T to move to the next trajectory point. (T again halts.)\n");
  printf("Some menu options only work with the controller off.\n");
  printf("\nA calibration file consists of a pot table for each joint concatenated\n");
  printf("together. Use 'G' to generate pot tables. (Turn controller off, press 'G',\n");
  printf("press 'e' (enable arm), press 'c' (controller), press ARM POWER ON button.)\n");
  printf("\nPress 'C' to spawn calibration. Turn on controller, enable ARM POWER, then\n");
  printf("press 'C' again to run calibration. When done, press 'C' to kill process.\n");
}


/* ******************************************************************** */
/*      MAIN                                                            */
/* ******************************************************************** */

int main (argc, argv)
    int argc;
    char *argv[];
{
  short         keepGoing = 1,
                potByte = 1,
	        val[6],
		 ans, i, j, movecount;
		/* MS Change value to a short from int */
  int           jtNum;
  short         short_ref[6];
  short         input_port,output_port,output_value;  /* MS */
  long	        value;
  short         short_value;
  long          longVal, longVect[8];
  static int    calibFlag = 0;
  int           jogFlag = 0;
  int           hand = 0;
  float         volts;
  char          filename[32] = {"trident.cfg"};
  char          calfile[128];
  cfigfile_t    *cfig;
  float         traj[8][7];
  int           trjCnt = 0, trjIndex = 0;
  
  monSpeedG = 75.0;     /* default speed for MOVEQ */
  moveqFreqG = 100.0;
  cntrlFlagG = 0;       /* turn off controller */
  oscFlagG = 0;         /* turn off oscillation generator */
  mqFlagG = 0;          /* Turn off moveq (trajectory) */

  /* Look for a command line argument for configuration file name. */
  if (argc > 1)
	 strncpy(filename,argv[1],31);

  /* Read the configuration file */
  printf("Using config file %s \n",filename);
  if ((cfig = cfigInit(filename,CFIG_R)) == NULL) {
    printf("Error: Bad config file -- missing 'TRC006' \n");
    exit();
  }

  /* Dummy read to make sure the correct config file is selected. */
  if (cfigCompulsory(cfig, "TRC006", &value, VT_FLOAT, 1) != I_OK){
    printf("Error: Bad config file -- missing 'TRC006' \n");
    exit();
  } /* endif */

  cfigCompulsory(cfig, "BASE_ADDRESS", &value, VT_SHORT, 1);
  pumaVarsG.base = value;
  
  printf("Base address is %04X \n",pumaVarsG.base);
  
  cfigCompulsory(cfig, "Kp_GAINS", KPosG, VT_FLOAT, 6);
  cfigCompulsory(cfig, "Kv_GAINS", KVelG, VT_FLOAT, 6);
  if (cfigOptional(cfig, "MOTOR_SCALE", motorScaleG, VT_FLOAT, 6) !=
                I_ERROR)
    printf("\nWARNING! Non-standard motor scale factors. \n\n");

  /*  cfigCompulsory(cfig, "MAGIC", magicNumG, VT_SHORT, 6);
   */
  cfigCompulsory(cfig, "CALIB_FILE", calfile, VT_STRING, 128);

  cfigOptional(cfig, "POT_BYTE", &potByte, VT_SHORT, 1);

  while (cfigOptional(cfig, "Q_POINT", &traj[trjCnt][0], VT_FLOAT, 7) == I_OK)
    if (++trjCnt >= 8)
      break;

  printf("Done reading config file\n");

  /* Done reading config file */

  /* --- This call sets up the timer interrupt. */
  
  system_init();
  /* Initialize the real-time control process under RTLinux */
  init_rtcontrol();

  /* MS write gains to real-time controller after RT memory allocated
     by init_rtcontrol()
  */

  for (i=0;i<6;i++) {
    val[i]=(short)KPosG[i];
    short_ref[i]=0;
  }
  write_gain(val); /* Write the gains to the RT controller */
  write_setpt(short_ref); /* Zero the setpoints */
  write_target(short_ref); /* Zero the targets */

  if (trjCnt > 0){
    printf("Trajectory initialized with %d points. \n",trjCnt);
  }


  /* endif */

  while(keepGoing){

    printf("Waiting for command > ");
    ans = getchar();
    switch (ans) {
    case 10:
      break;
    case 'a':       /* Read A/D */
      if (cntrlFlagG) break;
      printf("Enter joint number (1 - 6): ");
      scanf("%d",&jtNum);
      jtNum--;
      if ((jtNum >= 0) && (jtNum <= 5)){
	if (potByte == 0)
	  robot_potrd(POT, jtNum, &longVal);
	else
	  robot_longrd(POT, jtNum, &longVal);
	printf("A/D %d: %04lX\n",jtNum+1,longVal);
      } else {
	printf("Joint number out of range \n");
      } /* endif */
      break;
    case 'A':       /* Read 6 A/D Values */
      if (potByte == 0)
	robot_potrd(POT6,0,longVect);
      else
	robot_longrd(POT6,0,longVect);
      for(jtNum=0; jtNum<6; jtNum++)
	printf("%04lX  ",longVect[jtNum]);
      printf("\n");
      break;
    case 'c':       /* Control flag toggle */
      if (cntrlFlagG){
	stop_rtcontrol();
	printf("Controller off\n");
	cntrlFlagG = 0;
      } else {
	printf("Controller on\n");
	start_rtcontrol();
	cntrlFlagG = 1;
      }  /* endif */
      break;
    case 'C':               /* Repeatedly read position*/
      for(i=0;i<500;i++) {
	robot_shortrd(POSITION6, 0, val);
	printf("%6d  %6d  %6d \t%6d  %6d  %6d\n",val[0],
	       val[1], val[2],val[3],val[4], val[5]);
	sleep(1);
      }
      break;
    case 'd':       /* Disable arm power */
      pumaVarsG.discrete = pumaVarsG.discrete & ~POWER_BIT;
      robot_shortwr(STATUS, 0, &pumaVarsG.discrete);
      break;
    case 'D':       /* Discretes */
      if (cntrlFlagG) break;
      printf("Discretes are: %04X  enter new value in hex: ",
	     pumaVarsG.discrete);
      scanf("%hx",&pumaVarsG.discrete);
      robot_shortwr(STATUS, 0, &pumaVarsG.discrete);
      break;
    case 'e':       /* Enable arm power */
      pumaVarsG.discrete = pumaVarsG.discrete | POWER_BIT;
      robot_shortwr(STATUS, 0, &pumaVarsG.discrete);
      break;
    case 'g':
      if (cntrlFlagG) break;
      printf("Motor Scale Factors:\n");
      for (i=0; i<6; i++)
	printf("%7.2f  ",motorScaleG[i]);
      printf("\nKp Gains:\n");
      for (i=0; i<6; i++)
	printf("%8.2f  ",KPosG[i]);
      printf("\nKv Gains:\n");
      for (i=0; i<6; i++)
	printf("%7.2f  ",KVelG[i]);
      printf("\n");
      break;
    case 'G':
      /*
	if (potcalProc != NULL)
	sbsControl(potcalProc, SBS_KILL);
	if (cntrlFlagG) break;
	potcalProc = potcal_joint();
      */
      break;
    case 'h':  /* Toggle sense of hand */
      if(hand) {
	pumaVarsG.discrete = pumaVarsG.discrete | HAND_OPEN_BIT;
	pumaVarsG.discrete = pumaVarsG.discrete & ~HAND_CLOSED_BIT;
	hand=0;
      } else {
	pumaVarsG.discrete = pumaVarsG.discrete | HAND_CLOSED_BIT;
	pumaVarsG.discrete = pumaVarsG.discrete & ~HAND_OPEN_BIT;
	hand=1;
      }
      robot_shortwr(STATUS, 0, &pumaVarsG.discrete);
      break;
    case 'H':
      if (cntrlFlagG) break;
      print_help();
      break;
    case 'i':
      printf("Enter port to read from in hex: ");
      scanf("%hx",&input_port);
      robot_shortrd(ARB,input_port,&output_value);
      printf("Value read from port %04X = %04X \n",
	     input_port,output_value);
      break;
    case 'I':
      robot_longrd(INDEX6, 0, longVect);
      printf("Indices: \n");
      printf("%6ld  %6ld  %6ld \n",longVect[0], longVect[1],
	     longVect[2]);
      printf("%6ld  %6ld  %6ld \n",longVect[3], longVect[4],
	     longVect[5]);
      break;
    case 'J':       /* Jog any joint by pressing the number keys */
      /* ARM POWER must be on to set the jog flag */
      /* robot_longrd(STATUS,0,&longVal);
      if ((longVal & POWER_ON_MASK & 0x0000FFFF) && (jogFlag == 0) &&
      (mqFlagG == 0)){ */
      if(jogFlag == 0) {
	jogFlag = 1;
	printf("Joint jogging enabled\n");
      } else {
	jogFlag = 0;
	printf("Joint jogging disabled\n");
      } /* endif */
      break;
    case 'o':       /* Output value to arbitrary port */
      printf("Enter port to output from in hex: ");
      scanf("%hx",&output_port);
      printf("\n Enter output value in hex: ");
      scanf("%hx",&output_value);
      robot_shortwr(ARB,output_port,&output_value);
      printf("Value %04X sent to port %04X \n",
	     output_value,output_port);
      
      break;
    case 'O':       /* oscillatory movement */
      if ((oscFlagG == 0) && (mqFlagG == 0)){
	printf("Press a joint number (1 - 6) and 'Q' when done\n");
	oscFlagG = 10;
      } else if (oscFlagG != 0){
	printf("Oscillation off\n");
	oscFlagG = 0;
      } /* endif */

      break;
    case 'p':
      robot_shortrd(POSITION6, 0, val);
      printf("Positions: \n");
      printf("%6hd  %6hd  %6hd \t%6hd  %6hd  %6hd\n",val[0],
	     val[1], val[2],val[3],
	     val[4], val[5]);
      break;
    case 'P':
      printf("Position references: \n");
      printf("%5.3f  %5.3f  %5.3f \n",Q_RefG[0] ,Q_RefG[1] ,Q_RefG[2]);
      printf("%5.3f  %5.3f  %5.3f \n",Q_RefG[3] ,Q_RefG[4] ,Q_RefG[5]);
      break;
    case 'q':       /* Quit */
      keepGoing = 0;
      break;
    case 'r':       /* Reference position */
      if (cntrlFlagG){
	printf("controller must be off\n");
	break;
      } /* endif */
      printf("Enter joint number (1 - 6): ");
      scanf("%d",&jtNum);
      jtNum--;
      if ((jtNum >= 0) && (jtNum <= 5)){
	printf("Enter reference position in radians: ");
	scanf("%f",&Q_RefG[jtNum]);
      } else {
	printf("Joint number out of range \n");
      } /* endif */
      break;
    case 's':
      robot_longrd(STATUS, 0, &longVal);
      value = longVal;
      printf("Status = %04lX \n",value);
      break;
    case 't':
      if (mqFlagG == 0){
	if (trjCnt <= 0){
	  printf ("No trajectory points defined\n");
	} else {
	  for (i=0; i<trjCnt; i++){
	    for (j=0; j<7; j++)
	      printf("%f ",traj[i][j]);
	    printf("\n");
	  } /* endfor */
	} /* endif */
      } /* endif */
      break;
    case 'T':
      if (mqFlagG != 0){
	mqFlagG = 0;
      } else if ((trjCnt <= 0) || (trjCnt > 8)){
	printf("Invalid trajectory\n");
      } else if ((oscFlagG == 0) && (calibFlag == 0) && (jogFlag == 0)){

	for (i=0; i<6; i++)
	  val[i] = (short)traj[trjIndex][i];

	write_target(val);
	/* MS traj[i][7] is now a 1 or 0 to indicate hand open of closed */
	if(traj[trjIndex][7] > 0) {
	  pumaVarsG.discrete = pumaVarsG.discrete | HAND_OPEN_BIT;
	  pumaVarsG.discrete = pumaVarsG.discrete & ~HAND_CLOSED_BIT;
	} else {
	  pumaVarsG.discrete = pumaVarsG.discrete | HAND_CLOSED_BIT;
	  pumaVarsG.discrete = pumaVarsG.discrete & ~HAND_OPEN_BIT;
	}
	robot_shortwr(STATUS, 0, &pumaVarsG.discrete);
	for (i=0; i<6; i++)
	  printf("cmd[%d] = %f \n",i,traj[trjIndex][i]);

            /* Increment the list index */
	if (++trjIndex >= trjCnt)
	  trjIndex = 0;
      } else {
	printf("Conflict with other mode\n");
      } /* endif */
      break;
    case 'w':       /* Write DAC value */
      if (cntrlFlagG) break;
      printf("Enter joint number (1 - 6): ");
      scanf("%d",&jtNum);
      jtNum--;
      if ((jtNum >= 0) && (jtNum <= 5)){
	printf("Enter voltage in volts: ");
	scanf("%f",&volts);
	robot_fltwr(VOLTAGE, jtNum, &volts);
      } else {
	printf("Joint number out of range \n");
      } /* endif */
      break;
    case 'W':       /* Write DAC value */
      if (cntrlFlagG) break;
      printf("Enter joint number (1 - 6): ");
      scanf("%d",&jtNum);
      jtNum--;
      if ((jtNum >= 0) && (jtNum <= 5)){
	printf("Enter value in hex: ");
	scanf("%lx",&value);
	short_value = (short)value;
	robot_shortwr(TORQUE, jtNum, &short_value);
      } else {
	printf("Joint number out of range \n");
      } /* endif */
      break;
    case 'Z':       /* Zero the encoder counters */
      if (cntrlFlagG) break;
      val[0] = val[1] = val[2] = val[3] = val[4] = val[5] = 0;
      robot_shortwr(POSITION6, 0, val);
      break;
    case '?':
      while (print_menu() != -1);
      break;
    case '1':
    case '2':
    case '3':
    case '4':
    case '5':
    case '6':
      if (oscFlagG > 8){
	oscFlagG = ans - '0';
	/* Read the current position */
	read_position(val);
	movecount=0;
	while(movecount < 6) {
	  if (movecount%2 == 0) {
	    if(short_ref[oscFlagG-1]++ > 5000) movecount++;
	  } else
	    if(short_ref[oscFlagG-1]-- < -5000) movecount++;

	  write_target(short_ref);
	  /* MS Debug 	  read_position(val);*/
      robot_shortrd(POSITION6, 0, val);
	  printf("%6hd  %6hd  %6hd \t%6hd  %6hd  %6hd %0x\n",val[0],
		 val[1], val[2],val[3], val[4], val[5], val[5]);
	
	}
      } else if (jogFlag == 1){
	i = ans - '1';
      } /* endif */
      break;
    case '!':       /* Shift-1 */
      if (jogFlag == 1)
	write_setpt(short_ref);
      break;
    case '@':       /* Shift-2 */
      if (jogFlag == 1)
	write_setpt(short_ref);
      break;
    case '#':       /* Shift-3 */
    case '$':       /* Shift-4 */
    case '%':       /* Shift-5 */
      if (jogFlag == 1){
	i = ans - 33;
	write_setpt(short_ref);
      } /* endif */
      break;
    case '^':       /* Shift-6 */
      if (jogFlag == 1)
      break;
    default:
      printf("Bad command -- '?' for command list\n");
    } /* endswitch */
    
  } /* endwhile */
  

  
  /* !!! IMPORTANT !!!
     --- Release shared memory and io port */
  quit_rtcontrol();
  release_port();
  exit();
}

