/*	PID control*/#include <linux/kernel.h>#include <linux/module.h>#include <linux/errno.h>#include <rtai.h>#include <rtai_sched.h>#include <math.h>#include <asm/io.h>#include <rtai_leds.h>#include <rtai_fifos.h>#include <rtai_shm.h>#include "control.h"#include "formem.h"#include <asm/rtai.h>	#define axisnumber 0#define MINISECOND 1000000#define MICROSECOND 1000#define APLDA_SPI_ADS 0x10E			#define APLDA_SPI_BASE 0x10E#define APLDA_SPI_IDX 0#define DA_BASE 0x110#define BASE_PORT 0x100MODULE_LICENSE("GPL");EXPORT_NO_SYMBOLS;/*  * static int */struct str_data_mem *send_data;int MUTINUM=1;			//sampling period set to 1 msint control_for_in=1;int control_for_out=1;int savevalue=65536;float rk;int K=2;float e[3];int c[3];int checkmode;int add_all_sample=0;int realoutput;int checkfirsttime=0;int checksecondtime=0;float send_to_motor=0;float feed_back=0;int check_for_sign;int controlforsave=1;int okle;int loop=0;static int stop_task_nr=0;static RT_TASK task; /**	Fuctions for output*/	int spi_read(int base);void spi_end(int base);int spi_rrdy(int base){		return inb(base)&1;}int spi_wrdy(int base){	return inb(base)&2;}void spi_begin(int base,int idx,int clk){	int m;	m=idx+4+(clk<<3);	spi_end(base);	outb(m,base);}void spi_end(int base){	outb(0,base);}void spi_write(int base,int d){	while(spi_wrdy(base)==0);	outb(d,base+1);}int spi_read(int base){		while(spi_rrdy(base)==0);	return inb(base+1);}/*	check port parallel or serial*/int s_or_p(void)		{	int t;	if((t=inb(APLDA_SPI_ADS))&0x80)	{		printk("*********   The device is for serial port *********\n");		return 0;	}	else	{		printk("*********  The device is for parallel port  *********\n");		return 1;	}}/*	input axis position */int inport(int b)			{		int result,out;	inw(b);	result=inw(b);	out=savevalue-result;	savevalue=result;	if(control_for_in)		{				control_for_in--;			out=0;		}		else		if(out<0)			out=out+65536;	if(out>60000)		out=65536-out;	if(check_for_sign<6143)		out=0-out;	printk("input value is %d   ",out);	return out;}/*	output control values for serial port*/void outport(int i,int d){		inb(DA_BASE+7); 	//open all D/A channel	d=2047+d;	if(d<=1800)		d=1800;	if(d>2500)		d=2500;			d+=(((i<<2)+1)<<12);			spi_begin(APLDA_SPI_BASE,APLDA_SPI_IDX,2);			spi_write(APLDA_SPI_BASE,d>>8);			spi_read(APLDA_SPI_BASE);			spi_write(APLDA_SPI_BASE,d);			spi_read(APLDA_SPI_BASE);			spi_end(APLDA_SPI_BASE);			check_for_sign=d;//			printk("output value is : %d",d);	}/*	PID control Fuction*/int PIDcontrol(float stm,int fb,struct PID_struct m){		float STM,FB;			//just define for print use		float XXX,xxx,z,zz;	float x,y,uk;	float theory_work,sendout;	int FOR_PD;	sendout=stm;	theory_work=stm/10;	//caculate input V for period=10ms/* * 	Suspend RT-task *	Set motor speed to 0  */	if(stop_task_nr>=2)			{		stop_task_nr++;		sendout=0;		theory_work=0;					if(stop_task_nr==5)		{				stop_task_nr=0;			STM=send_to_motor;			FB=feed_back;			printk(KERN_ALERT"**send_to_motor value=%d, motor real work=%d**\n",(int)STM,(int)FB);			rt_task_suspend(&task);		}		return sendout;	}/* * 	Do not use first feed back value! */	  	if(checkfirsttime)	{		send_to_motor=send_to_motor+theory_work;		checkfirsttime=0;		realoutput=stm;		return stm;	}		feed_back=feed_back+fb;	A=m.Kp;					//Kp	B=m.Kp*m.T/m.Ti;			//Ki	C=m.Kp*m.TD/m.T;			//KD	e[2]=send_to_motor-feed_back;		//caculate e[2]		if(e[2]<=2&&e[2]>=0)		//if deviation is too small then	{		send_to_motor=send_to_motor+theory_work;		return realoutput;			//not do PID control		}	else	{		if(e[2]>=10||e[2]<0)			FOR_PD=0;		else			FOR_PD=1;				e[0]=e[2]+e[0];		x=e[0];		y=e[2];		xxx=A*e[2];		xxx=xxx+FOR_PD*B*e[0];		xxx=xxx+C*(e[2]-e[1]);					uk=sendout+xxx;		e[1]=e[2];		STM=send_to_motor;		//just for print		FB=feed_back;					printk(KERN_ALERT"\ne[2]=%d-%d=%d e[0]=%d   ",(int)STM,(int)FB,(int)y,(int)x);			XXX=xxx;		printk("uk=%d",(int)XXX);		/*	 *	caculate the float part */				z=uk;		zz=uk-(int)z;		z=10*zz;				if(zz>5)			realoutput=(int)uk+1;		else			realoutput=(int)uk;	//		printk("**send_to_motor value=%d, motor real work=%d**\n",(int)STM,(int)FB);		send_to_motor=send_to_motor+theory_work;		if(realoutput>200)			realoutput=200;		else if(realoutput<-200)			realoutput=-200;		printk(" \nRealoutput = %d  ",realoutput);				return realoutput;	}	return 0;}static void motor_task(int fifo)    		{	struct PID_struct PID;	int resultPID,control;	int okle2;	float FB2,STM2;	e[1]=e[0]=0;	/*  * pre-define PID values  */	PID.T=1;	PID.Ti=2;	PID.TD=3;	PID.Kp=4;	PID.speed=0;	control=0;while(1)	{//	printk("rk=%d\n",rk);	// get new values	rtf_get(7,&PID,sizeof(PID));	rk=PID.speed;	if(send_to_motor>=2500)		rk=0;//{	c[2]=inport(BASE_PORT);			//input axis position value	resultPID=PIDcontrol(rk,c[2],PID);	//do PID control	if(checkmode)	{					//serial port		outport(axisnumber,resultPID);	//out put control result		outport(axisnumber,resultPID);	// for 2 times	}	FB2=feed_back;	STM2=send_to_motor;	if(loop<MAXNUM)	{		okle2=(int)STM2;		send_data->stm[loop+1]=okle2;		okle2=(int)FB2;		send_data->fb[loop]=okle2;		printk("stm[%d]=%d",loop,send_data->stm[loop]);		printk("fb[%d]=%d",loop,send_data->fb[loop]);	}	loop++;//}		rt_task_wait_period();			}}static int myhandler(unsigned int fifo,int rw){	int msg;	int getcommand;	while((getcommand=rtf_get(0,&msg,sizeof(msg)))==sizeof(msg)){	if(!msg)	rt_task_make_periodic_relative_ns(&task,0,MUTINUM*MINISECOND);	else	{			stop_task_nr=2;//		rt_task_suspend(&task);		control_for_in=1;		control_for_out=1;		checkfirsttime=1;		send_to_motor=0;		feed_back=0;		e[0]=0;		loop=0;	}}	return 0;}static int myhandlerPID(unsigned int fifo,int rw){	struct PID_struct msg1;	int getcommand1;	while((getcommand1=rtf_get(5,&msg1,sizeof(msg1)))==sizeof(msg1)){	rtf_put(7,&msg1,sizeof(msg1));}	return 0;}int init_module(void){		outb(0x80,0x108);	checkmode=s_or_p();		printk("      *******Loading module checkmode=%d *******\n",checkmode);	outport(0,0);	outport(0,0);				//set output value=0	rtf_create(0,100);	rtf_create(1,100);	rtf_create(2,100);	rtf_create(5,100);	rtf_create(7,100);	send_data=(struct str_data_mem*)rtai_kmalloc(0xaaaa,sizeof(*send_data));	memset(send_data->stm,0,sizeof(int)*10000);	memset(send_data->fb,0,sizeof(int)*10000);	rt_task_use_fpu(&task,1);	rt_linux_use_fpu(1);	rtf_create_handler(0, X_FIFO_HANDLER(myhandler));	rtf_create_handler(5, X_FIFO_HANDLER(myhandlerPID));	rt_task_init(&task,motor_task,0,10000,0,1,0);		rt_set_oneshot_mode();	start_rt_timer(nano2count(MICROSECOND));	return 0;}void cleanup_module(void){		printk("********** stopping RT-task ***********\n");	rtf_destroy(0);	rtf_destroy(1);	rtf_destroy(2);	rtf_destroy(5);	rtf_destroy(7);	rtai_kfree(0xaaaa);	rt_task_delete(&task);	stop_rt_timer();	}