savedata.c

本人在Linux实时内核下写的PID算法的电机驱动程序

/*
	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 0x100
MODULE_LICENSE("GPL");
EXPORT_NO_SYMBOLS;
/* 
 * static int
 */
struct str_data_mem *send_data;
int MUTINUM=1;			//sampling period set to 1 ms
int 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();
	
}