line follower

#include <QTRSensors.h>
#define NUM_SENSORS             8  // number of sensors used
#define NUM_SAMPLES_PER_SENSOR  4  // average 4 analog samples per sensor reading
#define EMITTER_PIN             2  // emitter is controlled by digital pin 2

QTRSensorsAnalog qtra((unsigned char[]) {
  0, 1, 2, 3, 4, 5, 6, 7
}, NUM_SENSORS, NUM_SAMPLES_PER_SENSOR, EMITTER_PIN);
unsigned int sensorValues[NUM_SENSORS];

int MOTOR2_PIN1 = 3;
int MOTOR2_PIN2 = 5;
int MOTOR1_PIN1 = 6;
int MOTOR1_PIN2 = 9;



void setup()
{
  Serial.begin(9600);

  delay(500);
  pinMode(13, OUTPUT);
  digitalWrite(13, HIGH);

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

  for (int i = 0; i < NUM_SENSORS; i++)
  {
    Serial.print(qtra.calibratedMinimumOn[i]);
    Serial.print(' ');
  }
  Serial.println();

  for (int i = 0; i < NUM_SENSORS; i++)
  {
    Serial.print(qtra.calibratedMaximumOn[i]);
    Serial.print(' ');
  }

  Serial.println();
  Serial.println();
  delay(1000);

  pinMode(MOTOR1_PIN1, OUTPUT);
  pinMode(MOTOR1_PIN2, OUTPUT);
  pinMode(MOTOR2_PIN1, OUTPUT);
  pinMode(MOTOR2_PIN2, OUTPUT);

}

//float kp;

int Kp = 5;
int Kd = 5;
int pozitie_referinta;
int eroare_pozitie;
int eroare_pozitie_anterioara;
int derivativ;
int corectie_viteza;    //float corectie_viteza;
int vitezaM1;
int vitezaM2;
int viteza_croaziera = 40;


void loop()
{

  pozitie_referinta = 35;

  unsigned int position = qtra.readLine(sensorValues) / 100;

  eroare_pozitie = pozitie_referinta - position;

  derivativ = eroare_pozitie - eroare_pozitie_anterioara;

  eroare_pozitie_anterioara = eroare_pozitie;

  corectie_viteza = Kp * eroare_pozitie + Kd * derivativ;

  vitezaM1 = viteza_croaziera - corectie_viteza;
  vitezaM2 = viteza_croaziera + corectie_viteza;

  //go(vitezaM1, vitezaM2);

  Serial.print(vitezaM1);
  Serial.print(" ");
  Serial.println(vitezaM2);

  //Serial.println(eroare_pozitie);

  delay(250);
}


void go(int speedLeft, int speedRight)
{
  if (speedLeft > 0)
  {
    analogWrite(MOTOR1_PIN1, speedLeft);
    analogWrite(MOTOR1_PIN2, 0);
  }
  else
  {
    analogWrite(MOTOR1_PIN1, 0);
    analogWrite(MOTOR1_PIN2, -speedLeft);
  }

  if (speedRight > 0)
  {
    analogWrite(MOTOR2_PIN1, speedRight);
    analogWrite(MOTOR2_PIN2, 0);
  }
  else
  {
    analogWrite(MOTOR2_PIN1, 0);
    analogWrite(MOTOR2_PIN2, -speedRight);
  }
}