We need to build an Line-Follower / Obstacle-avoider to race in a circuit.
I've been having problems putting the two things together. When I test the Ultrasonic sensor, I WORKS. When I test the QTR sensor, IT WORKS. When I try to put both of them in the code, the qtr stops working.
I don't think that is an mechanical error, because it works perfectly when I put my qtr code on a circuit without obstacles.
Therefore, it's a code problem, and I don't get why it doesn't work.
This is the code:
#include <QTRSensors.h>
#include <Ultrasonic.h>
#define KP 0.50
#define KD 0
#define M1_MAX_SPEED 200
#define M2_MAX_SPEED 200
#define M1_DEFAULT_SPEED 140
#define M2_DEFAULT_SPEED 140
#define MIDDLE_SENSOR A2, A3
#define NUM_SENSORS 6 // number of sensors used
#define TIMEOUT 2500 // waits for 2500 us for sensor outputs to go low
#define EMITTER_PIN 2 // emitter is controlled by digital pin 2
#define DEBUG 1 // set to 1 if serial debug output needed
#define rightMotor1 7
#define rightMotor2 8
#define rightMotorPWM 6
#define leftMotor1 10
#define leftMotor2 9
#define leftMotorPWM 5
#define motorPower 7
#define pino_trigger 13
#define pino_echo 12
QTRSensorsRC qtrrc((unsigned char[]) { A0, A1, A2, A3, A4, A5} ,NUM_SENSORS, TIMEOUT, EMITTER_PIN);
unsigned int sensorValues[NUM_SENSORS];
int position, error, motorSpeed, rightMotorSpeed, leftMotorSpeed;
unsigned int sensors[6];
void manual_calibration() {
int i;
for (i = 0; i < 250; i++) // the calibration will take a few seconds
{
qtrrc.calibrate(QTR_EMITTERS_ON);
delay(50);
}
if (DEBUG) { // if true, generate sensor dats via serial output
Serial.begin(9600);
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();
Serial.println();
}
}
void setup()
{
pinMode(rightMotor1, OUTPUT);
pinMode(rightMotor2, OUTPUT);
pinMode(rightMotorPWM, OUTPUT);
pinMode(leftMotor1, OUTPUT);
pinMode(leftMotor2, OUTPUT);
pinMode(leftMotorPWM, OUTPUT);
pinMode(motorPower, OUTPUT);
manual_calibration();
}
int lastError = 0;
int last_proportional = 0;
int integral = 0;
long duration, cm;
void ler_distancia()
{
// Clears the trigPin condition
digitalWrite(pino_trigger, LOW);
delayMicroseconds(2);
// Sets the trigPin HIGH (ACTIVE) for 10 microseconds
digitalWrite(pino_trigger, HIGH);
delayMicroseconds(10);
digitalWrite(pino_echo, LOW);
// Reads the echoPin, returns the sound wave travel time in microseconds
duration = pulseIn(pino_echo, HIGH);
// Calculating the distance
cm = duration * 0.034 / 2; // Speed of sound wave divided by 2 (go and back)
}
void loop()
{
ler_distancia();
if (cm < 8) {
digitalWrite(motorPower, HIGH); //Stop
digitalWrite(rightMotor1, LOW);
digitalWrite(rightMotor2, LOW);
analogWrite(rightMotorPWM, rightMotorSpeed);
digitalWrite(leftMotor1, LOW);
digitalWrite(leftMotor2, LOW);
analogWrite(leftMotorPWM, leftMotorSpeed);
delay(500);
digitalWrite(motorPower, HIGH); //Turn Right
digitalWrite(rightMotor1, LOW);
digitalWrite(rightMotor2, LOW);
analogWrite(rightMotorPWM, rightMotorSpeed);
digitalWrite(leftMotor1, HIGH);
digitalWrite(leftMotor2, LOW);
analogWrite(leftMotorPWM, leftMotorSpeed);
delay(500);
digitalWrite(motorPower, HIGH); //Stop
digitalWrite(rightMotor1, LOW);
digitalWrite(rightMotor2, LOW);
analogWrite(rightMotorPWM, rightMotorSpeed);
digitalWrite(leftMotor1, LOW);
digitalWrite(leftMotor2, LOW);
analogWrite(leftMotorPWM, leftMotorSpeed);
delay(500);
digitalWrite(motorPower, HIGH); //Move forward
digitalWrite(rightMotor1, HIGH);
digitalWrite(rightMotor2, LOW);
analogWrite(rightMotorPWM, rightMotorSpeed);
digitalWrite(leftMotor1, HIGH);
digitalWrite(leftMotor2, LOW);
analogWrite(leftMotorPWM, leftMotorSpeed);
delay(2000);
digitalWrite(motorPower, HIGH); //Stop
digitalWrite(rightMotor1, LOW);
digitalWrite(rightMotor2, LOW);
analogWrite(rightMotorPWM, rightMotorSpeed);
digitalWrite(leftMotor1, LOW);
digitalWrite(leftMotor2, LOW);
analogWrite(leftMotorPWM, leftMotorSpeed);
delay(500);
digitalWrite(motorPower, HIGH); //Turn Left
digitalWrite(rightMotor1, LOW);
digitalWrite(rightMotor2, LOW);
analogWrite(rightMotorPWM, rightMotorSpeed);
digitalWrite(leftMotor1, HIGH);
digitalWrite(leftMotor2, LOW);
analogWrite(leftMotorPWM, leftMotorSpeed);
delay(500);
digitalWrite(motorPower, HIGH); //Stop
digitalWrite(rightMotor1, LOW);
digitalWrite(rightMotor2, LOW);
analogWrite(rightMotorPWM, rightMotorSpeed);
digitalWrite(leftMotor1, LOW);
digitalWrite(leftMotor2, LOW);
analogWrite(leftMotorPWM, leftMotorSpeed);
delay(500);
digitalWrite(motorPower, HIGH); //Move forward
digitalWrite(rightMotor1, HIGH);
digitalWrite(rightMotor2, LOW);
analogWrite(rightMotorPWM, rightMotorSpeed);
digitalWrite(leftMotor1, HIGH);
digitalWrite(leftMotor2, LOW);
analogWrite(leftMotorPWM, leftMotorSpeed);
delay(2000);
digitalWrite(motorPower, HIGH); //Stop
digitalWrite(rightMotor1, LOW);
digitalWrite(rightMotor2, LOW);
analogWrite(rightMotorPWM, rightMotorSpeed);
digitalWrite(leftMotor1, LOW);
digitalWrite(leftMotor2, LOW);
analogWrite(leftMotorPWM, leftMotorSpeed);
delay(500);
digitalWrite(motorPower, HIGH); //Turn Left
digitalWrite(rightMotor1, HIGH);
digitalWrite(rightMotor2, LOW);
analogWrite(rightMotorPWM, rightMotorSpeed);
digitalWrite(leftMotor1, LOW);
digitalWrite(leftMotor2, LOW);
analogWrite(leftMotorPWM, leftMotorSpeed);
delay(500);
digitalWrite(motorPower, HIGH); //Stop
digitalWrite(rightMotor1, LOW);
digitalWrite(rightMotor2, LOW);
analogWrite(rightMotorPWM, rightMotorSpeed);
digitalWrite(leftMotor1, LOW);
digitalWrite(leftMotor2, LOW);
analogWrite(leftMotorPWM, leftMotorSpeed);
delay(500);
digitalWrite(motorPower, HIGH); //Move forward
digitalWrite(rightMotor1, HIGH);
digitalWrite(rightMotor2, LOW);
analogWrite(rightMotorPWM, rightMotorSpeed);
digitalWrite(leftMotor1, HIGH);
digitalWrite(leftMotor2, LOW);
analogWrite(leftMotorPWM, leftMotorSpeed);
delay(2000);
digitalWrite(motorPower, HIGH); //Stop
digitalWrite(rightMotor1, LOW);
digitalWrite(rightMotor2, LOW);
analogWrite(rightMotorPWM, rightMotorSpeed);
digitalWrite(leftMotor1, LOW);
digitalWrite(leftMotor2, LOW);
analogWrite(leftMotorPWM, leftMotorSpeed);
delay(500);
digitalWrite(motorPower, HIGH); //Turn Right
digitalWrite(rightMotor1, LOW);
digitalWrite(rightMotor2, LOW);
analogWrite(rightMotorPWM, rightMotorSpeed);
digitalWrite(leftMotor1, HIGH);
digitalWrite(leftMotor2, LOW);
analogWrite(leftMotorPWM, leftMotorSpeed);
delay(500);
}
else if(cm > 8); {
position = qtrrc.readLine(sensors);
error = position - 2500;
motorSpeed = KP * error + KD * (error - lastError);
lastError = error;
rightMotorSpeed = M2_DEFAULT_SPEED + motorSpeed;
leftMotorSpeed = M1_DEFAULT_SPEED - motorSpeed;
if (rightMotorSpeed > M1_MAX_SPEED ) rightMotorSpeed = M1_MAX_SPEED; // limit top speed
if (leftMotorSpeed > M2_MAX_SPEED ) leftMotorSpeed = M2_MAX_SPEED; // limit top speed
if (rightMotorSpeed < 0) rightMotorSpeed = 0; // keep motor above 0
if (leftMotorSpeed < 0) leftMotorSpeed = 0; // keep motor speed above 0
{
digitalWrite(motorPower, HIGH);
digitalWrite(rightMotor1, HIGH);
digitalWrite(rightMotor2, LOW);
analogWrite(rightMotorPWM, rightMotorSpeed);
digitalWrite(motorPower, HIGH);
digitalWrite(leftMotor1, HIGH);
digitalWrite(leftMotor2, LOW);
analogWrite(leftMotorPWM, leftMotorSpeed);
}
}
}