/***************************************************************
 * PrPUMA560Defn.h
 *
 * This files contains constants for PUMA560.
 *
 ****************************************************************/

/*
 * modification history
 *----------------------
 *
 * 11/26/97: K.C. Chang: created.
 */

#ifndef _PrPUMA560Defn_h
#define _PrPUMA560Defn_h

#include "PrGlobalDefn.h"

#define PR_MANIP_ID             PR_PUMA560_ID
#define PR_MANIP_NOJ            6

// Saturated Torques
/*
#define T1MAX 97.6
#define T2MAX 156.4
#define T3MAX 89.4
#define T4MAX 24.2
#define T5MAX 20.1
#define T6MAX 21.3
*/

// amp/volt

#define PR_MANIP_AMP_CONSTANT_0      (1.0)
#define PR_MANIP_AMP_CONSTANT_1      (1.0)
#define PR_MANIP_AMP_CONSTANT_2      (1.0)
#define PR_MANIP_AMP_CONSTANT_3      (1.0)
#define PR_MANIP_AMP_CONSTANT_4      (1.0)
#define PR_MANIP_AMP_CONSTANT_5      (1.0)

// N-m/amp : guess
#define PR_MANIP_TORQUE_CONSTANT_0   (0.17460)
#define PR_MANIP_TORQUE_CONSTANT_1   (0.17460)
#define PR_MANIP_TORQUE_CONSTANT_2   (0.17460)
#define PR_MANIP_TORQUE_CONSTANT_3   (0.02667)
#define PR_MANIP_TORQUE_CONSTANT_4   (0.02667)
#define PR_MANIP_TORQUE_CONSTANT_5   (0.02667)

#define PR_MANIP_GEARRATIO_0   (-62.61)
#define PR_MANIP_GEARRATIO_1   (107.36)
#define PR_MANIP_GEARRATIO_2   (-53.69)
#define PR_MANIP_GEARRATIO_3   ( 76.01)
#define PR_MANIP_GEARRATIO_4   ( 71.91)
#define PR_MANIP_GEARRATIO_5   ( 76.73)
#define PR_MANIP_GEARRATIO { PR_MANIP_GEARRATIO_0, \
			     PR_MANIP_GEARRATIO_1, \
			     PR_MANIP_GEARRATIO_2, \
			     PR_MANIP_GEARRATIO_3, \
			     PR_MANIP_GEARRATIO_4, \
			     PR_MANIP_GEARRATIO_5 }


// volt/N-m = 1.0/(gear_ratio * N-m/amp * amp/volt)
#define PR_MANIP_MOTORSCALE { \
(1.0 \
 /(PR_MANIP_GEARRATIO_0*PR_MANIP_TORQUE_CONSTANT_0*PR_MANIP_AMP_CONSTANT_0)), \
(1.0 \
 /(PR_MANIP_GEARRATIO_1*PR_MANIP_TORQUE_CONSTANT_1*PR_MANIP_AMP_CONSTANT_1)), \
(1.0 \
 /(PR_MANIP_GEARRATIO_2*PR_MANIP_TORQUE_CONSTANT_2*PR_MANIP_AMP_CONSTANT_2)), \
(1.0 \
 /(PR_MANIP_GEARRATIO_3*PR_MANIP_TORQUE_CONSTANT_3*PR_MANIP_AMP_CONSTANT_3)), \
(1.0 \
 /(PR_MANIP_GEARRATIO_4*PR_MANIP_TORQUE_CONSTANT_4*PR_MANIP_AMP_CONSTANT_4)), \
(1.0 \
 /(PR_MANIP_GEARRATIO_5*PR_MANIP_TORQUE_CONSTANT_5*PR_MANIP_AMP_CONSTANT_5))}

#define  PR_MANIP_COUPLING34   ( 0.0139037) 
#define  PR_MANIP_COUPLING35   (-0.0130401) 
#define  PR_MANIP_COUPLING45   ( 0.1805560)

#define PR_MANIP_ENCODER_COUNT_PER_REV_0 0x03E8	// 250*4
#define PR_MANIP_ENCODER_COUNT_PER_REV_1 0x0320	// 200*4
#define PR_MANIP_ENCODER_COUNT_PER_REV_2 0x03E8	// 250*4
#define PR_MANIP_ENCODER_COUNT_PER_REV_3 0x03E8 // 250*4
#define PR_MANIP_ENCODER_COUNT_PER_REV_4 0x03E8	// 250*4
#define PR_MANIP_ENCODER_COUNT_PER_REV_5 0x03E8	// 250*4
#define PR_MANIP_ENCODER_COUNT_PER_REV { PR_MANIP_ENCODER_COUNT_PER_REV_0, \
				         PR_MANIP_ENCODER_COUNT_PER_REV_1, \
				         PR_MANIP_ENCODER_COUNT_PER_REV_2, \
				         PR_MANIP_ENCODER_COUNT_PER_REV_3, \
				         PR_MANIP_ENCODER_COUNT_PER_REV_4, \
				         PR_MANIP_ENCODER_COUNT_PER_REV_5 }

// no nest for PUMA560
#define PR_MANIP_POSITION_AT_NEST_0 (0.0)
#define PR_MANIP_POSITION_AT_NEST_1 (0.0)
#define PR_MANIP_POSITION_AT_NEST_2 (0.0)
#define PR_MANIP_POSITION_AT_NEST_3 (0.0)
#define PR_MANIP_POSITION_AT_NEST_4 (0.0)
#define PR_MANIP_POSITION_AT_NEST_5 (0.0)
#define PR_MANIP_POSITION_AT_NEST { PR_MANIP_POSITION_AT_NEST_0, \
		  		    PR_MANIP_POSITION_AT_NEST_1, \
				    PR_MANIP_POSITION_AT_NEST_2, \
				    PR_MANIP_POSITION_AT_NEST_3, \
				    PR_MANIP_POSITION_AT_NEST_4, \
				    PR_MANIP_POSITION_AT_NEST_5 }

#define PR_MANIP_POSITION_AT_MID_0 (0.0)
#define PR_MANIP_POSITION_AT_MID_1 (M_PI_2)
#define PR_MANIP_POSITION_AT_MID_2 (-M_PI_2)
#define PR_MANIP_POSITION_AT_MID_3 (-M_PI*38.0/180.0)
#define PR_MANIP_POSITION_AT_MID_4 (0.0)
#define PR_MANIP_POSITION_AT_MID_5 (0.0)

// find the correct offset(encoder mid position + position at nest)
// 2^16 / 2 = 2^15 = 0x8000
#define PR_MANIP_ENCODER_COUNT_ZERO { \
     (0x8000 + (int)((PR_MANIP_POSITION_AT_MID_0+PR_MANIP_POSITION_AT_NEST_0) \
		     /(2*M_PI)*PR_MANIP_ENCODER_COUNT_PER_REV_0 \
		     *PR_MANIP_GEARRATIO_0)), \
     (0x8000 + (int)((PR_MANIP_POSITION_AT_MID_1+PR_MANIP_POSITION_AT_NEST_1) \
		     /(2*M_PI)*PR_MANIP_ENCODER_COUNT_PER_REV_1 \
		     *PR_MANIP_GEARRATIO_1)), \
     (0x8000 + (int)((PR_MANIP_POSITION_AT_MID_2+PR_MANIP_POSITION_AT_NEST_2) \
		     /(2*M_PI)*PR_MANIP_ENCODER_COUNT_PER_REV_2 \
		     *PR_MANIP_GEARRATIO_2)), \
     (0x8000 + (int)((PR_MANIP_POSITION_AT_MID_3+PR_MANIP_POSITION_AT_NEST_3) \
		     /(2*M_PI)*PR_MANIP_ENCODER_COUNT_PER_REV_3 \
		     *PR_MANIP_GEARRATIO_3)), \
     (0x8000 + (int)((PR_MANIP_POSITION_AT_MID_4+PR_MANIP_POSITION_AT_NEST_4) \
		     /(2*M_PI)*PR_MANIP_ENCODER_COUNT_PER_REV_4 \
		     *PR_MANIP_GEARRATIO_4)), \
     (0x8000 + (int)((PR_MANIP_POSITION_AT_MID_5+PR_MANIP_POSITION_AT_NEST_5) \
		     /(2*M_PI)*PR_MANIP_ENCODER_COUNT_PER_REV_5 \
		     *PR_MANIP_GEARRATIO_5)) }

/*
  static float    mScale[3][6] = {
  {-0.08971, 0.12046, -0.10368, 0.45082, 0.51529,  0.43906}, // arm 0
  {-0.09018, 0.05166, -0.21789, 0.87804, 0.52545,  0.72232}, // arm 1
  {-0.08940, 0.05318, -0.10953, 0.49017, 0.49413,  0.52080}}; // arm 2

    {-0.10329, 0.05978, -0.11060, 0.41405, 0.47696, 0.46387},
    {-0.09761, 0.05357, -0.11060, 0.41405, 0.47696, 0.46387},
    {-0.09761, 0.05357, -0.11060, 0.41405, 0.47696, 0.46387}};

  volt = Tau*mScale[arm];

   static float    mScale[2][6] = { // guess 
    {-0.08971, 0.10500, -0.15368, 0.45082, 0.51529,  0.43906}, // samm 0
    {-0.09018, 0.04800, -0.21700, 0.87804, 0.52545,  0.72232}}; // samm 1
// without wrist
    {-0.08971, 0.09046, -0.15368, 0.45082, 0.51529,  0.43906},
    {-0.09018, 0.04500, -0.16500, 0.87804, 0.52545,  0.72232}};
//   Dave's
    {-0.08971, 0.12046, -0.10368, 0.45082, 0.51529,  0.43906},
    {-0.09018, 0.05166, -0.21789, 0.87804, 0.52545,  0.72232}};

*/

#endif // _PrPUMA560Defn_h

