motor.c

读取一个电机码盘信号

/*
	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 "control.h"
#include <asm/rtai.h>	
MODULE_LICENSE("GPL");
EXPORT_NO_SYMBOLS;
static RT_TASK task;
#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
int MUTINUM=1000;
int control_for_in=1;
int control_for_out=1;
int savevalue=65536;
int rk,uk;
int K=2;
float e[3];
int c[3];
int checkmode;
int spi_read(int base);
void spi_end(int base);
/*
	Fuctions for output
*/
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("t=%d \n",t);
		return 0;
	}
	else
	{
		printk("t=%d \n",t);
		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--;
			if(out<3&&out>=0)
				control_for_in++;
			return 0;
		}	
	else
		if(out<0)
			{	
				printk("input value is %d ",out+65536);
				return out+65536;
			}	
		else if(out>=3)
			{		
				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=d-2048; 
	if(control_for_out)
	{	
		control_for_out--;
		goto jumpout;
	}
	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);
	if(c[K]!=0)
	printk("output value is : %d\n",d-4096);	
	jumpout:
}
/*
	PID control Fuction
*/
int PIDcontrol(int send_to_motor,int feed_back,struct PID_struct m)
{	float xxx;
//	printk("T=%d,Ti=%d,Td=%d,Kp=%d",(int)m.T,(int)m.Ti,(int)m.TD,(int)m.Kp);
	A=m.Kp*(1.0+m.T/m.Ti+m.TD/m.T);
	B=m.Kp*(1.0+2.0*m.TD/m.T);
	C=m.Kp*m.TD/m.T;		
	e[K]=send_to_motor-feed_back;		//caculate e[K]
	xxx=A*e[K]-B*e[K-1]+C*e[K-2];
	uk=(int)xxx;
	e[K-2]=e[K-1];
	e[K-1]=e[K];
//	printk("uk=%d",uk);
	return uk;
}


static void motor_task(int fifo)    		
{	struct PID_struct PID;
	int resultPID,control;
	e[K-1]=e[K-2]=0;	
/* pre-define PID values */
	PID.T=1;
	PID.Ti=2;
	PID.TD=3;
	PID.Kp=4;
	PID.speed=0;
	control=0;
while(1)
	{
	// get new values
	rtf_get(7,&PID,sizeof(PID));
	rk=PID.speed;
	if(rk!=0)
{
	c[K]=inport(BASE_PORT);			//input axis position value
	resultPID=PIDcontrol(rk,c[K],PID);		//do PID control
	if(checkmode)				//serial port
	outport(0,resultPID);			//out put control result
}	
	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_resume(&task);
	else
	{	rt_task_suspend(&task);
		control_for_in=2;
		control_for_out=1;
	}
}
	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)
{	
	checkmode=s_or_p();	
	printk("checkmode=%d \n",checkmode);
	rtf_create(0,100);
	rtf_create(1,100);
	rtf_create(2,100);
	rtf_create(5,100);
	rtf_create(7,100);
	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,0,0);	
	rt_set_oneshot_mode();
	rt_task_make_periodic_relative_ns(&task,0,MUTINUM*MINISECOND);
	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);
	rt_task_delete(&task);
	stop_rt_timer();
	
}