標(biāo)題: 超聲波避障小車Arduino源碼 [打印本頁]
作者: Riton    時間: 2018-8-25 12:09
標(biāo)題: 超聲波避障小車Arduino源碼
硬件電路配置有Arduino、L293D驅(qū)動模塊、PCA9685驅(qū)動模塊各一個，四個直流電機(jī)（由L293D驅(qū)動模塊驅(qū)動），3個舵機(jī)（PCA9685驅(qū)動模塊驅(qū)動），三個超聲波探測器。代碼有很多的多余注釋，還有想要用定時器2的草稿，大家看著做個參考。
//四輪獨立驅(qū)動小車,注意程序已經(jīng)舵機(jī)角度改為不是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.                         // 當(dāng)IIC總 呱嫌 多片PCA9685或相同地址時才需焊接
  8.     #define PCA9685_SUBADR1 0x2
  9.     #define PCA9685_SUBADR2 0x3
  10.     #define PCA9685_SUBADR3 0x4

  12.     #define PCA9685_MODE1 0x0//PCA9685模式復(fù)位設(shè)置
  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度對應(yīng)4096的脈寬計數(shù)值
  28.     #define SERVO000  145 //0度對應(yīng)4096的脈寬計數(shù)值
  29.     #define SERVO180  520 //180度對應(yīng)4096的脈寬計算值，四個值可根據(jù)不同舵機(jī)修改

  31. AF_DCMotor LF_motor(1);//定義四個車輪電機(jī)對應(yīng)的內(nèi)存單元
  32. AF_DCMotor RF_motor(3);
  33. AF_DCMotor LB_motor(2);
  34. AF_DCMotor RB_motor(4);
  35. // 設(shè)定SR04連接的Arduino引腳
  36.     const int TrigPin = A0;//超聲波測距使能控制信號
  37.     const int EchoPin = A1;//超聲波測距模塊輸出，通過測得高電平持續(xù)時間獲得障礙物距離
  38.     //unsigned long distance;
  39.     const int IIC_SDA = A5;//串行數(shù)據(jù)線信號
  40.     const int IIC_SCL = A4;//串行時鐘線信號
  41.     const int EchoPin_L = A2;//左舵機(jī)超聲波探測器接收信號
  42.     const int EchoPin_R = A3;//右舵機(jī)超聲波探測器接收信號

  44. boolean stage1_ctrl=false;//前端超聲波云臺控制信號，true表示當(dāng)前從60度轉(zhuǎn)到0度；
  45. boolean stage23_ctrl=false;//左右端超聲波云臺控制信號，true表示當(dāng)前從60度轉(zhuǎn)到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. // 要檢測引腳上輸入的脈沖寬度，需要先設(shè)置為輸入狀態(tài)
  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("初始化結(jié)束");
  79.   setPWMFreq(50);
  80.   // Serial.println("初始化結(jié)束");
  81. //MsTimer2::set(500, stage1); // 500ms period定時器2初始化；
  82. // Serial.println("初始化結(jié)束");
  83. // MsTimer2::start();
  84. //Serial.println("初始化結(jié)束");
  85. }

  87. void loop()
  88. {//  Serial.println("主程序");
  89.   if(stage1_ctrl==true)
  90.    {
  91.     stage1_ctrl=false;
  92.     setPWM(0, 0, SERVO000);//第0路舵機(jī)從60度轉(zhuǎn)到0角度   0.12-0.13秒/60度
  93.    }
  94.    else
  95.    {
  96.     stage1_ctrl=true;
  97.     setPWM(0, 0,224);  //第0路舵機(jī)從0度轉(zhuǎn)到60角度   
  98.    }   
  99. for(l=0;l<=100;l++)   
  100. {
  101.     // 產(chǎn)生一個1。、0us的高脈沖去觸發(fā)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路舵機(jī)從60度轉(zhuǎn)到0角度   
  119.     setPWM(2, 0,224);//第2路舵機(jī)從0度轉(zhuǎn)到60角度   
  120.    }
  121.    else
  122.    {
  123.     stage23_ctrl=true;
  124.     setPWM(1, 0,224);  //第1路舵機(jī)從0度轉(zhuǎn)到60角度
  125.     setPWM(2, 0, SERVO000);  //第1路舵機(jī)從60度轉(zhuǎn)到0角度     
  126.    }   
  127.   // Serial.println("外循環(huán)");
  128.    for(m=0;m<=40;m++)
  129.    {
  130. //Serial.println("前端超聲波探測循環(huán)");
  131.    //delay(1);
  132.    digitalWrite(TrigPin, LOW); // 產(chǎn)生一個10us的高脈沖去觸發(fā)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("左轉(zhuǎn)");
  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("右轉(zhuǎn)");  
  185.    RF_motor.run(BACKWARD); //如果之前左轉(zhuǎn)，發(fā)現(xiàn)左端忽然有障礙物出現(xiàn)，則機(jī)器人實際變成后退動作；
  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;//注意內(nèi)層的break只能跳出內(nèi)層的for循環(huán)，兩個，三個break，也都只能跳出本層循環(huán)，及第二個Break根本不會執(zhí)行；

  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總線的物理結(jié)構(gòu)，總線上的START和IIC_stop信號必定是唯一的。
  241.                 //另外，IIC總線標(biāo)準(zhǔn)規(guī)定IIC_SDA線的數(shù)據(jù)轉(zhuǎn)換必須在IIC_SCL線的低電平期，
  242.                 //在IIC_SCL線的高電平期，IIC_SDA線的上數(shù)據(jù)是穩(wěn)定的。所以，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結(jié)束");
  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應(yīng)答
  269.     void ACK()//IIC_SDA應(yīng)答信號為0表示正常收發(fā)數(shù)據(jù)，為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.     //寫字節(jié)
  285.     void write_byte(unsigned char byte)
  286.     {
  287.     //Serial.println("地址寫入SDL開始");調(diào)試用
  288.     unsigned char i,temp;
  289.     temp=byte;
  290.     for(i=7;i<=7;i--)
  291.     {//unsigned char x;//調(diào)試用
  292.     //x=0;//調(diào)試用
  293.     //const int test = 2;//調(diào)試用
  294.     //pinMode(test,INPUT);//調(diào)試用
  295.     digitalWrite(IIC_SCL,LOW);
  296.     delayMicroseconds(4);
  297.     //Serial.println(temp);//調(diào)試用
  298.     //Serial.println(bitRead(temp,i));//調(diào)試用
  299.     digitalWrite(IIC_SDA,bitRead(temp,i));
  300.    //Serial.println(digitalRead(test));
  301.    // x=(digitalRead(test))|(x<<1);//調(diào)試用
  302.    //Serial.println(x);//調(diào)試用
  303.    //Serial.println(i);//調(diào)試用
  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對應(yīng)內(nèi)部哪個地址
  349.     ACK();
  350.     write_byte(date); //所寫入的數(shù)據(jù)
  351.     //Serial.println("所寫入的數(shù)據(jù):");
  352.     //Serial.println(date);
  353.     ACK();
  354.     IIC_stop();
  355.     //Serial.println("PCA9685_write結(jié)束");
  356.     }

  358.     //從PCA9685讀數(shù)據(jù)有返回值
  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對應(yīng)內(nèi)部哪個地址
  366.     ACK();
  367.     start();
  368.     write_byte(PCA9685_adrr|0x01); //表示此時狀態(tài)為從PCA9685讀取數(shù)據(jù)
  369.     ACK();
  370.     date=read_byte();
  371.    // Serial.println("date");
  372.     //Serial.println(date);
  373.     return date;
  374.     }

  376. //PCA9685復(fù)位
  377.     void reset(void)
  378.     {//Serial.println("RESET開始");
  379.     PCA9685_write(PCA9685_MODE1,0x0);
  380.      //Serial.println("RESET結(jié)束");
  381.     }

  383. //PCA9685修改頻率
  384. /*---------------------------------------------------------------
  385.                                 PCA9685修改角度函數(shù)
  386. num:舵機(jī)PWM輸出引腳0~15,on:PWM上升計數(shù)值0~4096,off:PWM下降計數(shù)值0~4096
  387. 一個PWM周期分成4096份,由0開始+1計數(shù),計到on時跳變?yōu)楦唠娖?繼續(xù)計數(shù)到off時
  388. 跳變?yōu)榈碗娖?直到計滿4096重新開始。所以當(dāng)on不等于0時可作延時,當(dāng)on等于0時,
  389. off/4096的值就是PWM的占空比。
  390. ----------------------------------------------------------------*/



  394.     void setPWMFreq(float freq)
  395.      { //Serial.print("設(shè)置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("周期頻率設(shè)置值：");
  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("當(dāng)前通道：");
  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路舵機(jī)從60度轉(zhuǎn)到0角度   
  438.    }
  439.    else
  440.    {
  441.     stage1_ctrl=true;
  442.     setPWM(0, 0,224);  //第0路舵機(jī)從0度轉(zhuǎn)到60角度   
  443.    }            
  444.                    //setPWM(0, 0, SERVO180);//第0路舵機(jī)轉(zhuǎn)到0角度     
  445.                    //setPWM(1, 0, SERVO000);//第1路舵機(jī)轉(zhuǎn)到0角度
  446. }
  447. */
復(fù)制代碼



作者: admin    時間: 2018-8-25 17:02
原理圖能分享下嗎？
作者: Riton    時間: 2018-8-26 11:55
admin 發(fā)表于 2018-8-25 17:02
原理圖能分享下嗎？

額，沒有畫原理圖，直接接線的。我等下簡單手畫一下吧
作者: Riton    時間: 2018-8-26 12:48
不懂得怎么發(fā)圖片啊。。。那樣成功了沒
作者: stu    時間: 2018-11-8 19:18
Riton 發(fā)表于 2018-8-26 12:48
不懂得怎么發(fā)圖片啊。。。那樣成功了沒

沒有看到圖片



歡迎光臨 (http://www.torrancerestoration.com/bbs/) Powered by Discuz! X3.1