YFROBOT创客社区

标题: 利用QTR-5RC红外传感器制作的arduino寻迹小车,寻迹采用PID调节 [打印本页]

作者: YFRobot    时间: 2014-4-3 20:49
标题: 利用QTR-5RC红外传感器制作的arduino寻迹小车,寻迹采用PID调节
QTR-5RC拥有5路红外探头,与传统的红外寻迹传感器不同,使用之前需要先给对应的某一路一个高电平脉冲,然后迅速将引脚设置为输入模式,启动单片机内部的定时器,计算出高电平的时间。接下来将利用该传感器来做一个智能化的寻迹机器人。
技术参数:[attach]864[/attach]
   使用之前请先将传感器固定到小车的正前下方位置,离地位置在3-5MM为宜,使用时应尽量避免在强光环境下使用,可能会产生干扰信号,探头对不同颜色的物体反射强度不同,对白色反应最为灵敏,对黑色无反射,所以非常适合用来做寻迹小车。接线时注意电源正负极,GND接电源负极,VCC接电源正极,IN1-IN5分别为从左到右5路传感器的输出端,接到单片机上用来检测寻迹信号。EN端口用来控制5路传感器的通断,高电平时传感器板工作,低电平则不工作,默认通过上拉电阻接到高电平,用户也可以用单片机的IO口来控制它。
   QTR-5RC库文件:[attach]863[/attach]
   请先下载上面附件,然后将其解压到arduino 安装目录下的“Libraries”文件夹内,才能正常使用,该库出自pololu公司!下面是几个常用的函数:
*   QTRSensorsRC qtrrc((unsigned char[]) {14, 15, 16, 17, 18},  NUM_SENSORS, TIMEOUT, EMITTER_PIN);
该函数用来进行设置连接IO口、所用传感数量、传感器检测溢出时间、EN引脚接口,例如这里14-18即对应arduino的A0-A4引脚,也可以使用数字引脚2-13来检测。
*    qtrrc.calibrate();
传感器初使值校验,在使用之前,需要先对传感器进行校验。校验时必须确保传感器能够完整的从最左侧移动到最右侧,如果校验环节出错,将影响下面的使用。可以可以参考下面给出的代码,程序通过马达的正反转完成了传感器的校验。
*    qtrrc.readLine(sensorValues);
读传感器的值,这个不需要多做解释的了,需要注意的是,这里的sensorValues是数组,不是变量,该数组用来存储检测的值。
寻迹代码如下:
[C++] 纯文本查看 复制代码
#include <QTRSensors.h>

// This example is designed for use with eight QTR-1RC sensors or the eight sensors of a
// QTR-8RC module.  These reflectance sensors should be connected to digital inputs 3 to 10.
// The QTR-8RC's emitter control pin (LEDON) can optionally be connected to digital pin 2,
// or you can leave it disconnected and change the EMITTER_PIN #define below from 2 to
// QTR_NO_EMITTER_PIN.

// The setup phase of this example calibrates the sensor for ten seconds and turns on
// the LED built in to the Arduino on pin 13 while calibration is going on.
// During this phase, you should expose each reflectance sensor to the lightest and
// darkest readings they will encounter.
// For example, if you are making a line follower, you should slide the sensors across the
// line during the calibration phase so that each sensor can get a reading of how dark the
// line is and how light the ground is.  Improper calibration will result in poor readings.
// If you want to skip the calibration phase, you can get the raw sensor readings
// (pulse times from 0 to 2500 us) by calling qtrrc.read(sensorValues) instead of
// qtrrc.readLine(sensorValues).

// The main loop of the example reads the calibrated sensor values and uses them to
// estimate the position of a line.  You can test this by taping a piece of 3/4" black
// electrical tape to a piece of white paper and sliding the sensor across it.  It
// prints the sensor values to the serial monitor as numbers from 0 (maximum reflectance)
// to 1000 (minimum reflectance) followed by the estimated location of the line as a number
// from 0 to 5000.  1000 means the line is directly under sensor 1, 2000 means directly
// under sensor 2, etc.  0 means the line is directly under sensor 0 or was last seen by
// sensor 0 before being lost.  5000 means the line is directly under sensor 5 or was
// last seen by sensor 5 before being lost.

#define NUM_SENSORS   5     // number of sensors used
#define TIMEOUT       2500  // waits for 2500 microseconds for sensor outputs to go low
#define EMITTER_PIN   2     // emitter is controlled by digital pin 2

// sensors 1 through 5 are connected to analog pins a0 through a
QTRSensorsRC qtrrc((unsigned char[]) {14, 15, 16, 17, 18},  
  NUM_SENSORS, TIMEOUT, EMITTER_PIN);
unsigned int sensorValues[NUM_SENSORS];//store the value of QTRSensor
int AIN1 = 6;  //DIRA1
int AIN2 = 5;  //DIRA2
int BIN1 = 10;  //DIRB1
int BIN2 = 9;  //DIRB2
int melody[]={330,330,330,262,392,200,280};
int noteDurations[]={8,4,4,8,4,4,2};
void setup(){
  delay(500);
  pinMode(AIN1,OUTPUT);
  pinMode(AIN2,OUTPUT);
  pinMode(BIN1,OUTPUT);
  pinMode(BIN2,OUTPUT);
  pinMode(13, OUTPUT);
  digitalWrite(13, HIGH);    // turn on Arduino's LED to indicate we are in calibration mode
  for (int i = 0; i < 80; i++)  // make the calibration take about 10 seconds
  {
    qtrrc.calibrate();       // reads all sensors 10 times at 2500 us per read (i.e. ~25 ms per call)
    if(i<20||i>=60)
       setMotor(80,-80);
    else
       setMotor(-80,80);//            */
  }
  digitalWrite(13, LOW);     // turn off Arduino's LED to indicate we are through with calibration
  setMotor(0,0);
  delay(500);
  for(int thisNote = 0;thisNote < 7;thisNote ++){
    tone(12,melody[thisNote],1000/noteDurations[thisNote]);
    delay(1300/noteDurations[thisNote]);
    noTone(12);
  }
// delay(2000);
  // print the calibration minimum values measured when emitters were on
  Serial.begin(9600);
}
void setMotor(int MOTORA,int MOTORB)
{
  if(MOTORA>=0)
  {
    digitalWrite(AIN2,HIGH);
    analogWrite(AIN1,255-MOTORA);
  }
  else
  {
   digitalWrite(AIN1,HIGH);
    analogWrite(AIN2,MOTORA+255);
  }
  if(MOTORB>0)
  {
    digitalWrite(BIN2,HIGH);
    analogWrite(BIN1,255-MOTORB);
  }
  else
  {
    digitalWrite(BIN1,HIGH);
    analogWrite(BIN2,255+MOTORB);
  }
}
void loop()
{
  // read calibrated sensor values and obtain a measure of the line position from 0 to 4000
  // To get raw sensor values, call:
  //  qtrrc.read(sensorValues); instead of unsigned int position = qtrrc.readLine(sensorValues);
  unsigned int last_proportional=0;
  long integral=0;
  unsigned int position =qtrrc.readLine(sensorValues);
  int proportional = ((int)position)-2000;
  int derivative = proportional-last_proportional;
  integral+=proportional;
  //setMotor(50,50);
  last_proportional = proportional;
  //int power_difference = proportional/12 ;// only P adjustment  
  int power_difference = proportional/15+integral/3000 + derivative*3/400;
  const int maxcount = 110;
  if(power_difference > maxcount)
        power_difference = maxcount;
  if(power_difference < -maxcount)
        power_difference = -maxcount;

  if(power_difference < 0)
       setMotor(maxcount+power_difference,maxcount);
  else
       setMotor(maxcount, maxcount-power_difference);
  // print the sensor values as numbers from 0 to 1000, where 0 means maximum reflectance and
  // 1000 means minimum reflectance, followed by the line position*/
/*  for (unsigned char i = 0; i < NUM_SENSORS; i++)
  {
    Serial.print(sensorValues);
    Serial.print('\t');
  }
  //Serial.println(); // uncomment this line if you are using raw values
  Serial.println(position); // comment this line out if you are using raw values*/
//  delay(250);
}

上面代码包含了电机驱动部分的代码,因为使用了DRV8833电机驱动的慢速衰减模式(带刹车功能),所以电机驱动部分的写法有些不同。如果你使用L298N,那么可以改成下面这种写法即可:

[C++] 纯文本查看 复制代码
void setMotor(int MOTORA,int MOTORB)
{
  if(MOTORA>=0)
  {
    digitalWrite(AIN2,LOW);
    analogWrite(AIN1,MOTORA);
  }
  else
  {
   digitalWrite(AIN1,HIGH);
    analogWrite(AIN2,-MOTORA);
  }
  if(MOTORB>0)
  {
    digitalWrite(BIN2,LOW);
    analogWrite(BIN1,MOTORB);
  }
  else
  {
    digitalWrite(BIN1,LOW);
    analogWrite(BIN2,-MOTORB);
  }
}



作者: tiantianyouyou    时间: 2019-5-15 08:17
寻迹采用PID调节,这个好




欢迎光临 YFROBOT创客社区 (http://yfrobot.com.cn/) Powered by Discuz! X3.1