 # include "Arduino.h"
 # include "OpenTB6612FNG.h"





OpenTB6612FNG::OpenTB6612FNG(int TP) {

	pinMode(AIN1, OUTPUT);
	pinMode(AIN2, OUTPUT);
	pinMode(PWMA, OUTPUT);
	pinMode(BIN1, OUTPUT);
	pinMode(BIN2, OUTPUT);
	pinMode(PWMB, OUTPUT);
	analogWriteFreq(20000);
	analogWriteRange(255);

	
}

void OpenTB6612FNG::MotorIz(int value) {
	/* if (value >= 0) {
		// si valor positivo vamos hacia adelante

		digitalWrite(AIN1, HIGH);
		digitalWrite(AIN2, LOW);
	} else {
		// si valor negativo vamos hacia atras

		digitalWrite(AIN1, LOW);
		digitalWrite(AIN2, HIGH);
		value *= -1;
	}
 */
	// Setea Velocidad

	analogWrite(MOTOR_IZQ, value);
}

void OpenTB6612FNG::MotorDe(int value) {
	/* if (value >= 0) {
		// si valor positivo vamos hacia adelante

		digitalWrite(BIN1, HIGH);
		digitalWrite(BIN2, LOW);
	} else {
		// si valor negativo vamos hacia atras

		digitalWrite(BIN1, LOW);
		digitalWrite(BIN2, HIGH);
		value *= -1;
	}
 */
	// Setea Velocidad

	analogWrite(MOTOR_DER, value);
}

void OpenTB6612FNG::Motores(int left, int righ) {
	MotorIz(left);
	MotorDe(righ);
}

void OpenTB6612FNG::Forward()
{
  digitalWrite(AIN1, HIGH);
  digitalWrite(AIN2, LOW);
  digitalWrite(BIN1, HIGH);
  digitalWrite(BIN2, LOW);
}

void OpenTB6612FNG::Backward()
{
  digitalWrite(AIN1, LOW);
  digitalWrite(AIN2, HIGH);
  digitalWrite(BIN1, LOW);
  digitalWrite(BIN2, HIGH);
}




void OpenTB6612FNG::Stop()
{
  digitalWrite(AIN1, LOW);
  digitalWrite(AIN2, LOW);
  digitalWrite(BIN1, LOW);
  digitalWrite(BIN2, LOW);
}