Help with a QTR 8A maze solving robot!

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);
}

Hello.

Can you post a video showing your robot’s behavior? If not, can you post a picture of the maze you are trying to solve and elaborate more on what you expect your robot to do and what it actually does (e.g. the robot should turn 90 degrees, but instead turns 60 degrees which causes it to lose the line)?

- Amanda