/*
  TCC - PONTE ROLANTE
  by losr
*/

#define X_inH 31   // Motor X Input Sentido Horário
#define X_inAH 35  // Motor X Input Sentido Anti-Horário
#define X_enable 42

#define Y_inH 39   // Motor Y Input Sentido Horário
#define Y_inAH 38  // Motor Y Input Sentido Anti-Horário
#define Y_enable 43

#define Z_inH 26    // Motor Z Input Sentido Horário
#define Z_inAH 36  // Motor Z Input Sentido Anti-Horário
#define Z_enable 44

#define btu 12
#define btd 13

#define pinXP 24  // Pino Joystick Horizontal +
#define pinXN 22  // Pino Joystick Horizontal -
#define pinYP 23  // Pino Joystick Vertical +
#define pinYN 25  // Pino Joystick Vertical -

bool btu_f, btd_f;
char sentido;

class motorDC {
  int pin1, pin2, pinEn;

public:

  void pinagem(int in1, int in2, int inEn) {  // Pinout é o método para a declaração dos pinos que vão controlar o objeto motor
    pin1 = in1;
    pin2 = in2;
    pinEn = inEn;
    pinMode(pin1, OUTPUT);
    pinMode(pin2, OUTPUT);
    pinMode(pinEn, OUTPUT);
  }

  void frente() {  // Forward é o método para fazer o motor girar para frente
    digitalWrite(pin1, HIGH);
    digitalWrite(pin2, LOW);
  }

  void tras() {  // Backward é o método para fazer o motor girar para trás
    digitalWrite(pin1, LOW);
    digitalWrite(pin2, HIGH);
  }
  void parar() {  // Stop é o metodo para fazer o motor ficar parado.
    digitalWrite(pin1, LOW);
    digitalWrite(pin2, LOW);
  }

  void velocidade(int pwm) {
    analogWrite(pinEn, pwm);
  }
};

motorDC motorX, motorY, motorZ;

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

  motorX.pinagem(X_inH, X_inAH, X_enable);
  motorY.pinagem(Y_inH, Y_inAH, Y_enable);
  motorZ.pinagem(Z_inH, Z_inAH, Z_enable);

  motorX.velocidade(255);
  motorY.velocidade(255);
  motorZ.velocidade(255);

  pinMode(pinXP, INPUT_PULLUP);
  pinMode(pinXN, INPUT_PULLUP);
  pinMode(pinYP, INPUT_PULLUP);
  pinMode(pinYN, INPUT_PULLUP);

  pinMode(btu, INPUT_PULLUP);
  pinMode(btd, INPUT_PULLUP);
}

void loop() {
  controle();
}

void controle() {
  // SUBIR E DESCER ELETROIMÃ

  if(!digitalRead(btu) && !digitalRead(btd)) motorZ.parar();
  else if (!digitalRead(btu)) motorZ.frente();
  else if (!digitalRead(btd)) motorZ.tras();
  else motorZ.parar();

  // MOVIMENTAÇÃO

  if (!digitalRead(pinXP)) motorX.frente();
  else if (!digitalRead(pinXN)) motorX.tras();
  else motorX.parar();

  if (!digitalRead(pinYP)) motorY.frente();
  else if (!digitalRead(pinYN)) motorY.tras();
  else motorY.parar();
}