void GameStateModule::advanceState()
{
switch (latest_data.state())
{
case STATE_INITIAL:
latest_data.set_state(STATE_READY);
break;
case STATE_READY:
latest_data.set_state(STATE_SET);
break;
case STATE_SET:
latest_data.set_state(STATE_PLAYING);
keep_time = true;
start_time = realtime_micro_time();
break;
case STATE_PLAYING:
manual_penalize();
break;
}
}
void GameStateModule::update()
{
// Check comm input last so we can reset button toggles.
if (buttonPressInput.message().toggle() != last_button)
{
last_button = !last_button;
if (!commInput.message().have_remote_gc()
|| latest_data.state() == STATE_PLAYING)
{
advanceState();
}
}
if (initialStateInput.message().toggle() != last_initial)
{
last_initial = !last_initial;
if (!commInput.message().have_remote_gc())
reset();
}
if (switchTeamInput.message().toggle() != last_team)
{
last_team = !last_team;
if (!commInput.message().have_remote_gc()
&& latest_data.state() == STATE_INITIAL)
switchTeam();
}
if (switchKickOffInput.message().toggle() != last_kickoff)
{
last_kickoff = !last_kickoff;
if (!commInput.message().have_remote_gc()
&& latest_data.state() == STATE_INITIAL)
switchKickOff();
}
if (commInput.message().have_remote_gc())
{
latest_data = commInput.message();
if (latest_data.state() != STATE_PLAYING)
{
keep_time = false;
start_time = 0;
}
else
{
keep_time = true;
if (!start_time)
{
start_time = realtime_micro_time();
}
}
// Did GC get our message yet??
for (int i = 0; i < latest_data.team_size(); ++i)
{
messages::TeamInfo* team = latest_data.mutable_team(i);
if (team->team_number() == team_number)
{
messages::RobotInfo* player = team->mutable_player(player_number-1);
if (response_status == GAMECONTROLLER_RETURN_MSG_MAN_PENALISE)
{
if(player->penalty())
{
response_status = GAMECONTROLLER_RETURN_MSG_ALIVE;
}
}
else if (response_status == GAMECONTROLLER_RETURN_MSG_MAN_UNPENALISE)
{
if(!player->penalty())
{
response_status == GAMECONTROLLER_RETURN_MSG_ALIVE;
}
}
}
}
}
if (keep_time && commInput.message().have_remote_gc())
{
long long diff_time = realtime_micro_time() - start_time;
latest_data.set_secs_remaining(600 -
static_cast<unsigned int>(diff_time/MICROS_PER_SECOND));
//TODO keep track of penalty times
}
}
// Overrides the RoboGram::run() method to do timing as well
void DiagramThread::RobotDiagram::run()
{
// Start timer
const long long startTime = realtime_micro_time();
if (name == "cognition")
{
PROF_ENTER(P_COGNITION_THREAD);
}
else if (name == "sensors")
{
PROF_ENTER(P_MOTION_THREAD);
}
else if (name == "comm")
{
PROF_ENTER(P_COMM_THREAD);
}
else if (name == "guardian")
{
PROF_ENTER(P_GUARDIAN_THREAD);
}
RoboGram::run();
if (name == "cognition")
{
PROF_EXIT(P_COGNITION_THREAD);
// Count cognition frames
PROF_NFRAME();
}
else if (name == "sensors")
{
PROF_EXIT(P_MOTION_THREAD);
}
else if (name == "comm")
{
PROF_EXIT(P_COMM_THREAD);
}
else if (name == "guardian")
{
PROF_EXIT(P_GUARDIAN_THREAD);
}
// Stop timer
const long long processTime = realtime_micro_time() - startTime;
// If we're under the frame length, this is good.
// We can sleep for the rest of the frame and let others process.
if (processTime < frameLengthMicro)
{
// Compute the time we should sleep from the amount of time
// we processed this frame and the amount of time allotted to a frame
const long int microSleepTime =
static_cast<long int>(frameLengthMicro - processTime);
const long int nanoSleepTime =
static_cast<long int>((microSleepTime %(1000 * 1000)) * 1000);
const long int secSleepTime =
static_cast<long int>(microSleepTime / (1000*1000));
interval.tv_sec = static_cast<time_t>(secSleepTime);
interval.tv_nsec = nanoSleepTime;
// Sleep!
nanosleep(&interval, &remainder);
}
#ifdef DEBUG_THREADS
else if (processTime > frameLengthMicro*1.5) {
std::cout<< "Warning: time spent in " << name << " thread longer"
<< " than frame length: "<< processTime << " uS" <<
std::endl;
}
#endif
}