找回密码
 立即注册

QQ登录

只需一步,快速开始

搜索
查看: 1297|回复: 0
打印 上一主题 下一主题
收起左侧

智能小车双舵机避障4WD小车单片机程序 舵机转向智能小车

[复制链接]
跳转到指定楼层
楼主
看了好多智能小车,都是两轮或者4轮麦克轮转向的,一直想模仿阿克曼智能ROS 机器人,利用舵机来转向的智能小车。经过多天学习c++语言和51芯片资料,通过改写程序,买了舵机转向前轮配件,用4wd小车改装的舵机转向智能小车,实现A舵机避障+B舵机转向功能,经过多次场地测试,前进速度尽量降低一点能实现完美避障。比以前4个tt电机驱动转向功耗小很多,玩耍时间更长最后附有程序和图文组装说明压缩包。
4轮小车双舵机避障零件清单
151主板1个+51单片机芯片
2SG90舵机1个+塑料云台空壳1个
3HC-SR04超声波测距1个
4TT电机2个
5L298N驱动1个
6、小车前轮舵机转向组1套
7、小车后轮2个
8、理电池7.4v一个。(5号电池不耐用,费钱)
9、铝合金或亚克力板底盘1个
10、螺丝、导线若干。
实物图片如下:

单片机程序如下:
  1. /*********************************************************************************
  2. * 【版    本】: 1.1
  3. * 【实验平台】: QX-A11智能小车  STC89C52
  4. * 【外部晶振】: 11.0592mhz        
  5. * 【主控芯片】: STC89C52
  6. * 【编译环境】: Keil μVisio4
  7. *        Copyright(C) QXMBOT
  8. *        All rights reserved
  9. ***********************************************************************************
  10. * 【程序功能】:QX-A11舵机云台避障参考程序                                                                                   
  11. * 【注意事项】:避免小车轮子堵转
  12. **********************************************************************************/
  13. #include <reg52.h>//51头文件
  14. #include <QX_A11.h>//QX_A11智能小车配置文件
  15. #include <intrins.h>
  16. bit Timer1Overflow;        //计数器1溢出标志位
  17. unsigned char disbuff[4]={0,0,0,0};//用于分别存放距离的值米,厘米,毫米
  18. unsigned int LeftDistance = 0, RightDistance = 0, FrontDistance = 0; //云台测距距离缓存
  19. unsigned int DistBuf[5] = {0};//distance data buffer
  20. unsigned char        pwm_val_left,pwm_val_right;        //中间变量,用户请勿修改。
  21. unsigned char         pwm_left,pwm_right;                        //定义PWM输出高电平的时间的变量。用户操作PWM的变量(括号内输入数值)。
  22. #define                PWM_DUTY                200                        //(用于舵机时不可修改)定义PWM的周期,数值为定时器0溢出周期,假如定时器溢出时间为100us,则PWM周期为20ms。
  23. #define                PWM_HIGH_MIN        70                        //限制PWM输出的最小占空比。用户请勿修改。
  24. #define                PWM_HIGH_MAX        PWM_DUTY        //限制PWM输出的最大占空比。用户请勿修改。

  25. void Timer0_Init(void); //定时器0初始化
  26. void Timer1_Init(void);//定时器1初始化
  27. void LoadPWM(void);//装入PWM输出值
  28. void Delay_Ms(unsigned int ms);//毫秒级延时函数
  29. void forward(unsigned char LeftSpeed,unsigned char RightSpeed);//QX_A11智能小车前进
  30. void left_run(unsigned char LeftSpeed,unsigned char RightSpeed);//QX_A11智能小车左转  
  31. void right_run(unsigned char LeftSpeed,unsigned char RightSpeed);//QX_A11智能小车右转
  32. void back_run(unsigned char LeftSpeed,unsigned char RightSpeed);//QX_A11智能小车后退
  33. void stop(void);//QX_A11智能小车停车
  34. void Delay18450us(void);        //@11.0592MHz
  35. void Delay1550us(void);                //@11.0592MHz
  36. void Delay19400us(void);        //@11.0592MHz
  37. void Delay600us(void);                //@11.0592MHz
  38. void Delay17500us(void);        //@11.0592MHz
  39. void Delay2500us(void);                //@11.0592MHz
  40. void QXMBOT_bubble(unsigned int *a,unsigned char n);//冒泡排序
  41. void QXMBOT_ServoFront(void);//舵机向前
  42. void QXMBOT_ServoLeft(void);//舵机左转
  43. void QXMBOT_ServoRight(void);//舵机右转
  44. void Servo1front(void);//舵机B向前
  45. void Servo1left(void);//舵机B左转
  46. void Servo1right(void);//舵机B右转
  47. void  QXMBOT_PTZ_Avoid(unsigned int val);//舵机云台避障
  48. unsigned int QXMBOT_GetDistance(void);//获取超声波距离
  49. void QXMBOT_TrigUltrasonic(void);// 触发超声波
  50. unsigned int QXMBOT_RefreshDistance(void);//超声波测距

  51. /*主函数*/     
  52. void main(void)
  53. {
  54.         Timer0_Init();//定时0初始化
  55.         Timer1_Init();//定时0初始化
  56.         EA_on;        //开中断
  57.         QXMBOT_ServoFront();//舵机向前
  58.         Servo1front(); //B舵机向前
  59.         Delay_Ms(1000);
  60.         forward(20,20);//前进5v速度,7.4v情况下forward全部调成(80,80)可调速。
  61.         while(1)
  62.         {
  63.                 QXMBOT_PTZ_Avoid(500);//舵机云台避障,LCD1602显示距离,设置触发距离300毫米
  64.         }        
  65. }


  66. /*********************************************
  67. 小车前进
  68. 入口参数:LeftSpeed,RightSpeed
  69. 说明:LeftSpeed,RightSpeed分别设置左右车轮转速
  70. **********************************************/
  71. void forward(unsigned char LeftSpeed,unsigned char RightSpeed)//不能放1号舵机参数Servo1front(),动力抖颤.
  72. {
  73.         pwm_left = LeftSpeed,pwm_right =  RightSpeed;//设置速度
  74.         left_motor_go; //左电机前进
  75.         right_motor_go; //右电机前进
  76. }
  77. /*********************************************
  78. 小车左转,入口参数:LeftSpeed,RightSpeed
  79. 说明:LeftSpeed,RightSpeed分别设置左右车轮转速
  80. **********************************************/
  81. void left_run(unsigned char LeftSpeed,unsigned char RightSpeed)
  82. {
  83.         pwm_left = LeftSpeed,pwm_right =  RightSpeed;//设置速度
  84.         left_motor_go; //左电机后退
  85.         right_motor_go; //右电机前进        
  86. }

  87. /*********************************************
  88. 小车右转,入口参数:LeftSpeed,RightSpeed
  89. 说明:LeftSpeed,RightSpeed分别设置左右车轮转速
  90. **********************************************/
  91. void right_run(unsigned char LeftSpeed,unsigned char RightSpeed)
  92. {
  93.         pwm_left = LeftSpeed,pwm_right =  RightSpeed;//设置速度
  94.         right_motor_go;//右电机后退
  95.         left_motor_go;    //左电机前进
  96. }
  97. /*********************************************
  98. 小车后退
  99. 入口参数:LeftSpeed,RightSpeed
  100. 说明:LeftSpeed,RightSpeed分别设置左右车轮转速
  101. **********************************************/
  102. void back_run(unsigned char LeftSpeed,unsigned char RightSpeed)
  103. {
  104. pwm_left = LeftSpeed,pwm_right =  RightSpeed;//设置速度
  105. right_motor_back;//右电机后退
  106. left_motor_back; //左电机后退
  107. }
  108. /*********************************************
  109. 小车停车
  110. 入口参数:无
  111. 说明:小车停车
  112. **********************************************/
  113. void stop(void)
  114. {
  115.         left_motor_stops;
  116.         right_motor_stops;
  117. }

  118. /*====================================
  119. 函数:void Delay_Ms(INT16U ms)
  120. 参数:ms,毫秒延时形参
  121. 描述:12T 51单片机自适应主时钟毫秒级延时函数
  122. ====================================*/
  123. void Delay_Ms(unsigned int ms)
  124. {
  125.      unsigned int i;
  126.          do{
  127.               i = MAIN_Fosc / 96000;
  128.                   while(--i);   //96T per loop
  129.      }while(--ms);
  130. }

  131. /*舵机方波延时朝向小车正前方*/
  132. void Delay1550us()                //@11.0592MHz
  133. {
  134.         unsigned char i, j;

  135.         i = 3;
  136.         j = 196;
  137.         do
  138.         {
  139.                 while (--j);
  140.         } while (--i);
  141. }

  142. void Delay18450us()                //@11.0592MHz
  143. {
  144.         unsigned char i, j;

  145.         _nop_();
  146.         i = 34;
  147.         j = 16;
  148.         do
  149.         {
  150.                 while (--j);
  151.         } while (--i);
  152. }
  153. /*舵机方波延时向小车右方*/
  154. void Delay600us()                //@11.0592MHz
  155. {
  156.         unsigned char i, j;

  157.         _nop_();
  158.         i = 2;
  159.         j = 15;
  160.         do
  161.         {
  162.                 while (--j);
  163.         } while (--i);
  164. }
  165. void Delay19400us()                //@11.0592MHz
  166. {
  167.         unsigned char i, j;

  168.         _nop_();
  169.         i = 35;
  170.         j = 197;
  171.         do
  172.         {
  173.                 while (--j);
  174.         } while (--i);
  175. }
  176. /*舵机方波延时朝向小车左方*/
  177. void Delay17500us()                //@11.0592MHz
  178. {
  179.         unsigned char i, j;

  180.         i = 32;
  181.         j = 93;
  182.         do
  183.         {
  184.                 while (--j);
  185.         } while (--i);
  186. }
  187. void Delay2500us()                //@11.0592MHz
  188. {
  189.         unsigned char i, j;

  190.         i = 5;
  191.         j = 120;
  192.         do
  193.         {
  194.                 while (--j);
  195.         } while (--i);
  196. }
  197. /*********************************************
  198. QX_A11智能小车PWM输出
  199. 入口参数:无
  200. 出口参数: 无
  201. 说明:装载PWM输出,如果设置全局变量pwm_left,pwm_right分别配置左右输出高电平时间
  202. **********************************************/
  203. void LoadPWM(void)
  204. {
  205.         if(pwm_left > PWM_HIGH_MAX)                pwm_left = PWM_HIGH_MAX;        //如果左输出写入大于最大占空比数据,则强制为最大占空比。
  206.         if(pwm_left < PWM_HIGH_MIN)                pwm_left = PWM_HIGH_MIN;        //如果左输出写入小于最小占空比数据,则强制为最小占空比。
  207.         if(pwm_right > PWM_HIGH_MAX)        pwm_right = PWM_HIGH_MAX;        //如果右输出写入大于最大占空比数据,则强制为最大占空比。
  208.         if(pwm_right < PWM_HIGH_MIN)        pwm_right = PWM_HIGH_MIN;        //如果右输出写入小于最小占空比数据,则强制为最小占空比。
  209.         if(pwm_val_left<=pwm_left)                Left_moto_pwm = 1;  //装载左PWM输出高电平时间
  210.         else Left_moto_pwm = 0;                                                     //装载左PWM输出低电平时间
  211.         if(pwm_val_left>=PWM_DUTY)                pwm_val_left = 0;        //如果左对比值大于等于最大占空比数据,则为零

  212.         if(pwm_val_right<=pwm_right)        Right_moto_pwm = 1; //装载右PWM输出高电平时间
  213.         else Right_moto_pwm = 0;                                                         //装载右PWM输出低电平时间
  214.         if(pwm_val_right>=PWM_DUTY)                pwm_val_right = 0;        //如果右对比值大于等于最大占空比数据,则为零
  215. }
  216. //冒泡排序
  217. void QXMBOT_bubble(unsigned int *a,unsigned char n) /*定义两个参数:数组首地址与数组大小*/
  218. {
  219.         unsigned int i,j,temp;        
  220.         for(i = 0;i < n-1; i++)        
  221.         {        
  222.                 for(j = i + 1; j < n; j++) /*注意循环的上下限*/
  223.                 {
  224.                         if(a[i] > a[j])
  225.                         {
  226.                                 temp = a[i];               
  227.                                 a[i] = a[j];               
  228.                                 a[j] = temp;                        
  229.                         }
  230.                 }
  231.         }

  232. }
  233. /*====================================
  234. 函数名        :QXMBOT_RefreshDistance距离单位:毫米
  235. 参数        :无
  236. 返回值        :经过冒泡排序后的距离
  237. 描述        :经过5次测距,去除最大值和最小值,取中间3次平均值
  238. ====================================*/
  239. unsigned int QXMBOT_RefreshDistance()
  240. {
  241.         unsigned char num;
  242.         unsigned int Dist;
  243.         for(num=0; num<5; num++)
  244.         {
  245.                 DistBuf[num] = QXMBOT_GetDistance();
  246.                 Delay_Ms(60);//测距周期不低于60毫秒        
  247.         }
  248.         QXMBOT_bubble(DistBuf, 5);//
  249.         Dist = (DistBuf[1]+DistBuf[2]+DistBuf[3])/3; //去掉最大和最小取中间平均值
  250.         return(Dist);
  251. }
  252. /*超声波触发*/
  253. void QXMBOT_TrigUltrasonic()
  254. {
  255.         TrigPin = 0; //超声波模块Trig        控制端
  256.         _nop_();
  257.         _nop_();
  258.         _nop_();
  259.         _nop_();
  260.         _nop_();
  261.         TrigPin = 1; //超声波模块Trig        控制端
  262.         _nop_();_nop_();_nop_();_nop_();_nop_();
  263.         _nop_();_nop_();_nop_();_nop_();_nop_();
  264.         _nop_();_nop_();_nop_();_nop_();_nop_();
  265.         TrigPin = 0; //超声波模块Trig        控制端
  266. }
  267. /*====================================
  268. 函数名        :QXMBOT_GetDistance
  269. 参数        :无
  270. 返回值        :获取距离单位毫米
  271. 描述        :超声波测距
  272. 通过发射信号到收到回响信号的时间测试距离
  273. 单片机晶振11.0592Mhz
  274. 注意测距周期为60ms以上
  275. ====================================*/
  276. unsigned int QXMBOT_GetDistance()
  277. {
  278.         unsigned int Distance = 0;        //超声波距离
  279.         unsigned int  Time=0;                //用于存放定时器时间值
  280.         QXMBOT_TrigUltrasonic();        //超声波触发
  281.         while(!EchoPin);          //判断回响信号是否为低电平
  282.         Timer1On;                        //启动定时器1
  283.         while(EchoPin);                //等待收到回响信号
  284.         Timer1Off;                        //关闭定时器1
  285.         Time=TH1*256+TL1;        //读取时间
  286.         TH1=0;
  287.         TL1=0;                                //清空定时器
  288.     Distance = (float)(Time*1.085)*0.17;//算出来是MM(一般超声测距公式、12mHZ的芯片是=Time*0.17)如果用0.017/算出来是cM,那么后面数据对应要改
  289.         return(Distance);//返回距离                                
  290. }
  291. /*====================================
  292. 函数名        :QXMBOT_PTZ_Avoid
  293. 参数        :val设置避障触发距离
  294. 返回值        :无
  295. 描述        :智能小车舵机云台避障
  296. 距离单位:毫米
  297. ====================================*/
  298. void  QXMBOT_PTZ_Avoid(unsigned int val)
  299. {
  300.         unsigned int Dis;//距离暂存变量
  301.         Dis = QXMBOT_GetDistance();//获取超声波测距距离,单位:毫米
  302.         if((Dis > 50) && (Dis < val))// 设置避障触发距离30~val=300
  303.         {
  304.                 LED1=0,LED2=0,beep=0;//LED1,2点亮,鸣笛        
  305.                 stop();        //停车
  306.                 Delay_Ms(100);
  307.                 LED1=1,LED2=1,beep=1;//LED1,2熄灭,静音

  308.                 /*舵机左转测距*/
  309.                 QXMBOT_ServoLeft();
  310.                 LeftDistance = QXMBOT_RefreshDistance();

  311.                 /*舵机右转测距*/
  312.                 QXMBOT_ServoRight();
  313.                 RightDistance = QXMBOT_RefreshDistance();

  314.                 /*舵机正前方测距*/
  315.                 QXMBOT_ServoFront();
  316.                 FrontDistance = QXMBOT_RefreshDistance();
  317.                 if((FrontDistance<150) && (LeftDistance<150) && (RightDistance<150))
  318.                 {
  319.                         do{
  320.                             Servo1right();
  321.                                 Delay_Ms(60);
  322.                                 left_run(20, 100);//原地左转,电机动力值(0~200)超出200则为高电平全速运行
  323.                                 Delay_Ms(200);//电机转动时间
  324.                                 Servo1front();
  325.                                 /*舵机正前方测距*/
  326.                                 QXMBOT_ServoFront();
  327.                                 Dis = QXMBOT_RefreshDistance();               
  328.                         }while(Dis < 200);//循环、距离
  329.            }else if(FrontDistance<100)
  330.                 {
  331.                         stop();        //停车
  332.                         back_run(60, 60);//后退,电机动力值
  333.                         Delay_Ms(150);
  334.            }else if((FrontDistance>LeftDistance) && (FrontDistance>RightDistance))
  335.                 {
  336.                         stop();        //停车
  337.                         Delay_Ms(100);
  338.                         forward(20, 20);//前进,电机动力值        
  339.                 }else if(LeftDistance > RightDistance)
  340.                 {
  341.                         LED1=1,LED2=0;//LED1灭,2点亮
  342.                         stop();        //停车
  343.                         Servo1right();
  344.                         Delay_Ms(60);
  345.                         left_run(20, 100);//原地左转,电机动力值
  346.                         Delay_Ms(200);//电机转动时间
  347.                         Servo1front();
  348.                         LED1=1,LED2=1;//LED1灭,2点灭               
  349.                 }else if(RightDistance > LeftDistance)
  350.                 {
  351.                         LED1=0,LED2=1;//LED1亮,2点灭
  352.                         stop();        //停车
  353.                            Servo1left();
  354.                         Delay_Ms(60);
  355.                         right_run(100, 20);//原地右转,电机动力值
  356.                         Delay_Ms(200);//电机转动时间,
  357.                         LED1=1,LED2=1;//LED1灭,2灭        
  358.                         Delay_Ms(60);
  359.                         Servo1front();
  360.                 }               
  361.         }
  362.         else
  363.         {
  364.                 forward(20, 20);//前进(不能放1号舵机参数Servo1front()动力抖颤)
  365.                 Delay_Ms(60);        
  366.         }                        
  367. }
  368. /*=================================================
  369. *函数名称:QXMBOT_ServoFront
  370. *函数功能:云台向前转动
  371. *调用:
  372. *输入:
  373. =================================================*/
  374. void QXMBOT_ServoFront()
  375. {
  376.         char i;
  377.         EA_off;        //关闭中断否则会影响舵机转向
  378.         for(i=0;i<10;i++)
  379.         {        
  380.                 ServoPin = 1;
  381.                 Delay1550us();
  382.                 ServoPin = 0;
  383.                 Delay18450us();
  384.         }
  385.         EA_on;        //开中断
  386.         Delay_Ms(100);
  387. }
  388. /*=================================================
  389. *函数名称:QXMBOT_ServoLeft
  390. *函数功能:云台向左转动
  391. =================================================*/
  392. void QXMBOT_ServoLeft()
  393. {
  394.         char i;
  395.         EA_off;        //关闭中断否则会影响舵机转向
  396.         for(i=0;i<10;i++)
  397.         {        
  398.                 ServoPin = 1;
  399.                 Delay2500us();
  400.                 ServoPin = 0;
  401.                 Delay17500us();
  402.         }
  403.         EA_on;        //开中断
  404.         Delay_Ms(100);
  405. }
  406. /*=================================================
  407. *函数名称:QXMBOT_ServoFront
  408. *函数功能:云台向右转动
  409. =================================================*/
  410. void QXMBOT_ServoRight()
  411. {
  412.         char i;
  413.         EA_off;        //关闭中断否则会影响舵机转向
  414.         for(i=0;i<10;i++)
  415.         {        
  416.                 ServoPin = 1;
  417.                 Delay600us();
  418.                 ServoPin = 0;
  419.                 Delay19400us();
  420.         }
  421.         EA_on;        //开中断
  422.         Delay_Ms(100);
  423. }
  424. /*=================================================
  425. *函数名称:QXMBOT_ServoFront
  426. *函数功能:云台B转动前(90°)
  427. =================================================*/
  428. void Servo1front()//舵机B向前
  429. {
  430.         char i;
  431.         EA_off;        //关闭中断否则会影响舵机转向
  432.         for(i=0;i<10;i++)
  433.         {        
  434.                 Servo1Pin = 1;
  435.                 Delay1550us();
  436.                 Servo1Pin = 0;
  437.                 Delay18450us();
  438.         }
  439.         EA_on;        //开中断
  440.         Delay_Ms(60);
  441. }
  442. /*=================================================
  443. *函数名称:QXMBOT_ServoFront
  444. *函数功能:云台B向左转动
  445. =================================================*/
  446. void Servo1left()//舵机B左转45度
  447. {
  448.         char i;
  449.         EA_off;        //关闭中断否则会影响舵机转向
  450.         for(i=0;i<10;i++)
  451.         {        
  452.                 Servo1Pin = 1;
  453.                 Delay_Ms(2);
  454.                 Servo1Pin = 0;
  455.                 Delay_Ms(18);
  456.         }
  457.         EA_on;        //开中断
  458.         Delay_Ms(60);
  459. }
  460. /*=================================================
  461. *函数名称:QXMBOT_ServoFront
  462. *函数功能:云台B向右转动
  463. =================================================*/
  464. void Servo1right()//舵机B右转45度
  465. {
  466.         char i;
  467.         EA_off;        //关闭中断否则会影响舵机转向
  468.         for(i=0;i<10;i++)
  469.         {        
  470.                 Servo1Pin = 1;
  471.                 Delay_Ms(1);
  472.                 Servo1Pin = 0;
  473.                 Delay_Ms(19);
  474.         }
  475.         EA_on;        //开中断
  476.         Delay_Ms(60);
  477. }
  478. /********************* Timer0初始化************************/
  479. void Timer0_Init(void)
  480. {
  481.         TMOD |= 0x02;//定时器0,8位自动重装模块
  482.         TH0 = 164;
  483.         TL0 = 164;//11.0592M晶振,12T溢出时间约等于100微秒
  484.         TR0 = 1;//启动定时器0
  485.         ET0 = 1;//允许定时器0中断        
  486. }
  487. /*定时器1初始化*/
  488. void Timer1_Init(void)               
  489. {
  490.         TMOD |= 0x10;        //定时器1工作模式1,16位定时模式。T1用测ECH0脉冲长度
  491.         TH1 = 0;                  
  492.     TL1 = 0;
  493.         ET1 = 1;             //允许T1中断
  494. }

  495. /********************* Timer0中断函数************************/
  496. void timer0_int (void) interrupt 1
  497. {
  498.          pwm_val_left++;
  499.          pwm_val_right++;
  500.          LoadPWM();//装载PWM输出
  501. }
  502. /* Timer0 interrupt routine */
  503. void tm1_isr() interrupt 3 using 1
  504. {
  505.         Timer1Overflow = 1;        //计数器1溢出标志位
  506.         EchoPin = 0;                //超声波接收端        
  507. }        
复制代码

图文附件请下载: 4轮小车双舵机避障程序 配件清单.zip (1.28 MB, 下载次数: 19)

评分

参与人数 1黑币 +50 收起 理由
admin + 50 共享资料的黑币奖励!

查看全部评分

分享到:  QQ好友和群QQ好友和群 QQ空间QQ空间 腾讯微博腾讯微博 腾讯朋友腾讯朋友
收藏收藏3 分享淘帖 顶1 踩
回复

使用道具 举报

您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

手机版|小黑屋|51黑电子论坛 |51黑电子论坛6群 QQ 管理员QQ:125739409;技术交流QQ群281945664

Powered by 单片机教程网

快速回复 返回顶部 返回列表