Arduino - Elevador código

#include <Servo.h>

// Servo
#define SERVO_PIN 13
// Controladores de piso
#define BTN_0 9
#define BTN_1 10
#define BTN_2 11
#define BTN_3 12
// Sensor de distancia
#define TRIG_PIN 8
#define ECHO_PIN 7
// Display de 7 segmentos
#define A 2
#define B 3
#define C 4
#define D 5
#define E 6
#define G A5
#define F A4
// Auxiliares
#define ALTURA 87
#define ALTURA_POR_PISO (long)(ALTURA/4)
#define TOLERANCIA_PORCENTUAL 5

/* Tipos de datos definidos para el problema */

// Rotador: Tipo que representa el servo de rotación continua.
typedef struct {
  // Objeto servo.
  Servo obj;
  // Dirección en la que gira el servo.
  int direccion;
} Rotador;

Rotador rotador_inicializar();
uint8_t rotador_attach(int pin);
// Cambia la dirección del servo. Por ejemplo: 0, 45, 90 (detener), 135, 180.
void rotador_set_dir(int direccion);
// Cambia la dirección del servo según su atributo "direccion".
void rotador_write();
// Cambia el atributo "direccion" e invoca a `rotador_write`.
void rotador_write_dir(int direccion);

// Sensor: Tipo que representa al sensor de distancia.
typedef struct {
  // Última distancia calculada.
  long int d;
  // Tiempo del último pulso.
  long int t;
} Sensor;

Sensor sensor_inicializar();
// Obtiene la última distancia registrada por el sensor.
// La distancia inicial, cuando todavía no se hizo ningún cálculo, es de -1.
long int sensor_distancia();
// Calcula la distancia al suelo medida en cm.
long int sensor_calcular_distancia();

// Elevador: Tipo que representa el comportamiento del elevador y sus estados.
typedef struct {
  short piso_actual;
  short piso_destino;
} Elevador;

Elevador elevador_inicializar();
// Escucha si se pulsó algún botón y modifica el estado interno según sea necesario.
void elevador_escuchar();
// Mueve el elevador en función de sus estados internos de `piso_actual` y `piso_destino`.
// En caso de que la distancia de movimiento sea 0, entonces no realiza acción alguna.
void elevador_mover();

// Funciones del display
void display_dibujar(int n);
void display_limpiar();

/* Estados y objetos de uso global para el programa en ejecución */

int debug = 1;

Rotador rotador;
Sensor sensor;
Elevador elevador;

// Representaciones de los valores para el display de 7 segmentos.
int display_figuras[4][7] = {
  {A, B, C, D, E, G, 0},
  {B, C, 0, 0, 0, 0, 0},
  {A, B, F, E, D, 0, 0},
  {A, B, C, D, F, 0, 0}
};

void setup()
{
  if (debug != 0) {
    Serial.begin(9600);
  }

  rotador = rotador_inicializar();
  rotador_attach(SERVO_PIN);
  rotador_write(); // El rotador inicia detenido (direccion = 90).

  sensor = sensor_inicializar();
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);

  elevador = elevador_inicializar();

  pinMode(BTN_0, INPUT);
  pinMode(BTN_1, INPUT);
  pinMode(BTN_2, INPUT);
  pinMode(BTN_3, INPUT);

  pinMode(A, OUTPUT);
  pinMode(B, OUTPUT);
  pinMode(C, OUTPUT);
  pinMode(D, OUTPUT);
  pinMode(E, OUTPUT);
  pinMode(F, OUTPUT);
  pinMode(G, OUTPUT);
}

void loop()
{
  display_limpiar();
  if (elevador.piso_destino != elevador.piso_actual) {
    display_dibujar(elevador.piso_destino);
  } else {
    display_dibujar(elevador.piso_actual);
  }

  elevador_escuchar();
  elevador_mover();
  delay(100);
}

// Display
void display_dibujar(int n) {
  for (int i = 0; i < 7; i++) {
    if (display_figuras[n][i] != 0) {
      digitalWrite(display_figuras[n][i], HIGH);
    }
  }
}

void display_limpiar() {
  digitalWrite(A, LOW);
  digitalWrite(B, LOW);
  digitalWrite(C, LOW);
  digitalWrite(D, LOW);
  digitalWrite(E, LOW);
  digitalWrite(F, LOW);
  digitalWrite(G, LOW);
}

// Elevador
Elevador elevador_inicializar()
{
  Elevador e;
  e.piso_actual = 0;
  e.piso_destino = 0;
  return e;
}

void elevador_mover()
{
  short actual = elevador.piso_actual;
  short destino = elevador.piso_destino;

  int distancia = abs((int)(destino) - (int)(actual));
  int direccion_rotador = 0;

  if (actual > destino) {
    direccion_rotador = -1;
  } else {
    if (actual < destino) {
      direccion_rotador = 1;
    }
  }

  // Salida prematura, ya que no se puede cambiar de piso.
  if (direccion_rotador == 0) {
    rotador_write_dir(90);
    return;
  }

  long int altura_destino_cm;
  if (direccion_rotador > 0) {
    altura_destino_cm = ALTURA - ALTURA_POR_PISO*destino;
    rotador_set_dir(110);
  } else {
    altura_destino_cm = ALTURA + ALTURA_POR_PISO*destino;
    rotador_set_dir(67);
  }

  long int distancia_actual_cm = sensor_calcular_distancia();
  long int tolerancia = ALTURA_POR_PISO*TOLERANCIA_PORCENTUAL/100;
  long int cota_superior = (long int)(altura_destino_cm + tolerancia);
  long int cota_inferior = (long int)(altura_destino_cm - tolerancia);

  if (debug != 0) {
    Serial.print("Actual: "); Serial.print(distancia_actual_cm); Serial.print("cm");
    Serial.print(" | Destino: "); Serial.print(altura_destino_cm); Serial.print("cm");
    Serial.print(" | ["); Serial.print(cota_inferior); Serial.print(", "); Serial.print(cota_superior); Serial.println("]");
    delay(100);
  }

  if (distancia_actual_cm >= cota_inferior && distancia_actual_cm <= cota_superior) {
    rotador_set_dir(90);
    elevador.piso_actual = destino;

    if (debug != 0) {
      Serial.print(">>> Llegada al piso: ");
      Serial.println(elevador.piso_actual);
    }
  }

  rotador_write();
}

void elevador_escuchar()
{
  int btn0 = digitalRead(BTN_0);
  int btn1 = digitalRead(BTN_1);
  int btn2 = digitalRead(BTN_2);
  int btn3 = digitalRead(BTN_3);

  if (btn0 == HIGH) {
    elevador.piso_destino = 0;
  }
  if (btn1 == HIGH) {
    elevador.piso_destino = 1;
  }
  if (btn2 == HIGH) {
    elevador.piso_destino = 2;
  }
  if (btn3 == HIGH) {
    elevador.piso_destino = 3;
  }
}

// Sensor
Sensor sensor_inicializar() {
  Sensor s;
  s.t = 0;
  s.d = -1;
}

long int sensor_distancia() {
  return sensor.d;
}

long int sensor_calcular_distancia()
{
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);

  sensor.t = pulseIn(ECHO_PIN, 1);
  sensor.d = sensor.t / 59;

  return sensor.d;
}

// Rotador
Rotador rotador_inicializar()
{
  Rotador r;
  r.obj = {};
  r.direccion = 90;
  return r;
}

uint8_t rotador_attach(int pin)
{
  return rotador.obj.attach(pin);
}

void rotador_set_dir(int direccion)
{
  rotador.direccion = direccion;
}

void rotador_write()
{
  rotador.obj.write(rotador.direccion);
}

void rotador_write_dir(int direccion)
{
  rotador_set_dir(direccion);
  rotador_write();
}