Código seguidor de linha

[henriquecm7]

Boas pessoal.
Estou com um probleminha, tenho um robô que tem que seguir linha, porém ele está com um problema. Ele ameaça seguir linha, mas segue reto antes de completar a curva. Vou deixar abaixo o código para vocês me ajudarem.

Os sensores de linha são 2, um de cada lado da linha preta.
No branco, o valor da em média 124, e no preto da em média 650.

Code Select Expand

#include <QTRSensors.h>
 
#define NUM_SENSORS   4     
#define TIMEOUT       2500 
#define EMITTER_PIN   2   

QTRSensorsRC qtrrc((unsigned char[]) {49, 51,47,53}, NUM_SENSORS, TIMEOUT, EMITTER_PIN);

unsigned int sensorValues[NUM_SENSORS];
int calibrateValue;

// PID STUFF
int mSpeed0 = 45;
int mSpeed1 = 47;
int m0Speed;
int m1Speed;

// por segurança, variável separada da velocidade que atualmente está nos motores
int m1spd = 0,m2spd = 0;

// variavel que controla a direção pra onde o robô vai (frente-trás)
int m1dir = 1,m2dir = 1;

// pinos dos motores
const int m1fpin = 42,m2fpin = 40,m1bpin =43 ,m2bpin = 41,m1pwm = 4,m2pwm = 3;
float Kp = 0.05;
float Kd = 5;

int error;
int last_error;

void setup()
{
  Serial.begin(9600);
  Calibrate();
  setMotors();
  setVelocity(mSpeed0,mSpeed1);
}


void loop()
{
  int posicao = (int)(Position() + 0.5);
 
  int prop = Kp * posicao;
  int turn = prop + integral;
 
  m1spd = mSpeed1 + turn;
  m2spd = mSpeed2 - turn;
 
  Run();
}

float Position()
{
  qtrrc.read(sensorValues);
  int position_value;
 
  int percentual = ((650 - calibrateValue) / 100);
 
  if(sensorValues[0] < calibrateValue)
    sensorValues[0] = calibrateValue;
   
  if(sensorValues[1] < calibrateValue)
  sensorValues[1] = calibrateValue;
 
  int porcentagem_left = (sensorValues[0] - calibrateValue) / percentual;
  int porcentagem_right = (sensorValues[1] - calibrateValue) / percentual;

  int position_left = -1000 - 10 * porcentagem_left;
  int position_right = 1000 + 10 * porcentagem_right;

  position_value = position_left + position_right;
 
  if (position_value > 1000)
    position_value = 1000;
   
  if (position_value < -1000)
    position_value = -1000;
 
  return position_value;
}

void Calibrate()
{
  qtrrc.read(sensorValues);
 
  float length;
  float value;

  delay(1000);
   
  pinMode(13, OUTPUT);
  digitalWrite(13, HIGH);
   
  while(millis() < 4000)
  {
    for(int i = 0; i < 2; i++ )
    {
      Serial.println(sensorValues[i]);
      value = value + sensorValues[i];
      length++;
    }
  } 
  digitalWrite(13, LOW);
 
  calibrateValue = value / length;
}

void setMotors() {
  pinMode(m1fpin,OUTPUT);
  pinMode(m2fpin,OUTPUT);
  pinMode(m1bpin,OUTPUT);
  pinMode(m2bpin,OUTPUT);
  digitalWrite(m1fpin,HIGH);
  digitalWrite(m2fpin,HIGH);
  digitalWrite(m1bpin,LOW);
  digitalWrite(m2bpin,LOW);
}

void setVelocity(int v1, int v2)
{
  m1spd = v1;
  m2spd = v2;
}

void Run () {
  analogWrite(m1pwm,map(m1spd, 0, 140, 0, 255));
  analogWrite(m2pwm,map(m2spd, 0, 140, 0, 255));
  Serial.println(m1spd);
}


[henriquecm7]

Ajudem-me, o que pode estar causando o problema?