/*
 * RTC  Version 2.0           Author :  Vincent Hayward
 *                                      School of Electrical Engineering
 *                                      Purdue University
 *      Dir     : rtc
 *      File    : cvae.c
 *      Remarks : Mapping functions encoder/angles/ranges cur/torques.
 *                Torque stuff not implemented for the stanford arm.
 *      Usage   : part of the library
 */

/*LINTLIBRARY*/

#include "../h/which.h"
#include "../h/umac.h"
#ifdef PUMA
#include "../h/pumadata.h"
#include "../h/pumaload.h"
#endif
#ifdef STAN
#include "../h/standata.h"
#include "../h/stanload.h"
#endif

#define J1      0
#define J2      1
#define J3      2
#define J4      3
#define J5      4
#define J6      5
#define PIT2    6.28318530717958650
#define PI      3.14159265358979320
#define DEGTORAD        0.01745329251994330


/*
 * take ranges, make encoder values
 * return : 01 02 04 010 020 040 'ored' codes if joint within 5 dg limit
 */

#define BOUND   5. * DEGTORAD

rngtoenc(e, r)  /*::*/
register unsigned short *e;
register double *r;
{
	register int code = 0;

	if (r[J1] < BOUND || r[J1] > JRNG1 - BOUND)
		code |= 01;
	if (r[J2] < BOUND || r[J2] > JRNG2 - BOUND)
		code |= 02;
	if (r[J3] < BOUND || r[J3] > JRNG3 - BOUND)
		code |= 04;
	if (r[J4] < BOUND || r[J4] > JRNG4 - BOUND)
		code |= 010;
	if (r[J5] < BOUND || r[J5] > JRNG5 - BOUND)
		code |= 020;
	if (r[J6] < BOUND || r[J6] > JRNG6 - BOUND)
		code |= 040;
	e[J1] = (int)(r[J1] * RATIO1) - OFFSET1;
	e[J2] = (int)(r[J2] * RATIO2) - OFFSET2;
	e[J3] = (int)(r[J3] * RATIO3) - OFFSET3;
	e[J4] = (int)(r[J4] * RATIO4) - OFFSET4;
#ifdef STAN
	e[J5] = (int)(r[J5] * RATIO5) - OFFSET5;
	e[J6] = (int)(r[J6] * RATIO6) - OFFSET6;
#endif
#ifdef PUMA
	e[J5] = (int)(r[J5] * RATIO5 + r[J4] * RATI54) - OFFSET5;
	e[J6] = (int)(r[J6] * RATIO6 + r[J5] * RATI65 + r[J4] * RATI64)
		 - OFFSET6;
#endif
	return(code);
}

/*
 * convert encoder values to ranges
 */

enctorng(r, e) /*::*/
register double *r;
register unsigned short *e;
{
	r[J1] = (double)(e[J1] + OFFSET1) / RATIO1;
	r[J2] = (double)(e[J2] + OFFSET2) / RATIO2;
	r[J3] = (double)(e[J3] + OFFSET3) / RATIO3;
	r[J4] = (double)(e[J4] + OFFSET4) / RATIO4;
#ifdef STAN
	r[J5] = (double)(e[J5] + OFFSET5) / RATIO5;
	r[J6] = (double)(e[J6] + OFFSET6) / RATIO6;
#endif
#ifdef PUMA
	r[J5] = ((double)(e[J5] + OFFSET5) - r[J4] * RATI54) / RATIO5;
	r[J6] = ((double)(e[J6] + OFFSET6) - r[J5] * RATI65 - r[J4] * RATI64)
		 / RATIO6;
#endif
}


static double rngpp(a) /*##*/
double a;
{
	while(a <= -PI)
		a += PIT2;
	while(a > PI)
		a -= PIT2;
	return(a);
}



static double rng0p(a) /*##*/
double a;
{
	while(a < 0.)
		a += PIT2;
	while(a >= PIT2)
		a -= PIT2;
	return(a);
}

/*
 * convert ranges to angles
 */

rngtoang(a ,r) /*::*/
register double *a, *r;
{
	a[J1] = rngpp(r[J1] + JMIN1);
	a[J2] = rngpp(r[J2] + JMIN2);
#ifdef PUMA
	a[J3] = rngpp(r[J3] + JMIN3);
#endif
#ifdef STAN
	a[J3] = r[J3] + JMIN3;
#endif
	a[J4] = rngpp(r[J4] + JMIN4);
	a[J5] = rngpp(r[J5] + JMIN5);
	a[J6] = rngpp(r[J6] + JMIN6);
}


/*
 * convert angles to ranges
 */

angtorng(r, a) /*::*/
register double *r, *a;
{
	r[J1] = rng0p(a[J1] - JMIN1);
	r[J2] = rng0p(a[J2] - JMIN2);
#ifdef PUMA
	r[J3] = rng0p(a[J3] - JMIN3);
#endif
#ifdef STAN
	r[J3] = a[J3] - JMIN3;
#endif
	r[J4] = rng0p(a[J4] - JMIN4);
	r[J5] = rng0p(a[J5] - JMIN5);
	r[J6] = rng0p(a[J6] - JMIN6);
}


/*
 * convert angles to encoders
 * return : 01 02 04 010 020 040 'ored' codes if joint within 5 dg limit
 */

angtoenc(e, a) /*::*/
register double *a;
register unsigned short *e;
{
	double r[6];

	angtorng(r, a);
	return(rngtoenc(e, r));
}


/*
 * convert encoders to angles
 */

enctoang(a, e) /*::*/
register unsigned short *e;
register double *a;
{
	enctorng(a, e);
	rngtoang(a, a);
}


/*
 * map torques to currents, add coulomb friction terms
 */

tortodac(d, t, v, o) /*::*/
register
short *d;
register
double *t;
register
unsigned short *v, *o;
{
	register int vp;        /* velocity positive predicate  */

	vp = v[J1] < o[J1];
	d[J1] = (v[J1] == o[J1]) ? (t[J1] * TDA1) :
		(t[J1] * ((vp) ? TDPA1 : TDNA1)) + ((vp) ? TDPB1 : TDNB1);

	vp = v[J2] > o[J2];
	d[J2] = (v[J2] == o[J2]) ? (t[J2] * TDA2) :
		(t[J2] * ((vp) ? TDPA2 : TDNA2)) + ((vp) ? TDPB2 : TDNB2);

	vp = v[J3] < o[J3];
	d[J3] = (v[J3] == o[J3]) ? (t[J3] * TDA3) :
		(t[J3] * ((vp) ? TDPA3 : TDNA3)) + ((vp) ? TDPB3 : TDNB3);

	vp = v[J4] > o[J4];
	d[J4] = (v[J4] == o[J4]) ? (t[J4] * TDA4) :
		(t[J4] * ((vp) ? TDPA4 : TDNA4)) + ((vp) ? TDPB4 : TDNB4);

	vp = v[J5] > o[J5];
	d[J5] = (v[J5] == o[J5]) ? (t[J5] * TDA5) :
		(t[J5] * ((vp) ? TDPA5 : TDNA5)) + ((vp) ? TDPB5 : TDNB5);

	vp = v[J6] > o[J6];
	d[J6] = (v[J6] == o[J6]) ? (t[J6] * TDA6) :
		(t[J6] * ((vp) ? TDPA6 : TDNA6)) + ((vp) ? TDPB6 : TDNB6);
}


/*
 * map currents to torques, remove coulomb friction terms
 */

adctotor(t, a, v, o) /*::*/
register
double *t;
register
short *a;
register
unsigned short *v, *o;
{
	register int vp;        /* velocity positive predicate  */

	vp = v[J1] < o[J1];
	t[J1] = (a[J1] * ((vp) ? ATPA1 : ATNA1)) + ((vp) ? ATPB1 : ATNB1);

	vp = v[J2] > o[J2];
	t[J2] = (a[J2] * ((vp) ? ATPA2 : ATNA2)) + ((vp) ? ATPB2 : ATNB2);

	vp = v[J3] < o[J3];
	t[J3] = (a[J3] * ((vp) ? ATPA3 : ATNA3)) + ((vp) ? ATPB3 : ATNB3);

	vp = v[J4] > o[J4];
	t[J4] = (a[J4] * ((vp) ? ATPA4 : ATNA4)) + ((vp) ? ATPB4 : ATNB4);

	vp = v[J5] > o[J5];
	t[J5] = (a[J5] * ((vp) ? ATPA5 : ATNA5)) + ((vp) ? ATPB5 : ATNB5);

	vp = v[J6] > o[J6];
	t[J6] = (a[J6] * ((vp) ? ATPA6 : ATNA6)) + ((vp) ? ATPB6 : ATNB6);
}
