int main(void) {
//adc_init();
i2c_init();
//initialize timer
TCCR1A = 0b00000000; // normal counter (0 to TOP = 0xFFFF)
TCCR1B = 0b00000011; // prescaler to 64
//initialize MiniURT library
initialize_baud(19200);
while(1) {
static int16_t oldCompass;
compass = cmps03_direction();
uint16_t time = TCNT1;
if(time >= MIN_VELOCITY_TIME) {
TCNT1 = 0;
angularVelocity = angleDifference(compass, oldCompass)/(((double)time)/(F_CPU/64))/10.0;
oldCompass = compass;
}
//accx = adc_read(0);
//accy = adc_read(1);
//accz = adc_read(2);
if(polled())
onPoll();
}
}
/* Message processing functions */
void purt_sd_buffer_process_message(void) {
switch(queue[0]) {
case POLL:
atomic_pop();
onPoll();
break;
case SUBSTATE:
onSubstate(&substate);
atomic_pop();
break;
case HANDSHAKE:
atomic_pop();
PURT_UART_BLOCK_SET_BYTE(4);
PURT_UART_BLOCK_SET_BYTE(0xFF);
PURT_UART_BLOCK_SET_BYTE(APPLICATION_TYPE);
PURT_UART_BLOCK_SET_BYTE(UID);
PURT_UART_BLOCK_SET_BYTE(~4);
onHandshake();
break;
case NONE:
break;
}
}
void Serializer::onMessageReceived(const sp<AMessage> &msg) {
switch (msg->what()) {
case kWhatAddSource:
{
ssize_t index = onAddSource(msg);
sp<AMessage> response = new AMessage;
if (index < 0) {
response->setInt32("err", index);
} else {
response->setSize("index", index);
}
uint32_t replyID;
CHECK(msg->senderAwaitsResponse(&replyID));
response->postReply(replyID);
break;
}
case kWhatStart:
case kWhatStop:
{
status_t err = (msg->what() == kWhatStart) ? onStart() : onStop();
sp<AMessage> response = new AMessage;
response->setInt32("err", err);
uint32_t replyID;
CHECK(msg->senderAwaitsResponse(&replyID));
response->postReply(replyID);
break;
}
case kWhatPoll:
{
int32_t generation;
CHECK(msg->findInt32("generation", &generation));
if (generation != mPollGeneration) {
break;
}
int64_t delayUs = onPoll();
if (delayUs >= 0ll) {
schedulePoll(delayUs);
}
break;
}
default:
TRESPASS();
}
}
bool Acceptor::poll( double timeout ) throw ( ConfigError, RuntimeError )
{
if( m_firstPoll )
{
m_stop = false;
onConfigure( m_settings );
onInitialize( m_settings );
m_firstPoll = false;
}
return onPoll( timeout );
}
Delegator::Delegator(zmq::context_t& context, const DelegatorSettings& settings, bool& running)
: context_(context),
requester_(context, ZMQ_ROUTER, "toRequester"),
heartbeat_(context, ZMQ_PAIR, "toHBRouter"),
network_(context, ZMQ_ROUTER, "toNetwork"),
router_("main", {&requester_, &heartbeat_, &network_}),
msPollRate_(settings.msPollRate),
hbSettings_(settings.heartbeat),
running_(running),
nextJobId_(0),
nJobTypes_(settings.nJobTypes)
{
// Initialise the local sockets
requester_.bind(DELEGATOR_SOCKET_ADDR);
heartbeat_.bind(SERVER_HB_SOCKET_ADDR);
network_.setFallback([&](const Message& m) { sendFailed(m); });
std::string address = "tcp://*:" + std::to_string(settings.port);
network_.bind(address);
LOG(INFO) << "Delegator listening on tcp://*:" + std::to_string(settings.port);
// Specify the Delegator functionality
auto fDisconnect = [&](const Message& m) { disconnectWorker(m); };
auto fNewWorker = [&](const Message &m)
{
//forwarding to HEARTBEAT
heartbeat_.send(m);
std::string worker = m.address.front();
if (workers_.count(worker) == 0)
connectWorker(m);
};
auto fRcvRequest = [&](const Message &m) { receiveRequest(m); };
auto fRcvResult = [&](const Message &m) { receiveResult(m); };
auto fForwardToHB = [&](const Message& m) { heartbeat_.send(m); };
auto fForwardToNetwork = [&](const Message& m) { network_.send(m); };
auto fForwardToHBAndDisconnect = [&](const Message& m)
{ fForwardToHB(m); fDisconnect(m); };
// Bind functionality to the router
const uint REQUESTER_SOCKET = 0, HB_SOCKET = 1, NETWORK_SOCKET = 2;
router_.bind(REQUESTER_SOCKET, REQUEST, fRcvRequest);
router_.bind(NETWORK_SOCKET, HELLO, fNewWorker);
router_.bind(NETWORK_SOCKET, RESULT, fRcvResult);
router_.bind(NETWORK_SOCKET, HEARTBEAT, fForwardToHB);
router_.bind(NETWORK_SOCKET, GOODBYE, fForwardToHBAndDisconnect);
router_.bind(HB_SOCKET, HEARTBEAT, fForwardToNetwork);
router_.bind(HB_SOCKET, GOODBYE, fDisconnect);
auto fOnPoll = [&] () {onPoll();};
router_.bindOnPoll(fOnPoll);
}