So I’ve been spending forever trying to make the robot solve a maze and this is the code I have so far…
The robot doesn’t seem to accurately turn or makes random decisions which I can’t seem to understand. Is there something wrong with my code?
#include <QTRSensors.h>
#include<AFMotor.h>
AF_DCMotor motor1(1, MOTOR12_64KHZ ); //create motor #1 using M1 output on Motor Drive Shield, set to 1kHz PWM frequency
AF_DCMotor motor2(2, MOTOR12_64KHZ ); //create motor #2 using M2 output on Motor Drive Shield, set to 1kHz PWM frequency
#define Kp 0.2 // experiment to determine this, start by something small that just makes your bot follow the line at a slow speed
#define Kd 3 // experiment to determine this, slowly increase the speeds and adjust this value. ( Note: Kp < Kd)
#define rightMaxSpeed 255 // max speed of the robot
#define leftMaxSpeed 255 // max speed of the robot
#define rightBaseSpeed 150 // this is the speed at which the motors should spin when the robot is perfectly on the line
#define leftBaseSpeed 150 // this is the speed at which the motors should spin when the robot is perfectly on the line
#define NUM_SENSORS 6 // number of sensors used
#define NUM_SAMPLES_PER_SENSOR 4 // average 4 analog samples per sensor reading
#define EMITTER_PIN QTR_NO_EMITTER_PIN // emitter is controlled by digital pin 2
int LastError =0;
// sensors 0 through 5 are connected to analog inputs 0 through 5, respectively
QTRSensorsAnalog qtra((unsigned char[]) {0, 1, 2, 3, 4, 5},
NUM_SENSORS, NUM_SAMPLES_PER_SENSOR, EMITTER_PIN);
unsigned int sensorValues[NUM_SENSORS];
void setup()
{
delay(500);
pinMode(13, OUTPUT);
digitalWrite(13, HIGH); // turn on Arduino's LED to indicate we are in calibration mode
for (int i = 0; i < 400; i++) // make the calibration take about 10 seconds
{
qtra.calibrate(); // reads all sensors 10 times at 2.5 ms per six sensors (i.e. ~25 ms per call)
}
digitalWrite(13, LOW); // turn off Arduino's LED to indicate we are through with calibration
// print the calibration minimum values measured when emitters were on
Serial.begin(9600);
for (int i = 0; i < NUM_SENSORS; i++)
{
Serial.print(qtra.calibratedMinimumOn[i]);
Serial.print(' ');
}
Serial.println();
// print the calibration maximum values measured when emitters were on
for (int i = 0; i < NUM_SENSORS; i++)
{
Serial.print(qtra.calibratedMaximumOn[i]);
Serial.print(' ');
}
Serial.println();
Serial.println();
delay(1000);
}
void loop()
{
// read calibrated sensor values and obtain a measure of the line position from 0 to 5000
// To get raw sensor values, call:
//int x = qtra.readCalibrated(sensorValues);
//qtra.read(sensorValues); //instead of unsigned int position = qtra.readLine(sensorValues);
unsigned int position = qtra.readLine(sensorValues);
// 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[i]);
Serial.print(" ");
Serial.print(i);
Serial.print('\t');
}
Serial.println();
if(sensorValues[5]>700){
turn_left();
Serial.println("TURNING LEFT");
delay(750);
} else if(sensorValues[2]>700 || sensorValues[3]>700){
Serial.println("KEEP GOING STRAIGHT");
} else if(sensorValues[0]>700)
{
turn_right();
Serial.println("TURNING RIGHT");
delay(750);
} else if(sensorValues[0]<100 && sensorValues[1]<100 && sensorValues[2]<100 && sensorValues[3]<100 && sensorValues[4]<100 && sensorValues[5]<100){
turn_around();
Serial.println("TURNING AROUND");
delay(1600);
}
int error = 2500 - position;
int motorSpeed = Kp*error + Kd*(error - LastError);
LastError = 0;
int rightMotorSpeed = rightBaseSpeed+ motorSpeed;
int leftMotorSpeed = leftBaseSpeed - motorSpeed;
if(rightMotorSpeed> rightMaxSpeed) rightMotorSpeed = rightMaxSpeed;
if(leftMotorSpeed > leftMaxSpeed) leftMotorSpeed = leftMaxSpeed;
if(rightMotorSpeed < 0) rightMotorSpeed = 0;
if(leftMotorSpeed< 0) leftMotorSpeed=0;
motor1.setSpeed(leftMotorSpeed);
motor2.setSpeed(rightMotorSpeed);
motor1.run(FORWARD);
motor2.run(FORWARD);
delay(250);
}
void turn_left(){
motor1.setSpeed(250);
motor2.setSpeed(250);
motor1.run(FORWARD);
motor2.run(FORWARD);
delay(150);
motor1.run(BACKWARD);
motor2.run(FORWARD);
}
void turn_around(){
motor1.setSpeed(250);
motor2.setSpeed(250);
motor1.run(BACKWARD);
motor2.run(FORWARD);
}
void turn_right(){
motor1.setSpeed(250);
motor2.setSpeed(250);
motor1.run(FORWARD);
motor2.run(FORWARD);
delay(150);
motor1.run(FORWARD);
motor2.run(BACKWARD);
}