/** * 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()); } }