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





OpenTB6612FNG::OpenTB6612FNG(int TP) {

	pinMode(PWMA, OUTPUT);
	pinMode(AIN1, OUTPUT);
	pinMode(AIN2, OUTPUT);
	pinMode(STBY, OUTPUT);
	pinMode(BIN1, OUTPUT);
	pinMode(BIN2, OUTPUT);
	pinMode(PWMB, OUTPUT);
   ledcSetup(ledChannel, freq, resolution);
   ledcSetup(ledChannel2, freq, resolution);
  
  // attach the channel to the GPIO to be controlled
  ledcAttachPin(PWMA, ledChannel);
  ledcAttachPin(PWMB, ledChannel2);
  
}

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

	ledcWrite(ledChannel, 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

	ledcWrite(ledChannel2, value);
}

void OpenTB6612FNG::Motores(int left, int righ) {
	MotorIz(left);
	MotorDe(righ);
	digitalWrite(STBY, LOW);


}

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