
/*	Robmain.c Main program for register interaction with TRC004 */

#include <stdio.h>
#include <string.h>
#include <conio.h>
#include "cfig.h"
#include "vtypes.h"
#include "robot.h"
#include "sched.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;
float           oscMagG,        /* oscillation magnitude */
                oscStepG,       /* oscillation step size */
                oscSpeedG;      /* oscillation speed */

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

float   Q_cmdG[8];       /* first 6 are joint positions, checksum, type */
float   Q_initG[6];
float   Q_deltaG[6];
int     mqStepnG;
float   mqStepSizeG;
float   moveqFreqG;
int     mqFlagG;
float   monSpeedG;
static int    line = 1;
extern short magicNumG[];

int print_menu()
{

  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\tHelp \n");
      break;
    case 12:
      printf("\ti\tIndex display \n");
      break;
    case 13:
      printf("\tJ\tJog any joint \n");
      break;
	 case 14:
		printf("\to\tOutput Word to any address \n");
		printf("\tO\tOscillation \n");

      break;
    case 15:
      printf("\tp\tPosition display \n");
      break;
    case 16:
      printf("\tP\tPosition reference display \n");
      break;
    case 17:
      printf("\tq\tQuit \n");
      break;
    case 18:
		printf("\tr\tReference position \n");
		printf("\tR\tRead from any address \n");

		break;
    case 19:
      printf("\ts\tStatus read \n");
      break;
    case 20:
      printf("\tT\tMove to next Trajectory point if defined \n");
      break;
    case 21:
      printf("\tw\tWrite a DAC voltage \n");
      break;
    case 22:
      printf("\tW\tWrite a DAC value in hex \n");
      break;
    case 23:
      printf("\tZ\tZero encoder counters \n");
      break;
	 case 24:
		printf("\t?\tprint this list \n");
		break;
    default:
      line = 1;
      return SBS_OFF;
  } /* 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, envp)
    int argc;
    char *argv[];
    char *envp;
{
  short         keepGoing = 1,
                potByte = 1,
					 jtNum,  val[6],
					 ans, i, j,value_short;
  short			 offset,read_value;

					 /* MS Change value to an long from short */
  float			 value;
  long          longVal, longVect[8];
  static int    calibFlag = 0;
  int           jogFlag = 0;
  float         actualFreq, reqFreq,
                volts;
  float         posRad[6];
  float         oscMag[6] = {0.5, 0.2, 0.5, 0.5, 1.0, 1.0};
  float         oscInc[6] = {0.003, 0.002, 0.003, 0.002, 0.003, 0.003};
  char          filename[32] = {"trident.cfg"};
  char          calfile[128];
  cfigfile_t    *cfig;
  processT      *ctrlProc, *oscProc, *menuProc, *calibProc, *potcalProc;
  processT      *moveqProc;
  float         traj[8][6];
  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) */
  potcalProc = NULL;

  /* 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)
	 return;
  /* 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");
	 return;
  } /* endif */


	 cfigCompulsory(cfig, "BASE_ADDRESS", &value_short, VT_SHORT, 1);
  pumaVarsG.base = value_short;

  cfigCompulsory(cfig, "FREQUENCY", &reqFreq, VT_FLOAT, 1);

  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, 6) == I_OK)
    if (++trjCnt >= 8)
      break;

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

  /* Done reading config file */


  while(keepGoing){
	 if (kbhit()){
		ans = getch();
		switch (ans) {
       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: %04X\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("%04X  ",longVect[jtNum]);
          printf("\n");
          break;
        case 'c':       /* Control flag toggle */
          if (cntrlFlagG){
				if (sbsControl(ctrlProc, SBS_OFF) != I_OK)
              printf("ERROR with sbsControl \n");
            printf("Controller off\n");
            cntrlFlagG = 0;
          } else {
            printf("Controller on\n");
            if (sbsControl(ctrlProc, SBS_ON) != I_OK)
              printf("ERROR with sbsControl \n");
            cntrlFlagG = 1;
          } /* endif */
          break;
        case 'C':               /* calibrate the robot */
          /* First 'C', spawn the process */
          if (calibFlag == 0){
            if (cntrlFlagG == 0){
              printf("Spawning calibration process\n");
              if ((calibProc = calib_init(calfile)) == NULL)
                printf("ERROR with calib sbsSpawn \n");
              else
                calibFlag = 1;
            } else {
              printf("Controller must be off to spawn calibration\n");
            } /* endif */

          /* second 'C', start the calibration process */
          } else if (calibFlag == 1){
            printf("Starting the calibration process \n");
            calib_joint(0,calibProc);
            calibFlag = 2;

          /* finally, kill the process */
          } else if (calibFlag == 2){
            printf("Killing calibration process\n");
            sbsControl(calibProc, SBS_KILL);
            calibFlag = 0;
          } /* endif */
          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("%x",&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':
          if (cntrlFlagG) break;
          print_help();
          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)){
            jogFlag = 1;
            printf("Joint jogging enabled\n");
          } else {
            jogFlag = 0;
            printf("Joint jogging disabled\n");
          } /* endif */
			 break;
			 case 'o':  /* Read any address */
			 printf("Enter offset address in hex: ");
			 scanf("%x",&offset);
			 printf("Enter value to output in hex: ");
			 scanf("%x",&read_value);
			 outport(pumaVarsG.base + offset, read_value);
			 printf("\nValue %x sent to %x\n",read_value, offset);
		  break;
        case 'O':       /* oscillatory movement */
          if ((oscFlagG == 0) && (mqFlagG == 0)){
            printf("Press a joint number (1 - 6)\n");
            if (sbsControl(oscProc, SBS_ON) != I_OK)
              printf("ERROR with sbsControl\n");
            oscFlagG = 10;
          } else if (oscFlagG != 0){
            printf("Oscillation off\n");
            if (sbsControl(oscProc, SBS_OFF) != I_OK)
              printf("ERROR with sbsControl\n");
            oscFlagG = 0;
          } /* endif */

          break;
        case 'p':
          robot_longrd(POSITION6, 0, &pumaVarsG.pos[0]);
          robot_fltrd(POSITION6, 0, posRad);
          printf("Positions: \n");
          printf("%6ld  %6ld  %6ld \t\t%6.3f  %6.3f  %6.3f\n",pumaVarsG.pos[0],
                                       pumaVarsG.pos[1], pumaVarsG.pos[2],
                                       posRad[0], posRad[1], posRad[2]);
          printf("%6ld  %6ld  %6ld \t\t%6.3f  %6.3f  %6.3f\n",pumaVarsG.pos[3],
                                       pumaVarsG.pos[4], pumaVarsG.pos[5],
                                       posRad[3], posRad[4], posRad[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;
			 /* MS */
		  case 'R':  /* Read any address */
			 printf("Enter offset address in hex: ");
			 scanf("%x",&offset);
			 read_value=inport(pumaVarsG.base + offset);
			 printf("\nRead value for %x is %x\n",offset,read_value);
		  break;
        case 's':
          robot_longrd(STATUS, 0, &longVal);
          value = longVal;
          printf("Status = %04X \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<6; j++)
                  printf("%f ",traj[i][j]);
                printf("\n");
              } /* endfor */
            } /* endif */
          } /* endif */
          break;
        case 'T':
          if (mqFlagG != 0){
            sbsControl(moveqProc, SBS_OFF);
            mqFlagG = 0;
          } else if ((trjCnt <= 0) || (trjCnt > 8)){
            printf("Invalid trajectory\n");
          } else if ((oscFlagG == 0) && (calibFlag == 0) && (jogFlag == 0)){
            /* ARM POWER must be on to run moveq */
            robot_longrd(STATUS,0,&longVal);
            if (!(longVal & POWER_ON_MASK & 0x0000FFFF)){
              printf("Arm Power OFF\n");
              break;
            } /* endif */

            /* Load the next goal into Q_cmdG */
            for (i=0; i<6; i++)
              Q_cmdG[i] = traj[trjIndex][i];

            for (i=0; i<6; i++)
              printf("cmd[%d] = %f \n",i,Q_cmdG[i]);

            /* Initialize the trajectory generator and start moving */
				/* MS if (moveqOn() == I_OK)
				  sbsControl(moveqProc, SBS_ON); */

            /* Increment the list index */
            if (++trjIndex >= trjCnt)
              trjIndex = 0;
          } else {
            printf("Conflict with other generator\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("%x",&value);
            robot_shortwr(TORQUE, jtNum, &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 '?':
			for (i=0; i<25;i++)
			print_menu();
			break;
        case '1':
        case '2':
        case '3':
        case '4':
        case '5':
        case '6':
          if (oscFlagG > 8){
            oscFlagG = ans - '0';
            oscMagG = oscMag[oscFlagG-1];
            oscStepG = oscSpeedG = oscInc[oscFlagG-1];
          } else if (jogFlag == 1){
            i = ans - '1';
            Q_RefG[i] += 10 * oscInc[i];
          } /* endif */
          break;
        case '!':       /* Shift-1 */
          if (jogFlag == 1)
            Q_RefG[0] -= 10 * oscInc[0];
          break;
        case '@':       /* Shift-2 */
          if (jogFlag == 1)
            Q_RefG[1] -= 10 * oscInc[1];
          break;
        case '#':       /* Shift-3 */
        case '$':       /* Shift-4 */
        case '%':       /* Shift-5 */
          if (jogFlag == 1){
            i = ans - 33;
            Q_RefG[i] -= 10 * oscInc[i];
          } /* endif */
          break;
        case '^':       /* Shift-6 */
          if (jogFlag == 1)
            Q_RefG[5] -= 10 * oscInc[5];
          break;
        default:
          printf("Bad command -- '?' for command list\n");
      } /* endswitch */
	 } /* endif */		
  }


}


