/* --- ROBOT.C          Version 2.1
   --- This file contains the main routines for running the PUMA
   --- robot with the Trident Robotics and Research, Inc. interface
   --- boards.
   ---
   --- These are only demo routines and are not complete.
   --- For serious operation, your own interrupt routines must
   --- handle the system clock properly and properly save and
   --- restore state. You are free to use these routines, or any
   --- part thereof, AT YOUR OWN RISK, provided this entire comment
   --- block and the included copyright notice remain intact.
   ---
   --- Initial coding by Richard Voyles, Vigilant Technologies.
   --- Further modifications by Richard Voyles, Trident Robotics
   ---
   --- Copyright 1993,1995,1996,1998 Trident Robotics and Research, Inc.
   ---
   --- 01/15/01 MS      Modified to work under Linux
   --- 12/02/93 RMV     Modified to work with TRC004 rev E.
   --- 02/21/95 RMV     Modified POT reading to perform byte reads.
   --- 11/08/95 RMV     Added POT6 and INDEX6 to robot_longrd().
   ---                  Added range checking to robot_fltwr(TORQUE6).
   --- 12/12/95 RMV     Added robot_potrd() to read A/D's with
   ---                  16-bit words. NOT RECOMMENDED FOR USERS!!
   --- 12/31/95 RMV     Changed motor and encoder scale factors.
   --- 02/02/96 RMV     Added robot_status(), robot_control(), and
   ---                  pdgControl(). Changed robot_intwr() to
   ---                  robot_shortwr().
   ---                  Added encoderOffsetG[] and changed the way
   ---                  encoders are read!!!             ROBOT 2.0
   --- 02/14/98 RMV     Added extra test to A/D read for fast CPUs.
   --- */

#define ROBOT_SRC
#include <stdio.h>
#include <errno.h>
#include <sys/time.h>
#include <sys/types.h>
#include <fcntl.h>
#include <unistd.h>
#include <sys/ioctl.h>
#include <rtl_fifo.h>
#include <rtl_time.h>
#include <math.h>

/* Added by MS */
#include <sys/perm.h>
#define REALLY_SLOW_IO
#include <asm/io.h>

#include "vtypes.h"
#include "robot.h"


/* --- Global variables (all globals end in "G") */
PUMAstatusT     pumaVarsG;      /* structure containing PUMA variables */
float           KPosG[6],       /* position error gain for PD cntrlr */
                KVelG[6];       /* velocity error gain for PD cntrlr */
float           Q_RefG[6],      /* joint reference positions */
                Qd_RefG[6];     /* joint reference velocity */
float           Q_MezG[6],      /* measured joint positions */
                Qd_MezG[6];     /* measured joint velocities */

/* --- The following values are valid for a PUMA 560 */

/* --- The motor scale converts N-m at the joint to DAC quanta */
/* These are taken from 1986 ICRA paper by Armstrong, Khatib, Burdick,
   and are measured values.    COMMENTED OUT
float   fltmtrScaleG[6] = {-21.0, 11.0, -22.9, 84.6, 102., 96.2}; */

/* The above numbers confirm the following theoretical values which
   are multiplied by -1 to account for the inverting amplifiers. */
float   fltmtrScaleG[6] = {19.99, -10.97, 22.65, -84.80, -97.68, -95.00};

/* These integer values are the above float values multiplied
   by 16. They result in faster computation and are within 3%.
   Shift the final result 4 bits to the right to equalize. */

/* For PUMA 560 */
/* short   smotorScaleG[6] = {320, -176, 362, -1357, -1563, -1520}; */
float   motorScaleG[6] = {19.99, -10.97, 22.65, -84.80, -97.68, -95.00};

/* For PUMA 550 Mark I */
/* short   smotorScaleG[6] = {-320, 176, -362, -1357, 0, 1520};*/
/* float   motorScaleG[6] = {-19.99, 10.97, -22.65, -84.80, 0, 95.00}; */

/* --- The encoder scale converts encoder counts to radians. */
/* This set of values is for the TRC004 Rev D. */
/* float   encoderScaleG[6] = {0.00010035, -0.000073156, 0.000117,
                            -0.000082663, -0.000087376, -0.000081885}; */

/* This set of values is for the TRC004 Rev E. */
float   encoderScaleG[6] = {0.00010035, -0.000073156, 0.000117,
                            -0.000082663, -0.000087376, -0.00016377};

short   encoderOffsetG[6] = {0,0,0,8000,0,0};

/* ************ Function Prototypes ************** */

short robot_fltwr(short option, short chan, float *val);
short robot_shortwr(short option, short chan, short *val);
short robot_fltrd(short option, short chan, float *val);
short robot_longrd(short option, short chan, long *val);
short robot_shortrd(short option, short chan, short *val);


/* --- This function initializes the system for robot control. It installs
   --- the interrupt handler and sets the desired interrupt frequency, in
   --- the proper order. It also initializes variables.
   --- It returns the acutal interrupt frequency, in hertz. */
void system_init()
{
  int   i;
  short init_val = 0;

  /* --- First initialize the PUMA structure. */
  /* --- Start by resetting everyting but the <Joint 6 Overflow> and
     --- <Calibration Status> bits and setting the discrete variable. */
  /* --- MS Under Linux, must obtain permission to write to port */
  /* --- Code imported from linux Mini-Howto IO-Port-Programming */
  /* Get access to the ports */
  if(pumaVarsG.base < 0x3ff) {
    if (ioperm(pumaVarsG.base, 64, 1)) {perror("ioperm"); exit(1);}
  } else 
    { iopl(3);
    }

  pumaVarsG.status = inw(pumaVarsG.base + STATUS_REG);
  /*  pumaVarsG.discrete = (pumaVarsG.status & 0xC000) >> 8;
      outw(pumaVarsG.discrete, pumaVarsG.base + DISCRETE_REG);*/
  pumaVarsG.status = inw(pumaVarsG.base + STATUS_REG);

  /* --- Now set position variables appropriately */
  if ((pumaVarsG.status & CALIB_MASK) == 0){
    /* --- Arm is NOT calibrated so set everything to zero. */
    for (i=0; i<6; i++){
      pumaVarsG.pos[i] = 0L;            /* reset the variable */
      robot_shortwr(POSITION, i, &init_val);    /* reset the encoder */
    } /* endfor */
    pumaVarsG.discrete = 0;           /* make sure all bits are cleared */
    outw(pumaVarsG.discrete,pumaVarsG.base + DISCRETE_REG);
  } else {
    /* --- Arm IS calibrated */
    robot_longrd(POSITION6,0,&pumaVarsG.pos[0]);
  } /* endif */

}

/* --- This function writes 1 or 6 float values to the robot. The particular
   --- value is chosen with the OPTION parameter, which is one out of a
   --- list of enumerated ioOptionsT. CHAN ranges from 0 to 5 (corresponding
   --- to PUMA joints 1 - 6). It returns one of the enumerated ioErrorsT. */
short robot_fltwr(short option, short chan, float *val)
{
  short intVal, shortVal,
        i;
  long  longVal;

  switch (option) {
    case TORQUE6:       /* Output to all DACs -- val is in N-m */
      for (i=0; i<6; i++){
        longVal = (long)(val[i] * motorScaleG[i]) + 0x0800;
        if (longVal > DAC_MAX_CNT)
          shortVal = DAC_MAX_CNT;
        else if (longVal < DAC_MIN_CNT)
          shortVal = DAC_MIN_CNT;
        else
          shortVal = (short)longVal;
        outw(shortVal,pumaVarsG.base + DAC_BASE + 2*i);
      } /* endfor */
      return I_OK;
    case TORQUE:        /* Output to single DAC -- val is in N-m */
      longVal = (long)(*val * motorScaleG[chan]) + 0x0800;
      if (longVal > DAC_MAX_CNT)
        shortVal = DAC_MAX_CNT;
      else if (longVal < DAC_MIN_CNT)
        shortVal = DAC_MIN_CNT;
      else
        shortVal = (short)longVal;
      outw(shortVal,pumaVarsG.base + DAC_BASE + 2*chan);
      return I_OK;
    case VOLTAGE:       /* Output to single DAC -- val is in volts */
      if (*val >= 10.0){
        intVal = 0;
      } else if (*val <= -9.995){
        intVal = 0x0FFF;
      } else {
        intVal = (int)(*val * DAC_BITS_PER_VOLT) + 0x0800;
      } /* endif */
      outw(intVal,pumaVarsG.base + DAC_BASE + 2*chan);
      return I_OK;
    case POSITION6:     /* Preset all encoders -- val is in radians */
      for (i=0; i<6; i++){
        intVal = (int)(val[i] / encoderScaleG[i]) + encoderOffsetG[i];
        outw(intVal,pumaVarsG.base + ENC_LOAD_BASE + 2*i);
      } /* endfor */
      return I_OK;
    case POSITION:      /* Preset a single encoder -- val is in radians */
      intVal = (int)(*val / encoderScaleG[chan]) + encoderOffsetG[chan];
      outw(intVal,pumaVarsG.base + ENC_LOAD_BASE + 2*chan);
      return I_OK;
    default:
      return I_BAD_OPTION;
  } /* endswitch */
}

/* --- This function writes 1 or 6 integer values to the robot. The particular
   --- value is chosen with the OPTION parameter, which is one out of a
   --- list of enumerated ioOptionsT. CHAN ranges from 0 to 5 (corresponding
   --- to PUMA joints 1 - 6). It returns one of the enumerated ioErrorsT. */
short robot_shortwr(short option, short chan, short *val)
{
  short i;

  switch (option) {
    case TORQUE6:       /* Output to all DACs */
      for (i=0; i<6; i++){
        outw(val[i],pumaVarsG.base + DAC_BASE + 2*i);
      } /* endfor */
      return I_OK;
    case TORQUE:        /* Output to single DAC */
      outw(*val,pumaVarsG.base + DAC_BASE + 2*chan);
      return I_OK;
    case POSITION6:     /* Preset all encoders */
      for (i=0; i<6; i++){
        outw(val[i] + encoderOffsetG[i],
	     pumaVarsG.base + ENC_LOAD_BASE + 2*i);
        outw(val[i] + encoderOffsetG[i],
	     pumaVarsG.base + ENC_LOAD_BASE + 2*i);
      } /* endfor */
      return I_OK;
    case POSITION:      /* Preset a single encoder */
      outw(             *val + encoderOffsetG[chan],
           pumaVarsG.base + ENC_LOAD_BASE + 2*chan);
      return I_OK;
    case STATUS:        /* Write the entire status word */
      outw( *val,pumaVarsG.base + DISCRETE_REG);
      return I_OK;
     case ARB:
      outw( *val,pumaVarsG.base + chan);
      return I_OK;
    default:
      return I_BAD_OPTION;
  } /* endswitch */
}

/* --- This function reads 1 or 6 float values from the robot. The particular
   --- value is chosen with the OPTION parameter, which is one out of a
   --- list of enumerated ioOptionsT. CHAN ranges from 0 to 5 (corresponding
   --- to PUMA joints 1 - 6). It returns one of the enumerated ioErrorsT. */
short robot_fltrd(short option, short chan, float *val)
{
  short i;

  switch (option) {
    case POSITION6:     /* Read all encoders -- val is in radians */
      for (i=0; i<6; i++){
        val[i] = encoderScaleG[i] * (inw(pumaVarsG.base +
                        ENC_COUNT_BASE + 2*i) - encoderOffsetG[i]);
      } /* endfor */
      return I_OK;
    case POSITION:      /* Read a single encoder */
      *val = encoderScaleG[chan]*(inw(pumaVarsG.base +
                        ENC_COUNT_BASE + 2*chan) - encoderOffsetG[chan]);
      return I_OK;
    case POT:           /* Read a single ADC -- val is in volts */
      outb(chan,pumaVarsG.base + ADC_MUX_SELECT); /* select the MUX channel */
      inb(pumaVarsG.base + ADC_START);             /* trigger the conversion */
      /* --- Enable the ADC conversion complete bit. */
      pumaVarsG.discrete = pumaVarsG.discrete | ADC_MASK_BIT;
      outw(pumaVarsG.discrete,pumaVarsG.base + DISCRETE_REG);
      /* --- Wait for the conversion to start. TEST */
      while ((inw(pumaVarsG.base + STATUS_REG) & ADC_STAT_MASK) == 0);
      /* --- Wait for the conversion complete flag. */
      while ((inw(pumaVarsG.base + STATUS_REG) & ADC_STAT_MASK) != 0);
      *val = 0.0196 * (inb(pumaVarsG.base + ADC_VALUE) & 0x00FF);
      /* --- Disable and clear the ADC conversion complete bit. */
      pumaVarsG.discrete = pumaVarsG.discrete & (~ADC_MASK_BIT);
      outw(pumaVarsG.discrete,pumaVarsG.base + DISCRETE_REG);
      return I_OK;
    default:
      return I_BAD_OPTION;
  } /* endswitch */
} 

/* --- This function reads 1 or 6 LONG values to the robot. The particular
   --- value is chosen with the OPTION parameter, which is one out of a
   --- list of enumerated ioOptionsT. CHAN ranges from 0 to 5 (corresponding
   --- to PUMA joints 1 - 6). It returns one of the enumerated ioErrorsT. 
   --- Double reads were put in by MS for use in fast PC.  Double read 
   --- equivalent to inw_p() as per Linux I/O Port mini howto.
   */
short robot_longrd(short option, short chan, long *val)
{
  short i;
  long temp_val;

  switch (option) {
    case POSITION6:     /* Read all encoders */
      for (i=0; i<6; i++){
	val[i] = ((long)(short)inw(pumaVarsG.base + ENC_COUNT_BASE + 2*i)) -
                        encoderOffsetG[i];
	val[i] = ((long)(short)inw(pumaVarsG.base + ENC_COUNT_BASE + 2*i)) -
                        encoderOffsetG[i];
	
      } /* endfor */
      return I_OK;
    case POSITION:      /* Read a single encoder */
      *val = (long)(short)inw(pumaVarsG.base + ENC_COUNT_BASE + 2*chan) -
                        encoderOffsetG[chan];
      *val = (long)(short)inw(pumaVarsG.base + ENC_COUNT_BASE + 2*chan) -
                        encoderOffsetG[chan];
      return I_OK;
    case STATUS:        /* Read the entire status word into long int */
      *val = (long)((unsigned short)inw(pumaVarsG.base + STATUS_REG));
      *val = (long)((unsigned short)inw(pumaVarsG.base + STATUS_REG));
      return I_OK;
    case POT:           /* Read a single ADC */
      /* --- Enable the ADC conversion complete bit. */
      pumaVarsG.discrete = pumaVarsG.discrete | ADC_MASK_BIT;
      outw(pumaVarsG.discrete,pumaVarsG.base + DISCRETE_REG);

      outw(chan,pumaVarsG.base + ADC_MUX_SELECT); /* select the MUX channel */
				     /*MS rev E AND trigger the conversion */
      /* --- Wait for the conversion complete flag. */
      temp_val=0;
      while ((inw(pumaVarsG.base + STATUS_REG) & ADC_STAT_MASK) != 0)
	{
	  if (temp_val++ > 100000){
	    printf("Timed out waiting for A/D channed %d to complete\n",chan);
	    break;
	  }
	}
      *val = (long)(inw(pumaVarsG.base + ADC_VALUE) & 0xFFF0);
      /* --- Reset the ADC conversion complete bit. */
      pumaVarsG.discrete = pumaVarsG.discrete & (~ADC_MASK_BIT);
      outw(pumaVarsG.discrete,pumaVarsG.base + DISCRETE_REG);
      return I_OK;
    case POT6:          /* Read all ADC's */
      for(i=0; i<6; i++){
        outb(i, pumaVarsG.base + ADC_MUX_SELECT); /* select the MUX channel */
        inb(pumaVarsG.base + ADC_START);             /* trigger the conversion */
        /* --- Enable the ADC conversion complete bit. */
        pumaVarsG.discrete = pumaVarsG.discrete | ADC_MASK_BIT;
        outw(pumaVarsG.discrete,pumaVarsG.base + DISCRETE_REG);
        /* --- Wait for the conversion to start. TEST */
        while ((inw(pumaVarsG.base + STATUS_REG) & ADC_STAT_MASK) == 0);
        /* --- Wait for the conversion complete flag. */
        while ((inw(pumaVarsG.base + STATUS_REG) & ADC_STAT_MASK) != 0);
        val[i] = (long)(inb(pumaVarsG.base + ADC_VALUE) & 0x00FF);
        /* --- Disable and clear the ADC conversion complete bit. */
        pumaVarsG.discrete = pumaVarsG.discrete & (~ADC_MASK_BIT);
        outw(pumaVarsG.discrete,pumaVarsG.base + DISCRETE_REG);
      } /* endfor */
      return I_OK;
    case INDEX:     /* Read one index register */
      *val = (long)(short)inw(pumaVarsG.base + 2*chan) - encoderOffsetG[chan];
      *val = (long)(short)inw(pumaVarsG.base + 2*chan) - encoderOffsetG[chan];
      return I_OK;
    case INDEX6:     /* Read all index registers */
      for (i=0; i<6; i++){
        val[i] = (long)(short)inw(pumaVarsG.base + 2*i) - encoderOffsetG[i];
        val[i] = (long)(short)inw(pumaVarsG.base + 2*i) - encoderOffsetG[i];
      } /* endfor */
      return I_OK;
    default:
      return I_BAD_OPTION;
  } /* endswitch */
}

/* --- This function writes 1 or 6 integer values to the robot. The particular
   --- value is chosen with the OPTION parameter, which is one out of a
   --- list of enumerated ioOptionsT. CHAN ranges from 0 to 5 (corresponding
   --- to PUMA joints 1 - 6). It returns one of the enumerated ioErrorsT. */
short robot_control(short option, short chan, short *val)
{
  short tmp;

  switch (option) {
    case CLR_INDEX_BIT:         /* Clear one index bit */
      tmp = JT1_INDEX_BIT << chan;
      /* Clear the index bit */
      outw(pumaVarsG.discrete & ~tmp,pumaVarsG.base + DISCRETE_REG);
      tmp = pumaVarsG.discrete = pumaVarsG.discrete | tmp;
      /* Re-enable the index bit */
      outw(tmp,pumaVarsG.base + DISCRETE_REG);
      return I_OK;
    case CLR_INDEX_BITS:        /* Clear all index bits */
      /* Clear all index bits */
      outw(pumaVarsG.discrete & ~((short)ALL_INDEX_BITS),pumaVarsG.base + DISCRETE_REG);
      tmp = pumaVarsG.discrete = pumaVarsG.discrete | ALL_INDEX_BITS;
      /* Re-enable all index bits */
      outw(tmp,pumaVarsG.base + DISCRETE_REG);
      return I_OK;
    case CLR_ADC_BIT:   /* Clear A/D status bit */
      outw(pumaVarsG.discrete & ~((short)ADC_MASK_BIT),
	   pumaVarsG.base + DISCRETE_REG);
      tmp = pumaVarsG.discrete = pumaVarsG.discrete | ADC_MASK_BIT;
      outw(tmp,pumaVarsG.base + DISCRETE_REG );
      return I_OK;
    case POWER:         /* Enable/disable arm power */
      if (*val != 0){
        tmp = pumaVarsG.discrete = pumaVarsG.discrete | POWER_BIT;
        outw(tmp,pumaVarsG.base + DISCRETE_REG);
      } else {
        tmp = pumaVarsG.discrete = pumaVarsG.discrete & ~(POWER_BIT);
        outw(tmp,pumaVarsG.base + DISCRETE_REG);
      } /* endif */
      return I_OK;
    case STATUS:        /* Write the entire status word */
      outw(*val,pumaVarsG.base + DISCRETE_REG);
      return I_OK;
    default:
      return I_BAD_OPTION;
  } /* endswitch */
}

/* --- This function reads 1 or 6 short values of the robot. The particular
   --- value is chosen with the OPTION parameter, which is one out of a
   --- list of enumerated ioOptionsT. CHAN ranges from 0 to 5 (corresponding
   --- to PUMA joints 1 - 6). It returns one of the enumerated ioErrorsT. */
short robot_status(short option, short chan, short *val)
{
  short tmp;

  switch (option) {
    case INDEX_BIT:             /* Read one index bit */
      *val = inw(pumaVarsG.base + STATUS_REG);
      tmp = JT1_INDEX_BIT << chan;
      *val = *val & tmp;
      if (*val !=0)
        *val = 1;
      return I_OK;
    case ALL_INDEX_BITS:        /* Read all index bits */
      *val = inw(pumaVarsG.base + STATUS_REG);
      *val = *val & ALL_INDEX_BITS;
      return I_OK;
    case STATUS:        /* Read the entire status word */
      *val = inw(pumaVarsG.base + STATUS_REG);
      return I_OK;
    default:
      return I_BAD_OPTION;
  } /* endswitch */
}

/* --- This function reads a short values from the robot. 
   --- from the arbitrary port specified by chan */
short robot_shortrd(short option, short chan, short *val)
{
  short i;

  switch (option) {
    case POSITION6:     /* Read all encoders */
      for (i=0; i<6; i++){
        val[i] = inw(pumaVarsG.base + ENC_COUNT_BASE + 2*i) -
                        encoderOffsetG[i];
      } /* endfor */
      
      return I_OK;
    case ARB:      /* Read from arbitrary location */
      
      *val = inw(pumaVarsG.base + chan);
      *val = inw(pumaVarsG.base + chan);
      
      return I_OK;
  }
  return I_OK;
}


/* --- This function reads the POTs using 16-bit words. It is similar
   --- to robot_longrd(). This is *NOT* the recommended interface to
   --- the A/D's and is provided for backward compatibility and testing
   --- only. It returns one of the enumerated ioErrorsT. */
short robot_potrd(short option, short chan, long *val)
{
  short i;

  switch (option) {
    case POT:           /* Read a single ADC */
      outw(chan,pumaVarsG.base + ADC_MUX_SELECT); /* select the MUX channel */
						   /* trigger the conversion */
      /* --- Enable the ADC conversion complete bit. */
      pumaVarsG.discrete = pumaVarsG.discrete | ADC_MASK_BIT;
      outw(pumaVarsG.discrete,pumaVarsG.base + DISCRETE_REG);
      /* --- Wait for the conversion complete flag. */
      while ((inw(pumaVarsG.base + STATUS_REG) & ADC_STAT_MASK) != 0);
      *val = (long)(inw(pumaVarsG.base + ADC_VALUE) & 0x00FF);
      /* --- Disable and clear the ADC conversion complete bit. */
      pumaVarsG.discrete = pumaVarsG.discrete & (~ADC_MASK_BIT);
      outw(pumaVarsG.discrete,pumaVarsG.base + DISCRETE_REG);
      return I_OK;
    case POT6:          /* Read all ADC's */
      for(i=0; i<6; i++){
        outw(i, pumaVarsG.base + ADC_MUX_SELECT); /* select the MUX channel */
        inw(pumaVarsG.base + ADC_START);             /* trigger the conversion */
        /* --- Enable the ADC conversion complete bit. */
        pumaVarsG.discrete = pumaVarsG.discrete | ADC_MASK_BIT;
        outw(pumaVarsG.discrete,pumaVarsG.base + DISCRETE_REG);
        /* --- Wait for the conversion complete flag. */
        while ((inw(pumaVarsG.base + STATUS_REG) & ADC_STAT_MASK) != 0);
        val[i] = (long)(inw(pumaVarsG.base + ADC_VALUE) & 0x00FF);
        /* --- Disable and clear the ADC conversion complete bit. */
        pumaVarsG.discrete = pumaVarsG.discrete & (~ADC_MASK_BIT);
        outw(pumaVarsG.discrete,pumaVarsG.base + DISCRETE_REG);
      } /* endfor */
      return I_OK;
    default:
      return I_BAD_OPTION;
  } /* endswitch */
}

/* --- Simple PD controller with gravity compensation on joints 2 and 3. */
int pdgControl()
{
  float gravity[3];
  float torque[6];
  float factor;
  static float  Q_Old[2][6];

  robot_fltrd(POSITION6, 0, Q_MezG);            /* get current robot pos */
  /* calculate velocities */
  factor = 0.5;
  Qd_MezG[0] = (Q_MezG[0] - Q_Old[1][0]) * factor;
  Qd_MezG[1] = (Q_MezG[1] - Q_Old[1][1]) * factor;
  Qd_MezG[2] = (Q_MezG[2] - Q_Old[1][2]) * factor;
  Qd_MezG[3] = (Q_MezG[3] - Q_Old[1][3]) * factor;
  Qd_MezG[4] = (Q_MezG[4] - Q_Old[1][4]) * factor;
  Qd_MezG[5] = (Q_MezG[5] - Q_Old[1][5]) * factor;

  torque[0] = KPosG[0]*(Q_RefG[0] - Q_MezG[0]) + KVelG[0]*(Qd_RefG[0] - Qd_MezG[0]);
  torque[1] = KPosG[1]*(Q_RefG[1] - Q_MezG[1]) + KVelG[1]*(Qd_RefG[1] - Qd_MezG[1]);
  torque[2] = KPosG[2]*(Q_RefG[2] - Q_MezG[2]) + KVelG[2]*(Qd_RefG[2] - Qd_MezG[2]);
  torque[3] = KPosG[3]*(Q_RefG[3] - Q_MezG[3]) + KVelG[3]*(Qd_RefG[3] - Qd_MezG[3]);
  torque[4] = KPosG[4]*(Q_RefG[4] - Q_MezG[4]) + KVelG[4]*(Qd_RefG[4] - Qd_MezG[4]);
  torque[5] = KPosG[5]*(Q_RefG[5] - Q_MezG[5]) + KVelG[5]*(Qd_RefG[5] - Qd_MezG[5]);

  /* calculate gravity compensation for joints 2 and 3 
  gravity[2] = 0.75 * sin(Q_MezG[1] - Q_MezG[2]);
  gravity[1] = -2.0 * sin(Q_MezG[1]) - gravity[2];*/

  torque[1] += gravity[1];
  torque[2] += gravity[2];

  robot_fltwr(TORQUE6, 0, torque);           /* write torque values */

  /* --- Time shift the position values */
  Q_Old[1][0] = Q_Old[0][0];
  Q_Old[0][0] = Q_MezG[0];
  Q_Old[1][1] = Q_Old[0][1];
  Q_Old[0][1] = Q_MezG[1];
  Q_Old[1][2] = Q_Old[0][2];
  Q_Old[0][2] = Q_MezG[2];
  Q_Old[1][3] = Q_Old[0][3];
  Q_Old[0][3] = Q_MezG[3];
  Q_Old[1][4] = Q_Old[0][4];
  Q_Old[0][4] = Q_MezG[4];
  Q_Old[1][5] = Q_Old[0][5];
  Q_Old[0][5] = Q_MezG[5];

  return I_OK;
}
void
release_port()
{
  /* We don't need the ports anymore */
  if(pumaVarsG.base < 0x3ff) {
   if (ioperm(pumaVarsG.base, 64, 0)) {perror("ioperm"); exit(1);}
  }
  else iopl(0);
}
