7 сегментный индикатор

Пока разбирался с часиками нашел сперва готовые библиотеки для работы с 7-сегментным индикатором, часть не завелась, а то что завелось по коду было просто жутко, с отдельными функциями на каждую цифру и нелепыми delay() в коде.

В общем решил переписать с нуля. Общий смысл в том чтобы часики отображались не когда им это «сказали», а по прерыванию.

#include <MsTimer2.h>
int DIGS[4] = {7, 8, 9, 10}; //Выходы DIG от 1 до 4
int SEGS[8] = {2,3,4,5,11,12,13,6};//Выходы сегментов от "a" до "g" + "dp" (точка)
char SHOW[4]= {' ',' ',' ',' '};  //Массив текущего отображения
bool DOTS[4]= {0,0,0,0}; //Массив подсвечиваемых точек
int TableN=0; //Номер обрабатываемого сегмента
void setup()
{
  //пины на вывод
  for (int i=0; i<4; i++) {pinMode(DIGS[i],OUTPUT);}
  for (int i=0; i<8; i++) {pinMode(SEGS[i],OUTPUT);}
  MsTimer2::set(1, ShowTable); // прерывание по таймеру 1 мс, учитывая что один запуск - это одна цифра из 4
  MsTimer2::start();           // разрешение прерывания 
}
void loop()
{
  //Тут какой то код, который может менять содержимое массива SHOW для отображения информации,
  //а так же играться с DOTS массивом чтобы показывать точки, например:
  //DOTS[1]=now.second()%2; для мигания средней точки на часиках
}
void ShowTable()
{
    TableN++; //переходим на следующий сегмент
    if (TableN==4) {TableN=0;}
    //Включаем нужный DIG
    for (int i=0; i<4; i++)
    {
      if (TableN!=i) {digitalWrite(DIGS[i],HIGH);} //Гасим цифру
      else {digitalWrite(DIGS[i],LOW);} //Поджигаем
    }
    //Поджигаем или гасим точку
    digitalWrite(SEGS[7], DOTS[TableN]);
    //"Рисуем" цифры и символы в бинарке
    byte DATA;
    switch (SHOW[TableN]) {
      case '0': DATA=B1111110; break;
      case '1': DATA=B0110000; break;
      case '2': DATA=B1101101; break;
      case '3': DATA=B1111001; break;
      case '4': DATA=B0110011; break;
      case '5': DATA=B1011011; break;
      case '6': DATA=B1011111; break;
      case '7': DATA=B1110000; break;
      case '8': DATA=B1111111; break;
      case '9': DATA=B1111011; break;
      case '-': DATA=B0000001; break;
      case 'E': DATA=B1001111; break;
      default:  DATA=B0000000; break;
    }
    for (int i=0; i<7; i++) {digitalWrite(SEGS[i], bitRead(DATA, 6-i));}
}