/*-=-=-=-=-=-=-=-=-=-=-=- RCCL/RCI Notice of Copyright -=-=-=-=-=-=-=-=-=-=-*
 *									    *
 *	Copyright (C) 1991 by John E. Lloyd.				    *
 *									    *
 *	Information on copyright permissions, as well as a complete	    *
 *	disclaimer of warranty, is given in the file COPYRIGHT.		    *
 *									    *
 *-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=*/
/*---------------------------------------------------------------------------
 *		
 * MODULE NAME:	potcalCtrl
 *
 * DESCRIPTION:	Control level routines to read potentiometer values while
 *		the robot is in motion, and gather data to allow the
 *		contruction of a line fit relating pot values to
 *		encoder values.
 *
 *		These routines are used by the programs 'pumacal' and
 *		potcal.
 *
 * AUTHOR/DATE:	John Lloyd, July 25, 1990
 *
 * MODIFICATION HISTORY:
 *
 *--------------------------------------------------------------------------*/

#include "../h/rci.h"
#include "../h/chase.h"

#include "potcal.h"

unsigned int encSetVel[MAXJNTS];

potvalMonitor(cb, td, arg)
CHASE_CTRL *cb;
RCI_DESC *td;
int arg;
{
	POTCAL *pc;
	RCI_RBT *rbt;
	HOW *how;
	JLS *jls;
	LINE_DATA *data;
	register int j;

	if ((pc = (POTCAL*)getMemByAddr((void*)arg)) == NULL)
	 { rciAbort (0, "potvalMonitor() -- can't get POTCAL structure\n");
	   return;
	 }
	rbt = TASKROBOT(td,0);
	how = rbt->how;
	jls = rbt->jls;

	if (pc->initialize)
	 { for (j=0; j<jls->numJ; j++)
	    { data = &pc->data[j];

	      data->sumEnc = 0.0;
	      data->sumPot = 0.0;
	      data->sumEncEnc = 0.0;
	      data->sumEncPot = 0.0;
	      data->n = 0;
	      data->spurious = 0;
	    }
	   pc->sampling = 0;
	   pc->complete = 0;
	   pc->initialize = 0;
	   return;
	 }
	for (j=0; j<jls->numJ; j++)
	 { 	   
	   if (pc->jntSelect & (1<<j))
	    { 
	      int positiveTravel;

	      /* if we are within the sample interval, aquire sample */

	      positiveTravel = (pc->sampleStop[j] > pc->sampleStart[j]);

	      if ((positiveTravel  && (how->encSet[j] > pc->sampleStart[j])
				   && (how->encSet[j] < pc->sampleStop[j])) ||
		  (!positiveTravel && (how->encSet[j] < pc->sampleStart[j])
				   && (how->encSet[j] > pc->sampleStop[j])) )
	       { int potval;

		 data = &pc->data[j];
		 if (!(pc->sampling & (1<<j)))
		  { pc->sampling |= (1<<j);
		    data->firstEnc = how->enc[j];
		  }
		 data->lastEnc = how->enc[j];

		 potval = (pc->testMode ? fakePotValue(j, how->enc) 
					: how->potInput[j]);

		 data->n++;
		 data->sumEnc += how->enc[j];
		 data->sumPot += potval;
		 data->sumEncEnc += how->enc[j] * (double)how->enc[j];
		 data->sumEncPot += how->enc[j] * (double)potval;
	       }
	      else if (pc->sampling & (1<<j))
	       { /* if we are past the end of the sample interval, set the
		    completed flag */
		 pc->complete |= (1<<j);
		 pc->sampling &= ~(1<<j);
		 if (pc->setCalibMode)
		  { SET_CALIBMODE (rbt, pc->complete);
		  }
	       }
	    }
	 }
}

fakePotValue(j, e)
int j;
int *e;
{
	/* returns a simulated pot input so that the potentiometer
	   slope/intercept values for each joint is (11+j) and 111*(j+1). */
	   
	return ( (e[j]-111*(j+1)) / (float)(11+j) );	
}

