// ======================== Configuración TB6612 ==========================
constexpr uint8_t AI1 = 11;
constexpr uint8_t AI2 = 3;
constexpr uint8_t BI1 = 5;
constexpr uint8_t BI2 = 6;


// ======================== Funciones Motores ==========================
void motorControl(uint8_t pin1, uint8_t pin2, int vel) {
  vel = constrain(vel, -255, 255);
  if (vel > 0) {
    analogWrite(pin1, vel);
    analogWrite(pin2, 0);
  } else if (vel < 0) {
    analogWrite(pin1, 0);
    analogWrite(pin2, -vel);
  } else {
    analogWrite(pin1, 0);
    analogWrite(pin2, 0);
  }
}

inline void motorA(int vel) {
  motorControl(AI1, AI2, vel);
}
inline void motorB(int vel) {
  motorControl(BI1, BI2, vel);
}

void motors(int velB, int velA) {
  motorA(velA);
  motorB(velB);
}

void stopMotors() {
  analogWrite(AI1, 0);
  analogWrite(AI2, 0);
  
  analogWrite(BI1, 0);
  analogWrite(BI2, 0);
}

// ======================== SETUP ==========================
void setup() {
  delay(1000);
}

// ======================== LOOP ==========================
void loop() {
  motors(60,60);
  delay(500);
  motors(-60,-60);
  delay(500);
  stopMotors();
}