/***************************************************************
 * PrPUMA260Defn.h
 *
 * This files contains constants for PUMA260.
 *
 ****************************************************************/

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

#ifndef _PrPUMA260Defn_h
#define _PrPUMA260Defn_h

#include "PrGlobalDefn.h"

#define PR_MANIP_ID             PR_PUMA260_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
#define PR_MANIP_TORQUE_CONSTANT_0   (0.8125*PR_LB_TO_KG*PR_GRAVITY_CONSTANT*PR_INCH_TO_METER)
#define PR_MANIP_TORQUE_CONSTANT_1   (0.8125*PR_LB_TO_KG*PR_GRAVITY_CONSTANT*PR_INCH_TO_METER)
#define PR_MANIP_TORQUE_CONSTANT_2   (0.8125*PR_LB_TO_KG*PR_GRAVITY_CONSTANT*PR_INCH_TO_METER)
#define PR_MANIP_TORQUE_CONSTANT_3   (0.3000*PR_LB_TO_KG*PR_GRAVITY_CONSTANT*PR_INCH_TO_METER)
#define PR_MANIP_TORQUE_CONSTANT_4   (0.3000*PR_LB_TO_KG*PR_GRAVITY_CONSTANT*PR_INCH_TO_METER)
#define PR_MANIP_TORQUE_CONSTANT_5   (0.3000*PR_LB_TO_KG*PR_GRAVITY_CONSTANT*PR_INCH_TO_METER)

#define PR_MANIP_GEARRATIO_0   (46.753247)      // 360/7.70
#define PR_MANIP_GEARRATIO_1   (-70.038911)     // 360/5.14
#define PR_MANIP_GEARRATIO_2   (42.857143)      // 360/8.40
#define PR_MANIP_GEARRATIO_3   (-43.530834)     // 360/8.27
#define PR_MANIP_GEARRATIO_4   (-39.387309)     // 360/9.14
#define PR_MANIP_GEARRATIO_5   (-31.774051)     // 360/11.33
#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.25) 
#define PR_MANIP_COUPLING35    (0.025) 
#define PR_MANIP_COUPLING45    (-0.17)

#define PR_MANIP_ENCODER_COUNT_PER_REV_0 0x03E8
#define PR_MANIP_ENCODER_COUNT_PER_REV_1 0x03E8
#define PR_MANIP_ENCODER_COUNT_PER_REV_2 0x03E8
#define PR_MANIP_ENCODER_COUNT_PER_REV_3 0x0320
#define PR_MANIP_ENCODER_COUNT_PER_REV_4 0x0320
#define PR_MANIP_ENCODER_COUNT_PER_REV_5 0x0320
#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 }


#define PR_MANIP_POSITION_AT_NEST_0 (-270.0000*M_PI/180.0)
#define PR_MANIP_POSITION_AT_NEST_1 (-202.8394*M_PI/180.0)
#define PR_MANIP_POSITION_AT_NEST_2 ( -14.6845*M_PI/180.0)
#define PR_MANIP_POSITION_AT_NEST_3 (  90.0000*M_PI/180.0)
#define PR_MANIP_POSITION_AT_NEST_4 (  90.0000*M_PI/180.0)
#define PR_MANIP_POSITION_AT_NEST_5 (  (52.3465 - 22.5)*M_PI/180.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 ( M_PI_2)
#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_2)
#define PR_MANIP_POSITION_AT_MID_4 ( 0.0)
#define PR_MANIP_POSITION_AT_MID_5 (-M_PI_4)

// 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)) }

#endif // _PrPUMA260Defn_h
