save.c

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

#include <stdio.h>
#include <pthread.h>
#include <stdlib.h>
#include <errno.h>
#include <sys/time.h>
#include <sys/types.h>
#include <fcntl.h>
#include <unistd.h>
#include <sys/ioctl.h>
#include <bits/pthreadtypes.h>
#include "control.h"
#include "formem.h"
#include <rtai_fifos.h>
#include <rtai_shm.h>
#define BUFSIZE 70
int choose=1;
static int i=0;
static int stm[MAXNUM],fb[MAXNUM];
static int midstm,midfb;
static int controlfordata;
static int controldata=0;
static int savestm,savefb;
FILE *fp,*fp1;
struct str_data_mem *get_data;
void thread(void)
{	
/*
 * 	get data from RT-task
 */
	fprintf(stderr,"now in thread!!!!i=%d",i);
	get_data=(struct str_data_mem*)rtai_malloc(0xaaaa,sizeof(*get_data));
	while(i<MAXNUM)
	{
		stm[i]=get_data->stm[i];
		fb[i]=get_data->fb[i];
//		fprintf(stderr,"stm[%d]=%d",i,get_data->stm[i]);
	i++;
	}
		for(i=0;i<20;i++)
		fprintf(stderr,"stm[%d]=%d",i,get_data->stm[i]);
/*
 * 	write the data to file
 */	
	
	if((fp=fopen("/motordriver/data","w"))==NULL)
		fprintf(stderr,"cant open");
	if(fwrite(stm,sizeof(stm[MAXNUM]),MAXNUM,fp)!=MAXNUM)
	{
		fprintf(stderr,"error");
		fclose(fp);
		exit(1);
	}			
	else
		fprintf(stderr,"\nsucess write to file data");
	fclose(fp);
	
	if((fp1=fopen("/motordriver/data1","w"))==NULL)
		fprintf(stderr,"cant open 1");
	if(fwrite(fb,sizeof(fb[MAXNUM]),MAXNUM,fp1)!=MAXNUM)
	{
		fprintf(stderr,"error 1");
		fclose(fp1);
		exit(1);
	}
	else
		fprintf(stderr,"\nsucess write to file data1\n");
	fclose(fp1);

}




int main(int argc,char *argv[])
{	int CMD,CMD1,CMDPID;
	int msg,choose,ready;
	int Finish=30;
	int Finish1;
	int ch,letdo,checkall,a,b,c,save;
	int ret;
	pthread_t getvalue;
	struct PID_struct PID;
	for(i=0;i<MAXNUM;i++)
	{
		stm[i]=0;
		fb[i]=8;
	}
	i=0;
	letdo=1;
	c=0;
while(letdo){
	ready=1;	// For the first time set all values
	if(check){
	PID.T=0.001;
	PID.Ti=0.7;
	PID.Kp=0.315;
	PID.TD=0.038;
	check=0;
	/* Open fifo device */
	if ((CMD = open("/dev/rtf0", O_WRONLY))<0) 
{
	fprintf(stderr, "Error opening /dev/rtf0\n");
	exit(1);
}
	if((CMD1=open("/dev/rtf2",O_RDONLY))<0)
{
	fprintf(stderr,"Error opening /dev/rtf2\n");
	exit(1);
}	
	if((CMDPID=open("/dev/rtf5",O_WRONLY))<0)
{
	fprintf(stderr,"Error opening /dev/rtf0\n");
	exit(1);
}	
	if((savestm=open("/dev/rtf8",O_RDONLY))<0)
{
	fprintf(stderr,"Error opening /dev/rtf0\n");
	exit(1);
}	

	if((savefb=open("/dev/rtf9",O_RDONLY))<0)
{	
	fprintf(stderr,"Error opening /dev/rtf0\n");
	exit(1);
}
	if((controlfordata=open("/dev/rtf3",O_RDONLY))<0)
{	
	fprintf(stderr,"Error opening /dev/rtf3\n");
	exit(1);
}
}	
	while(ready)	//set all values
	
{	fprintf(stderr,"now T=%f,Kp=%f,Ti=%f,TD=%f\n",PID.T,PID.Kp,PID.Ti,PID.TD);
	fprintf(stderr,"do you want to reset the value?\n 1 for T, 2 for Kp, 3 for Ti, 4 for TD,5 for exit\n");
	scanf("%d",&choose);
//	getchar();
	switch(choose)
	{	case 1:
		fprintf(stderr,"input for T\n");
		scanf("%f",&PID.T);
		fprintf(stderr,"the T is %f",PID.T);
//		getchar();
		break;
		case 2:
		fprintf(stderr,"input for Kp\n");
		scanf("%f",&PID.Kp);
//		getchar();
		fprintf(stderr,"Kp is %fnow\n",PID.Kp);
		break;
		case 3:
		fprintf(stderr,"input for Ti\n");
		scanf("%f",&PID.Ti);
		getchar();
		fprintf(stderr,"Ti is %fnow\n",PID.Ti);
		break;
		case 4:
		fprintf(stderr,"input for TD\n");
		scanf("%f",&PID.TD);
//		getchar();
		fprintf(stderr,"TD is %fnow\n",PID.TD);
		break;
		case 5:
		fprintf(stderr, "no value reset");
		break;
		default:
		fprintf(stderr,"wrong input again!\n");
		break;
	}
		fprintf(stderr, "\n Quit or set again? 1 for Quit 2 for set\n");
		scanf("%d",&ch);
	//	getchar();
		fprintf(stderr," value set done!!\n");
		if(ch==1)
		{//	fprintf(stderr,"%d",ch);
			ready=0;
		}
	}

/* send the speed command to RT-task */
	fprintf(stderr,"set the speed V=:\n");
	scanf("%f",&PID.speed);
	fprintf(stderr,"PID.speed set to be  %f\n ",PID.speed);
//	fprintf(stderr,"input for Ti= :\n");
//	scanf("%d",&Ti);
	if((write(CMDPID,&PID,sizeof(PID)))<0)
	{
	fprintf(stderr,"error when change the value of PID\n");
	exit(1);
	}	
	fprintf(stderr,"start the RT-task press 0 ,suspend it press 1");
	while(1)
	{	
		
	scanf("%d",&Finish1);
	if((write(CMD, &Finish1, sizeof(Finish1))) < 0)
		{
       		 fprintf(stderr, "Can't send a command to RT-task\n");
		 exit(1);
		}	
	else
	{
		if(Finish1==0)
			i=MAXNUM;
		else
		{
			i=0;
			ret=pthread_create(&getvalue,NULL,(void*)thread,NULL);
		}
	}
	if(choose)
	{	
		choose=0;
	//	ret=pthread_create(&getvalue,NULL,(void*)thread,NULL);
	}	

	if(Finish1)
	break;
	}
	c=1;
	fprintf(stderr,"\n#######do you want set all value again?######\n 1 for yes 2 for no\n");
	scanf("%d",&checkall);
	if(checkall==2)
	letdo=0;
}

	return 0;

}