设为首页收藏本站
开启辅助访问 切换到宽版

 找回密码
 立即注册

QQ登录

只需一步，快速开始

热门： 51单片机 | 24小时必答区 | 单片机教程 | 单片机DIY制作 | STM32 | Cortex M3 | 模数电子 | 电子DIY制作 | 音响/功放 | 拆机乐园 | Arduino | 嵌入式OS | 程序设计

搜索
»论坛 › 嵌入式/单片机论坛 › Arduino › 超声波避障小车Arduino源码
返回列表 发新帖
查看: 12360|回复: 4
收起左侧

超声波避障小车Arduino源码

[复制链接]
ID:389885 发表于 2018-8-25 12:09 | 显示全部楼层 |阅读模式
硬件电路配置有Arduino、L293D驱动模块、PCA9685驱动模块各一个，四个直流电机（由L293D驱动模块驱动），3个舵机（PCA9685驱动模块驱动），三个超声波探测器。代码有很多的多余注释，还有想要用定时器2的草稿，大家看着做个参考。
//四轮独立驱动小车,注意程序已经舵机角度改为不是0-60之间变化，范围有所缩小。

  1. #include <AFMotor.h>
  2. //#include <MsTimer2.h>


  5.     #define PCA9685_adrr 0x80//  1+A5+A4+A3+A2+A1+A0+w/r
  6.                         //片选地址，将焊接点置1可改变地址，
  7.                         // 当IIC总 呱嫌 多片PCA9685或相同地址时才需焊接
  8.     #define PCA9685_SUBADR1 0x2
  9.     #define PCA9685_SUBADR2 0x3
  10.     #define PCA9685_SUBADR3 0x4

  12.     #define PCA9685_MODE1 0x0//PCA9685模式复位设置
  13.     #define PCA9685_PRESCALE 0xFE//控制周期的寄存器

  15.     #define LED0_ON_L 0x6
  16.     #define LED0_ON_H 0x7
  17.     #define LED0_OFF_L 0x8
  18.     #define LED0_OFF_H 0x9

  20.     #define ALLLED_ON_L 0xFA
  21.     #define ALLLED_ON_H 0xFB
  22.     #define ALLLED_OFF_L 0xFC
  23.     #define ALLLED_OFF_H 0xFD   

  25.     #define SERVOMIN  115 // this is the 'minimum' pulse length count (out of 4096)
  26.     #define SERVOMAX  590 // this is the 'maximum' pulse length count (out of 4096)
  27.     //#define SERVO000  130 //0度对应4096的脉宽计数值
  28.     #define SERVO000  145 //0度对应4096的脉宽计数值
  29.     #define SERVO180  520 //180度对应4096的脉宽计算值，四个值可根据不同舵机修改

  31. AF_DCMotor LF_motor(1);//定义四个车轮电机对应的内存单元
  32. AF_DCMotor RF_motor(3);
  33. AF_DCMotor LB_motor(2);
  34. AF_DCMotor RB_motor(4);
  35. // 设定SR04连接的Arduino引脚
  36.     const int TrigPin = A0;//超声波测距使能控制信号
  37.     const int EchoPin = A1;//超声波测距模块输出，通过测得高电平持续时间获得障碍物距离
  38.     //unsigned long distance;
  39.     const int IIC_SDA = A5;//串行数据线信号
  40.     const int IIC_SCL = A4;//串行时钟线信号
  41.     const int EchoPin_L = A2;//左舵机超声波探测器接收信号
  42.     const int EchoPin_R = A3;//右舵机超声波探测器接收信号

  44. boolean stage1_ctrl=false;//前端超声波云台控制信号，true表示当前从60度转到0度；
  45. boolean stage23_ctrl=false;//左右端超声波云台控制信号，true表示当前从60度转到0度；
  46. boolean direction_ctrl_r=true;
  47. boolean direction_ctrl_l=true;


  50. int l=0;
  51. int m=0;
  52. int stage1_dis;
  53. int stage2_dis;
  54. int stage3_dis;


  57. void setup()
  58. {   // 初始化串口通信及连接SR04的引脚
  59. //Serial.begin(9600);
  60. pinMode(TrigPin, OUTPUT);
  61. // 要检测引脚上输入的脉冲宽度，需要先设置为输入状态
  62. pinMode(EchoPin, INPUT);
  63. //Serial.println("开始");
  64. LF_motor.setSpeed(150);
  65. RF_motor.setSpeed(150);
  66. LB_motor.setSpeed(150);
  67. RB_motor.setSpeed(150);
  68. LF_motor.run(FORWARD);
  69. RF_motor.run(FORWARD);
  70. LB_motor.run(FORWARD);
  71. RB_motor.run(FORWARD);
  72.   pinMode(IIC_SDA,OUTPUT);
  73.   pinMode(IIC_SCL,OUTPUT);
  74. //Serial.println("SDA,SCL引脚初始化");
  75. IIC_init();
  76. // Serial.println("IIC初始化");
  77. reset();
  78.   // Serial.println("初始化结束");
  79.   setPWMFreq(50);
  80.   // Serial.println("初始化结束");
  81. //MsTimer2::set(500, stage1); // 500ms period定时器2初始化；
  82. // Serial.println("初始化结束");
  83. // MsTimer2::start();
  84. //Serial.println("初始化结束");
  85. }

  87. void loop()
  88. {//  Serial.println("主程序");
  89.   if(stage1_ctrl==true)
  90.    {
  91.     stage1_ctrl=false;
  92.     setPWM(0, 0, SERVO000);//第0路舵机从60度转到0角度   0.12-0.13秒/60度
  93.    }
  94.    else
  95.    {
  96.     stage1_ctrl=true;
  97.     setPWM(0, 0,224);  //第0路舵机从0度转到60角度   
  98.    }   
  99. for(l=0;l<=100;l++)   
  100. {
  101.     // 产生一个1。、0us的高脉冲去触发TrigPin,即开启超声波测距
  102. digitalWrite(TrigPin, LOW);
  103. delayMicroseconds(2);
  104. digitalWrite(TrigPin, HIGH);
  105. delayMicroseconds(10);
  106. digitalWrite(TrigPin, LOW);

  108.     // 检测脉冲宽度，并计算出距离
  109. delayMicroseconds(2);
  110. int stage1_dis=pulseIn(EchoPin, HIGH,2000);
  111. //Serial.println(stage1_dis);
  112. //distance = pulseIn(EchoPin, HIGH) / 58.00;
  113. if(stage1_dis>0)//stage1_dis=0表示超过超声波等待回收信号范围，即距离超过（1300/58）cm；
  114.   {
  115.     if(stage23_ctrl==true)
  116.      {
  117.     stage23_ctrl=false;
  118.     setPWM(1, 0, SERVO000);//第1路舵机从60度转到0角度   
  119.     setPWM(2, 0,224);//第2路舵机从0度转到60角度   
  120.    }
  121.    else
  122.    {
  123.     stage23_ctrl=true;
  124.     setPWM(1, 0,224);  //第1路舵机从0度转到60角度
  125.     setPWM(2, 0, SERVO000);  //第1路舵机从60度转到0角度     
  126.    }   
  127.   // Serial.println("外循环");
  128.    for(m=0;m<=40;m++)
  129.    {
  130. //Serial.println("前端超声波探测循环");
  131.    //delay(1);
  132.    digitalWrite(TrigPin, LOW); // 产生一个10us的高脉冲去触发TrigPin,即开启超声波测距
  133.    delayMicroseconds(2);
  134.    digitalWrite(TrigPin, HIGH);
  135.    delayMicroseconds(10);
  136.    digitalWrite(TrigPin, LOW);
  137.    delayMicroseconds(2);
  138.    stage2_dis=pulseIn(EchoPin_L, HIGH,1500);
  139.    delay(1);//如果不延时处理，stage3_dis测得距离会有问题；
  140.    digitalWrite(TrigPin, LOW);
  141.    delayMicroseconds(2);
  142.    digitalWrite(TrigPin, HIGH);
  143.    delayMicroseconds(10);
  144.    digitalWrite(TrigPin, LOW);
  145.    delayMicroseconds(2);
  146.    stage3_dis=pulseIn(EchoPin_R, HIGH,1500);
  147. //Serial.println(stage1_dis);
  148. //Serial.println(stage2_dis);
  149. // Serial.println(stage3_dis);
  150. //delay(500);
  151.    if(stage2_dis>0)
  152.    {
  153.    direction_ctrl_l=false;
  154.    }
  155.    else
  156.    {
  157.    direction_ctrl_l=true;
  158.    }
  159.    if(stage3_dis>0)
  160.    {
  161.    direction_ctrl_r=false;
  162.    }
  163.    else
  164.    {
  165.    direction_ctrl_r=true;
  166.    }
  167.    }
  168. // Serial.println(direction_ctrl_l);
  169. //Serial.println(direction_ctrl_r);
  170.   if(direction_ctrl_l==true)
  171.   {
  172.   // Serial.println("左转");
  173.    LF_motor.run(BACKWARD);
  174.    LB_motor.run(BACKWARD);
  175.   // RF_motor.run(FORWARD);
  176.   // RB_motor.run(FORWARD);
  177.    delay(1000);
  178. //  LF_motor.run(FORWARD);
  179.   // LB_motor.run(FORWARD);

  181.   }
  182.   else if(direction_ctrl_r==true)
  183.   {
  184.    // Serial.println("右转");  
  185.    RF_motor.run(BACKWARD); //如果之前左转，发现左端忽然有障碍物出现，则机器人实际变成后退动作；
  186.    RB_motor.run(BACKWARD);
  187.    //LF_motor.run(FORWARD);
  188.    //LB_motor.run(FORWARD);
  189.    delay(1000);
  190.   // RF_motor.run(FORWARD);
  191.   // RB_motor.run(FORWARD);

  193.   }
  194.   else
  195.   {
  196.    // Serial.println("后退");
  197.    RF_motor.run(BACKWARD);
  198.    RB_motor.run(BACKWARD);
  199.    LF_motor.run(BACKWARD);
  200.    LB_motor.run(BACKWARD);
  201.    delay(1000);
  202.   // RF_motor.run(FORWARD);
  203. //  RB_motor.run(FORWARD);
  204.   // LF_motor.run(FORWARD);
  205.   // LB_motor.run(FORWARD);


  208.   }

  210.   break;//注意内层的break只能跳出内层的for循环，两个，三个break，也都只能跳出本层循环，及第二个Break根本不会执行；

  212.    //digitalWrite(LF, HIGH);
  213.    //digitalWrite(LB, HIGH);

  215. }
  216. else
  217. {
  218.    RF_motor.run(FORWARD);
  219.    RB_motor.run(FORWARD);
  220.    LF_motor.run(FORWARD);
  221.    LB_motor.run(FORWARD);
  222. }

  224. }  
  225. //Serial.print(distance);
  226. //Serial.print("cm");
  227. //Serial.println();
  228. //delay(500);
  229. }
  230.   //IIC初始化
  231.     void IIC_init()
  232.     {
  233.     digitalWrite(IIC_SDA,HIGH); //IIC_SDA IIC_SCL使用前被拉高
  234.     delayMicroseconds(4);
  235.     digitalWrite(IIC_SCL,HIGH);
  236.     delayMicroseconds(4);
  237.     }


  240.   void start()//基于IIC总线的物理结构，总线上的START和IIC_stop信号必定是唯一的。
  241.                 //另外，IIC总线标准规定IIC_SDA线的数据转换必须在IIC_SCL线的低电平期，
  242.                 //在IIC_SCL线的高电平期，IIC_SDA线的上数据是稳定的。所以，start()，IIC_stop()的
  243.                 //特殊情况可以作为指示的起止信号。
  244.     {
  245.    //   Serial.println("start开始");
  246.     digitalWrite(IIC_SDA,HIGH);
  247.     delayMicroseconds(4);
  248.     digitalWrite(IIC_SCL,HIGH); //IIC_SCL高 IIC_SDA拉低表示可以IIC启动
  249.     delayMicroseconds(4);
  250.     digitalWrite(IIC_SDA,LOW);
  251.     delayMicroseconds(4);
  252.     digitalWrite(IIC_SCL,LOW);
  253.     delayMicroseconds(4);
  254.   //  Serial.println("start结束");
  255.     }

  257.     //IIC停止
  258.     void IIC_stop()
  259.     {
  260.     digitalWrite(IIC_SDA,LOW);
  261.     delayMicroseconds(4);
  262.     digitalWrite(IIC_SCL,HIGH);
  263.     delayMicroseconds(4);
  264.     digitalWrite(IIC_SDA,HIGH);
  265.     delayMicroseconds(4);
  266.     }

  268.     //IIC应答
  269.     void ACK()//IIC_SDA应答信号为0表示正常收发数据，为1表示停止，
  270.     {
  271.     unsigned char i;
  272.     digitalWrite(IIC_SCL,HIGH);
  273.     delayMicroseconds(2);
  274.     pinMode(IIC_SDA,INPUT);
  275.     while((digitalRead(IIC_SDA))&&(i<=255))   
  276.     {
  277.     i++;
  278.     }
  279.     pinMode(IIC_SDA,OUTPUT);
  280.     digitalWrite(IIC_SCL,LOW);
  281.     delayMicroseconds(4);
  282.     }

  284.     //写字节
  285.     void write_byte(unsigned char byte)
  286.     {
  287.     //Serial.println("地址写入SDL开始");调试用
  288.     unsigned char i,temp;
  289.     temp=byte;
  290.     for(i=7;i<=7;i--)
  291.     {//unsigned char x;//调试用
  292.     //x=0;//调试用
  293.     //const int test = 2;//调试用
  294.     //pinMode(test,INPUT);//调试用
  295.     digitalWrite(IIC_SCL,LOW);
  296.     delayMicroseconds(4);
  297.     //Serial.println(temp);//调试用
  298.     //Serial.println(bitRead(temp,i));//调试用
  299.     digitalWrite(IIC_SDA,bitRead(temp,i));
  300.    //Serial.println(digitalRead(test));
  301.    // x=(digitalRead(test))|(x<<1);//调试用
  302.    //Serial.println(x);//调试用
  303.    //Serial.println(i);//调试用
  304.     delayMicroseconds(4);
  305.     digitalWrite(IIC_SCL,HIGH);
  306.     delayMicroseconds(4);
  307.     }
  308.     digitalWrite(IIC_SCL,LOW);
  309.     delayMicroseconds(4);
  310.     digitalWrite(IIC_SDA,HIGH);
  311.     delayMicroseconds(4);
  312.     }
  313.     unsigned char read_byte()
  314.     {
  315.     unsigned char i,j,k;
  316.     digitalWrite(IIC_SCL,LOW);
  317.     delayMicroseconds(4);
  318.     digitalWrite(IIC_SDA,HIGH);
  319.     delayMicroseconds(4);
  320.     for(i=0;i<8;i++)
  321.     {
  322.     delayMicroseconds(4);
  323.     digitalWrite(IIC_SCL,HIGH);
  324.     delayMicroseconds(4);
  325.     pinMode(IIC_SDA,INPUT);
  326.     if(digitalRead(IIC_SDA))
  327.     {
  328.       j=1;
  329.     }
  330.     else
  331.     {
  332.       j=0;
  333.     }
  334.     pinMode(IIC_SDA,OUTPUT);
  335.     k=(k<<1)|j;
  336.     digitalWrite(IIC_SCL,LOW);
  337.     }
  338.     delayMicroseconds(4);
  339.     return k;
  340.     }

  342.     void PCA9685_write(unsigned char address,unsigned char date)
  343.     {
  344.    //Serial.println("PCA9685_write开始");
  345.     start();
  346.     write_byte(PCA9685_adrr); //PCA9685
  347.     ACK();
  348.     write_byte(address); //PCA9685对应内部哪个地址
  349.     ACK();
  350.     write_byte(date); //所写入的数据
  351.     //Serial.println("所写入的数据:");
  352.     //Serial.println(date);
  353.     ACK();
  354.     IIC_stop();
  355.     //Serial.println("PCA9685_write结束");
  356.     }

  358.     //从PCA9685读数据有返回值
  359.     unsigned char PCA9685_read(unsigned char address)
  360.     {
  361.     unsigned char date;
  362.     start();
  363.     write_byte(PCA9685_adrr); //PCA9685
  364.     ACK();
  365.     write_byte(address);//PCA9685对应内部哪个地址
  366.     ACK();
  367.     start();
  368.     write_byte(PCA9685_adrr|0x01); //表示此时状态为从PCA9685读取数据
  369.     ACK();
  370.     date=read_byte();
  371.    // Serial.println("date");
  372.     //Serial.println(date);
  373.     return date;
  374.     }

  376. //PCA9685复位
  377.     void reset(void)
  378.     {//Serial.println("RESET开始");
  379.     PCA9685_write(PCA9685_MODE1,0x0);
  380.      //Serial.println("RESET结束");
  381.     }

  383. //PCA9685修改频率
  384. /*---------------------------------------------------------------
  385.                                 PCA9685修改角度函数
  386. num:舵机PWM输出引脚0~15,on:PWM上升计数值0~4096,off:PWM下降计数值0~4096
  387. 一个PWM周期分成4096份,由0开始+1计数,计到on时跳变为高电平,继续计数到off时
  388. 跳变为低电平,直到计满4096重新开始。所以当on不等于0时可作延时,当on等于0时,
  389. off/4096的值就是PWM的占空比。
  390. ----------------------------------------------------------------*/



  394.     void setPWMFreq(float freq)
  395.      { //Serial.print("设置PWM周期：");
  396.                 unsigned int prescale,oldmode,newmode;
  397.                 float prescaleval;
  398.                 freq *= 0.915;
  399.                 prescaleval = 25000000;
  400.                 prescaleval /= 4096;
  401.                 prescaleval /= freq;
  402.                 prescaleval -= 1;
  403.                 prescale = floor(prescaleval + 0.5);

  405.                 oldmode = PCA9685_read(PCA9685_MODE1);
  406.                 newmode = (oldmode&0x7F) | 0x10; // sleep
  407.                 //Serial.println("新模式：");
  408.                 PCA9685_write(PCA9685_MODE1,newmode); // go to sleep
  409.                // Serial.println("新模式：");
  410.                 // Serial.println(newmode);
  411.                 PCA9685_write(PCA9685_PRESCALE,prescale); // set the prescaler
  412.                 // Serial.println("周期频率设置值：");
  413.                 // Serial.println(prescale);
  414.                 PCA9685_write(PCA9685_MODE1,oldmode);
  415.                // Serial.println("旧模式：");
  416.                 // Serial.println(oldmode);
  417.                 delay(2);
  418.                 PCA9685_write(PCA9685_MODE1,oldmode | 0xa1);
  419.      }
  420.           void setPWM(unsigned int num, unsigned int on, unsigned int off)
  421.         {
  422.                //Serial.print("当前通道：");
  423.                //Serial.print(num);
  424.                 PCA9685_write(LED0_ON_L+4*num,on);
  425.                 PCA9685_write(LED0_ON_H+4*num,on>>8);
  426.                 PCA9685_write(LED0_OFF_L+4*num,off);
  427.                 PCA9685_write(LED0_OFF_H+4*num,off>>8);
  428.         }

  430. /*void stage1()
  431. {
  432.   Serial.print("timer");

  434. if(stage1_ctrl==true)
  435.    {
  436.     stage1_ctrl=false;
  437.     setPWM(0, 0, SERVO000);//第0路舵机从60度转到0角度   
  438.    }
  439.    else
  440.    {
  441.     stage1_ctrl=true;
  442.     setPWM(0, 0,224);  //第0路舵机从0度转到60角度   
  443.    }            
  444.                    //setPWM(0, 0, SERVO180);//第0路舵机转到0角度     
  445.                    //setPWM(1, 0, SERVO000);//第1路舵机转到0角度
  446. }
  447. */
复制代码



评分

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

查看全部评分

回复

使用道具 举报

ID:1 发表于 2018-8-25 17:02 | 显示全部楼层
原理图能分享下吗？
回复

使用道具 举报

ID:389885 发表于 2018-8-26 11:55 | 显示全部楼层
admin 发表于 2018-8-25 17:02
原理图能分享下吗？

额，没有画原理图，直接接线的。我等下简单手画一下吧
回复

使用道具 举报

ID:389885 发表于 2018-8-26 12:48 | 显示全部楼层
不懂得怎么发图片啊。。。那样成功了没
回复

使用道具 举报

ID:390585 发表于 2018-11-8 19:18 | 显示全部楼层
Riton 发表于 2018-8-26 12:48
不懂得怎么发图片啊。。。那样成功了没

没有看到图片
回复

使用道具 举报

返回列表 发新帖
高级模式
B Color Image Link Quote Code Smilies
您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

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

Powered by 单片机教程网

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