/*
 * RCCL Version 1.0           Author :  Vincent Hayward
 * RTC          2.0                     School of Electrical Engineering
 * MAC          2.0                     Purdue University
 *      Dir     : h
 *      File    : pumac.c
 *      Remarks : generates the include file standata.h containing all the
 *                precalculated constants of the puma Stanford arm.
 *                Due to hardware problems, most of the constants have
 *                to be revised.
 *      Usage   : cc stanc.c -lm ; a.out
 */

/*************************************************************************
 notations : { E } : E expressed in degrees converted in radians
	     [ E ] : E of type double converted in type int
	     < E > : E ....... int    ................. short
	     | E | : mod 2*pi ; E + n*2*pi , n so that   0 <= | E | < 2*pi
	     constants in upercase letters, variables in lower.
	     (d) type double, (i) type int, (s) type short

      min is chosen as the furthest position clockwise
      angles increase in counter-clockwise motion
      rng is the angle between min and max positions
      relative coordinates measure angles in respect with the min pos

 let :
       DMIN  (d) the minimum position in deg.  absolute coordinates
       JMIN  (d) .................... in rad.  ....................
       DRNG  (d) the range in deg.
       JRNG  (d) ............ rad.
       DCAL  (d) the calibration position in deg. absolute coordinates
       ACAL  (d) ........................ in rad  ....................
       JCAL  (d) ........................ in rad. relative coordinates
       ECCL  (i) the counters content at the calibration position
       RATIO (d) the ratio  encoder pulses / physical unitd (mm,rad)
       sol   (d) be the raw angle given by the solution equation
       j     (d) a joint value expressed in relative coordinates
       e     (s) the final encoder value
       DMXV  (d) the maximum joint velocity in deg. / sec.
       JMXV  (d) the ......................... rd. / sec.

  this program generates the following constantes :

     JMIN = { DMIN }
     JRNG = { DRNG }
     RATIO = RATIO
     OFFSET = [ | { DCAL - DMIN } | * RATIO ] - ECCL
     JCAL = | { DCAL - DMIN } |
     ACAL = { DCAL }
     JMXV = { DMXV }

  to be used by :
     calibrate :
	sets  j = JCAL
	computes the REST transform from the joints values

     tr_to_jns :
	computes   j = | sol - JMIN |   and checks if  j < JRNG

     putenco  :
	e = < [ j * RATIO ] - OFFSET >

     setpoint :
	to monitor joint velocities

********************************************************************/

#include <stdio.h>

#define PIB2      1.57079632679489660
#define PI        3.14159265358979320
#define PIT2      6.28318530717958650
#define RADTODEG 57.29577951308232100
#define DEGTORAD  0.01745329251994330
#define YES 1
#define NO  0

#define D2 174.00   /* mm */

#define DMIN1 220.0
#define DRNG1 280.0

#define DMIN2   0.0
#define DRNG2 180.0

#define DMIN3 195.0
#define DRNG3 690.0

#define DMIN4 180.0
#define DRNG4 360.0

#define DMIN5 260.0
#define DRNG5 200.0

#define DMIN6 180.0
#define DRNG6 360.0

#define RATIO1 -12732.39545  /* pls/rad */
#define RATIO2  15915.49431  /* pls/rad */
#define RATIO3  35.64625147  /* pls/mm */
#define RATIO4 -4583.662360  /* pls/rad */
#define RATIO5  4583.662360  /* pls/rad */
#define RATIO6 -5570.423007  /* pls/rad */


#define ECCL1 32768
#define ECCL2 32768
#define ECCL3 26468
#define ECCL4 32768
#define ECCL5 25568
#define ECCL6 32768

#define ESQR1   551             /* how much to bring the arm in */
#define ESQR2   263             /* a square configuration from the */
#define ESQR3   581             /* zero index configuration */
#define ESQR4   228
#define ESQR5   -16
#define ESQR6   -13


#define DCAL1   0.0
#define DCAL2  90.0
#define DCAL3 385.0
#define DCAL4   0.0
#define DCAL5  90.0
#define DCAL6   0.0


#define DMXV1   600.
#define DMXV2   600.
#define DMXV3   500.    /* mm/sec */
#define DMXV4   600.
#define DMXV5   600.
#define DMXV6   600.

#define MXDC1   300
#define MXDC2   300
#define MXDC3   200
#define MXDC4   200
#define MXDC5   200
#define MXDC6   200


#define MXOC1   250
#define MXOC2   250
#define MXOC3   200
#define MXOC4   200
#define MXOC5   200
#define MXOC6   200

double range(a)
double a;
{
	while(a < 0.)
		a += PIT2;
	while(a >= PIT2)
		a -= PIT2;
	return(a);
}


#define ABS(a) ((a < 0) ? (-(a)) : (a))
#define ORDER(a, b) {int t; if (a > b) {t = b; b = a; a = t;}}

/*
 * create #define constants for stanford arm
 */

main()
{
	int o1, o2, o3, o4, o5, o6;
	int mx1, mx2, mx3, mx4, mx5, mx6;
	int mn1, mn2, mn3, mn4, mn5, mn6;
	FILE *fph;

	fph = fdopen(creat("standata.h", 0644), "w");

	fprintf(fph, "#define D2      %20.12f\n", D2);
	fprintf(fph, "#define D22     %20.12f\n", D2 * D2);
	fprintf(fph, "#define JMIN1   %20.12f\n", DEGTORAD * DMIN1);
	fprintf(fph, "#define JRNG1   %20.12f\n", DEGTORAD * DRNG1);
	fprintf(fph, "#define ACAL1   %20.12f\n", DEGTORAD * DCAL1);
	fprintf(fph, "#define JMIN2   %20.12f\n", DEGTORAD * DMIN2);
	fprintf(fph, "#define JRNG2   %20.12f\n", DEGTORAD * DRNG2);
	fprintf(fph, "#define ACAL2   %20.12f\n", DEGTORAD * DCAL2);
	fprintf(fph, "#define JMIN3   %20.12f\n",            DMIN3);
	fprintf(fph, "#define JRNG3   %20.12f\n",            DRNG3);
	fprintf(fph, "#define ACAL3   %20.12f\n",            DCAL3);
	fprintf(fph, "#define JMIN4   %20.12f\n", DEGTORAD * DMIN4);
	fprintf(fph, "#define JRNG4   %20.12f\n", DEGTORAD * DRNG4);
	fprintf(fph, "#define ACAL4   %20.12f\n", DEGTORAD * DCAL4);
	fprintf(fph, "#define JMIN5   %20.12f\n", DEGTORAD * DMIN5);
	fprintf(fph, "#define JRNG5   %20.12f\n", DEGTORAD * DRNG5);
	fprintf(fph, "#define ACAL5   %20.12f\n", DEGTORAD * DCAL5);
	fprintf(fph, "#define JMIN6   %20.12f\n", DEGTORAD * DMIN6);
	fprintf(fph, "#define JRNG6   %20.12f\n", DEGTORAD * DRNG6);
	fprintf(fph, "#define ACAL6   %20.12f\n", DEGTORAD * DCAL6);

	fprintf(fph, "#define JCAL1   %20.12f\n",
	range(DEGTORAD * (DCAL1 - DMIN1)));
	fprintf(fph, "#define JCAL2   %20.12f\n",
	range(DEGTORAD * (DCAL2 - DMIN2)));
	fprintf(fph, "#define JCAL3   %20.12f\n",
	 /* LIN JOINT */  DCAL3 - DMIN3);
	fprintf(fph, "#define JCAL4   %20.12f\n",
	range(DEGTORAD * (DCAL4 - DMIN4)));
	fprintf(fph, "#define JCAL5   %20.12f\n",
	range(DEGTORAD * (DCAL5 - DMIN5)));
	fprintf(fph, "#define JCAL6   %20.12f\n",
	range(DEGTORAD * (DCAL6 - DMIN6)));

	fprintf(fph, "#define RATIO1  %20.12f\n", RATIO1);
	fprintf(fph, "#define RATIO2  %20.12f\n", RATIO2);
	fprintf(fph, "#define RATIO3  %20.12f\n", RATIO3);
	fprintf(fph, "#define RATIO4  %20.12f\n", RATIO4);
	fprintf(fph, "#define RATIO5  %20.12f\n", RATIO5);
	fprintf(fph, "#define RATIO6  %20.12f\n", RATIO6);

	fprintf(fph, "#define OFFSET1   (%6d)\n", o1 =
	(int)(range((DCAL1 - DMIN1) * DEGTORAD) * RATIO1) - ECCL1);

	fprintf(fph, "#define OFFSET2   (%6d)\n", o2 =
	(int)(range((DCAL2 - DMIN2) * DEGTORAD) * RATIO2) - ECCL2);

	fprintf(fph, "#define OFFSET3   (%6d)\n", o3 =
	(int)((DCAL3 - DMIN3) * RATIO3) - ECCL3);

	fprintf(fph, "#define OFFSET4   (%6d)\n", o4 =
	(int)(range((DCAL4 - DMIN4) * DEGTORAD) * RATIO4) - ECCL4);

	fprintf(fph, "#define OFFSET5   (%6d)\n", o5 =
	(int)(range((DCAL5 - DMIN5) * DEGTORAD) * RATIO5) - ECCL5);

	fprintf(fph, "#define OFFSET6   (%6d)\n", o6 =
	(int)(range((DCAL6 - DMIN6) * DEGTORAD) * RATIO6) - ECCL6);

	fprintf(fph, "#define JMXV1 %f\n",  DEGTORAD * DMXV1);
	fprintf(fph, "#define JMXV2 %f\n",  DEGTORAD * DMXV2);
	fprintf(fph, "#define JMXV3 %f\n",             DMXV3);
	fprintf(fph, "#define JMXV4 %f\n",  DEGTORAD * DMXV4);
	fprintf(fph, "#define JMXV5 %f\n",  DEGTORAD * DMXV5);
	fprintf(fph, "#define JMXV6 %f\n",  DEGTORAD * DMXV6);

	fprintf(fph, "#define EMXV1 0%o\n",
	(int)(DEGTORAD * DMXV1 * ABS(RATIO1) / 1000.));
	fprintf(fph, "#define EMXV2 0%o\n",
	(int)(DEGTORAD * DMXV2 * ABS(RATIO2) / 1000.));
	fprintf(fph, "#define EMXV3 0%o\n",
	(int)(           DMXV3 * ABS(RATIO3) / 1000.));
	fprintf(fph, "#define EMXV4 0%o\n",
	(int)(DEGTORAD * DMXV4 * ABS(RATIO4) / 1000.));
	fprintf(fph, "#define EMXV5 0%o\n",
	(int)(DEGTORAD * DMXV5 * ABS(RATIO5) / 1000.));
	fprintf(fph, "#define EMXV6 0%o\n",
	(int)(DEGTORAD * DMXV6 * ABS(RATIO6) / 1000.));

	fprintf(fph, "#define ECCL1 0%o\n", ECCL1);
	fprintf(fph, "#define ECCL2 0%o\n", ECCL2);
	fprintf(fph, "#define ECCL3 0%o\n", ECCL3);
	fprintf(fph, "#define ECCL4 0%o\n", ECCL4);
	fprintf(fph, "#define ECCL5 0%o\n", ECCL5);
	fprintf(fph, "#define ECCL6 0%o\n", ECCL6);

	fprintf(fph, "#define XCCL1 0%o\n", ECCL1 + ESQR1);
	fprintf(fph, "#define XCCL2 0%o\n", ECCL2 + ESQR2);
	fprintf(fph, "#define XCCL3 0%o\n", ECCL3 + ESQR3);
	fprintf(fph, "#define XCCL4 0%o\n", ECCL4 + ESQR4);
	fprintf(fph, "#define XCCL5 0%o\n", ECCL5 + ESQR5);
	fprintf(fph, "#define XCCL6 0%o\n", ECCL6 + ESQR6);

	mn1 = -o1;
	mx1 = (int)(DEGTORAD * DRNG1 * RATIO1) - o1;
	ORDER(mn1, mx1);
	mn2 = -o2;
	mx2 = (int)(DEGTORAD * DRNG2 * RATIO2) - o2;
	ORDER(mn2, mx2);
	mn3 = -o3;
	mx3 = (int)(           DRNG3 * RATIO3) - o3;
	ORDER(mn3, mx3);
	mn4 = -o4;
	mx4 = (int)(DEGTORAD * DRNG4 * RATIO4) - o4;
	ORDER(mn4, mx4);
	mn5 = -o5;
	mx5 = (int)(DEGTORAD * DRNG5 * RATIO5) - o5;
	ORDER(mn5, mx5);
	mn6 = -o6;
	mx6 = (int)(DEGTORAD * DRNG6 * RATIO6) - o6;
	ORDER(mn6, mx6);

	fprintf(fph, "#define ECMN1 0%o\n", mn1);
	fprintf(fph, "#define ECMN2 0%o\n", mn2);
	fprintf(fph, "#define ECMN3 0%o\n", mn3);
	fprintf(fph, "#define ECMN4 0%o\n", mn4);
	fprintf(fph, "#define ECMN5 0%o\n", mn5);
	fprintf(fph, "#define ECMN6 0%o\n", mn6);

	fprintf(fph, "#define ECMX1 0%o\n", mx1);
	fprintf(fph, "#define ECMX2 0%o\n", mx2);
	fprintf(fph, "#define ECMX3 0%o\n", mx3);
	fprintf(fph, "#define ECMX4 0%o\n", mx4);
	fprintf(fph, "#define ECMX5 0%o\n", mx5);
	fprintf(fph, "#define ECMX6 0%o\n", mx6);

	fprintf(fph, "#define MXDC1 0%o\n", MXDC1);
	fprintf(fph, "#define MXDC2 0%o\n", MXDC2);
	fprintf(fph, "#define MXDC3 0%o\n", MXDC3);
	fprintf(fph, "#define MXDC4 0%o\n", MXDC4);
	fprintf(fph, "#define MXDC5 0%o\n", MXDC5);
	fprintf(fph, "#define MXDC6 0%o\n", MXDC6);

	fprintf(fph, "#define MXOC1 0%o\n", MXOC1);
	fprintf(fph, "#define MXOC2 0%o\n", MXOC2);
	fprintf(fph, "#define MXOC3 0%o\n", MXOC3);
	fprintf(fph, "#define MXOC4 0%o\n", MXOC4);
	fprintf(fph, "#define MXOC5 0%o\n", MXOC5);
	fprintf(fph, "#define MXOC6 0%o\n", MXOC6);
}
