diff --git a/main.cpp b/main.cpp
index 7a9e253..9cf7177 100644
--- a/main.cpp
+++ b/main.cpp
@@ -12,57 +12,46 @@
 #define MAX_TRAINS 16
 #define COMMAND_BUFFER_SIZE 64
 #define SNPRINTF_BUFFER_SIZE 64
+#define EPPROM_SIZE 1024
 
 char buffer[SNPRINTF_BUFFER_SIZE];
 
 uint16_t storedTrains = 0;
 Train trains[MAX_TRAINS];
 
-#define TICK_MAX 255
-uint8_t tick = 0;
-
 bool autoff = true;
 bool powerIsOn = true;
 
+#define TICK_MAX 255
+volatile uint8_t tick = 0;
+
+volatile bool resendEvent = false;
+
 ISR(TIMER0_OVF_vect) 
 {
-    if(powerIsOn)
-    {
-        ++tick;
-        if(tick == TICK_MAX) 
-        {
-            for(uint16_t i = 0; i < storedTrains; i++) trains[i].resendSpeed();
-            if(autoff) 
-            {
-                bool trainsRunning = false;
-                for(uint16_t i = 0; i < storedTrains; i++) trainsRunning = trainsRunning || trains[i].getSpeed();
-                if(!trainsRunning)
-                {
-                    powerIsOn = false;
-                    Train::setOutput(Train::OFF);
-                }
-            }
-            tick = 0;
-        }
-        else
-        {
-            
-            Train::setOutput(Train::HIGH);
-            _delay_us(200);
-            Train::setOutput(Train::LOW);
-        }
-    }
+    ++tick;
+    if(tick  == 0) resendEvent = true;
+    if((tick & 0b00000111) < 1) Train::setOutput(Train::HIGH);
+    else Train::setOutput(Train::LOW);
+}
+
+void timer0InterruptEnable(const bool enable)
+{
+     if(enable) TIMSK0 = 0b00000001;
+     else TIMSK0 = 0b00000000;
 }
 
 void save_state()
 {
+    cli();
     EEPROM_write_char( 0, storedTrains );
     EEPROM_write_char( 1, autoff );
     for(uint16_t i = 0; i < storedTrains; i++)
     {
-        EEPROM_write_char( i+32, trains[i].getAddress());
+        EEPROM_write_char( i*2+32, trains[i].getAddress());
+        EEPROM_write_char( i*2+32+1, trains[i].getProtocol());
     }
-    
+    sei();
 }
 
 void restore_state()
@@ -71,13 +60,13 @@ void restore_state()
     autoff = EEPROM_read_char(1);
     if(storedTrains > MAX_TRAINS ) 
     {
-        for(uint16_t i = 0; i < MAX_TRAINS+32; i++) EEPROM_write_char(i, 0);
+        for(uint16_t i = 0; i < EPPROM_SIZE; i++) EEPROM_write_char(i, 0);
         storedTrains = 0;
     }
-    else for(uint8_t i = 0; i <= storedTrains; i++)
+    else for(uint8_t i = 0; i < storedTrains; i++)
     {
-        uint8_t address = EEPROM_read_char(32+i);
-        trains[i].setAddress(address); 
+        trains[i].setAddress(EEPROM_read_char(32+i*2)); 
+        trains[i].setProtocol(EEPROM_read_char(32+1+i*2));
     }
 }
 
@@ -104,27 +93,26 @@ inline static void printHelp(Serial* serial)
 void trainDispatch(char* inBuffer, Serial* serial)
 {
     cli();
-    if(powerIsOn == false)
-    {
-        powerIsOn = true;
-        Train::setOutput(Train::LOW);
-        _delay_ms(100);
-    }
     if( strcmp(inBuffer, "add") == 0 )
     {
         char* token = strtok(NULL, " ");
-        uint8_t address = strtol(token, nullptr, 2 );
+        uint8_t address = 0;
+        if(token != NULL) address = strtol(token, nullptr, 2 );
         if(address != 0 && storedTrains < MAX_TRAINS) 
         {
             trains[storedTrains].setAddress(address);
             
-            uint8_t size = snprintf(buffer, SNPRINTF_BUFFER_SIZE, "TRAIN saved! NUMBER: %u ADRESS: %s%s\n", storedTrains, bit_rep[address >> 4], bit_rep[address & 0x0F]);
+            token = strtok(NULL, " ");
+            if(token != NULL && strcmp(token, "delta") == 0 )  trains[storedTrains].setProtocol(Train::M_DELTA);
+            else trains[storedTrains].setProtocol(Train::M_DIGITAL);
+            
+            uint8_t size = snprintf(buffer, SNPRINTF_BUFFER_SIZE, "TRAIN saved! NUMBER: %u ADRESS: %s%s PROTCOL: %s\n", storedTrains, bit_rep[address >> 4], bit_rep[address & 0x0F], trains[storedTrains].getProtocol() == Train::M_DELTA ? "delta" : "digital" );
             serial->write(buffer, size);
             
             storedTrains++;
             save_state();
         }
-        else serial->write_p(PSTR("Usage: train add [address]"));
+        serial->write_p(PSTR("Usage: train add [address]"));
     }
     else if( strcmp(inBuffer, "delete") == 0)
     {
@@ -138,8 +126,11 @@ void trainDispatch(char* inBuffer, Serial* serial)
         serial->write_p(PSTR("Trains:\n"));
         for(uint8_t i = 0; i < storedTrains; i++)
         {
-            snprintf(buffer, SNPRINTF_BUFFER_SIZE, "NUMBER: %u ID: %s%s CURRENT SPD: %u\n", i, bit_rep[trains[i].getAddress() >> 4], bit_rep[trains[i].getAddress() & 0x0F], trains[i].getSpeed());
+            snprintf(buffer, SNPRINTF_BUFFER_SIZE, "NUMBER: %u ID: %s%s CURRENT PACKET: %x", i, bit_rep[trains[i].getAddress() >> 4], bit_rep[trains[i].getAddress() & 0x0F], trains[i].getLastPacket());
             serial->write(buffer, SNPRINTF_BUFFER_SIZE);
+            serial->write_p(PSTR(" PROTOCOL: "));
+            if(trains[i].getProtocol() == Train::M_DELTA)serial->write_p(PSTR("delta\n"));
+            else serial->write_p(PSTR("digital\n"));
         }
         serial->putChar('\n');
     }
@@ -148,6 +139,13 @@ void trainDispatch(char* inBuffer, Serial* serial)
         uint8_t id = strtol(inBuffer, nullptr, 10);
         if(id < storedTrains )
         {
+            if(powerIsOn == false)
+            {
+                powerIsOn = true;
+                Train::setOutput(Train::LOW);
+                _delay_ms(100);
+                timer0InterruptEnable(true);
+            }
             char* token = strtok(NULL, " ");
             if( token != NULL &&  strcmp(token, "speed") == 0 )
             {
@@ -156,32 +154,56 @@ void trainDispatch(char* inBuffer, Serial* serial)
                 serial->write_p(PSTR("Set Train speed\n"));
             }
             else if(token != NULL && strcmp(token, "function") == 0 ) 
+            {
+                token = strtok(NULL, " ");
+                char* boolToken = strtok(NULL, " ");
+                if(token != NULL && boolToken !=  NULL)
+                {
+                    uint8_t functionId = atoi(token);
+                    bool on = (strcmp(boolToken, "on") == 0);
+                    trains[id].sendFunction(functionId, on );
+                    serial->write_p(PSTR("Set Train function "));
+                    serial->write(functionId);
+                    serial->write(on ? " on\n" : " off\n");
+                }
+            }
+            else if(token != NULL && strcmp(token, "probe") == 0 ) 
+            {
+                for(uint16_t j = 0; j < 1024; j++)
+                {
+                    trains[id].sendRaw(j);
+                    snprintf(buffer, SNPRINTF_BUFFER_SIZE, "TRYING: %x\n", j);
+                    serial->write(buffer, strlen(buffer));
+                    sei();
+                    _delay_ms(250);
+                    cli();
+                    
+                }
+            }
+            else if(token != NULL && strcmp(token, "raw") == 0 ) 
             {
                 token = strtok(NULL, " ");
                 if(token != NULL)
                 {
-                    for(uint16_t j = 0; j < 8; j++)
+                    cli();
+                    uint16_t i = strtol(token, nullptr, 16 );
+                    snprintf(buffer, SNPRINTF_BUFFER_SIZE, "SENDING: %x\n", i);
+                    serial->write(buffer, strlen(buffer));
+                    for(uint8_t j = 0; j < 100; j++)
                     {
-                        uint16_t i = strtol(token, nullptr, 16 );
-                        trains[id].sendFunction(i);
-                        snprintf(buffer, SNPRINTF_BUFFER_SIZE, "TRYING: %x\n", i);
-                        serial->write(buffer, strlen(buffer));
-                        sei();
-                        _delay_ms(250);
-                        cli();
-                        
+                        trains[id].sendRaw(i);
+                        _delay_ms(20);
                     }
+                    serial->write_p(PSTR("Finished\n"));
+                    sei();
                 }
-                /*
-                trains[id].sendFunction(atoi(token));
-                serial->write_p(PSTR("Set Train function\n"));*/
             }
             else if( token != NULL && strcmp(token, "reverse") == 0 )
             {
                 trains[id].reverse();
                 serial->write_p(PSTR("Reversed Train\n"));
             }
-            else if( token != NULL && strcmp(token, "stop") )trains[id].setSpeed(0);
+            else if( token != NULL && strcmp(token, "stop") == 0 )trains[id].stop();
             else serial->write_p(PSTR("Not a valid command\n"));
         }
         else serial->write_p(PSTR("Id out of range.\n"));
@@ -193,10 +215,7 @@ void powerDispatch(char* token, Serial* serial)
 {
     if(strcmp(token, "off") == 0)
     {
-        for(uint16_t i = 0; i < storedTrains; i++)
-        {
-            trains[i].setSpeed(0);
-        }
+        timer0InterruptEnable(false);
         Train::setOutput(Train::OFF);
         powerIsOn = false;
     }
@@ -207,6 +226,7 @@ void powerDispatch(char* token, Serial* serial)
             trains[i].setSpeed(0);
         }
         Train::setOutput(Train::LOW);
+        timer0InterruptEnable(true);
     }
     else if(strcmp(token, "auto") == 0)
     {
@@ -252,13 +272,13 @@ void serialDispatch(Serial* serial)
             }
             else if(length > 4 && strncmp(token, "erase", 4) == 0)
             {
-                for(uint16_t i = 0; i < MAX_TRAINS+1; i++) EEPROM_write_char(i, 0);
+                for(uint16_t i = 0; i < EPPROM_SIZE; i++) EEPROM_write_char(i, 0);
                 serial->write_p(PSTR("EEPROM erased\n"));
                 storedTrains = 0;
             }
             else if(length > 3 && strcmp(token, "dump") == 0)
             {
-                for(uint16_t i = 0; i < MAX_TRAINS+32; i++) 
+                for(uint16_t i = 0; i < EPPROM_SIZE; i++) 
                 {
                     if(i != 0) serial->putChar(',');
                     serial->write((uint16_t)EEPROM_read_char(i));
@@ -270,7 +290,7 @@ void serialDispatch(Serial* serial)
                 for(uint16_t i = 0; i < storedTrains; i++)
                 {
                     cli();
-                    trains[i].setSpeed(0);
+                    trains[i].stop();
                     sei();
                 }
             }
@@ -297,22 +317,54 @@ void serialDispatch(Serial* serial)
 int main() 
 {
     TCNT0 = 0;
-    TIMSK0 = 0b00000001; //turn on timer0 interupt on OCIEB
-    TCCR0B = (1<<CS02) /*| (1<<CS00)*/; // run timer0 with /1024 scaler
+    TCCR0B = (1<<CS01) /*| (1<<CS00)*/; // run timer0 with /64 scaler
     
     DDRD =  (1 << PD2) | (1 << PD3) | (1 << PD4) | (1 << PD5);
     
     restore_state();
     
-    autoff ? Train::setOutput(Train::OFF) : Train::setOutput(Train::LOW);
+    if(autoff) 
+    {
+        timer0InterruptEnable(false);
+        Train::setOutput(Train::OFF);
+    }
+    else
+    {
+        timer0InterruptEnable(true);
+        Train::setOutput(Train::LOW);
+    }
     
     sei();
     
     Serial serial;
 
     serial.write_p(PSTR("TrainController v0.1 starting\n"));
+    
+    uint8_t trainToResend = 0;
+    
     while(true)
     {        
+        if(resendEvent && storedTrains > 0) 
+        {
+            timer0InterruptEnable(false);
+            trains[trainToResend].resendData();
+            trainToResend++;
+            if(storedTrains <= trainToResend) trainToResend = 0;
+            resendEvent = false;
+            timer0InterruptEnable(true);
+        }
+        
+        if(autoff) 
+        {
+            bool trainsRunning = false;
+            for(uint16_t i = 0; i < storedTrains; i++) trainsRunning = trainsRunning || trains[i].isActive();
+            if(!trainsRunning)
+            {
+                powerIsOn = false;
+                timer0InterruptEnable(false);
+                Train::setOutput(Train::OFF);
+            }
+        }
         serialDispatch(&serial);
     }
     return 0;
diff --git a/train.cpp b/train.cpp
index d372d5a..e5c61a7 100644
--- a/train.cpp
+++ b/train.cpp
@@ -2,7 +2,7 @@
 
 static volatile unsigned char *_port = &PORTD;
 
-Train::Train(const uint8_t address):  _address(address)
+Train::Train(const uint8_t address, uint8_t protocol):  _address(address), _protocol(protocol)
 {
 }
 
@@ -21,6 +21,18 @@ uint8_t Train::getAddress()
     return _address;
 }
 
+void Train::stop()
+{
+    lastDataPacket = 0;
+    resendData();
+}
+
+bool Train::isActive()
+{
+    return lastDataPacket & 0b0000000111111111;
+}
+
+
 void Train::off()
 {
     writePin(_port, _pinHighA, false);
@@ -34,14 +46,14 @@ void Train::setOutput(const uint8_t state)
     if(state == HIGH)
     {
         off();
-        _delay_us(5);
+        _delay_us(3);
         writePin(_port, _pinHighA, true);
         writePin(_port, _pinLowB,  false);
     }
     else if (state == LOW)
     {
         off();
-        _delay_us(5);
+        _delay_us(3);
         writePin(_port, _pinHighB, true);
         writePin(_port, _pinLowA,  false);
     }
@@ -77,12 +89,12 @@ void Train::sendAddress()
     }
 }
 
-void Train::sendRaw(const uint8_t data)
+void Train::sendRaw(const uint16_t data)
 {
     for(uint8_t j = 0; j < SEND_COUNT; j++)
     {
         sendAddress();
-        for(uint8_t i = 0; i < 5; i++)
+        for(uint8_t i = 0; i < 10; i++)
         {
             sendBit(data & (1 << i));
         }
@@ -92,31 +104,50 @@ void Train::sendRaw(const uint8_t data)
 
 void Train::setSpeed(uint8_t speed)
 {
-    if(speed > 0) ++speed;
-    if(speed <= 15)
+    if(speed != 0)speed = speed + 1;
+    else if(speed > 15) speed = 15;
+    for(uint8_t i = 0; i < 4; i++)
     {
-        lastSpeed = speed;
-        sendData((speed << 1));
+                                 lastDataPacket = (lastDataPacket & ~(1 << (i+1)*2))     | (((uint16_t)speed & (1 << i)) << (i+1)*2-i);
+        if(_protocol == M_DELTA) lastDataPacket = (lastDataPacket & ~(1 << ((i+1)*2+1))) | (((uint16_t)speed & (1 << i)) << ((i+1)*2+1-i));
     }
+    resendData();
 }
 
-void Train::resendSpeed()
+void Train::resendData()
 {
-    uint8_t data = lastSpeed;
-    sendData((data << 1));
+    sendRaw(lastDataPacket);
 }
 
-uint8_t Train::getSpeed()
+uint16_t Train::getLastPacket()
 {
-    return lastSpeed;
+    return lastDataPacket;
 }
 
 void Train::reverse()
 {
-    sendData((1 << 1));
+    sendRaw(0b000001100);
+    sendRaw(0b000001100);
 }
 
-void Train::sendFunction(const uint16_t function)
+void Train::sendFunction(const uint8_t function, bool enable)
 {
-    sendDataUndoubled(function);
+    if(function == 0) lastDataPacket = (lastDataPacket & ~0b00000011) | (enable ? 0b00000011 : 0b00000000);
+    else if(_protocol == M_DIGITAL && function <= 3)
+    {
+        lastDataPacket = lastDataPacket | 0b1000000000;
+        if(enable) lastDataPacket = lastDataPacket |  (1 << (9-function*2));
+        else       lastDataPacket = lastDataPacket & ~(1 << (9-function*2));
+    }
+    resendData();
+}
+
+void Train::setProtocol(const uint8_t protocol)
+{
+   _protocol = protocol;
+}
+
+uint8_t Train::getProtocol()
+{
+    return _protocol;
 }
diff --git a/train.h b/train.h
index 7338559..6e7b6f3 100644
--- a/train.h
+++ b/train.h
@@ -23,11 +23,11 @@ private:
     static const unsigned char _pinHighB = PD2;
     static const unsigned char _pinLowB = PD3;
     
-    static const uint8_t SEND_COUNT = 4;
+    static const uint8_t SEND_COUNT = 2;
     
     uint8_t _protocol = M_DIGITAL;
 
-    uint16_t lastdatapacket = 0;
+    uint16_t lastDataPacket = 0;
     
     inline static void off();
     void sendBit(const bool bit);
@@ -37,16 +37,20 @@ public:
     
     static void setOutput(const uint8_t state);
     
-    Train(const uint8_t address);
+    Train(const uint8_t address, uint8_t protocol = M_DIGITAL);
     Train();
     
-    void resendSpeed();
+    void resendData();
     
     void reverse();
     
+    void stop();
+    
+    bool isActive();
+    
     uint8_t getAddress();
     
-    uint8_t getSpeed();
+    uint16_t getLastPacket();
     
     uint8_t getProtocol();
     
@@ -56,7 +60,7 @@ public:
     
     void setAddress(const uint8_t address);
     
-    void sendFunction(const uint16_t function);
+    void sendFunction(const uint8_t function, bool enable = true);
     
     void sendRaw(const uint16_t data);