PRJ2：智能小车V0.1
作者：Sam山野人，coolwyc@21cn.com
<?xml:namespace prefix = o ns = "urn:schemas-microsoft-com:office:office" />
硬件：
1. 车体
2. S3C2410A开发板
3. 红外测距传感器
4. 伺服器（舵机）
5. 直流电机
6. 电机驱动板
7. 电源
软件：
1. Linux2.6.14内核，包括：Nand Flash、USB、NET、PWM、ADC等驱动；Yaffs等文件系统。
2. VIVI
3. Busybox1.6
主要目标：
使车体能自主地在平地行驶，能躲避一定高度和大小的障碍物。
下一目标：
优化行走算法、行走路线预测、自选行走路线等。
加入编码轮，实现PID。
开发简述：
1. 在前几个月已完成了PRJ1：智能小车的开发，PRJ2硬件跟PRJ1相同，实现功能也基本相同，最大不同的是PRJ1采用UCOSII操作系统，PRJ2采用Linux，由于工作比较忙，PRJ1的介绍稍后撰写发布。
2. 开发板是一年多前买的，自带的linux是2.4的版本，现在很多开发板都已经用上2.6了，我当然也想用2.6做平台了，于是开始了linux2.6.14的移植，用了大概花了3个星期左右，其中也尝试了2.6.18和2.6.22，编译后运行时老是出现visual address什么什么的错误，再网上也没查到原因，于是放弃了。linux2.6.14的移植也算比较顺利，Nand Flash、USB、NET、Yaffs等都很快弄好了。我是在业余时间做的，平时要上班，花了三个星期自己感觉也不算慢了。
3. 内核移植完了下一步当然是文件系统了，首先是移植Busybox1.6，然后建立目录结构、编写好linuxrc还有hosts、passwd等配置文件，在mtd上划出一个分区建立yaffs，把目录结构、Busybox等都copy过去，启动时用这个yaffs分区做根目录。具体的网上也有很多文章参考，这里就从简了。
4. 要使车体工作需要控制三个硬件：
a. 红外测距传感器---车体的唯一一个传感器，最大探测距离80cm，负责探测车体周围的障碍物，车体上没有其他接触开关等传感器，所以车体的安全保障就靠它了。
b. 伺服器---负责带动红外测距传感器旋转，使它能探测到不同角度的距离，这里设计是前方140度范围旋转，也就是说能探测前方140度范围的障碍物的距离。
c. 直流电机---负责车体移动。通过了PWM信号驱动电机驱动板来调节车体的运行速度，前方障碍物距离远时运行速度可以快些，当离障碍物距离近时运行速度可以慢些或者停车。
5. 红外测距传感器输出的是模拟电压信号（当障碍物距离远输出电压低，当障碍物距离近输出电压高），所以要用到ADC，需要编写S3C2410_adc驱动。
6. 伺服器和直流电机都需要用到PWM信号，PWM信号是由定时器产生的，S3C2410有五路定时器，linux占用了一个，查看linux内核源码，确定是占用了timer4，其余的4个就可以选用了，选用timer0驱动伺服器，timer1驱动直流电机，分配好了立刻动手编写S3C2410_pwm驱动；车体运行除了要控制速度还需要控制方向（前进、后退、左转、右转等），需要通过I/O对电机驱动板进行控制，本来应该另外编写驱动程序来完成这些的，但考虑到控制对象是一样的（电机），而且控制方向和控制速度关系比较密切，所以把控制I/O的代码也写到S3C2410_pwm里面去了，用起来也方便。
7. 到此，内核和驱动都已经准备好了，下一步要实现的就是应用程序了，包括小车的行走模式、行走算法、障碍物探测等，使用了两个线程来完成这些任务：
a. MeasureThread()---探测线程：主要实现伺服器在140度区间来回转动，带动红外测距传感器也随之转动；每转过一定角度就测量障碍物距离；根据测量结果通知MotionThread()做相应的动作，例如：前方障碍物距离在安全范围，通知MotionThread()前进；左前方有障碍物，通知MotionThread()往右转；前方障碍物距离比较近，通知MotionThread()减速，前方障碍物距离比较远，通知MotionThread()加速。通过全局变量Motion_status实现通信。
b. MotionThread()---运动控制线程：根据MeasureThread()设置的状态控制车体运动。当有障碍物时车体需要左转或右转以避开障碍物，当障碍物刚好在安全距离阀值附近时，会出现抖动（有障碍-à左转à无障碍à停à有障碍-à左转），这里采取了每次转动要至少保持一定时间（例如：200ms），在这段时间内禁止改变车体运动状态，通过互斥锁mutex_turning来实现这种同步。
<?xml:namespace prefix = v ns = "urn:schemas-microsoft-com:vml" />
8. 代码：car.c
#include
#include
#include
#include
#include
#include
#include
#define ADC_DEV "/dev/s3c2410_adc"
#define PWM_DEV "/dev/s3c2410_pwm"
#define PWMIOCTL_PRESALE0 0
#define PWMIOCTL_PRESALE1 1
#define PWMIOCTL_MUX0 2
#define PWMIOCTL_MUX1 3
#define PWMIOCTL_MUX2 4
#define PWMIOCTL_MUX3 5
#define PWMIOCTL_MUX4 6
#define PWMIOCTL_AUTORELOAD0 7
#define PWMIOCTL_AUTORELOAD1 8
#define PWMIOCTL_AUTORELOAD2 9
#define PWMIOCTL_AUTORELOAD3 10
#define PWMIOCTL_INVERTER0 12
#define PWMIOCTL_INVERTER1 13
#define PWMIOCTL_INVERTER2 14
#define PWMIOCTL_INVERTER3 15
#define PWMIOCTL_INVERTER4 16
#define PWMIOCTL_UPDATE0 17
#define PWMIOCTL_UPDATE1 18
#define PWMIOCTL_UPDATE2 19
#define PWMIOCTL_UPDATE3 20
#define PWMIOCTL_UPDATE4 21
#define PWMIOCTL_START0 22
#define PWMIOCTL_START1 23
#define PWMIOCTL_START2 24
#define PWMIOCTL_START3 25
#define PWMIOCTL_START4 26
#define PWMIOCTL_TCNTB0 27
#define PWMIOCTL_TCNTB1 28
#define PWMIOCTL_TCNTB2 29
#define PWMIOCTL_TCNTB3 30
#define PWMIOCTL_TCNTB4 31
#define PWMIOCTL_TCMPB0 32
#define PWMIOCTL_TCMPB1 33
#define PWMIOCTL_TCMPB2 34
#define PWMIOCTL_TCMPB3 35
#define PWMIOCTL_TCMPB4 36
#define MOTOIOCTL_GPCDAT 200
#define MOTION_FORWARD 1
#define MOTION_STOP 2
#define MOTION_LEFTTURN 3
#define MOTION_RIGHTTURN 4
#define MOTION_BACKTURN 5
#define MOTION_BACKWARD 6
#define SERVO_STEP 2
#define SERVO_LEFT_MAX 7
#define SERVO_RIGHT_MAX 21
#define SERVO_MIDDLE 14
#define SERVO_DIRECT_LEFT 0
#define SERVO_DIRECT_RIGHT 1
#define SAVE_ 0x140 //障碍物安全距离值
#define SAVE_1 0x70
#define SAVE_2 0x50
unsigned char Motion_Status = MOTION_STOP ;
unsigned short GP2D12_[30] ;
unsigned char Servo_val[30] = {51,57,63,69,75,81,87,93,99,105,111,117,123,129,135,141,147,153,159,165,171,177,183,189,195,201,207,213,219,225} ;
unsigned char Direct_val[8] = {0x0, 0x48, 0x0, 0x28, 0x42, 0x42, 0x22, 0x0} ;
unsigned char Speed_val[8] = {0x15, 0x20, 0x30, 0x46, 0x50, 0x5a, 0x60, 0x63} ;
unsigned char curSpeed = 0 ;
unsigned char curDegree = 0 ;
unsigned char curDegree_d = 0 ;
unsigned short turn_count = 0 ;
unsigned short lrturn_count = 0 ;
pthread_mutex_t mutex_turning;
int adc_fd,pwm_fd;
void InitServo() ;
void InitMoto() ;
void SetServoDegree(unsigned char degree) ;
void SetMotoSpeed(unsigned char speed) ;
void SetMotoDirect(unsigned char direct) ;
int GetAdc() ;
void MeasureThread() ;
void MotionThread() ;
void InitServo() {
int i = 0 ;
i=0x20 ;
ioctl(pwm_fd,PWMIOCTL_PRESALE0,&i);
i=0x3 ;
ioctl(pwm_fd,PWMIOCTL_MUX0,&i);
i=1953 ; //i=1800 ;
ioctl(pwm_fd,PWMIOCTL_TCNTB0,&i);
SetServoDegree(14) ;
}
void InitMoto() {
SetMotoDirect(MOTION_STOP) ;
SetMotoSpeed(curSpeed) ;
}
void SetServoDegree(unsigned char degree) {
int i = 0 ;
i=Servo_val[degree] ;
ioctl(pwm_fd,PWMIOCTL_TCMPB0,&i);
}
void SetMotoSpeed(unsigned char speed) {
int i = 0 ;
i=Speed_val[speed] ;
ioctl(pwm_fd,PWMIOCTL_TCMPB1,&i);
}
void SetMotoDirect(unsigned char direct) {
int i = 0 ;
i=Direct_val[direct] ;
ioctl(pwm_fd,MOTOIOCTL_GPCDAT,&i);
}
int GetAdc() {
int i = 0, ret = 0, tmp = 0 ;
for(i=0;i<5;i++) {
read(adc_fd,&tmp,sizeof(unsigned long));
if(i>0) ret += tmp ;
}
return ret/4 ;
}
void MeasureThread()
{
unsigned short adcData0 = 0 ;
unsigned char save_count = 0 ;
unsigned char save_count1 = 0 ;
unsigned char save_count2 = 0 ;
// Motion_Status = MOTION_STOP ;
printf("MeasureThread,start\n");
while(1)
{
switch (Motion_Status) {
case MOTION_LEFTTURN:
case MOTION_RIGHTTURN:
usleep(20000);
pthread_mutex_lock(&mutex_turning); //等待转动一定角度后才开始检测
pthread_mutex_unlock(&mutex_turning);
while(1) {
adcData0 = GetAdc() ;
if(adcData0<=SAVE_) {
SetMotoDirect(MOTION_STOP) ;
Motion_Status = MOTION_STOP ;
break ;
}
if(Motion_Status != MOTION_LEFTTURN && Motion_Status != MOTION_RIGHTTURN)
break ;
}
break ;
case MOTION_BACKWARD:
case MOTION_BACKTURN:
break ;
default:
if(curDegree
curDegree = SERVO_LEFT_MAX ;
curDegree_d = SERVO_DIRECT_RIGHT ;
}
if(curDegree>SERVO_RIGHT_MAX) {
curDegree = SERVO_RIGHT_MAX ;
curDegree_d = SERVO_DIRECT_LEFT ;
}
while(1) {
adcData0 = GetAdc() ;
if(adcData0>=SAVE_) {
//stop and sleep to do.
//
lrturn_count++ ;
if(curDegree>SERVO_MIDDLE) {
Motion_Status = MOTION_LEFTTURN ;
curDegree_d=SERVO_DIRECT_RIGHT ;
} else {
Motion_Status = MOTION_RIGHTTURN ;
curDegree_d=SERVO_DIRECT_LEFT ;
}
save_count = 0 ;
save_count1 = 0 ;
save_count2 = 0 ;
curSpeed = 0 ;
SetMotoDirect(MOTION_STOP) ;
SetMotoSpeed(curSpeed) ;
break ;
} else if(adcData0
save_count2++ ;
save_count1++ ;
save_count++ ;
if(save_count2>8) {
curSpeed = 2 ;
save_count2 = 8 ;
}
} else if(adcData0
save_count2 = 0 ;
save_count1++ ;
save_count++ ;
if(curSpeed == 2) curSpeed = 1 ;
if(save_count1>8) {
curSpeed = 1 ;
save_count1 = 8 ;
}
} else if(adcData0
save_count1 = 0 ;
save_count2 = 0 ;
save_count++ ;
curSpeed = 0 ;
}
if(save_count>15) {
Motion_Status = MOTION_FORWARD ;
lrturn_count = 0 ;
save_count = 15 ;
}
SetMotoSpeed(curSpeed) ;
if(curDegree_d==SERVO_DIRECT_LEFT) { //控制伺服器旋转
curDegree -= SERVO_STEP ;
if(curDegree
curDegree=SERVO_LEFT_MAX ;
curDegree_d=SERVO_DIRECT_RIGHT ;
}
} else {
curDegree += SERVO_STEP ;
if(curDegree>SERVO_RIGHT_MAX) {
curDegree=SERVO_RIGHT_MAX ;
curDegree_d=SERVO_DIRECT_LEFT ;
}
}
SetServoDegree(curDegree) ;
usleep(40000);
}
break ;
}
}
}
void MotionThread()
{
printf("MotionThread start. \n");
while(1)
{
switch (Motion_Status) {
case MOTION_FORWARD:
while(Motion_Status==MOTION_FORWARD) {
SetMotoDirect(MOTION_FORWARD) ;
usleep(5000) ;
}
SetMotoDirect(MOTION_STOP) ;
break ;
case MOTION_STOP:
usleep(5000) ;
break ;
case MOTION_LEFTTURN:
SetMotoDirect(MOTION_LEFTTURN) ;
pthread_mutex_lock(&mutex_turning); //每次转动要保持转动一定时间
if(lrturn_count>10) {
lrturn_count = 0 ;
usleep(1000000) ;
} else {
usleep(200000) ;
}
pthread_mutex_unlock(&mutex_turning);
while(Motion_Status==MOTION_LEFTTURN) {
SetMotoDirect(MOTION_LEFTTURN) ;
usleep(20000) ;
}
SetMotoDirect(MOTION_STOP) ;
usleep(100000) ;
break ;
case MOTION_RIGHTTURN:
SetMotoDirect(MOTION_RIGHTTURN) ;
pthread_mutex_lock(&mutex_turning);
if(lrturn_count>10) {
lrturn_count = 0 ;
usleep(1000000) ;
} else {
usleep(200000) ;
}
pthread_mutex_unlock(&mutex_turning);
while(Motion_Status==MOTION_RIGHTTURN) {
SetMotoDirect(MOTION_RIGHTTURN) ;
usleep(20000) ;
}
SetMotoDirect(MOTION_STOP) ;
usleep(100000) ;
break ;
default:
SetMotoDirect(MOTION_STOP) ;
Motion_Status = MOTION_STOP ;
usleep(5000) ;
break ;
}
}
printf("MotionThread end. \n");
}
int main(void)
{
int ret;
pthread_t thread_id;
unsigned long tmp;
adc_fd=open(ADC_DEV,O_RDWR);
pwm_fd=open(PWM_DEV,O_RDWR);
if(adc_fd < 0)
{
printf("Error opening %s adc device\n", ADC_DEV);
return -1;
}
if(pwm_fd < 0)
{
printf("Error opening %s adc device\n", ADC_DEV);
return -1;
}
else
printf("device id is %d\n",adc_fd) ;
InitServo();
InitMoto() ;
pthread_mutex_init (&mutex_turning,NULL);
ret=pthread_create(&thread_id,NULL,(void *)MeasureThread,NULL);
if(ret!=0){
printf ("Create pthread error!\n");
return -1;
}
MotionThread() ;
pthread_join(thread_id,NULL);
close(adc_fd);
close(pwm_fd);
return 0;
}
用户377235 2012-3-23 19:22
用户99191 2007-8-24 19:50
用户75640 2007-8-23 10:45
用户1184930 2007-8-22 20:59
呵呵！有兴趣一齐研究研究！
用户1318172 2007-8-22 17:37