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
}