diff --git a/CMakeLists.txt b/CMakeLists.txt
index 1ec872e..3f6eea0 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -31,7 +31,7 @@ set(CMAKE_LINKER /usr/bin/avr-ld)
 
 # Compiler flags
 add_definitions(-mmcu=${MCU} -DF_CPU=${CPU_SPEED})
-add_definitions(-s -c -g -Os -Wall -std=c++11 )
+add_definitions(-s -c -g -Os -Wall -std=c++17 )
 add_definitions(-fno-exceptions -ffunction-sections -fdata-sections)
 
 # Linker flags
diff --git a/main.cpp b/main.cpp
index a15fc02..e130cef 100644
--- a/main.cpp
+++ b/main.cpp
@@ -28,20 +28,27 @@ volatile uint8_t tick = 0;
 
 volatile bool resendEvent = false;
 
+constexpr bool USE_PULSES = false;
+
 ISR(TIMER0_OVF_vect) 
 {
 	++tick;
 	if(tick  == 0)
 		resendEvent = true;
-	if((tick & 0b00000111) < 1)
-		Train::setOutput(Train::HIGH);
-	else Train::setOutput(Train::LOW);
+	if constexpr(USE_PULSES)
+	{
+		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;
+	if(enable)
+		TIMSK0 = 0b00000001;
+	else
+		TIMSK0 = 0b00000000;
 }
 
 void save_state()
@@ -109,11 +116,11 @@ int trainDispatch(char* inBuffer, Serial* serial)
 			if(token != NULL) 
 				trains[storedTrains].setFunctionMask(strtol(token, nullptr, 2 ));
 			
-			uint8_t size = snprintf(buffer, SNPRINTF_BUFFER_SIZE, "TRAIN saved! NUMBER: %u ADDRESS: %u FUNCTIONS: %u FUNCTIONMASK: %u\n", 
+			uint8_t size = snprintf(buffer, SNPRINTF_BUFFER_SIZE, "TRAIN saved! NUMBER: %u ADDRESS: %u FUNCTIONS: %s FUNCTIONMASK: %s\n", 
 									storedTrains,
 									address,
-									trains[storedTrains].getFunctions(),
-									trains[storedTrains].getFunctionMask());
+									bit_rep[trains[storedTrains].getFunctions() & 0x0F],
+									bit_rep[trains[storedTrains].getFunctionMask() & 0x0F]);
 			serial->write(buffer, size);
 			
 			storedTrains++;
@@ -138,10 +145,10 @@ int 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: %u CURRENT PACKET: %x SPEED: %i FUNCTIONS: %u FUNCTIONMASK: %u\n", 
+			snprintf(buffer, SNPRINTF_BUFFER_SIZE, "NUMBER: %u ID: %u CURRENT PACKET: %x SPEED: %i FUNCTIONS: %s FUNCTIONMASK: %s\n", 
 						i, trains[i].getAddress(),
 						trains[i].getLastPacket(), trains[i].getSpeed(),
-						trains[i].getFunctions(), trains[i].getFunctionMask());
+						bit_rep[trains[i].getFunctions() & 0x0F], bit_rep[trains[i].getFunctionMask() & 0x0F]);
 			serial->write(buffer, SNPRINTF_BUFFER_SIZE);
 		}
 		serial->putChar('\n');
@@ -226,7 +233,7 @@ int trainDispatch(char* inBuffer, Serial* serial)
 					uint16_t i = strtol(token, nullptr, 16 );
 					snprintf(buffer, SNPRINTF_BUFFER_SIZE, "SENDING: %x to %x\n", i, trains[id].getAddress());
 					serial->write(buffer, strlen(buffer));
-					for(uint8_t j = 0; j < 100; j++)
+					//for(uint8_t j = 0; j < 100; j++)
 					{
 						trains[id].sendRaw(i);
 						_delay_ms(20);
@@ -392,7 +399,7 @@ void serialDispatch(Serial* serial)
 int main() 
 {
 	TCNT0 = 0;
-	TCCR0B = (1<<CS01) /*| (1<<CS00)*/; // run timer0 with /64 scaler
+	TCCR0B = (1<<CS00); // run timer0 with /1 scaler
 
 	DDRD =  (1 << PD2) | (1 << PD3) | (1 << PD4) | (1 << PD5);
 
@@ -421,9 +428,9 @@ int main()
 	{        
 		if(resendEvent && storedTrains > 0) 
 		{
+			//_delay_ms(100);
 			timer0InterruptEnable(false);
-			_delay_us(255);
-			trains[trainToResend].resendData();
+			trains[trainToResend].sendData();
 			trainToResend++;
 			if(storedTrains <= trainToResend)
 				trainToResend = 0;
diff --git a/train.cpp b/train.cpp
index 3c6db5a..506b278 100644
--- a/train.cpp
+++ b/train.cpp
@@ -26,7 +26,7 @@ void Train::stop()
 {
     _speed = 0;
     _function = 0;
-    resendData();
+    sendData();
 }
 
 void Train::off()
@@ -95,7 +95,7 @@ void Train::sendRawAddr(const uint16_t address, const uint16_t data)
         {
             sendBit(data & (1 << i));
         }
-        _delay_ms(2);
+        _delay_ms(1);
     }
 }
 
@@ -173,7 +173,7 @@ uint16_t Train::packetAddFunction(const uint8_t function)
 void Train::setSpeed(uint8_t speed)
 {
     _speed = speed;
-    resendData();
+    sendData();
 }
 
 uint8_t Train::getSpeed()
@@ -187,14 +187,23 @@ uint16_t Train::assemblePacket()
     return packet;
 }
 
-void Train::resendData()
+void Train::sendData()
 {
     sendRaw(assemblePacket());
-    for(uint8_t i = 1; i < 4; ++i)
-    {
-        if(_functionmask & (1 << i))
-        sendRaw(packetAddSpeed() | packetAddFunction(i) | packetAddFunction(0));
-    }
+	if(_functionmask)
+	{
+		uint8_t functionToResend = (_function & 0xF0) >> 4;
+		++functionToResend;
+		if (functionToResend == 4)
+			functionToResend = 1;
+		if(_functionmask & (1 << functionToResend))
+		{
+			_delay_ms(2);
+			sendRaw(packetAddSpeed() | packetAddFunction(functionToResend) | packetAddFunction(0));
+		}
+		_function &= ~0xF0;
+		_function |= functionToResend << 4;
+	}
 }
 
 uint16_t Train::getLastPacket()
@@ -205,13 +214,13 @@ uint16_t Train::getLastPacket()
 void Train::reverse()
 {
     _direction = !_direction;
-    resendData();
+    sendData();
 }
 
 
 uint8_t Train::getFunctions()
 {
-    return _function;
+    return _function & 0x0F;
 }
     
 uint8_t Train::getFunctionMask()
@@ -224,5 +233,5 @@ void Train::sendFunction(const uint8_t function, bool enable)
     if(function > 3)
         return;
     _function = (_function & ~(1 << function)) | (enable << function);
-    resendData();
+    sendData();
 }
diff --git a/train.h b/train.h
index bf8e4f2..96876a7 100644
--- a/train.h
+++ b/train.h
@@ -43,13 +43,13 @@ public:
     Train(const uint8_t address, uint8_t functionmask = 0b1111);
     Train();
 
-    void resendData();
+    void sendData();
     
     void reverse();
     
     void stop();
     
-    bool isActive() {return _speed || _function;}
+    bool isActive() {return getSpeed() || getFunctions();}
     
     uint8_t getAddress();