/*-=-=-=-=-=-=-=-=-=-=-=- RCCL/RCI Notice of Copyright -=-=-=-=-=-=-=-=-=-=-*
 *									    *
 *	Copyright (C) 1987, 1991 by John E. Lloyd and Vincent Hayward.	    *
 *									    *
 *	Information on copyright permissions, as well as a complete	    *
 *	disclaimer of warranty, is given in the file COPYRIGHT.		    *
 *      Modified by Matthew Stein 2001                                      *
 *									    *
 *-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=*/

/*-=-=-=-=-=-=-=-=-=-=-=- Description -=-=-=-=-=-=-=-=-=-=-=-=-=-=-==-=-=-=-*
  This module is an supplement to RCI/RCCL for RTLINUX using a single 
  processor to perform both the joint-level servo, the control level and
  the planning level.

  Under RTLinux, this program will be loaded into the kernel using insmod
  and run on a real-time interrupt.  The interrupt scheduling is *not -
  preemptive* and this means the control level formerly implemented by
  rci will have to share interrupts with the JLS interrupt.  The control
  level must execute on an integral number of JLS interrupt periods and 
  execution will be scheduled by the JLS task.

  Communication with the JLS task will be through shared memory and by
  real-time fifo as provided by the RTLinux package.  The fifo will activate
  and de-activate the JLS interrupt routine.  Once running, it will communicate
  with the planning task via shared memory and semaphore.

  The JLS routine will be as lean as possible and will operate on the following 
  principles:
  * Each interrupt it will read 6 positions (into shared memory), calculate
    6 control outputs and send these to the DACs
  * Other operations it will only do on request from the control level and it
    will only do one of these each cycle
 *-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=*/

/* These included for Real-Time Linux */
#include <linux/errno.h>
#include <rtl.h>
#include <time.h>
#include <unistd.h>
#include <rtl_sched.h>
#include <rtl_fifo.h>
#include "mbuff.h"

/* Definition of the real-time fifo numbers */
#include "../h/fifos.h"

pthread_t jlstask;

/* These includes for RCI */


#include "../h/controlLevel.h"
#include "../h/pumaInterface.h"

/* MS Include the block of parameters originally loaded into the joint control
cards on the Mark-II standard 560.  Our JLS routine will use these parameters.
*/
#include "mk2_std.560.lin.c"

/* the contents of shared memory may change at any time, thus volatile */
volatile char * shm1;
static	robot_JLS_type *robot_JLS;
static	int started_flag;
static  int started_cl=0;
static  int Alarm_count=0;
static  int Beat_count=0;
static  int Read_pots=0; 
/* Pipe for dumping data */
static  int dump_pipe;

struct dump_data {
  int u;
  int setpt;
  int pos;
  int deriv;
} dump;


/* Number of .9 ms intervals to run the control level */
static  int jlsBeatInterval = 10; /* This sets the rci system clock to a
				     9 ms interval.  Can be changed by
				     contol level
				  */
static  int jlsAlarmInterval = 0; /* This should be set by the control level
				     requesting an alarm signal */

static short prev_local_pos[6];  /* Storage of previous value for derivitive 
				    calculation */
static short e_d[6];             /* Local storage of desired encoder value, 
				    updated at servo rate */

/* This task will read start and stop commands from the TASK_CONTROL_FIFO  */
void *thread_code(void *t)
{
  jlsMessageType msg;
  RciTimerEvent tev;
  short u[6];
  short e_delta[6];
  short old_setpt[6];
  short count[6];
  short local_pos[6];
  short INTENABLE[6];
  short deriv;
  int i,j;

  for(i=0;i<6;i++) {
    e_delta[i]=0;
    old_setpt[i]=0;
    count[i]=0;
  }

  while (1) {
    int ret;
    int err;
    
    /* This function waits until the next time period to return, i.e. this is the
       function that makes this thread periodic.  When this returns, perform all the
       tasks necessary and then return to this line due to the while(1) above.
    */
    ret = pthread_wait_np();
    /* Fist check if new commands have been sent over the fifo, and process 
       as necessary
    */
    if ((err = 
     rtf_get ( TASK_CONTROL_FIFO, &msg, sizeof(msg))) == sizeof(msg)) {
      rtl_printf("Executing the \"%d\" command \n",  msg.command);
      switch (msg.command) {
      case START_TASK:
	started_flag = 1;
	/* period is expressed in nanoseconds, jntClkRate specified in 
	   mk2_std.560.c as units of (.1 msec), thus the multiplier of
	   100000.  My how times change.*/
	pthread_make_periodic_np(pthread_self(), gethrtime(), 
				 jntClkRate * 100000);
	
	/* These statements initialize the robot each time the
	   real-time task is re-activated */
	/* Make sure there is a default number here, the number 30
	   will result in 27 milliseconds */	
	robot_JLS->ninter=30; 
	
	/* Initialize the discrete output to enable the index mark
	   found flags by setting the index masks 1.  Take special care
	   to preserve the calib bit, this is set by software and is used
	   between programs to indicate calibration
	*/
	robot_JLS->status=pumaDesRd();
	robot_JLS->discrete=(robot_JLS->status & CALIB_BIT )| ALL_INDEX_BITS;
	pumaControl(CLR_INDEX_BITS,robot_JLS->discrete,0,0);
	/* Initialize pos vector and local control setpoint to current position
	 */
	for (j=0; j<6; j++){
	  robot_JLS->pos[j] = pumaEncRd(j);
	  e_d[j]=robot_JLS->pos[j];     
	}

	break;
      case STOP_TASK:
	started_flag = 0;
	/* Suspend right here until my_handler wakes us up with a call to
	   pthread_wakeup_np.  This happens when data arrives on the fifo, and
	   thus we do not set started_flag until the the fifo is read and the
	   command is START_TASK
	*/ 
	pthread_suspend_np(pthread_self());
	break;
      case START_ALRM_MSG:
	/* Send am alarm message to the CONTROL_LEVEL_FIFO to
	   activate the alarm task task
	*/
	
	if(msg.arg > 0 && msg.arg < 1000 ) /* This 1000 is arbitrary */
	  jlsAlarmInterval = msg.arg; 
	break;
      case STOP_ALRM_MSG:
	/* Stop sending alarm messages to the CONTROL_LEVEL_FIFO to
	   activate the alarm task.   However,
	   stopping beat messages does not stop the beat messages. 
	*/ 
	jlsAlarmInterval =0;
	break;

	/* MS Beat messages are activated by the planning task 
	   when creating monitor tasks.  These are the main heartbeat
	   of the planning task.
	*/
      case START_BEAT_MSG:
	/* Start sending periodic beat messages to the CONTROL_LEVEL_FIFO to
	   activate the rci control task.  Alarm messages must already be active
	   for beat messages to be activated.
	*/ 
	started_cl = 1;

	if(msg.arg > 0 && msg.arg < 1000 ) /* This 1000 is arbitrary */
	  jlsBeatInterval = msg.arg;
	
	pthread_make_periodic_np(pthread_self(), gethrtime(), 
				 jntClkRate * 100000);
	
	break;
      case STOP_BEAT_MSG:
	
	/* Stop sending periodic beat messages to the CONTROL_LEVEL_FIFO. 
	   Halting these should also stop the
	   periodic task from running
	*/ 
	started_cl = 0;
	
	pthread_suspend_np(pthread_self());
	
	break;
      default:
	rtl_printf("RTL task: bad command\n");
      }
    }
    
    if(started_flag) {
      /* Check for new setpoint and implement without requiring explicit 
	 handshake with planning level */
      for(j=0;j<6;j++) {
	if(old_setpt[j] !=  robot_JLS->setpt[j]) {
	  e_delta[j]= 
	    (robot_JLS->setpt[j]-e_d[j])/ 
	    (robot_JLS->ninter > 0 ? robot_JLS->ninter : 1);
	  count[j]=robot_JLS->ninter;
	  old_setpt[j]=robot_JLS->setpt[j];
	}
      }

      /* Get all positions at once */
      for (j=0; j<6; j++){
	local_pos[j] = pumaEncRd(j);
      }

      /* This function implements the  P-D control for
	 each joint based on the Joint Servo Card control algorithm reported
	 in Peter Corke's "The Unimation Puma Servo System" report.
	 Peter Corke is my hero.  Variable names are consistent with those
	 used in the report.
	 defaultRamTable is in mk2_std.560.c
	 
      */

      for (i=0; i<6; i++){

	e_d[i] += e_delta[i];
	if(--count[i] == 0) e_delta[i]=0;
	
	err=e_d[i]-local_pos[i];
	
	if(abs(err-defaultRamTable[POSTOL][i+1]))
	  robot_JLS->TOL[i]=1;
	else 
	  robot_JLS->TOL[i]=0;

	if(abs(err-defaultRamTable[INTTOL][i+1]))
	  INTENABLE[i]=1;
	else 
	  INTENABLE[i]=0;

	/* 
	 * PD Control
	 */

	u[i]=
	  err*defaultRamTable[PGAIN][i+1];

	deriv = prev_local_pos[i]-local_pos[i];

	
	u[i]+=deriv*defaultRamTable[VGAIN][i+1];
	
	pumaDacWr(u[i],i);

	robot_JLS->pos[i] = local_pos[i];
	prev_local_pos[i] = local_pos[i];
      }
      /* MS Debugging */
      /* Dump the data for graphing */

      dump.u=u[5];
      dump.setpt=setpt[5];
      dump.pos=local_pos[5];
      dump.deriv=deriv;
      rtf_put(6,&dump, sizeof(dump));


      /* Read the discrete state every control cycle, this allows polling
	 for power on.
      */
      robot_JLS->status=pumaDesRd();

      if(Read_pots) {
	for (i=0;i<6;i++) 
	  robot_JLS->pot[i]=pumaPotRd(i,robot_JLS->discrete);
      }

      switch(robot_JLS->cl_request) {
      case JLS_NOREQ:         /* No request to the JLS process */
	break;
      case JLS_READ_STATUS:   /* Request a read of the status word */
	robot_JLS->status=pumaDesRd();
	robot_JLS->cl_request=JLS_NOREQ;
	break;
      case JLS_READ_INDEX:    /* Request a read of the indexes */
	for (i=0;i<6;i++) 
	  robot_JLS->index[i]=pumaIndRd(i);
	robot_JLS->cl_request=JLS_NOREQ;
	break;
      case JLS_READ_POT:      /* Request continuous reading of  pots */
	Read_pots=1;
	robot_JLS->cl_request=JLS_NOREQ;
	break;
	
      case JLS_DONT_READ_POT:      /* Halt continuous reading of  pots */
	Read_pots=0;
	robot_JLS->cl_request=JLS_NOREQ;
	break;
	
      case JLS_WRITE_DES:     /* Request a write of the discrete outputs */
	pumaDesWr(robot_JLS->discrete);
	robot_JLS->cl_request=JLS_NOREQ;
	break;
      case JLS_STOP_REQ:     /* Request to stop received when puma_release  */
	robot_JLS->discrete=robot_JLS->discrete & (~POWER_BIT);
	pumaDesWr(robot_JLS->discrete);
	robot_JLS->cl_request=JLS_NOREQ;
	break;
      case JLS_WRITE_ENC:     /* Request a write of encoder preloads */
	for (i=0;i<6;i++) { 
	  pumaEncWr(robot_JLS->setpt[i],i); /* Here we use the setpt variables
					       to pre-load the encoders */
	  e_d[i]=robot_JLS->setpt[i];       /* also set the internal desired
					       setpoint to these values
					    */
	  old_setpt[i]=robot_JLS->setpt[i];
	  e_delta[i]=0;
	}
	robot_JLS->cl_request=JLS_NOREQ;
	break;
      default:
	break;
      }

    } /* End if(started_flag) */
    
    /* Control does not have to be started to activate the RciCtrlTask */
    /* JLS process activates control level process on
       an integral number of samples, as defined in mk2_std.560.c.  */
    if(started_cl) {
      if(jlsAlarmInterval != 0) {
	if(++Alarm_count >= jlsAlarmInterval) {
	  
	  tev.req=RCI_ALARM_READY;
	  rtf_put(CONTROL_LEVEL_FIFO,&tev, sizeof(tev));
	  Alarm_count=0;
	  jlsAlarmInterval=0; /* Alarms occur once */
	}
	
      }
      if(++Beat_count >= jlsBeatInterval) {
	
	tev.req=RCI_BEAT_READY;
	rtf_put(CONTROL_LEVEL_FIFO,&tev, sizeof(tev));
	Beat_count=0;
      }
    }
    
  } /* End of while(1) */
}

int my_handler(unsigned int fifo)
{
  jlsMessageType msg;
  int err;
	
  while ((err = rtf_get(ACTIVATION_FIFO, &msg, sizeof(msg))) == sizeof(msg)) {
    /* I have been activated because message is available on ACTIVATION_FIFO,
       Immediately pass the message to TASK_CONTROL_FIFO and then wake up 
       the control task 
    */
    rtl_printf("FIFO handler: sending the \"%d\" command to task \n", msg.command);

    rtf_put (TASK_CONTROL_FIFO, &msg, sizeof(msg));
    
    if((msg.command == START_BEAT_MSG) || (msg.command == START_TASK))
      {
	pthread_wakeup_np (jlstask);
      }
  }
  if (err != 0) {
    rtl_printf("FIFO handler: error in \"%d\" \n", msg.command);
    return -EINVAL;
  }
  return 0;
}


int init_module(void)
{
  /* This function called by system function insmod when module is loaded 
   */ 
     
	int c[5];
	pthread_attr_t attr;
	struct sched_param sched_param;
	int ret;

	Alarm_count = 0;
	Beat_count = 0;

	rtf_destroy(1);
	rtf_destroy(2);
	rtf_destroy(3);
	rtf_destroy(4);
	rtf_destroy(5);
	/* Debugging rtp for dumping data */
     	rtf_destroy(6);

 	c[0] = rtf_create(1, 4000);
	c[1] = rtf_create(2, 4000);
	c[2] = rtf_create(3, 200);	
	c[3] = rtf_create(4, 200);	/* JLS control channel */
	c[4] = rtf_create(5, 200);	/* Control-Level control channel */

        dump_pipe = rtf_create(6, 4000);   /* Pipe for data dump */

	pthread_attr_init (&attr);
	/* Set the JLS as the highest priority task */
	sched_param.sched_priority = 1;
	pthread_attr_setschedparam (&attr, &sched_param);
	/* Specify the routine thread_code  (above) as a real-time thread */
	ret = pthread_create (&jlstask,  &attr, thread_code, (void *)1);
	
	/* specify my_handler as the function to be called when data is read or 
	   written to real-time fifo #3 
	*/
	
	rtf_create_handler(ACTIVATION_FIFO, &my_handler);


	shm1 = (volatile char*) mbuff_alloc("control",sizeof(robot_JLS_type));
	robot_JLS = (robot_JLS_type *) shm1;
	started_flag = 0;

	return 0;
}


void cleanup_module(void)
{
  /* This function called by system call rmmod when the module is removed.  */
	rtf_destroy(1);
	rtf_destroy(2);
	rtf_destroy(3);
	rtf_destroy(4);
	rtf_destroy(5);

	/* Debug */
	rtf_destroy(6);

	mbuff_free("control",(void*)shm1);
	pthread_delete_np (jlstask);

}
