147 lines
2.4 KiB
C++
147 lines
2.4 KiB
C++
#include "train.h"
|
|
#include <stdlib.h>
|
|
|
|
Train::Train(const uint8_t address, uint8_t functionmask): Item(address), _functionmask(functionmask)
|
|
{
|
|
}
|
|
|
|
uint16_t Train::packetAddSpeed()
|
|
{
|
|
uint16_t packet = 0;
|
|
if(_speed > 14)
|
|
_speed = 14;
|
|
for(uint8_t i = 0; i < 4; 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;
|
|
sendData();
|
|
}
|
|
|
|
uint8_t Train::getSpeed()
|
|
{
|
|
return _speed;
|
|
}
|
|
|
|
void Train::stop()
|
|
{
|
|
_speed = 0;
|
|
_function = 0;
|
|
sendData();
|
|
}
|
|
|
|
|
|
uint16_t Train::assembleSpeedPacket()
|
|
{
|
|
uint16_t packet = packetAddSpeed() | packetAddFunction(0) | packetAddDirection();
|
|
return packet;
|
|
}
|
|
|
|
void Train::sendData()
|
|
{
|
|
sendRaw(assembleSpeedPacket());
|
|
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()
|
|
{
|
|
return assembleSpeedPacket();
|
|
}
|
|
|
|
void Train::reverse()
|
|
{
|
|
_direction = !_direction;
|
|
sendData();
|
|
}
|
|
|
|
|
|
uint8_t Train::getFunctions()
|
|
{
|
|
return _function & 0x0F;
|
|
}
|
|
|
|
uint8_t Train::getFunctionMask()
|
|
{
|
|
return _functionmask;
|
|
}
|
|
|
|
void Train::sendFunction(const uint8_t function, bool enable)
|
|
{
|
|
if(function > 3)
|
|
return;
|
|
_function = (_function & ~(1 << function)) | (enable << function);
|
|
sendData();
|
|
}
|