PRJ2：智能小车V0.1
作者：Sam山野人，coolwyc@21cn.com(coolwyc@21cn.com)
http://www.roboticfan.com/blog/UploadFiles/2007-8/221435205355.jpghttp://www.roboticfan.com/blog/UploadFiles/2007-8/221435770766.jpg
硬件：
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来实现这种同步。
[url=http://www.roboticfan.com/blog/UploadFiles/2007-8/221417629139.png]http://www.roboticfan.com/blog/UploadFiles/2007-8/221417629139.png[/url]
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
");
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.
");
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.
");
}
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
", ADC_DEV);
return -1;
}
if(pwm_fd < 0)
{
printf("Error opening %s adc device
", ADC_DEV);
return -1;
}
else
printf("device id is %d
",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!
");
return -1;
}
MotionThread() ;
pthread_join(thread_id,NULL);
close(adc_fd);
close(pwm_fd);
return 0;
}
图片+视频+代码下载
相关链接:http://www.mcublog.com/blog/blog2007/coolwyc/archives/2007/24607.html |
|
楼主
标题置顶
标题高亮
