send sucess message if command is processed sucessfully

train.cpp

@@ -1,8 +1,9 @@
 #include "train.h"
+#include <stdlib.h>
 
 static volatile unsigned char *_port = &PORTD;
 
-Train::Train(const uint8_t address, uint8_t protocol):  _address(address), _protocol(protocol)
+Train::Train(const uint8_t address, uint8_t functionmask):  _address(address), _functionmask(functionmask)
 {
 }
 
@@ -23,16 +24,11 @@ uint8_t Train::getAddress()
 
 void Train::stop()
 {
-    lastDataPacket = 0;
+    _speed = 0;
+    _function = 0;
     resendData();
 }
 
-bool Train::isActive()
-{
-    return lastDataPacket & 0b0000000111111111;
-}
-
-
 void Train::off()
 {
     writePin(_port, _pinHighA, false);
@@ -81,19 +77,20 @@ void Train::sendBit(const bool bit)
     }
 }
 
-void Train::sendAddress()
+void Train::sendAddress(uint8_t address)
 {
     for(uint8_t i = 0; i < 8; i++) 
     {
-        sendBit(_address & (1 << i));
+        sendBit(address & (1 << i));
     }
 }
 
-void Train::sendRaw(const uint16_t data)
+
+void Train::sendRawAddr(const uint16_t address, const uint16_t data)
 {
     for(uint8_t j = 0; j < SEND_COUNT; j++)
     {
-        sendAddress();
+        sendAddress(address);
         for(uint8_t i = 0; i < 10; i++)
         {
             sendBit(data & (1 << i));
@@ -102,97 +99,130 @@ void Train::sendRaw(const uint16_t data)
     }
 }
 
-void Train::setSpeed(uint8_t speed)
+void Train::sendRaw(const uint16_t data)
 {
-    if(speed != 0)speed = speed + 1;
-    else if(speed > 15) speed = 15;
+    sendRawAddr(_address, data);
+}
+
+uint16_t Train::packetAddSpeed()
+{
+    uint16_t packet = 0;
+    if(_speed > 14)
+        _speed = 14;
     for(uint8_t i = 0; i < 4; i++)
     {
-                                 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));
+        packet |= ((bool)((_speed+1) & (1 << i))) << (i*2+2);
     }
+    return packet;
+}
+
+uint16_t Train::packetAddDirection()
+{
+    uint16_t packet = 0;
+
+    uint8_t data = 0;
+    if(!_direction)
+    {
+        if(_speed > 6)
+            data = 0b0100;
+        else if(_speed > 0)
+            data = 0b0101;
+    }
+    else
+    {
+        if(_speed > 0)
+            data = 0b1011;
+        else if (_speed > 6)
+            data = 0b1010;
+    }
+
+    if(_speed == 0)
+        data = !_direction ?  0b0101 : 0b1011;
+
+    for(uint8_t i = 0; i < 4; i++)
+    {
+        uint8_t bit = (data & (1 << (3-i))) >> (3-i);
+        packet |= bit << ((i*2)+3);
+    }
+    return packet;
+}
+
+uint16_t Train::packetAddFunction(const uint8_t function)
+{
+    uint16_t packet = 0;
+    bool enabled = _function & (1 << function);
+    if(function == 0)
+    {
+        packet |= enabled ? 0b00000011 : 0b00000000;
+    }
+    else
+    {
+        if(enabled)
+            packet |=  0b1000000000;
+        for(uint8_t i = 0; i < 4; ++i )
+        {
+            if(function > i)
+              packet = packet |  (1 << (7-i*2));
+            else
+              packet = packet & ~(1 << (7-i*2));
+        }
+    }
+    return packet;
+}
+
+void Train::setSpeed(uint8_t speed)
+{
+    _speed = 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;
+    return _speed;
+}
+
+uint16_t Train::assemblePacket()
+{
+    uint16_t packet = packetAddSpeed() | packetAddFunction(0) | packetAddDirection();
+    return packet;
 }
 
 void Train::resendData()
 {
-    sendRaw(lastDataPacket);
-    if(functionTimer > 0)
+    sendRaw(assemblePacket());
+    for(uint8_t i = 1; i < 4; ++i)
     {
-        --functionTimer;
-    }
-    else if(functionTimer == 0)
-    {
-        functionClear();
-        --functionTimer;
+        if(_functionmask & (1 << i))
+        sendRaw(packetAddSpeed() | packetAddFunction(i) | packetAddFunction(0));
     }
 }
 
 uint16_t Train::getLastPacket()
 {
-    return lastDataPacket;
+    return assemblePacket();
 }
 
 void Train::reverse()
 {
-    functionClear();
+    _direction = !_direction;
     resendData();
-    resendData();
-    if(getSpeed() > 0)
-    {
-        setSpeed(0);
-        resendData();
-        resendData();
-        _delay_ms(2000);
-    }
-    sendRaw(0b000001100);
-    sendRaw(0b000001100);
 }
 
-void Train::functionClear()
+
+uint8_t Train::getFunctions()
 {
-	lastDataPacket = lastDataPacket & ~(0b10101010);
+    return _function;
+}
+    
+uint8_t Train::getFunctionMask()
+{
+    return _functionmask;
 }
 
 void Train::sendFunction(const uint8_t function, bool enable)
 {
-    if(function == 0)
-    {
-        lastDataPacket = (lastDataPacket & ~0b00000001) | (enable ? 0b00000001 : 0b00000000);
-        //lastDataPacket = (lastDataPacket & ~0b1100000000) | (enable ? 0b1100000000 : 0);
-    }
-    else if(_protocol == M_DIGITAL && function <= 3)
-    {
-        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));
-        }
-    }
+    if(function > 3)
+        return;
+    _function = (_function & ~(1 << function)) | (enable << function);
     resendData();
-    functionTimer = 10;
-}
-
-void Train::setProtocol(const uint8_t protocol)
-{
-   _protocol = protocol;
-}
-
-uint8_t Train::getProtocol()
-{
-    return _protocol;
 }