bool Ant::goTo(Pos dest) {
Path newPath = PathFinder::findPath(this->pos(), dest);
if (newPath.isValid()) {
// Win - there is a path
m_path = newPath;
LOG_ANT(this, "goTo " << dest);
m_state = STATE_GOING_TO_POS;
Square& s = g_map->square(dest);
if (s.destinyAnt) {
// Some-one else was header here.
if (s.destinyAnt->state() == STATE_GOING_TO_FOOD)
s.destinyAnt->stop(); // It should do something else.
}
s.destinyAnt = this;
return true;
} else {
// Fail
LOG_ANT(this, "goTo failed");
//stop();
return false;
}
}
void Ant::updateState() {
LOG_ANT(this, "Ant::updateState");
if (!m_path.isValid()) {
LOG_ANT(this, "Invalid path, stopping");
stop();
}
if (m_state&STATE_GOING_TO_POS) {
if (pos() == m_path.dest()) {
LOG_DEBUG("At destination");
stop();
}
}
if (m_state==STATE_GOING_TO_HILL && !g_map->square(m_path.dest()).hillAlive)
stop(); // Hill dead.
if (m_state==STATE_GOING_TO_FOOD) {
Square& s = g_map->square(m_path.dest());
if (s.visible() && !s.isFood) {
LOG_DEBUG("FOOD GONE!");
stop();
}
}
if (m_state==STATE_GOING_TO_ROOM) {
if (g_map->roomAt(pos()) == g_map->roomAt(m_path.dest())) {
LOG_ANT(this, "Arrived to room");
m_state = STATE_NONE;
}
}
}
bool Ant::goToRoom(Room* room) {
stop();
if (room == g_map->roomAt(pos()))
LOG_ANT(this, "Asked to go to room it is in");
LOG_ANT(this, "goToRoom " << room);
if (goTo(room->centerPos())) {
LOG_ANT(this, "Going to room");
m_state = STATE_GOING_TO_ROOM;
return true;
}
return false;
}
uint32_t ant_hrm_init(ant_hrm_profile_t * p_profile, ant_channel_config_t const * p_channel_config,
ant_hrm_tx_config_t const * p_tx_config)
{
ASSERT(p_profile != NULL);
ASSERT(p_channel_config != NULL);
p_profile->channel_number = p_channel_config->channel_number;
if (p_tx_config != NULL)
{
p_profile->p_cb = p_tx_config->p_cb_buffer;
ant_hrm_tx_cb_t * p_hrm_cb = (ant_hrm_tx_cb_t *)p_profile->p_cb;
p_hrm_cb->page_1_present = p_tx_config->page_1_present;
ASSERT((p_tx_config->main_page_number == ANT_HRM_PAGE_0) || (p_tx_config->main_page_number == ANT_HRM_PAGE_4));
p_hrm_cb->main_page_number = p_tx_config->main_page_number;
p_hrm_cb->ext_page_number = p_hrm_cb->page_1_present ? ANT_HRM_PAGE_1 : ANT_HRM_PAGE_2;
p_hrm_cb->message_counter = 0;
p_hrm_cb->toggle_bit = true;
}
p_profile->page_0 = (ant_hrm_page0_data_t)DEFAULT_ANT_HRM_PAGE0();
p_profile->page_1 = (ant_hrm_page1_data_t)DEFAULT_ANT_HRM_PAGE1();
p_profile->page_2 = (ant_hrm_page2_data_t)DEFAULT_ANT_HRM_PAGE2();
p_profile->page_3 = (ant_hrm_page3_data_t)DEFAULT_ANT_HRM_PAGE3();
p_profile->page_4 = (ant_hrm_page4_data_t)DEFAULT_ANT_HRM_PAGE4();
LOG_ANT("ANT HRM channel %u init\n\r", p_profile->channel_number);
return ant_channel_init(p_channel_config);
}
void Ant::calcDesire() {
LOG_ANT(this, "calcDesire");
m_desire.clear();
if (m_state == STATE_NONE) {
// TODO: priorities.
//m_desire.push_back( g_map->getLocation(pos(), rand()%4) );
} else {
m_desire = m_path.getNextStep(pos());
if (m_desire.size()==0 || (m_desire.size()==1 && m_desire[0]==pos())) {
LOG_ANT(this, "Resetting state - at target?");
stop();
}
}
}
bool Ant::goToFoodAt(Pos dest) {
LOG_ANT(this, "goToFoodAt " << dest);
if (goTo(dest)) {
// Win
m_state = STATE_GOING_TO_FOOD;
return true;
}
return false;
}
/**@brief Function for encoding HRM message.
*
* @note Assume to be call each time when Rx window will occur.
*/
static void decode_hrm_rx_message(ant_hrm_profile_t * p_profile, uint8_t * p_message_payload)
{
const ant_hrm_message_layout_t * p_hrm_message_payload = (ant_hrm_message_layout_t *)p_message_payload;
LOG_ANT("HRM RX Page Number: %u\r\n", p_hrm_message_payload->page_number);
ant_hrm_page_0_decode(p_hrm_message_payload->page_payload, &(p_profile->page_0)); // Page 0 is present in each message
printf("@HRM=%u\n\r", (unsigned int)p_profile->page_0.computed_heart_rate);
switch (p_hrm_message_payload->page_number)
{
case ANT_HRM_PAGE_0:
// No implementation needed
break;
case ANT_HRM_PAGE_1:
ant_hrm_page_1_decode(p_hrm_message_payload->page_payload, &(p_profile->page_1));
break;
case ANT_HRM_PAGE_2:
ant_hrm_page_2_decode(p_hrm_message_payload->page_payload, &(p_profile->page_2));
break;
case ANT_HRM_PAGE_3:
ant_hrm_page_3_decode(p_hrm_message_payload->page_payload, &(p_profile->page_3));
break;
case ANT_HRM_PAGE_4:
ant_hrm_page_4_decode(p_hrm_message_payload->page_payload, &(p_profile->page_4));
break;
default:
break;
}
LOG_ANT("\r\n");
}
/**@brief Function for encoding HRM message.
*
* @note Assume to be call each time when Tx window will occur.
*/
static void encode_tx_message(ant_hrm_profile_t * p_profile, uint8_t * p_message_payload)
{
ant_hrm_message_layout_t * p_hrm_message_payload = (ant_hrm_message_layout_t *)p_message_payload;
ant_hrm_tx_cb_t * p_hrm_cb = (ant_hrm_tx_cb_t *)p_profile->p_cb;
ASSERT(p_hrm_cb != NULL);
if (p_hrm_cb->message_counter == (BACKGROUND_DATA_INTERVAL))
{
p_hrm_message_payload->page_number = p_hrm_cb->ext_page_number;
p_hrm_cb->message_counter = 0;
p_hrm_cb->ext_page_number++;
if (p_hrm_cb->ext_page_number > ANT_HRM_PAGE_3)
{
p_hrm_cb->ext_page_number = p_hrm_cb->page_1_present ? ANT_HRM_PAGE_1 : ANT_HRM_PAGE_2;
}
}
else
{
p_hrm_message_payload->page_number = p_hrm_cb->main_page_number;
}
if (p_hrm_cb->message_counter % TX_TOGGLE_DIVISOR == 0)
{
p_hrm_cb->toggle_bit ^= 1;
}
p_hrm_cb->message_counter++;
p_hrm_message_payload->toggle_bit = p_hrm_cb->toggle_bit;
LOG_ANT("HRM TX Page number: %u\r\n", p_hrm_message_payload->page_number);
ant_hrm_page_0_encode(p_hrm_message_payload->page_payload, &(p_profile->page_0)); // Page 0 is present in each message
switch (p_hrm_message_payload->page_number)
{
case ANT_HRM_PAGE_0:
// No implementation needed
break;
case ANT_HRM_PAGE_1:
ant_hrm_page_1_encode(p_hrm_message_payload->page_payload, &(p_profile->page_1));
break;
case ANT_HRM_PAGE_2:
ant_hrm_page_2_encode(p_hrm_message_payload->page_payload, &(p_profile->page_2));
break;
case ANT_HRM_PAGE_3:
ant_hrm_page_3_encode(p_hrm_message_payload->page_payload, &(p_profile->page_3));
break;
case ANT_HRM_PAGE_4:
ant_hrm_page_4_encode(p_hrm_message_payload->page_payload, &(p_profile->page_4));
break;
default:
APP_ERROR_HANDLER(NRF_ERROR_NOT_FOUND);
break;
}
LOG_ANT("\r\n");
}
uint32_t ant_hrm_open(ant_hrm_profile_t * p_profile)
{
LOG_ANT("ANT HRM channel %u open\n\r", p_profile->channel_number);
return sd_ant_channel_open(p_profile->channel_number);
}