diff --git a/main.cpp b/main.cpp
index fd64933..7a9e253 100644
--- a/main.cpp
+++ b/main.cpp
@@ -7,14 +7,7 @@
 #include "writepin.h"
 #include "train.h"
 #include "eeprom.h"
-
-const char *bit_rep[16] = 
-{
-    [ 0] = "0000", [ 1] = "0001", [ 2] = "0010", [ 3] = "0011",
-    [ 4] = "0100", [ 5] = "0101", [ 6] = "0110", [ 7] = "0111",
-    [ 8] = "1000", [ 9] = "1001", [10] = "1010", [11] = "1011",
-    [12] = "1100", [13] = "1101", [14] = "1110", [15] = "1111",
-};
+#include "bitrep.h"
 
 #define MAX_TRAINS 16
 #define COMMAND_BUFFER_SIZE 64
@@ -35,29 +28,29 @@ ISR(TIMER0_OVF_vect)
 {
     if(powerIsOn)
     {
-    ++tick;
-    if(tick == TICK_MAX) 
-    {
-        for(uint16_t i = 0; i < storedTrains; i++) trains[i].resendSpeed();
-        if(autoff) 
+        ++tick;
+        if(tick == TICK_MAX) 
         {
-            bool trainsRunning = false;
-            for(uint16_t i = 0; i < storedTrains; i++) trainsRunning = trainsRunning || trains[i].getSpeed();
-            if(!trainsRunning)
+            for(uint16_t i = 0; i < storedTrains; i++) trains[i].resendSpeed();
+            if(autoff) 
             {
-                powerIsOn = false;
-                Train::setOutput(Train::OFF);
+                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 = 0;
-    }
-    else
-    {
-        
-        Train::setOutput(Train::HIGH);
-        _delay_us(200);
-        Train::setOutput(Train::LOW);
-    }
     }
 }
 
@@ -110,6 +103,7 @@ inline static void printHelp(Serial* serial)
 
 void trainDispatch(char* inBuffer, Serial* serial)
 {
+    cli();
     if(powerIsOn == false)
     {
         powerIsOn = true;
@@ -149,28 +143,50 @@ void trainDispatch(char* inBuffer, Serial* serial)
         }
         serial->putChar('\n');
     }
-    else\
+    else
     {
         uint8_t id = strtol(inBuffer, nullptr, 10);
         if(id < storedTrains )
         {
             char* token = strtok(NULL, " ");
-            if( strcmp(token, "speed") == 0 )
+            if( token != NULL &&  strcmp(token, "speed") == 0 )
             {
                 token = strtok(NULL, " ");
                 trains[id].setSpeed(atoi(token));
+                serial->write_p(PSTR("Set Train speed\n"));
             }
-            else if( strcmp(token, "function") == 0 ) 
+            else if(token != NULL && strcmp(token, "function") == 0 ) 
             {
                 token = strtok(NULL, " ");
+                if(token != NULL)
+                {
+                    for(uint16_t j = 0; j < 8; 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].sendFunction(atoi(token));
+                serial->write_p(PSTR("Set Train function\n"));*/
             }
-            else if( strcmp(token, "reverse") == 0 ) trains[id].reverse();
-            else if( strcmp(token, "stop") )trains[id].setSpeed(0);
+            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 serial->write_p(PSTR("Not a valid command\n"));
         }
         else serial->write_p(PSTR("Id out of range.\n"));
     }
+    sei();
 }
 
 void powerDispatch(char* token, Serial* serial)
@@ -195,13 +211,13 @@ void powerDispatch(char* token, Serial* serial)
     else if(strcmp(token, "auto") == 0)
     {
         token = strtok(NULL, " ");
-        if(strcmp(token, "on") == 0)
+        if(token != NULL && strcmp(token, "on") == 0)
         {
             autoff = true;
             serial->write_p(PSTR("auto power off turned on.\n"));
             save_state();
         }
-        else if(strcmp(token, "off") == 0)
+        else if(token != NULL && strcmp(token, "off") == 0)
         {
             autoff = false;
             serial->write_p(PSTR("auto power off turned off.\n"));
@@ -232,7 +248,7 @@ void serialDispatch(Serial* serial)
             if(length > 4 && strcmp(token, "train") == 0)
             {
                 token = strtok(NULL, " ");
-                trainDispatch(token, serial);
+                if(token != NULL)trainDispatch(token, serial);
             }
             else if(length > 4 && strncmp(token, "erase", 4) == 0)
             {
@@ -253,13 +269,15 @@ void serialDispatch(Serial* serial)
             {
                 for(uint16_t i = 0; i < storedTrains; i++)
                 {
+                    cli();
                     trains[i].setSpeed(0);
+                    sei();
                 }
             }
             else if(length > 3 && strcmp(token, "power") == 0)
             {
                 token = strtok(NULL, " ");
-                powerDispatch(token, serial);
+                if(token != NULL)powerDispatch(token, serial);
             }
             else if(length > 3 && strcmp(token, "help") == 0)
             {
@@ -282,7 +300,7 @@ int main()
     TIMSK0 = 0b00000001; //turn on timer0 interupt on OCIEB
     TCCR0B = (1<<CS02) /*| (1<<CS00)*/; // run timer0 with /1024 scaler
     
-    DDRD =  (1 << PD2) | (1 << PD3);
+    DDRD =  (1 << PD2) | (1 << PD3) | (1 << PD4) | (1 << PD5);
     
     restore_state();
     
@@ -294,7 +312,7 @@ int main()
 
     serial.write_p(PSTR("TrainController v0.1 starting\n"));
     while(true)
-    {
+    {        
         serialDispatch(&serial);
     }
     return 0;
diff --git a/train.cpp b/train.cpp
index ab2cada..d372d5a 100644
--- a/train.cpp
+++ b/train.cpp
@@ -8,8 +8,6 @@ Train::Train(const uint8_t address):  _address(address)
 
 Train::Train()
 {
-    writePin(_port, _pinHigh, false);
-    writePin(_port, _pinLow, true);
     _address = 0;
 }
 
@@ -23,24 +21,33 @@ uint8_t Train::getAddress()
     return _address;
 }
 
+void Train::off()
+{
+    writePin(_port, _pinHighA, false);
+    writePin(_port, _pinHighB, false);
+    writePin(_port, _pinLowA,  true);
+    writePin(_port, _pinLowB,  true);
+}
+
 void Train::setOutput(const uint8_t state)
 {
     if(state == HIGH)
     {
-        writePin(_port, _pinLow, true);
+        off();
         _delay_us(5);
-        writePin(_port, _pinHigh, true);
+        writePin(_port, _pinHighA, true);
+        writePin(_port, _pinLowB,  false);
     }
     else if (state == LOW)
     {
-        writePin(_port, _pinHigh, false);
+        off();
         _delay_us(5);
-        writePin(_port, _pinLow, false);
+        writePin(_port, _pinHighB, true);
+        writePin(_port, _pinLowA,  false);
     }
     else
     {
-        writePin(_port, _pinHigh, false);
-        writePin(_port, _pinLow, true);
+        off();
     }
 }
 
@@ -70,7 +77,7 @@ void Train::sendAddress()
     }
 }
 
-void Train::sendData(const uint8_t data)
+void Train::sendRaw(const uint8_t data)
 {
     for(uint8_t j = 0; j < SEND_COUNT; j++)
     {
@@ -78,7 +85,6 @@ void Train::sendData(const uint8_t data)
         for(uint8_t i = 0; i < 5; i++)
         {
             sendBit(data & (1 << i));
-            sendBit(data & (1 << i));
         }
         _delay_ms(2);
     }
@@ -90,14 +96,14 @@ void Train::setSpeed(uint8_t speed)
     if(speed <= 15)
     {
         lastSpeed = speed;
-        sendData(speed << 1);
+        sendData((speed << 1));
     }
 }
 
 void Train::resendSpeed()
 {
     uint8_t data = lastSpeed;
-    sendData(data << 1);
+    sendData((data << 1));
 }
 
 uint8_t Train::getSpeed()
@@ -107,10 +113,10 @@ uint8_t Train::getSpeed()
 
 void Train::reverse()
 {
-    sendData(1 << 1);
+    sendData((1 << 1));
 }
 
-void Train::sendFunction(const uint8_t function)
+void Train::sendFunction(const uint16_t function)
 {
-    sendData((function << 1) | 1);
+    sendDataUndoubled(function);
 }
diff --git a/train.h b/train.h
index 07ed7b1..7338559 100644
--- a/train.h
+++ b/train.h
@@ -7,26 +7,34 @@
 
 class Train
 {
-private:
-    uint8_t _address;
-    
-    static const unsigned char _pinHigh = PD3;
-    static const unsigned char _pinLow = PD2;
-    
-    static const uint8_t SEND_COUNT = 4;
-
-    uint8_t lastSpeed = 0;
-    
-    void sendBit(const bool bit);
-    void sendAddress();
-    void sendData(const uint8_t data);
-    
 public:
+    static const uint8_t M_DELTA = 0;
+    static const uint8_t M_DIGITAL = 1;
     
     static const uint8_t HIGH = 2;
     static const uint8_t LOW = 1;
     static const uint8_t OFF = 0;
     
+private:
+    uint8_t _address;
+    
+    static const unsigned char _pinHighA = PD5;
+    static const unsigned char _pinLowA = PD4;
+    static const unsigned char _pinHighB = PD2;
+    static const unsigned char _pinLowB = PD3;
+    
+    static const uint8_t SEND_COUNT = 4;
+    
+    uint8_t _protocol = M_DIGITAL;
+
+    uint16_t lastdatapacket = 0;
+    
+    inline static void off();
+    void sendBit(const bool bit);
+    void sendAddress();
+    
+public:
+    
     static void setOutput(const uint8_t state);
     
     Train(const uint8_t address);
@@ -34,16 +42,22 @@ public:
     
     void resendSpeed();
     
-    void setSpeed(uint8_t speed);
-    
     void reverse();
     
     uint8_t getAddress();
     
     uint8_t getSpeed();
     
+    uint8_t getProtocol();
+    
+    void setSpeed(uint8_t speed);
+    
+    void setProtocol(const uint8_t protocol);
+    
     void setAddress(const uint8_t address);
     
-    void sendFunction(const uint8_t function);
+    void sendFunction(const uint16_t function);
+    
+    void sendRaw(const uint16_t data);
     
 };