Hey everyone,
I am trying to get my robot to cross a line when all of the sensors ( Pollolu QTR 8 RC) get a maximum reflectance using a if statement. It will then call the function " seeek" to seek objects across the line. I am not sure if I am not using the right numbers for the sensors, or if I can’t call a function inside an if loop. By the number of the sensor, I mean sensor in the code snippet below.
Any help would be appreciated!
The specific part of the code is :
if((sensors[0] >1000) || (sensors[4] >1000))
{
motor1.run(FORWARD);
motor2.run(FORWARD);
delay(2000);
seeek();
}
All of my code:
#include <QTRSensors.h>
// includes the library for the pololu qtr-8rc reflectance sensor
#include <AFMotor.h>
// includes the library for the adafruit motor shield.
#include <arduino.h>
AF_DCMotor motor1(1, MOTOR12_8KHZ );
// Sets up motor 1 using the motor 1 port, sets the motor frequency to 8khz.
AF_DCMotor motor2(2, MOTOR12_8KHZ );
// Sets up motor 2 using the motor 3 port, sets the motor frequency to 8khz.
#define KP 0.2
#define KD 5.05
// Defines the constants for the PID Control
#define M1_REG_SPEED 125
#define M2_REG_SPEED 125
// Defines the default speeds for the robots motors
#define M1_MAX_SPEED 150
#define M2_MAX_SPEED 150
// Defines the max speed for the robots motors
#define MIDDLE_SENSOR 3
#define NUM_SENSORS 5
// Defines the amount of sensors used
#define TIMEOUT 2500
//Sets a time of 2.5 seconds to the QTR sesnors call.
#define EMITTER_PIN 14
// Defines which pin will be used for turning on and off the IR led's
#define ANALOGPIN 0
QTRSensorsRC qtrrc((unsigned char[]) { 19,18,17,16,15} ,NUM_SENSORS, TIMEOUT, EMITTER_PIN);
// Sets up the QTR reflectance sensor using the QTR sensor library, sets the pins as inputs and reads them. Digital pins 14-19 or analog 0-5.
unsigned int sensorValues[NUM_SENSORS];
// Sets the incoming sensor values to be unsigned integers.
void setup()
{
manual_calibration();
set_motors(0,0);
seeek();
}
/* sets up the program, delays for 1 second, and then calls the manual calibration function, which we use to calibrate the robots position on the line. It then sets the motor speed to 0.*/
int lastError = 0;
int last_proportional = 0;
int integral = 0;
/* sets the last error, last proportional, and last integral all equal to 0. This is par tof the PID algorithm.*/
void loop()
// Main program loop
{
unsigned int sensors[5];
// tells
float position = qtrrc.readLine(sensors);
// sets the received position from the QTR sensors as a float value, and stores them.
float error = 2000-position ;
// Sets the error to be 2500-position, and stores it as a float value.
float motorSpeed = KP * error + KD * (error - lastError);
lastError = error;
/* This is the heart of the PID algorithm. This sets the main motor speed equal to the proportional multiplied by the error and adds the derivitive multiplied by the error minus last error. This basically tells the robot how to figure out the
error in the line and where its position is, allowing it to fine tune the motor control for whatever direction it needs to go.*/
float leftMotorSpeed = M1_REG_SPEED + motorSpeed;
// Sets the left motor to be the default speed plus whatever the speed the algorithm determines to adjust it to, and sets it as a float value.
float rightMotorSpeed = M2_REG_SPEED - motorSpeed;
// Sets the right motor to be the default speed minus whatever the speed th algorith has determined to adjust it to, and sets it as a float value.
/* The reason one is adding the value and one is subtracting the value is because the wheels need to spin the same way, not the opposite.*/
set_motors(leftMotorSpeed, rightMotorSpeed);
if((sensors[0] >1000) || (sensors[4] >1000))
{
motor1.run(FORWARD);
motor2.run(FORWARD);
delay(2000);
seeek();
}
// Calls the set_motors function.
}
// End of the main loop.
void manual_calibration() {
// This is the function called in the setup function to calibrate the sensors and find the line position.
int i;
for (i = 0; i < 250; i++)
/* Sets i equal to an integer, and then start at zero and increments up to 250*/
{
qtrrc.calibrate(QTR_EMITTERS_ON);
delay(20);
/* This allows the calibrate setting from the QTR library to do its internal calculations and finish determining where the line is in relevance to the robot before starting the program. Also turns on the IR lights.*/
}
// End of if conditional
}
//End of the manual calibration function.
void set_motors(int motor1speed, int motor2speed)
// This function is the main motor control function, which is called in the main program loop.
{
if (motor1speed > M1_MAX_SPEED ) motor1speed = M1_MAX_SPEED;
// Basically this limits the top speed so we can't get any error values for motor 1.
if (motor2speed > M2_MAX_SPEED ) motor2speed = M2_MAX_SPEED;
// this one limits the top speed for motor 2 so that we can't get any error values.
if (motor1speed < 0) motor1speed = 0;
// This makes it so that we can't have a negative motor speed value and get an error.
if (motor2speed < 0) motor2speed = 0;
// This makes it so that we can't have a negative motor speed value and get an error.
motor1.setSpeed(motor1speed);
// Sets the speed of motor 1 to the value obtained from motor1speed.
motor2.setSpeed(motor2speed);
// Sets the speed of motor 2 to the value obtained from motor2speed.
motor1.run(FORWARD);
// Instructs motor 1 to run forwards
motor2.run(FORWARD);
//instructs motor two to run forwards.
}
// End of void set_motors().
void seeek()
{
float analogPin = A0;
float sensorValue = analogRead(analogPin);
if (sensorValue < 700 && sensorValue > 300)
{
motor1.setSpeed(100);
motor2.setSpeed(100);
motor1.run(FORWARD);
motor2.run(FORWARD);
}
if (sensorValue >700 || sensorValue < 300)
{ motor1.setSpeed(75);
motor2.setSpeed(75);
motor1.run(BACKWARD);
motor2.run(FORWARD);
}
}