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