|
PRJ2：智能小车 V0.1作者：Sam山野人，coolwyc@21cn.com   硬件： 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. 伺服器--- 负责带动红外测距传感器旋转，使它能探测到不同角度的距离，这里设计是前方 lang="EN-US" style="font-family: Times New Roman">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来实现这种同步。
 8. 代码：car.c ＃i nclude <sys/stat.h> ＃i nclude <fcntl.h> ＃i nclude <stdio.h> ＃i nclude <sys/time.h> ＃i nclude <sys/types.h> ＃i nclude <unistd.h> ＃i nclude <pthread.h> #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 ; style="font-size: 12pt; font-family: Times New Roman"> // Motion_Status = MOTION_STOP ; printf("MeasureThread,start\n"); style="font-size: 12pt; font-family: Times New Roman"> 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 ; style="font-size: 12pt; font-family: Times New Roman"> case MOTION_BACKWARD: case MOTION_BACKTURN: break ;
default:
if(curDegree<SERVO_LEFT_MAX) { 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_2) { save_count2++ ; save_count1++ ; save_count++ ; if(save_count2>8) { curSpeed = 2 ; save_count2 = 8 ; } } else if(adcData0<SAVE_1) { 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_) { 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<SERVO_LEFT_MAX) { 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 pwm device\n", PWM_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; } |
 你的电机驱动板是有伺服电机和直流电机驱动的吗？？我看网上很多都是各自的驱动板，想找一块混合的，不知道哪里有~我是菜鸟~刚接触~谢谢指点~ |
| 我也想做一个，但我才六年级，知识不够，能教教我吗?我的QQ是961311939. |
我很佩服你 |
向你学习，我也在做移动小车机器人，请多赐教! |
|
 能冒昧的问问你的联系方式吗？ 我是一名学生想做一个类似的机器，由于目前学的东西还比较少，所以想向您请教请教，我的手机号是15901011922，希望能得到您的帮助~~ |
| 做的不错！想你学习！一定向您多交流！ |
| 厉害啊,佩服 |
| 强! |
| 控制的不错啊,good! 第一次见到用linux控制的小车。 |