/*
COPYRIGHT (C) 2004 Alvaro CASADO RODRIGUEZ

  Next code has been developed based on examples of rtai by 
  COPYRIGHT (C) 2000  Paolo Mantegazza (mantegazza@aero.polimi.it)

 This library is free software; you can redistribute it and/or
 modify it under the terms of the GNU Lesser General Public
 License as published by the Free Software Foundation; either
 version 2 of the License, or (at your option) any later version.
 
  This library is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  Lesser General Public License for more details.
  
   You should have received a copy of the GNU Lesser General Public
   License along with this library; if not, write to the Free Software
   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA.
*/

/*Compile :
	gcc -I/usr/src/rtai/include -I/usr/src/comedi -g -o -Wall servo servo.c 
*/

//#include <comedilib.h>

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <sys/types.h>
#include <sys/mman.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <sched.h>
#include "rtai_shm.h"


#define KEEP_STATIC_INLINE
#include <rtai_lxrt_user.h>
#include <rtai_lxrt.h>
#include <rtai_comedi_lxrt.h>
int main(int argc,char *argv[])
{
	void * device;
	RT_TASK *t2;
	RTIME t1;
	float alfa=0.014;//this value depends on the servo we use, its the Ts see information in the web site
	struct sched_param mysched;
	int subdevice =3; //digital output subdevice , see comedi reference manual or execute ./info in comedilib/demo directory 
	int stype;
	long PERIOD;
	unsigned int *adress,*stop;
	
	//shm
	adress=rtai_malloc(nam2num("SERVO1"),8);
	stop=rtai_malloc(nam2num("CONDICION"),1);
	//open device
	device=comedi_open("/dev/comedi0");
	if(!device){
	  //comedi_perror("/dev/comedi0");
		exit(0);
	}
    
	//check subdevice selected
	stype = comedi_get_subdevice_type(device,subdevice);
	if(stype!=COMEDI_SUBD_DO){
		printf("%d is not a digital I/O subdevice\n",subdevice);
		exit(0);
	}
	//this is necesary to use in user-space
	rt_allow_nonroot_hrt();
	
	//Scheduler policy and priority
	mysched.sched_priority = sched_get_priority_max(SCHED_FIFO) - 1;
	if( sched_setscheduler( 0, SCHED_FIFO, &mysched ) == -1 ) {
		puts("ERROR IN SETTING THE SCHEDULER");
		perror("errno");
		exit(0);
	}
	//task init
	if (!(t2 = rt_task_init(nam2num(argv[1]), 1, 0, 0))) {
		printf("CANNOT INIT %d TASK\n",atoi(argv[1]));
		exit(1);
	}
	
	PERIOD=20000000; //50 Hz signal
	//locks pages in mem
	mlockall(MCL_CURRENT | MCL_FUTURE);
	rt_set_oneshot_mode();
	start_rt_timer(0);
	t1 = rt_get_time();
	while(stop[0]) {
		//wait until next period
		rt_sleep_until(t1);
		//write  '1'
		comedi_dio_write(device,subdevice,atoi(argv[1]),1);
		//sleep during a pulse wide (Ton)
		alfa=adress[atoi(argv[1])]*0.001;
		rt_sleep(nano2count(alfa*PERIOD));
		t1=t1+nano2count(PERIOD);
		//write'0' 
		comedi_dio_write(device,subdevice,atoi(argv[1]),0);
	}
	rt_task_delete(t2);
	stop_rt_timer();
	comedi_close(device);
	exit(0);
}