194 lines
3.1 KiB
C++
194 lines
3.1 KiB
C++
#include "train.h"
|
|
#include <stdlib.h>
|
|
#include <avr/interrupt.h>
|
|
|
|
Train::Train(const uint8_t address, uint8_t functionmask, uint8_t quirks):
|
|
Item(address), _functionmask(functionmask), _quirks(quirks)
|
|
{
|
|
}
|
|
|
|
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(int8_t speed)
|
|
{
|
|
bool direction = _direction;
|
|
|
|
if(_quirks & (1 << 1))
|
|
speed = 0-speed;
|
|
|
|
if(speed < 0)
|
|
_direction = true;
|
|
else if(speed > 0)
|
|
_direction = false;
|
|
|
|
_speed = abs(speed);
|
|
|
|
if((_quirks & 1) && direction != _direction)
|
|
{
|
|
cli();
|
|
for(uint8_t i = 0; i < 2; ++i)
|
|
{
|
|
sendRaw(0x28c);
|
|
_delay_ms(4);
|
|
}
|
|
if(!_direction)
|
|
{
|
|
for(uint8_t i = 0; i < 2; ++i)
|
|
{
|
|
sendRaw(0x224);
|
|
_delay_ms(4);
|
|
}
|
|
}
|
|
sei();
|
|
}
|
|
else if(!directSendBlock)
|
|
{
|
|
sendData();
|
|
}
|
|
}
|
|
|
|
int8_t Train::getSpeed()
|
|
{
|
|
int8_t speed = _direction ? 0-_speed : _speed;
|
|
return _quirks & (1 << 1) ? 0-speed : speed;
|
|
}
|
|
|
|
void Train::stop()
|
|
{
|
|
_speed = 0;
|
|
_function = 0;
|
|
if(!directSendBlock)
|
|
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;
|
|
if(!directSendBlock)
|
|
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);
|
|
if(!directSendBlock)
|
|
sendData();
|
|
}
|
|
|
|
uint8_t Train::getQuirks()
|
|
{
|
|
return _quirks;
|
|
}
|
|
|
|
void Train::setQuirks(uint8_t quirks)
|
|
{
|
|
_quirks = quirks;
|
|
}
|