Example #1
0
  /**
   * 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;
    }
  }
Example #2
0
  /** 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());
    }
  }