appsonar.c

在linux下通过298N控制的步进电机驱动程序

#include <stdio.h>  /*标准的输入输出定义*/
#include <unistd.h> /*UNIX标准函数定义*/
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>  /*文件控制定义*/
#include <assert.h>
#include <stdlib.h>

#include <sys/ioctl.h>
#include <sys/mman.h>
#include <error.h>  /*错误号定义*/

//#include <delay.h> //udelay()

#include "sonar.h"
#include "stepper.h"
#define  perimeter  31.5
#define  accel_no   0
#define  accel_yes  1
#define  shortpath_yes 1
#define  shortpath_no  0
#define     r       29
#define    PI      3.14159
int main()
{
	int fd1,fd2,steps,range,i;
	float   range_front,range_left,range_right;
	int average_speed =10000;
	int limit_speed =8000,accelstep=40;
	int accelmin=12000;
	int accelfrom=12000;
	int  maxsteps,minsteps,left_steps,right_steps,left_curve,right_curve;
	int   buf[2] = {5000, -2000};
	maxsteps=  (500/perimeter)*200;
	minsteps= (44/perimeter)*200;
	left_steps= right_steps=2*PI*r*200/(4*perimeter); 
	left_curve = 1000;
	right_curve = 1000;
	fd1=open("/dev/stepper",O_WRONLY);
	if (fd1<0)
	{
		printf("Steppe error:Unnable to open stepper device\n");
	    
        }
	else{
		ioctl(fd1,Stepper_On);
		ioctl(fd1,Limit_Speed,limit_speed);
		ioctl(fd1,Average_Speed,average_speed);
		ioctl(fd1,Accel_Step,accelstep);
		ioctl(fd1,Accel_Min,accelmin);
		ioctl(fd1,Accel_From,accelfrom);
		ioctl(fd1,Set_Zero);
		ioctl(fd1,Set_ShortPath,shortpath_no);

		ioctl(fd1,Stepper_Half);
		ioctl(fd1,Use_Accel,accel_no);
		ioctl(fd1,Stepper_Front,buf[0]);
 
             	ioctl(fd1,Stepper_Half);
             	ioctl(fd1,Use_Accel,accel_yes);
             	ioctl(fd1,Stepper_Front,buf[0]);           

                ioctl(fd1,Stepper_Full);
                ioctl(fd1,Use_Accel,accel_no);
                ioctl(fd1,Stepper_Front,buf[0]);

                ioctl(fd1,Stepper_Full);
                ioctl(fd1,Use_Accel,accel_no);
                ioctl(fd1,Stepper_Front,buf[1]);
}

		/*ioctl(fd1,Stepper_Full);
		ioctl(fd1,Average_Speed,average_speed);
		ioctl(fd1,Use_Accel,accel_no);
		ioctl(fd1,Stepper_Back,buf[1]);
		
		ioctl(fd1,Stepper_Half);
		ioctl(fd1,Use_Accel,accel_yes);
		ioctl(fd1,Stepper_Back,buf[1]);
		
		ioctl(fd1,Stepper_Full);
		ioctl(fd1,Use_Accel,accel_no);
		ioctl(fd1,Average_Speed,average_speed);
		ioctl(fd1,Stepper_Right,right_steps);
		ioctl(fd1,Stepper_Left,left_steps);
		
		ioctl(fd1,Stepper_Left_Curve,left_curve);
		ioctl(fd1,Stepper_Right_Curve,right_curve);
	}

/*	fd2=open("/dev/sonar",O_RDONLY);
	if (fd2<0)
	{
		printf("Sonar error:Unnable to open sonar device\n");
	}
	else
	{
		ioctl(fd2,Sonar_Front_On);
		//ioctl(fd2,Sonar_Left_On);
		ioctl(fd1,Stepper_Full);
		ioctl(fd1,Average_Speed,average_speed);
		ioctl(fd1,Use_Accel,accel_no);
		ioctl(fd1,Stepper_Front,buf[0]);

		/*while(1)
		{ 
			range_front=ioctl(fd2,Sonar_Front_Range);  
			range_front= range_front/10000*1.00004-4.104814;//ni he
			printf("\n\nrange_front is %.2fcm \n",range_front);
			if (range_front>=45)
			{              
				printf("\nsonar_front distasnce %.3f",range_front);
				steps=((range_front-10)/perimeter);
				ioctl(fd1,Stepper_Half);
				ioctl(fd1,Use_Accel,accel_yes);
				ioctl(fd1,Stepper_Front,steps);
			}
			else
			{
				range_left=ioctl(fd2,Sonar_Left_Range);
				range_left=range/10000*1.00004-4.104814;
				if(range_left<45)
				{
					ioctl(fd1,Use_Accel,accel_no);
					ioctl(fd1,Average_Speed,average_speed);
					ioctl(fd1,Stepper_Full);
					ioctl(fd1,Stepper_Right,right_steps);
				}
				else
				{
					ioctl(fd1,Use_Accel,accel_no);
                                        ioctl(fd1,Average_Speed,average_speed);
                                        ioctl(fd1,Stepper_Full);
                                        ioctl(fd1,Stepper_Left,left_steps);
				}
			}

		}*/

        close(fd1);
        //close(fd2);
        exit(0);                             		
}