// ---------------- Pines ----------------
#define AI1 0 /**< Pin 1 del motor A. */
#define AI2 1 /**< Pin 2 del motor A. */
#define BI1 3 /**< Pin 1 del motor B. */
#define BI2 5 /**< Pin 2 del motor B. */

void motorControl(uint8_t pin1, uint8_t pin2, int vel);
void motorA(int vel);
void motorB(int vel);
void motors(int velB, int velA);
void stopMotors(void);
void initMotors(void);
void testMotors(void);

// ---------------- Función 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(void) {
  analogWrite(AI1, 0);
  analogWrite(AI2, 0);
  analogWrite(BI1, 0);
  analogWrite(BI2, 0);
}

void initMotors(void) {
  pinMode(AI1, OUTPUT);
  pinMode(AI2, OUTPUT);
  pinMode(BI1, OUTPUT);
  pinMode(BI2, OUTPUT);
}

void testMotors(void) {
  motors(60, 60);
  delay(500);
  motors(-60, -60);
  delay(500);
}

// ---------------- Setup ----------------
void setup() {
  initMotors();
}

// ---------------- Loop principal ----------------
void loop() {
  testMotors();
}