|
|
< >作者：Sam山野人，<A target=_blank href="mailto:coolwyc@21cn.com">coolwyc@21cn.com</A></P><DIV align=left><IMG style="CURSOR: pointer" onclick=javascript:window.open(this.src); src="http://www.roboticfan.com/blog/UploadFiles/2007-8/221435205355.jpg" onload=rsimg(this,500)><BR><BR><IMG style="CURSOR: pointer" onclick=javascript:window.open(this.src); src="http://www.roboticfan.com/blog/UploadFiles/2007-8/221435770766.jpg" onload=rsimg(this,500)></DIV><DIV align=left><EMBED pluginspage=http://www.microsoft.com/Windows/Downloads/Contents/MediaPlayer/ src=http://www.shop8.us/a/21-08-07_1951.rm width=176 height=144 type=application/x-mplayer2 filename="http://www.shop8.us/a/21-08-07_1951.rm" autostart="true" showcontrols="true" showstatusbar="false" showdisplay="false" autorewind="true"></EMBED></DIV><P>硬件：</P><P>1. 车体</P><P>2. S3C2410A开发板</P><P>3. 红外测距传感器</P><P>4. 伺服器（舵机）</P><P>5. 直流电机</P><P>6. 电机驱动板</P><P>7. 电源</P><P>软件：</P><P>1. Linux2.6.14内核，包括：Nand Flash、USB、NET、PWM、ADC等驱动；Yaffs等文件系统。</P><P>2. VIVI</P><P>3. Busybox1.6</P><P>主要目标：</P><P>使车体能自主地在平地行驶，能躲避一定高度和大小的障碍物。</P><P>下一目标：</P><P>优化行走算法、行走路线预测、自选行走路线等。</P><P>加入编码轮，实现PID。</P><P>开发简述：</P><P>1. 在前几个月已完成了PRJ1：智能小车的开发，PRJ2硬件跟PRJ1相同，实现功能也基本相同，最大不同的是PRJ1采用UCOSII操作系统，PRJ2采用Linux，由于工作比较忙，PRJ1的介绍稍后撰写发布。</P><P>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等都很快弄好了。我是在业余时间做的，平时要上班，花了三个星期自己感觉也不算慢了。</P><P>3. 内核移植完了下一步当然是文件系统了，首先是移植Busybox1.6 ，然后建立目录结构、编写好linuxrc 还有hosts 、passwd 等配置文件，在mtd 上划出一个分区建立yaffs ，把目录结构、Busybox 等都copy 过去，启动时用这个yaffs 分区做根目录。具体的网上也有很多文章参考，这里就从简了。</P><P>4. 要使车体工作需要控制三个硬件：</P><P>a. 红外测距传感器--- 车体的唯一一个传感器，最大探测距离80cm ，负责探测车体周围的障碍物，车体上没有其他接触开关等传感器，所以车体的安全保障就靠它了。</P><P>b. 伺服器--- 负责带动红外测距传感器旋转，使它能探测到不同角度的距离，这里设计是前方 lang="EN-US" style="font-family: Times New Roman">140度范围旋转，也就是说能探测前方 140度范围的障碍物的距离。</P><P>c. 直流电机--- 负责车体移动。通过了PWM 信号驱动电机驱动板来调节车体的运行速度，前方障碍物距离远时运行速度可以快些，当离障碍物距离近时运行速度可以慢些或者停车。</P><P>5. 红外测距传感器输出的是模拟电压信号（当障碍物距离远输出电压低，当障碍物距离近输出电压高），所以要用到ADC ，需要编写S3C2410_adc 驱动。</P><P>6. 伺服器和直流电机都需要用到PWM信号，PWM信号是由定时器产生的，S3C2410 有五路定时器，linux 占用了一个，查看linux 内核源码，确定是占用了timer4 ，其余的4 个就可以选用了，选用timer0 驱动伺服器，timer1 驱动直流电机，分配好了立刻动手编写S3C2410_pwm 驱动；车体运行除了要控制速度还需要控制方向（前进、后退、左转、右转等），需要通过 I/O对电机驱动板进行控制，本来应该另外编写驱动程序来完成这些的，但考虑到控制对象是一样的（电机），而且控制方向和控制速度关系比较密切，所以把控制 I/O的代码也写到 S3C2410_pwm里面去了，用起来也方便。</P><P>7. 到此，内核和驱动都已经准备好了，下一步要实现的就是应用程序了，包括小车的行走模式、行走算法、障碍物探测等，使用了两个线程来完成这些任务：</P><P>a. MeasureThread()--- 探测线程：主要实现伺服器在 140度区间来回转动，带动红外测距传感器也随之转动；每转过一定角度就测量障碍物距离；根据测量结果通知 MotionThread()做相应的动作，例如：前方障碍物距离在安全范围，通知 MotionThread()前进；左前方有障碍物，通知 MotionThread()往右转；前方障碍物距离比较近，通知 MotionThread()减速，前方障碍物距离比较远，通知 MotionThread()加速。通过全局变量 Motion_status实现通信。</P><P>b. MotionThread()--- 运动控制线程：根据MeasureThread()设置的状态控制车体运动。当有障碍物时车体需要左转或右转以避开障碍物，当障碍物刚好在安全距离阀值附近时，会出现抖动（有障碍 - 左转 - 无障碍- 停- 有障碍- 左转），这里采取了每次转动要至少保持一定时间（例如： 200ms），在这段时间内禁止改变车体运动状态，通过互斥锁 mutex_turning来实现这种同步。</P><P> </P><DIV><IMG style="CURSOR: pointer" onclick=javascript:window.open(this.src); src="http://www.roboticfan.com/college/UploadFiles_7562/200708/20070823175933282.png" onload=rsimg(this,500)></DIV><P>8. 代码：car.c</P><P>＃i nclude <sys/stat.h> </P><P>＃i nclude <fcntl.h> </P><P>＃i nclude <stdio.h> </P><P>＃i nclude <sys/time.h> </P><P>＃i nclude <sys/types.h> </P><P>＃i nclude <unistd.h> </P><P>＃i nclude <pthread.h></P><P>#define ADC_DEV "/dev/s3c2410_adc" </P><P>#define PWM_DEV "/dev/s3c2410_pwm" </P><P> </P><P>#define PWMIOCTL_PRESALE0 0</P><P>#define PWMIOCTL_PRESALE1 1</P><P>#define PWMIOCTL_MUX0 2</P><P>#define PWMIOCTL_MUX1 3</P><P>#define PWMIOCTL_MUX2 4</P><P>#define PWMIOCTL_MUX3 5</P><P>#define PWMIOCTL_MUX4 6</P><P>#define PWMIOCTL_AUTORELOAD0 7</P><P>#define PWMIOCTL_AUTORELOAD1 8</P><P>#define PWMIOCTL_AUTORELOAD2 9</P><P>#define PWMIOCTL_AUTORELOAD3 10</P><P>#define PWMIOCTL_INVERTER0 12</P><P>#define PWMIOCTL_INVERTER1 13</P><P>#define PWMIOCTL_INVERTER2 14</P><P>#define PWMIOCTL_INVERTER3 15</P><P>#define PWMIOCTL_INVERTER4 16</P><P>#define PWMIOCTL_UPDATE0 17</P><P>#define PWMIOCTL_UPDATE1 18</P><P>#define PWMIOCTL_UPDATE2 19</P><P>#define PWMIOCTL_UPDATE3 20</P><P>#define PWMIOCTL_UPDATE4 21</P><P>#define PWMIOCTL_START0 22</P><P>#define PWMIOCTL_START1 23</P><P>#define PWMIOCTL_START2 24</P><P>#define PWMIOCTL_START3 25</P><P>#define PWMIOCTL_START4 26</P><P>#define PWMIOCTL_TCNTB0 27</P><P>#define PWMIOCTL_TCNTB1 28</P><P>#define PWMIOCTL_TCNTB2 29</P><P>#define PWMIOCTL_TCNTB3 30</P><P>#define PWMIOCTL_TCNTB4 31</P><P>#define PWMIOCTL_TCMPB0 32</P><P>#define PWMIOCTL_TCMPB1 33</P><P>#define PWMIOCTL_TCMPB2 34</P><P>#define PWMIOCTL_TCMPB3 35</P><P>#define PWMIOCTL_TCMPB4 36</P><P> </P><P>#define MOTOIOCTL_GPCDAT 200</P><P> </P><P>#define MOTION_FORWARD 1</P><P>#define MOTION_STOP 2</P><P>#define MOTION_LEFTTURN 3</P><P>#define MOTION_RIGHTTURN 4</P><P>#define MOTION_BACKTURN 5</P><P>#define MOTION_BACKWARD 6</P><P> </P><P>#define SERVO_STEP 2</P><P>#define SERVO_LEFT_MAX 7</P><P>#define SERVO_RIGHT_MAX 21</P><P>#define SERVO_MIDDLE 14</P><P>#define SERVO_DIRECT_LEFT 0</P><P>#define SERVO_DIRECT_RIGHT 1</P><P> </P><P>#define SAVE_ 0x140 //障碍物安全距离值</P><P>#define SAVE_1 0x70</P><P>#define SAVE_2 0x50</P><P> </P><P>unsigned char Motion_Status = MOTION_STOP ;</P><P>unsigned short GP2D12_[30] ;</P><P>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} ;</P><P>unsigned char Direct_val[8] = {0x0, 0x48, 0x0, 0x28, 0x42, 0x42, 0x22, 0x0} ;</P><P>unsigned char Speed_val[8] = {0x15, 0x20, 0x30, 0x46, 0x50, 0x5a, 0x60, 0x63} ;</P><P> </P><P>unsigned char curSpeed = 0 ;</P><P>unsigned char curDegree = 0 ;</P><P>unsigned char curDegree_d = 0 ;</P><P> </P><P>unsigned short turn_count = 0 ;</P><P>unsigned short lrturn_count = 0 ;</P><P>pthread_mutex_t mutex_turning;</P><P> </P><P>int adc_fd,pwm_fd; </P><P> </P><P>void InitServo() ;</P><P>void InitMoto() ;</P><P>void SetServoDegree(unsigned char degree) ;</P><P>void SetMotoSpeed(unsigned char speed) ;</P><P>void SetMotoDirect(unsigned char direct) ;</P><P>int GetAdc() ;</P><P>void MeasureThread() ;</P><P>void MotionThread() ;</P><P> </P><P>void InitServo() {</P><P> int i = 0 ;</P><P> i=0x20 ;</P><P> ioctl(pwm_fd,PWMIOCTL_PRESALE0,&i); </P><P> i=0x3 ;</P><P> ioctl(pwm_fd,PWMIOCTL_MUX0,&i); </P><P> i=1953 ; //i=1800 ;</P><P> ioctl(pwm_fd,PWMIOCTL_TCNTB0,&i); </P><P> SetServoDegree(14) ;</P><P>}</P><P> </P><P>void InitMoto() {</P><P> SetMotoDirect(MOTION_STOP) ;</P><P> SetMotoSpeed(curSpeed) ;</P><P>}</P><P> </P><P>void SetServoDegree(unsigned char degree) {</P><P> int i = 0 ;</P><P> i=Servo_val[degree] ;</P><P> ioctl(pwm_fd,PWMIOCTL_TCMPB0,&i); </P><P>}</P><P> </P><P>void SetMotoSpeed(unsigned char speed) {</P><P> int i = 0 ;</P><P> i=Speed_val[speed] ;</P><P> ioctl(pwm_fd,PWMIOCTL_TCMPB1,&i);</P><P>}</P><P> </P><P>void SetMotoDirect(unsigned char direct) {</P><P> int i = 0 ;</P><P> i=Direct_val[direct] ;</P><P> ioctl(pwm_fd,MOTOIOCTL_GPCDAT,&i); </P><P>}</P><P> </P><P>int GetAdc() {</P><P> int i = 0, ret = 0, tmp = 0 ;</P><P> for(i=0;i<5;i++) {</P><P> read(adc_fd,&tmp,sizeof(unsigned long));</P><P> if(i>0) ret += tmp ;</P><P> }</P><P> return ret/4 ;</P><P>}</P><P> </P><P>void MeasureThread()</P><P>{</P><P> unsigned short adcData0 = 0 ;</P><P> unsigned char save_count = 0 ;</P><P> unsigned char save_count1 = 0 ;</P><P> unsigned char save_count2 = 0 ;</P><P>style="font-size: 12pt; font-family: Times New Roman"> </P><P>// Motion_Status = MOTION_STOP ;</P><P> printf("MeasureThread,start\n");</P><P>style="font-size: 12pt; font-family: Times New Roman"> </P><P> while(1)</P><P> {</P><P> </P><P> switch (Motion_Status) {</P><P> case MOTION_LEFTTURN:</P><P> case MOTION_RIGHTTURN:</P><P> usleep(20000);</P><P> pthread_mutex_lock(&mutex_turning); //等待转动一定角度后才开始检测</P><P> pthread_mutex_unlock(&mutex_turning);</P><P> while(1) {</P><P> adcData0 = GetAdc() ;</P><P> if(adcData0<=SAVE_) {</P><P> SetMotoDirect(MOTION_STOP) ;</P><P> Motion_Status = MOTION_STOP ;</P><P> break ;</P><P> }</P><P> if(Motion_Status != MOTION_LEFTTURN && Motion_Status != MOTION_RIGHTTURN)</P><P> break ;</P><P> }</P><P> break ;</P><P>style="font-size: 12pt; font-family: Times New Roman"> </P><P> case MOTION_BACKWARD:</P><P> case MOTION_BACKTURN:</P><P> break ;</P><P> </P><P> default:</P><P> </P><P> if(curDegree<SERVO_LEFT_MAX) {</P><P> curDegree = SERVO_LEFT_MAX ;</P><P> curDegree_d = SERVO_DIRECT_RIGHT ;</P><P> }</P><P> if(curDegree>SERVO_RIGHT_MAX) {</P><P> curDegree = SERVO_RIGHT_MAX ;</P><P> curDegree_d = SERVO_DIRECT_LEFT ;</P><P> }</P><P> </P><P> while(1) {</P><P> adcData0 = GetAdc() ;</P><P> if(adcData0>=SAVE_) {</P><P> //stop and sleep to do.</P><P> //</P><P> lrturn_count++ ;</P><P> if(curDegree>SERVO_MIDDLE) {</P><P> Motion_Status = MOTION_LEFTTURN ;</P><P> curDegree_d=SERVO_DIRECT_RIGHT ;</P><P> </P><P> } else {</P><P> Motion_Status = MOTION_RIGHTTURN ;</P><P> curDegree_d=SERVO_DIRECT_LEFT ;</P><P> }</P><P> save_count = 0 ;</P><P> save_count1 = 0 ;</P><P> save_count2 = 0 ;</P><P> curSpeed = 0 ;</P><P> SetMotoDirect(MOTION_STOP) ;</P><P> SetMotoSpeed(curSpeed) ;</P><P> break ;</P><P> </P><P> } else if(adcData0<SAVE_2) {</P><P> save_count2++ ;</P><P> save_count1++ ;</P><P> save_count++ ;</P><P> if(save_count2>8) {</P><P> curSpeed = 2 ;</P><P> save_count2 = 8 ;</P><P> }</P><P> } else if(adcData0<SAVE_1) {</P><P> save_count2 = 0 ;</P><P> save_count1++ ;</P><P> save_count++ ;</P><P> if(curSpeed == 2) curSpeed = 1 ;</P><P> if(save_count1>8) {</P><P> curSpeed = 1 ;</P><P> save_count1 = 8 ;</P><P> }</P><P> } else if(adcData0<SAVE_) {</P><P> save_count1 = 0 ;</P><P> save_count2 = 0 ;</P><P> save_count++ ;</P><P> curSpeed = 0 ;</P><P> }</P><P> if(save_count>15) {</P><P> Motion_Status = MOTION_FORWARD ;</P><P> lrturn_count = 0 ;</P><P> save_count = 15 ;</P><P> }</P><P> SetMotoSpeed(curSpeed) ;</P><P> </P><P> if(curDegree_d==SERVO_DIRECT_LEFT) { //控制伺服器旋转</P><P> curDegree -= SERVO_STEP ;</P><P> if(curDegree<SERVO_LEFT_MAX) {</P><P> curDegree=SERVO_LEFT_MAX ;</P><P> curDegree_d=SERVO_DIRECT_RIGHT ;</P><P> }</P><P> } else {</P><P> curDegree += SERVO_STEP ;</P><P> if(curDegree>SERVO_RIGHT_MAX) {</P><P> curDegree=SERVO_RIGHT_MAX ;</P><P> curDegree_d=SERVO_DIRECT_LEFT ;</P><P> }</P><P> }</P><P> SetServoDegree(curDegree) ;</P><P> usleep(40000);</P><P> }</P><P> break ;</P><P>> </P><P> }</P><P> </P><P> }</P><P>}</P><P> </P><P>void MotionThread()</P><P>{</P><P> </P><P> printf("MotionThread start. \n");</P><P> while(1)</P><P> {</P><P> </P><P> switch (Motion_Status) {</P><P> case MOTION_FORWARD:</P><P> while(Motion_Status==MOTION_FORWARD) {</P><P> SetMotoDirect(MOTION_FORWARD) ;</P><P> usleep(5000) ;</P><P> }</P><P> SetMotoDirect(MOTION_STOP) ;</P><P> break ;</P><P> case MOTION_STOP:</P><P> usleep(5000) ;</P><P> break ;</P><P> case MOTION_LEFTTURN:</P><P> SetMotoDirect(MOTION_LEFTTURN) ;</P><P> pthread_mutex_lock(&mutex_turning); //每次转动要保持转动一定时间</P><P> if(lrturn_count>10) {</P><P> lrturn_count = 0 ;</P><P> usleep(1000000) ;</P><P> } else {</P><P> usleep(200000) ;</P><P> }</P><P> pthread_mutex_unlock(&mutex_turning);</P><P> while(Motion_Status==MOTION_LEFTTURN) {</P><P> SetMotoDirect(MOTION_LEFTTURN) ;</P><P> usleep(20000) ;</P><P> }</P><P> SetMotoDirect(MOTION_STOP) ;</P><P> usleep(100000) ;</P><P> break ;</P><P> case MOTION_RIGHTTURN:</P><P> SetMotoDirect(MOTION_RIGHTTURN) ;</P><P> pthread_mutex_lock(&mutex_turning);</P><P> if(lrturn_count>10) {</P><P> lrturn_count = 0 ;</P><P> usleep(1000000) ;</P><P> } else {</P><P> usleep(200000) ;</P><P> }</P><P> pthread_mutex_unlock(&mutex_turning);</P><P> while(Motion_Status==MOTION_RIGHTTURN) {</P><P> SetMotoDirect(MOTION_RIGHTTURN) ;</P><P> usleep(20000) ;</P><P> }</P><P> SetMotoDirect(MOTION_STOP) ;</P><P> usleep(100000) ;</P><P> break ;</P><P> default:</P><P> SetMotoDirect(MOTION_STOP) ;</P><P> Motion_Status = MOTION_STOP ;</P><P> usleep(5000) ;</P><P> break ;</P><P> }</P><P> }</P><P> printf("MotionThread end. \n");</P><P>}</P><P>int main(void) </P><P>{ </P><P> int ret; </P><P> pthread_t thread_id;</P><P> unsigned long tmp; </P><P> adc_fd=open(ADC_DEV,O_RDWR); </P><P> pwm_fd=open(PWM_DEV,O_RDWR); </P><P> if(adc_fd < 0) </P><P> { </P><P> printf("Error opening %s adc device\n", ADC_DEV); </P><P> return -1; </P><P> }</P><P> if(pwm_fd < 0) </P><P> { </P><P> printf("Error opening %s pwm device\n", PWM_DEV); </P><P> return -1; </P><P> } </P><P> else </P><P> printf("device id is %d\n",adc_fd) ; </P><P> </P><P> InitServo();</P><P> InitMoto() ;</P><P> pthread_mutex_init (&mutex_turning,NULL);</P><P> ret=pthread_create(&thread_id,NULL,(void *)MeasureThread,NULL);</P><P> if(ret!=0){</P><P> printf ("Create pthread error!\n");</P><P> return -1;</P><P> }</P><P> MotionThread() ;</P><P> pthread_join(thread_id,NULL);</P><P> close(adc_fd); </P><P> close(pwm_fd); </P><P> return 0; </P><P>}</P><P><A title=图片+视频+代码 href="http://www.shop8.us/a/car.rar">图片+视频+代码下载</A></P> |
|
发表于 2008-5-1 07:14:58
微信
收藏
转播
分享
顶
踩
提升卡
变色卡
抢沙发
楼主