/**
* Sets the LEDs whenever the state they visualize changes.
* Regularily sends the return packet to the GameController.
*/
void handleOutput()
{
unsigned now = (unsigned) proxy->getTime(0);
if(teamNumber && *playerNumber &&
*playerNumber <= gameCtrlData.playersPerTeam &&
(gameCtrlData.teams[0].teamNumber == teamNumber ||
gameCtrlData.teams[1].teamNumber == teamNumber))
{
const TeamInfo& team = gameCtrlData.teams[gameCtrlData.teams[0].teamNumber == teamNumber ? 0 : 1];
if(gameCtrlData.state != previousState ||
gameCtrlData.gamePhase != previousGamePhase ||
gameCtrlData.kickingTeam != previousKickingTeam ||
team.teamColour != previousTeamColour ||
team.players[*playerNumber - 1].penalty != previousPenalty)
{
switch(team.teamColour)
{
case TEAM_BLUE:
setLED(leftFootRed, 0.f, 0.f, 1.f);
break;
case TEAM_RED:
setLED(leftFootRed, 1.f, 0.f, 0.f);
break;
case TEAM_YELLOW:
setLED(leftFootRed, 1.f, 1.f, 0.f);
break;
case TEAM_WHITE:
setLED(leftFootRed, 1.f, 1.f, 1.f);
break;
case TEAM_GREEN:
setLED(leftFootRed, 0.f, 1.f, 0.f);
break;
case TEAM_ORANGE:
setLED(leftFootRed, 1.f, 0.5f, 0.f);
break;
case TEAM_PURPLE:
setLED(leftFootRed, 1.f, 0.f, 1.f);
break;
case TEAM_BROWN:
setLED(leftFootRed, 0.2f, 0.1f, 0.f);
break;
case TEAM_GRAY:
setLED(leftFootRed, 0.2f, 0.2f, 0.2f);
break;
case TEAM_BLACK:
default:
setLED(leftFootRed, 0.f, 0.f, 0.f);
}
if(gameCtrlData.state == STATE_INITIAL &&
gameCtrlData.gamePhase == GAME_PHASE_PENALTYSHOOT &&
gameCtrlData.kickingTeam == team.teamNumber)
setLED(rightFootRed, 0.f, 1.f, 0.f);
else if(gameCtrlData.state == STATE_INITIAL &&
gameCtrlData.gamePhase == GAME_PHASE_PENALTYSHOOT &&
gameCtrlData.kickingTeam != team.teamNumber)
setLED(rightFootRed, 1.f, 1.0f, 0.f);
else if(now - whenPacketWasReceived < GAMECONTROLLER_TIMEOUT &&
gameCtrlData.state <= STATE_SET &&
gameCtrlData.kickingTeam == team.teamNumber)
setLED(rightFootRed, 1.f, 1.f, 1.f);
else
setLED(rightFootRed, 0.f, 0.f, 0.f);
if(team.players[*playerNumber - 1].penalty != PENALTY_NONE)
setLED(chestRed, 1.f, 0.f, 0.f);
else
switch(gameCtrlData.state)
{
case STATE_READY:
setLED(chestRed, 0.f, 0.f, 1.f);
break;
case STATE_SET:
setLED(chestRed, 1.f, 0.4f, 0.f);
break;
case STATE_PLAYING:
setLED(chestRed, 0.f, 1.f, 0.f);
break;
default:
setLED(chestRed, 0.f, 0.f, 0.f);
}
ledRequest[4][0] = (int) now;
proxy->setAlias(ledRequest);
previousState = gameCtrlData.state;
previousGamePhase = gameCtrlData.gamePhase;
previousKickingTeam = gameCtrlData.kickingTeam;
previousTeamColour = team.teamColour;
previousPenalty = team.players[*playerNumber - 1].penalty;
}
if(now - whenPacketWasReceived < GAMECONTROLLER_TIMEOUT &&
now - whenPacketWasSent >= ALIVE_DELAY &&
send(GAMECONTROLLER_RETURN_MSG_ALIVE))
whenPacketWasSent = now;
}
}
/** The method sets all actuators. */
void setActuators()
{
// set all actuator values according to the values in the shared memory block
try
{
dcmTime = proxy->getTime(0);
data->readingActuators = data->newestActuators;
if(data->readingActuators == lastReadingActuators)
{
if(actuatorDrops == 0)
fprintf(stderr, "libbhuman: missed actuator request.\n");
++actuatorDrops;
}
else
actuatorDrops = 0;
lastReadingActuators = data->readingActuators;
float* readingActuators = data->actuators[data->readingActuators];
float* actuators = handleState(readingActuators);
if(state != standing)
{
if(frameDrops > 0 || state == shuttingDown)
setEyeLeds(actuators);
else
copyNonServos(readingActuators, actuators);
}
setBatteryLeds(actuators);
// set position actuators
positionRequest[4][0] = dcmTime; // 0 delay!
for(int i = 0; i < lbhNumOfPositionActuatorIds; ++i)
positionRequest[5][i][0] = actuators[i];
proxy->setAlias(positionRequest);
// set hardness actuators
bool requestedHardness = false;
for(int i = headYawHardnessActuator; i < headYawHardnessActuator + lbhNumOfHardnessActuatorIds; ++i)
if(actuators[i] != requestedActuators[i])
{
hardnessRequest[4][0] = dcmTime; // 0 delay!
for(int j = 0; j < lbhNumOfHardnessActuatorIds; ++j)
hardnessRequest[5][j][0] = requestedActuators[headYawHardnessActuator + j] = actuators[headYawHardnessActuator + j];
proxy->setAlias(hardnessRequest);
requestedHardness = true;
break;
}
// set us actuator
bool requestedUs = false;
if(requestedActuators[usActuator] != actuators[usActuator])
{
requestedActuators[usActuator] = actuators[usActuator];
if(actuators[usActuator] >= 0.f)
{
resetUsMeasurements();
usRequest[4][0] = dcmTime;
usRequest[5][0][0] = actuators[usActuator];
proxy->setAlias(usRequest);
requestedUs = true;
}
}
// set led
if(!requestedHardness && !requestedUs)
for(int i = 0; i < lbhNumOfLedActuatorIds; ++i)
{
int index = faceLedRedLeft0DegActuator + ledIndex;
if(++ledIndex == lbhNumOfLedActuatorIds)
ledIndex = 0;
if(actuators[index] != requestedActuators[index])
{
ledRequest[0] = std::string(actuatorNames[index]);
ledRequest[2][0][0] = requestedActuators[index] = actuators[index];
ledRequest[2][0][1] = dcmTime;
proxy->set(ledRequest);
break;
}
}
// set team info
// since this should very rarely, we don't use a proxy here
if(data->bhumanStartTime != lastBHumanStartTime)
{
for(int i = 0; i < lbhNumOfTeamInfoIds; ++i)
memory->insertData(teamInfoNames[i], data->teamInfo[i]);
lastBHumanStartTime = data->bhumanStartTime;
}
}
catch(AL::ALError& e)
{
fprintf(stderr, "libbhuman: %s\n", e.toString().c_str());
}
}