Pololu Robotics & Electronics
Menu
My account Comments or questions? About Pololu Contact Ordering information Distributors

Pololu Forum

QTR-8-RC with Adafruit motorshield V2


#1

I am using a QTR8RC sensor with an Adafruit motorshield V2. My power source is a 12 volt lithium Ion that is run through the motorshield that is connected to an Arduino Mega. My question is on how to properly write a code that will only read and intersection one time (with an outer sensor) as the robot passes over it. In my code I have attempted to do this, however sometimes I get two values (that I assign to a linecounter) as I pass over the intersection. I have tried using different speeds and altered the timeout condition. Also, is it possible to set a specified sensor to execute a command as soon as it reads a black line. Example: If I pass a certain intersection, I want to turn right and stop turning when the middle sensor is over the intersecting line.

This is my code so far.

//Preprocessor Includes---------------------------------------
#include <Adafruit_TCS34725.h>
#include <Servo.h>
#include <QTRSensors.h>
#include <Adafruit_MotorShield.h>
#include <Wire.h>
#include "utility/Adafruit_MS_PWMServoDriver.h"
// Figure out if it would be best to make this code a library
//------------------------------------------------------------

//Definitions-------------------------------------------------
Adafruit_MotorShield AFMS = Adafruit_MotorShield();
Adafruit_DCMotor *Motor_1 = AFMS.getMotor(1);
Adafruit_DCMotor *Motor_2 = AFMS.getMotor(2);
Adafruit_DCMotor *Motor_3 = AFMS.getMotor(3);
Adafruit_DCMotor *Motor_4 = AFMS.getMotor(4);
// Change the values below to suit your robot's motors, weight, wheel type, etc.
#define KP .01  //Part of P.I.D values .009 works  ,.01
#define KD .04                  //  works    ,.3
#define M1_DEFAULT_SPEED 70 //Will have to be adjusted. Drive motors
#define M2_DEFAULT_SPEED 70 //Will have to be adjusted
#define M1_MAX_SPEED 130 //Will have to be adjusted
#define M2_MAX_SPEED 130 //Will have to be adjusted
#define MIDDLE_SENSOR 6 //This does not assign MS to 4 it is for if else while stuff
#define leftSensor 8 ////
#define rightSensor 3 ////
#define NUM_SENSORS  6      // number of sensors used
#define TIMEOUT       2600  // waits for 2500 us for sensor outputs to go low
#define TIMEOUT2       2450  // waits for 2500 us for sensor outputs to go low
#define EMITTER_PIN 2     // emitter is controlled by digital pin 2
#define DEBUG 0 // set to 1 if serial debug output needed
//#define LEFTsensor
//#define RIGHTsensor
//------------------------------------------------------------

//NOTES************
//void read(unsigned int *sensorValues, unsigned char readMode = QTR_EMITTERS_ON)
//Reads the raw sensor values into an array. There MUST be space for as many values as there were sensors specified in the constructor. The values returned are a measure of the reflectance in units that depend on the type of sensor being used, with higher values corresponding to lower reflectance (a black surface or a void). QTR-xA sensors will return a raw value between 0 and 1023. QTR-xRC sensors will return a raw value between 0 and the timeout argument (in units of microseconds) provided in the constructor (which defaults to 2500).
//The functions that read values from the sensors all take an argument readMode, which specifies the kind of read that will be performed. Several options are defined: QTR_EMITTERS_OFF specifies that the reading should be made without turning on the infrared (IR) emitters, in which case the reading represents ambient light levels near the sensor; QTR_EMITTERS_ON specifies that the emitters should be turned on for the reading, which results in a measure of reflectance; and QTR_EMITTERS_ON_AND_OFF specifies that a reading should be made in both the on and off states. The values returned when the QTR_EMITTERS_ON_AND_OFF option is used are given by on + max – off, where on is the reading with the emitters on, off is the reading with the emitters off, and max is the maximum sensor reading. This option can reduce the amount of interference from uneven ambient lighting. Note that emitter control will only work if you specify a valid emitter pin in the constructor.
//Dimmable QTR sensors with separate CTRL ODD and CTRL EVEN pins also support QTR_EMITTERS_ODD_EVEN, where first the odd sensors are read with only the odd emitters on, then the even sensors are read with only the even emitters on, as well as QTR_EMITTERS_ODD_EVEN_AND_OFF, which works like a combination of ODD_EVEN and ON_AND_OFF: the odd sensors are read with the odd emitters on, the even sensors are read with the even emitters off, and all sensors are read with all emitters off. These two modes only work if you specify separate odd and even emitter control pins in the constructor.

//void QTRSensorsRC::init(unsigned char* digitalPins, unsigned char numSensors, unsigned int timeout = 2500, unsigned char emitterPin = QTR_NO_EMITTER_PIN)
//Initializes a QTR-RC (digital) sensor array.
//The array digitalPins should contain the Arduino digital pin numbers for each sensor.
//numSensors specifies the length of the digitalPins array (the number of QTR-RC sensors you are using). numSensors must be no greater than 16.
//timeout specifies the length of time in microseconds beyond which you consider the sensor reading completely black. That is to say, if the pulse length for a pin exceeds timeout, pulse timing will stop and the reading for that pin will be considered full black. It is recommended that you set timeout to be between 1000 and 3000 us, depending on factors like the height of your sensors and ambient lighting. This allows you to shorten the duration of a sensor-reading cycle while maintaining useful measurements of reflectance.
//emitterPin is the Arduino digital pin that controls whether the IR LEDs are on or off. This pin is optional and does not exist on some of the QTR sensor arrays. If a valid pin is specified, the emitters will only be turned on during a reading. If the value QTR_NO_EMITTER_PIN (255) is used, you can leave the emitter pin disconnected and the IR emitters will always be on.

//Global------------------------------------------------------
int State = LOW;
int LastState = LOW;
unsigned int leftsensor;
unsigned int rightsensor;
unsigned int numDesignation;
char side;

//A12, A11, A10, A9, A8
QTRSensorsRC qtrrc((unsigned char[]) {
7, 6, 5, 4, 3
} , NUM_SENSORS, TIMEOUT, EMITTER_PIN); //Wire sensor to these pin#


QTRSensorsRC LEFT((unsigned char[]) { 8
  } , leftsensor, TIMEOUT2, EMITTER_PIN); //Wire sensor to these pin#

//QTRSensorsRC RIGHT((unsigned char[]) { 3
// } , rightsensor, TIMEOUT2, EMITTER_PIN); //Wire sensor to these pin#

unsigned int sensorValues[NUM_SENSORS];
//unsigned int s[LEFTsensor];
int lineCounter = 0;
int count = 0;
unsigned long previousMillis = 0;
const long interval = 1000;
//unsigned int s[5]; //
//unsigned int LEFT = qtrrc.readLine(NUM_SENSORS);
//unsigned int RIGHT = qtrrc.readLine(Rightsensor);
//unsigned int Left = digitalRead(s[5]);


void Designator(unsigned int i)
{
  if(i == 5)
    {
      side = 'L';
      numDesignation = 1;
    }
    else if(i == 4)
    {
      side = 'L';
      numDesignation = 2;
    }
    else if(i == 3)
    {
      side = 'L';
      numDesignation = 3;
    }
    else if(i == 2)
    {
      side = 'R';
      numDesignation = 3;
    }
    else if(i == 1)
    {
      side = 'R';
      numDesignation = 2;
    }
    else if(i == 0)
    {
      side = 'R';
      numDesignation = 1;
    }
}

//------------------------------------------------------------






//************************************************ begin setup *******
void setup()
{
  Serial.begin(9600);
  AFMS.begin();
  manual_calibration(); //Call function to calibrate
  set_motors(0, 0); //Call function to set motors
  //LEFT.digitalRead(s);
  // Serial.println();
  // Motor_1->setSpeed(225); // MAX is 225
  // Motor_2->setSpeed(225); 
  //Motor_3->setSpeed(225);
  //Motor_4->setSpeed(225);
}
//******************************************* end setup **************

int lastError = 0;
int last_proportional = 0;
int integral = 0;

//*************************************************** begin loop ************
void loop()  ///sensor  [6  5  4  3  2  1]
{
  //unsigned int sensor4[5];  //pid based on 4


  //****************************  BEGIN PID  *****************************************
  //unsigned int sensors[5];//
  unsigned int position = qtrrc.readLine(sensorValues); // sensor4[6]
  int error = position - 2000; //2478
  int motorSpeed = KP * error + KD * (error - lastError);//PID stuff for better correction factor
  lastError = error;
  int leftMotorSpeed = M1_DEFAULT_SPEED + motorSpeed;
  int rightMotorSpeed = M2_DEFAULT_SPEED - motorSpeed;
  // set motor speeds using the two motor speed variables above
  set_motors(leftMotorSpeed, rightMotorSpeed);
  //**************************  END PID  **************************************************


  //begin print

  
 // for (int i = NUM_SENSORS-1; i >= 0; i--)
 // {
  //  Designator(i);
  //  Serial.print("[Sensor ");
  //  Serial.print(side);
  //  Serial.print(numDesignation);
  //  Serial.print(": ");
  //  Serial.print(sensorValues[i]);
  //  Serial.print(" ]");
  //  Serial.print("  ");
 // }
  Serial.print(" \n");
  Serial.print("[readLine Output: ");
  Serial.print(qtrrc.readLine(sensorValues,QTR_EMITTERS_ON,0)/1000);
  Serial.print(" ]");
  Serial.print("[lineCounter: ");
  Serial.print(lineCounter);
  Serial.print(" ]");
 Serial.print(" \n");

  // Serial.print(position);
  // Serial.print("         ");
  // Serial.print(s[5]);
  // Serial.print("         ");


  //get sensor values*****************************
  // qtrrc.read(s);
  //LEFT.read(s);


  unsigned long currentMillis = millis();

  if (currentMillis - previousMillis >= 10)
  {
    previousMillis = currentMillis;

    if (sensorValues[4] > 750) //s[0] is far left sensor
    {
      lineCounter++;

      if (lineCounter == 2 )
      {
        Motor_1->setSpeed(225); 
        Motor_2->setSpeed(225); 
        Motor_1->run(FORWARD);
        Motor_2->run(BACKWARD);
        delay (165);
      }
      if (lineCounter == 4 )
      {
        Motor_1->setSpeed(225); 
        Motor_2->setSpeed(225);
        Motor_1->run(BACKWARD);
        Motor_2->run(FORWARD);
        delay (150);
      }
      if (lineCounter == 9)
      {
        Motor_1->setSpeed(225); 
        Motor_2->setSpeed(225);
        Motor_1->run(BACKWARD);
        Motor_2->run(FORWARD);
        delay (150);
      }
      if (lineCounter == 11)
      {
        Motor_1->setSpeed(225); 
        Motor_2->setSpeed(225);
        Motor_1->run(BACKWARD);
        Motor_2->run(FORWARD);
        delay (150);
      }
      if (lineCounter == 16)
      {
        Motor_1->setSpeed(225); 
        Motor_2->setSpeed(225);
        Motor_1->run(BACKWARD);
        Motor_2->run(FORWARD);
        delay (150);
      }
      if (lineCounter == 17)
      {
        Motor_1->setSpeed(225); 
        Motor_2->setSpeed(225);
        Motor_1->run(FORWARD);
        Motor_2->run(BACKWARD);
        delay (150);
      }
     // END Update Interval
     }}
 //if (lineCounter >= 3 && sensorValues[5] > 750 && lineCounter == 4) // if at 2nd intersection and not reading a line
 // {
   // Motor_1->run(BACKWARD); // turn right
  //  Motor_2->run(FORWARD);
    //delay (500);
   // Motor_1->run(FORWARD); // turn right
   // Motor_2->run(BACKWARD); 
  //}
  

  /*LEFT.read(s);
    unsigned int s[8];
    ///RUN COURSE*******************************************
    if (s[8] == 2500) //s[5] is far left sensor
    {
    lineCounter++;
    if (lineCounter>=2 && lineCounter<=3)
    //{
    //Motor_1->run(RELEASE);
    //Motor_2->run(RELEASE);
    //delay (1000);
    /
    while (lineCounter>=3 && lineCounter< 4)
    {
    Motor_1->run(FORWARD);
    Motor_2->run(BACKWARD);
    // delay (1000);
    }
    if (lineCounter>=4 && lineCounter<=5)
    {
    Motor_1->run(FORWARD);
    Motor_2->run(FORWARD);
    // delay (1000);
    }
  */


  //first turn ****************************************
  /* while (lineCounter>=2 && lineCounter<=3)
    {
    Motor_1->run(FORWARD);
    Motor_2->run(BACKWARD);
    LEFT.read(s);
    if (s[5]==2500)
      {
       lineCounter++;
      }
    }*/

  //Serial.print(" \n");
  //Serial.print(lineCounter);
  //Serial.print(" \n");
}
//********************************************** end loop **********************



void set_motors(int motor1speed, int motor2speed)
{
  if (motor1speed > M1_MAX_SPEED ) motor1speed = M1_MAX_SPEED; // limit top speed
  if (motor2speed > M2_MAX_SPEED ) motor2speed = M2_MAX_SPEED; // limit top speed
  if (motor1speed < 0) motor1speed = 0; // keep motor above 0
  if (motor2speed < 0) motor2speed = 0; // keep motor speed above 0
  Motor_1->setSpeed(motor1speed);     // set motor speed
  Motor_2->setSpeed(motor2speed);     // set motor speed
  Motor_1->run(FORWARD);
  Motor_2->run(FORWARD);
}

void manual_calibration()
{
  pinMode(13, OUTPUT);
  digitalWrite(13, HIGH);  // turn on Arduino's LED to indicate we are in calibration mode

  for (int i = 0; i < 250; i++)  // the calibration will take a few seconds //was 250
  { // instead of 400
    qtrrc.calibrate(QTR_EMITTERS_ON); //QTR_EMITTERS_ON
    //    delay(20);
  }

  if (DEBUG) // if true, generate sensor dats via serial output
  {
    Serial.begin(74880); //Try this to get better response
    for (int i = 0; i < NUM_SENSORS; i++)
    {
      Serial.print(qtrrc.calibratedMinimumOn[i]);
      Serial.print(' ');
    }
    Serial.println();

    for (int i = 0; i < NUM_SENSORS; i++)
    {
      Serial.print(qtrrc.calibratedMaximumOn[i]);
      Serial.print(' ');
    }
    Serial.println();
  }
  digitalWrite(13, LOW);     // turn off Arduino's LED to indicate we are through with calibration
}

#2

Hello.

It is not entirely clear to me what problem you are having, but from a brief look through your code, it looks like you are using a counter to keep track of which turn you are on, and it is sometimes incrementing by more than 1 on a single turn. If that’s the case, you could probably use some kind of state variable that will only allow the counter to increment after some event (e.g. after some amount of time has elapsed or it finds the line again).

It should be possible to break from your normal routine when a specific sensor is reading black like you described. You could achieve this by entering a while loop once the corresponding reading in the sensorValues[] array is higher than a some threshold (which it looks like you are doing already to sense the turns). For example, you once you detect a right turn, you could enter a while loop that just turns and reads the sensors until the middle sensor is black, then returns to your normal code. Note that when you do this, you might need to only take into account the readings after some short amount of time, to give the robot enough time to move the right sensor off of the line and not break out of the while loop immediately.

Brandon


#3

Thank you Brandon, I will try and implement your suggestions. Would something like the following code work for breaking out of the while loop?

 if (currentMillis - previousMillis >= 10)
  {
    previousMillis = currentMillis;

  if (sensorValues[5] == 1000  ) //s[0] is far left sensor
    {
 Serial.print("lineCounter: ");
     Serial.print(lineCounter);
    Serial.print("  ");
    Serial.print(" \n");
      lineCounter++;

      while (lineCounter == 2 && sensorValues[3] == 1000 )
      {
        delay(500);
        Motor_1->setSpeed(225); 
        Motor_2->setSpeed(225); 
        Motor_1->run(FORWARD);
        Motor_2->run(BACKWARD);
        return 1;
      }
   while (1)
   {

#4

Your original program has a lot going on in it, so it is not easy to say for sure if your purposed adjustments will work out of context from the rest of your program. You should probably just try it out. The timing of it will probably depend heavily on your robot and line course, so you will probably have to do some trial and error to get it working as you want.

By the way, you will need to make sure you are updating your sensorValues[] array each time you want to check for the line, so you might need to run the readLine() command inside of your while loop. Also, you probably don’t want to be using a blocking delay inside of the while loop (the way it is now, it looks like it will only check to exit the while loop once every half second).

Brandon