improve it

train.cpp

@@ -114,9 +114,26 @@ void Train::setSpeed(uint8_t speed)
     resendData();
 }
 
+uint8_t Train::getSpeed()
+{
+    uint8_t speed = 0;
+    for(uint8_t i = 0; i < 4; i++)
+        speed |= ((bool)(lastDataPacket & (1 << (i+1)*2))) << i;
+    return speed;
+}
+
 void Train::resendData()
 {
     sendRaw(lastDataPacket);
+    if(functionTimer > 0)
+    {
+        --functionTimer;
+    }
+    else if(functionTimer == 0)
+    {
+        functionClear();
+        --functionTimer;
+    }
 }
 
 uint16_t Train::getLastPacket()
@@ -126,20 +143,48 @@ uint16_t Train::getLastPacket()
 
 void Train::reverse()
 {
+    functionClear();
+    resendData();
+    resendData();
+    if(getSpeed() > 0)
+    {
+        setSpeed(0);
+        resendData();
+        resendData();
+        _delay_ms(2000);
+    }
     sendRaw(0b000001100);
     sendRaw(0b000001100);
 }
 
+void Train::functionClear()
+{
+	lastDataPacket = lastDataPacket & ~(0b10101010);
+}
+
 void Train::sendFunction(const uint8_t function, bool enable)
 {
-    if(function == 0) lastDataPacket = (lastDataPacket & ~0b00000011) | (enable ? 0b00000011 : 0b00000000);
+    if(function == 0)
+    {
+        lastDataPacket = (lastDataPacket & ~0b00000001) | (enable ? 0b00000001 : 0b00000000);
+        //lastDataPacket = (lastDataPacket & ~0b1100000000) | (enable ? 0b1100000000 : 0);
+    }
     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));
+        if(enable)
+            lastDataPacket |=  0b1000000000;
+        else
+            lastDataPacket &= ~0b1000000000;
+        for(uint8_t i = 0; i < 4; ++i )
+        {
+            if(function > i)
+              lastDataPacket = lastDataPacket |  (1 << (7-i*2));
+            else
+              lastDataPacket = lastDataPacket & ~(1 << (7-i*2));
+        }
     }
     resendData();
+    functionTimer = 10;
 }
 
 void Train::setProtocol(const uint8_t protocol)