robot care nu merge

#include <QTRSensors.h>

//TODO: Inlocuiti x cu pinul la care este conectat pe Arduino
const unsigned int MOTOR1A = 3;
const unsigned int MOTOR1B = 5;
const unsigned int MOTOR2A = 9;
const unsigned int MOTOR2B = 6;

const unsigned int LINIE8 = A3;
const unsigned int LINIE7 = A2;
const unsigned int LINIE6 = A1;
const unsigned int LINIE5 = A0;
const unsigned int LINIE4 = 13;
const unsigned int LINIE3 = 12;
const unsigned int LINIE2 = 11;
const unsigned int LINIE1 = 10;
//ENDTODO

//PID
const float KP = 0.5;
const float KD = 2.0;
const float KI = 0.0;

double last_p = 0;
double integral = 0;
//ENDPID

const int SPEED = 30;
int motorTested = 1;

QTRSensorsRC qtr((unsigned char[]) {LINIE1, LINIE2, LINIE3, LINIE4, LINIE5, LINIE6, LINIE7, LINIE8}, 8);
unsigned int senzori[8];

void setMotors(int a, int b)
{
	/*
	 * TODO: implementati controlul motoarelor
	 * a = valoarea pentru motorul stang
	 * b = valoarea pentru motorul drept
	 * Daca primeste valoare negativa, motorul se invarte inapoi.
	 * Pentru a roti motorul intr-o directie, setati unul din pini pe LOW si pe celalalt scrieti
	 * valoarea PWM. Aveti grija la semn, analogWrite primeste o valoare pozitiva (0 - 255).
	 */

  if(a <0)
  {
   analogWrite(MOTOR1A, -a);
   analogWrite(MOTOR1B, 0);
  }
  if(a >0)
  {
    analogWrite(MOTOR1A, 0);
    analogWrite(MOTOR1B, a);

  }
  if(b <0)
  {
    analogWrite(MOTOR2A, -b);
    analogWrite(MOTOR2B, 0);
  }
  if(b >0)
  {
    analogWrite(MOTOR2A, 0);
    analogWrite(MOTOR2B, b);
  }
//  if(a == 0)
//  {
//     analogWrite(MOTOR1A, 0);
//     analogWrite(MOTOR1B, 0);
//  }
//  if(b == 0)
//  {
//     analogWrite(MOTOR2A, 0);
//     analogWrite(MOTOR2B, 0);
//  }

}

void printLine()
{
	/*
	 * TODO: Cititi senzorii si trimiteti valorile citite prin interfata seriala.
	 * Pentru citire folositi qtr.read(senzori). Functia va citi senzorii care au fost initializati
	 * la declararea lui qtr si va pune valorile in vectorul senzori.
	 */
   qtr.read(senzori);


}

long getProportional()
{
	  long proportional;
	/*
	 * TODO: Functie ce va returna termenul proportional.
	 *
	 * Folositi functia qtr.readLine(senzori).
	 * Aceasta copiaza valorile citite in vectorul senzori si returneaza un int intre 0 si 7000,
	 * 0 insemnand ca linia este sub senzorul 1 si 7000 insemnand ca linia este sub senzorul 8.
	 *
	 * Termenul proportional este reprezentat de devierea de la conditia dorita. Se doreste ca
	 * linia sa se afle pe centru, deci functia va returna o valoare proportionala cu devierea
	 * liniei de la centru. Aceasta va fi negativa daca se afla in stanga si pozitiva daca se
	 * afla in dreapta.
	 */
	 proportional = qtr.readLine(senzori);

	 proportional -= 3500;
   proportional /= 10;


	return proportional;
}

double computePID()
{
	double proportional;
	double derivative;
	/*
	 * Aplicati algoritmul PID
	 * integral este o variabila globala definita mai sus.
	 * Folositi last_p pentru a calcula termenul derivat.
	 */
   proportional =  getProportional();
	 derivative = proportional - last_p;
   integral += proportional;
   last_p = proportional;

	 return KP * proportional + KI * integral + KD * derivative;
}

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

	pinMode(MOTOR1A, OUTPUT);
	pinMode(MOTOR1B, OUTPUT);
	pinMode(MOTOR2A, OUTPUT);
	pinMode(MOTOR2B, OUTPUT);
	pinMode(13, OUTPUT); //pinul cu led

	digitalWrite(13, HIGH);
	for (int i = 0; i < 125; i++)  { //calibrare senzori
		 //semnalare calibrare prin led-ul legat la pinul 13
		qtr.calibrate();
		delay(20);

	}
	digitalWrite(13, LOW);
}


void loop()
{
//  if(!motorTested) {
//    setMotors(0,0);
//
//	for(int i = 0 ; i < 20 ; i++) {
//		printLine();
//		delay(50);
//	}
//  setMotors(40, 0);
//  delay(500);
//  setMotors(0, 40);
//  delay(500);
//  setMotors(-40, 0);
//  delay(500);
//  setMotors(0, -40);
//  delay(500);
//
//  motorTested = 1;
//  setMotors(0,0);
//  }

  float pid = computePID();

//  for(int i=0; i<8; i++)
//  {
//    Serial.print(senzori[i]);
//    Serial.print(" ");
//  }
  Serial.println(qtr.readLine(senzori));
  Serial.println(pid);
  if(pid > 0) {
    int a = constrain(SPEED + pid, 0, 255);
    int b = constrain(SPEED - pid, 0, 255);
    setMotors(a, b);
  } else if(pid < 0) {
    int a = constrain(SPEED + pid, 0, 255);
    int b = constrain(SPEED - pid, 0, 255);
    setMotors(a, b);
  } else {
    setMotors(SPEED, SPEED);
  }


}