/*
 * RTC  Version 2.0           Author :  Vincent Hayward
 * MAC          2.0                     School of Electrical Engineering
 *                                      Purdue University
 *      Dir     : h
 *      File    : pumac.c
 *      Remarks : generates the include file pumaload.h containing all the
 *                precalculated friction constants of the puma 600.
 *      Usage   : cc pumal.c -lm ; a.out
 */

/*
 * friction and gravity loadings for PUMA 600 robot
 * installed in Potter Building, room B20.
 * After Zhang Hong (TR..., spring, summer 1983)
 *
 * Joints 1, 3    have a positive velocity when encoder values decrease.
 * .......2 4 5 6.produce positive torque for negative current
 *
 *
 * torque to dac mapping:
 * dac = t * TDPA + TDPB  (+)
 * dac = t * TDNA + TDNB  (-)
 *
 * Those coeff are first determined by inverting the relations above.
 *
 * adc to torque mapping:
 * t = adc * ATPA + ATPB  (+)
 * t = adc * ATNA + ATNB  (-)
 *
 * dac to adc mapping:
 * dac = ALP adc + BET
 *
 * All values in dac, adc, N-mm        (from measurements in oz-in)
 */


#include <stdio.h>

#define PA1     (9.0623652 )
#define NA1     (8.6431404 )
#define PA2     (13.4400000)
#define NA2     (14.1120000)
#define PA3     (7.2788060 )
#define NA3     (7.0163582 )
#define PA4     (1.7217002 )
#define NA4     (1.7705593 )
#define PA5     (1.4585102 )
#define NA5     (1.4546531 )
#define PA6     (1.8182966 )
#define NA6     (1.7624517 )

#define PB1     (-2016.536 )
#define NB1     (1800.536  )
#define PB2     (-1500.0   )    /*(-1088.000 )*/
#define NB2     (1500.0    )    /*(1088.00   )*/
#define PB3     (-632.846  )
#define NB3     (631.385   )
#define PB4     (-116.0    )
#define NB4     (115.0     )
#define PB5     (-100.648  )
#define NB5     (146.648   )
#define PB6     (-90.0     )
#define NB6     (88.0      )

#define ALP1    (-5.8)
#define BET1    (-80.)
#define ALP2    (-5.8)
#define BET2    (-80.)
#define ALP3    (-5.8)
#define BET3    (-80.)
#define ALP4    (-5.8)
#define BET4    (-80.)
#define ALP5    (-5.8)
#define BET5    (-80.)
#define ALP6    (-5.8)
#define BET6    (-80.)



#define CV      7.05       /* oz in -> N mm */
#define ONE     1.
#define TWO     2.

main()
{
	FILE *fp = fdopen(creat("pumaload.h",  0644),"w");
	double pa1, pb1, na1, nb1;
	double pa2, pb2, na2, nb2;
	double pa3, pb3, na3, nb3;
	double pa4, pb4, na4, nb4;
	double pa5, pb5, na5, nb5;
	double pa6, pb6, na6, nb6;


	fprintf(fp, "#define TDPA1\t%f\n", pa1 =  ONE / (PA1 * CV));
	fprintf(fp, "#define TDPB1\t%f\n", pb1 = -(PB1) / (PA1));

	fprintf(fp, "#define TDPA2\t%f\n", pa2 = -ONE / (PA2 * CV));
	fprintf(fp, "#define TDPB2\t%f\n", pb2 =  (PB2) / (PA2));

	fprintf(fp, "#define TDPA3\t%f\n", pa3 =  ONE / (PA3 * CV));
	fprintf(fp, "#define TDPB3\t%f\n", pb3 = -(PB3) / (PA3));

	fprintf(fp, "#define TDPA4\t%f\n", pa4 = -ONE / (PA4 * CV));
	fprintf(fp, "#define TDPB4\t%f\n", pb4 =  (PB4) / (PA4));

	fprintf(fp, "#define TDPA5\t%f\n", pa5 = -ONE / (PA5 * CV));
	fprintf(fp, "#define TDPB5\t%f\n", pb5 =  (PB5) / (PA5));

	fprintf(fp, "#define TDPA6\t%f\n", pa6 = -ONE / (PA6 * CV));
	fprintf(fp, "#define TDPB6\t%f\n", pb6 =  (PB6) / (PA6));

	fprintf(fp, "#define TDNA1\t%f\n", na1 =  ONE / (NA1 * CV));
	fprintf(fp, "#define TDNB1\t%f\n", nb1 = -(NB1) / (NA1));

	fprintf(fp, "#define TDNA2\t%f\n", na2 = -ONE / (NA2 * CV));
	fprintf(fp, "#define TDNB2\t%f\n", nb2 =  (NB2) / (NA2));

	fprintf(fp, "#define TDNA3\t%f\n", na3 =  ONE / (NA3 * CV));
	fprintf(fp, "#define TDNB3\t%f\n", nb3 =  -(NB3) / (NA3));

	fprintf(fp, "#define TDNA4\t%f\n", na4 =  -ONE / (NA4 * CV));
	fprintf(fp, "#define TDNB4\t%f\n", nb4 =  (NB4) / (NA4));

	fprintf(fp, "#define TDNA5\t%f\n", na5 =  -ONE / (NA5 * CV));
	fprintf(fp, "#define TDNB5\t%f\n", nb5 =  (NB5) / (NA5));

	fprintf(fp, "#define TDNA6\t%f\n", na6 = -ONE / (NA6 * CV));
	fprintf(fp, "#define TDNB6\t%f\n", nb6 =  (NB6) / (NA6));

	fprintf(fp, "#define TDA1\t%f\n",  TWO / ((PA1 + NA1) * CV));
	fprintf(fp, "#define TDA2\t%f\n", -TWO / ((PA2 + NA2) * CV));
	fprintf(fp, "#define TDA3\t%f\n",  TWO / ((PA3 + NA3) * CV));
	fprintf(fp, "#define TDA4\t%f\n", -TWO / ((PA4 + NA4) * CV));
	fprintf(fp, "#define TDA5\t%f\n", -TWO / ((PA5 + NA5) * CV));
	fprintf(fp, "#define TDA6\t%f\n", -TWO / ((PA6 + NA6) * CV));


	fprintf(fp, "#define ATPA1\t%f\n", ALP1 / pa1);
	fprintf(fp, "#define ATPA2\t%f\n", ALP2 / pa2);
	fprintf(fp, "#define ATPA3\t%f\n", ALP3 / pa3);
	fprintf(fp, "#define ATPA4\t%f\n", ALP4 / pa4);
	fprintf(fp, "#define ATPA5\t%f\n", ALP5 / pa5);
	fprintf(fp, "#define ATPA6\t%f\n", ALP6 / pa6);
	fprintf(fp, "#define ATNA1\t%f\n", ALP1 / na1);
	fprintf(fp, "#define ATNA2\t%f\n", ALP2 / na2);
	fprintf(fp, "#define ATNA3\t%f\n", ALP3 / na3);
	fprintf(fp, "#define ATNA4\t%f\n", ALP4 / na4);
	fprintf(fp, "#define ATNA5\t%f\n", ALP5 / na5);
	fprintf(fp, "#define ATNA6\t%f\n", ALP6 / na6);

	fprintf(fp, "#define ATPB1\t%f\n", (BET1 - pb1) / pa1);
	fprintf(fp, "#define ATPB2\t%f\n", (BET2 - pb2) / pa2);
	fprintf(fp, "#define ATPB3\t%f\n", (BET3 - pb3) / pa3);
	fprintf(fp, "#define ATPB4\t%f\n", (BET4 - pb4) / pa4);
	fprintf(fp, "#define ATPB5\t%f\n", (BET5 - pb5) / pa5);
	fprintf(fp, "#define ATPB6\t%f\n", (BET6 - pb6) / pa6);
	fprintf(fp, "#define ATNB1\t%f\n", (BET1 - nb1) / na1);
	fprintf(fp, "#define ATNB2\t%f\n", (BET2 - nb2) / na2);
	fprintf(fp, "#define ATNB3\t%f\n", (BET3 - nb3) / na3);
	fprintf(fp, "#define ATNB4\t%f\n", (BET4 - nb4) / na4);
	fprintf(fp, "#define ATNB5\t%f\n", (BET5 - nb5) / na5);
	fprintf(fp, "#define ATNB6\t%f\n", (BET6 - nb6) / na6);
}
