<commit_before>/* 
 * File:   WebotsController.cpp
 * Author: <NAME>
 *
 * Created on 11. Dezember 2008, 16:06
 */

#include <cmath>


#include "WebotsController.h"
#include "CommunicationCollectionImpl.h"
#include "PlatformInterface/Platform.h"

int main() {
  WebotsController theWebotsController;
  theWebotsController.init();
  theWebotsController.main();
  return 0;
}//end main


WebotsController::WebotsController()
  :PlatformInterface("Webots", 40),
  key(0)
{
  wb_robot_init();

  theBasicTimeStep = (int)wb_robot_get_basic_time_step(); // simulation-step starts every 40ms

  get_Devices();

  cout << "Constructor finished" << endl;
}

WebotsController::~WebotsController()
{
  wb_robot_cleanup();
}

//get all devices of the robot and enable them with the basic update frequency
void WebotsController::get_Devices()
{
  
  camera = wb_robot_get_device("camera");
  wb_camera_enable(camera,theBasicTimeStep);

  cameraSelect = wb_robot_get_device("CameraSelect");
  wb_servo_enable_position(cameraSelect,theBasicTimeStep);

  for(int i=0;i<FSRData::numOfFSR;i++)
  {
    fsr[(FSRData::FSRID) i] = wb_robot_get_device(FSRData::getFSRName((FSRData::FSRID) i).c_str());
    wb_touch_sensor_enable(fsr[i],theBasicTimeStep);
  }//end for

  for(int i=0;i<JointData::numOfJoint;i++)
  {  
    joint[(JointData::JointID) i] = wb_robot_get_device(JointData::getJointName((JointData::JointID)i).c_str());
    wb_servo_enable_position(joint[i], theBasicTimeStep);
  }//end for

  accelerometer = wb_robot_get_device("accelerometer");
  wb_accelerometer_enable(accelerometer, theBasicTimeStep);

  gyrometer = wb_robot_get_device("gyro");
  wb_gyro_enable(gyrometer, theBasicTimeStep);

  gps = wb_robot_get_device("gps");
  wb_gps_enable(gps, theBasicTimeStep);

  leds[EarLeft] = wb_robot_get_device("Ears/Led/Left");
  leds[EarRight] = wb_robot_get_device("Ears/Led/Right");
  leds[FaceLeft] = wb_robot_get_device("Face/Led/Left");
  leds[FaceRight] = wb_robot_get_device("Face/Led/Right");
  leds[ChestButton] = wb_robot_get_device("ChestBoard/Led");
  leds[FootRight] = wb_robot_get_device("RFoot/Led");
  leds[FootLeft] = wb_robot_get_device("LFoot/Led");

  bumper[BumperData::LeftBumperLeft] = wb_robot_get_device("LFoot/Bumper/Left");
  bumper[BumperData::LeftBumperRight] = wb_robot_get_device("LFoot/Bumper/Right");
  bumper[BumperData::RightBumperLeft] = wb_robot_get_device("RFoot/Bumper/Left");
  bumper[BumperData::RightBumperRight] = wb_robot_get_device("RFoot/Bumper/Right");
  
  for(int i=0;i<BumperData::numOfBumper;i++)
  {
    wb_touch_sensor_enable(bumper[i], theBasicTimeStep);
  }

  wb_robot_keyboard_enable(theBasicTimeStep);

}//end get_Devices

void WebotsController::init()
{
  PlayerInfo::TeamColor teamColor = PlayerInfo::red;
  unsigned char playerNumber = 0;


  // get player-info from webots
  std::string robotName(wb_robot_get_name());

  if(robotName.substr(0,3) != "red")
    teamColor = PlayerInfo::blue;

  if(teamColor == PlayerInfo::blue)
  {
    if(robotName != "blue goal keeper")
    {
      int n = 1;
      sscanf(robotName.c_str(), "blue player %d", &n);
      playerNumber = n;
    }
  }
  else if(teamColor == PlayerInfo::red)
  {
    if(robotName != "red goal keeper")
    {
      int n = 1;
      sscanf(robotName.c_str(), "red player %d", &n);
      playerNumber = n;
    }
  }

  // calculate port
  unsigned short port = 5401;
  if(teamColor == PlayerInfo::red)
  {
    port = 5400 + playerNumber;
  }
  else if(teamColor == PlayerInfo::blue)
  {
    port = 5500 + playerNumber;
  }

  Platform::getInstance().init(this, new WebotsCommunicationCollection(port));
  Cognition::getInstance().init();
  Motion::getInstance().init();

}

void WebotsController::main()
{
  cout << "Run WebotsController" << endl;

  while(wb_robot_step(theBasicTimeStep) != -1)
  {
    //cout << "Step WebotsController" << endl;
    key = wb_robot_keyboard_get_key();
    if(key >= '0' && key <='9')
    {
      key-='0';
    }

    ((WebotsCommunicationCollection&)(*Platform::getInstance().commCollection)).receiveMessages();
    
    Cognition::getInstance().main();

    Motion::getInstance().main();
  }//end while

  cout << "Run finished" << endl;
}//end main

void WebotsController::get(FrameInfo& data)
{
  data.frameNumber++;
  data.time += theBasicTimeStep;
}

void WebotsController::get(SensorJointData& data)
{
  double dt = theBasicTimeStep / 1000.0;
  for (int i = 0; i < JointData::numOfJoint; i++) {
    double p = wb_servo_get_position(joint[i]);
    double dp = (p - data.position[i]) / dt;
    data.ddp[i] = (dp - data.dp[i]) / dt;<commit_msg>- don't use Cognition and Motion (since we don't know which callbackfunction is used by the project) - calculate port without using the PlayerInfo:TeamColor enum<commit_after>/* 
 * File:   WebotsController.cpp
 * Author: <NAME>
 *
 * Created on 11. Dezember 2008, 16:06
 */

#include <cmath>


#include "WebotsController.h"
#include "CommunicationCollectionImpl.h"
#include "PlatformInterface/Platform.h"

int main() {
  WebotsController theWebotsController;
  theWebotsController.init();
  theWebotsController.main();
  return 0;
}//end main


WebotsController::WebotsController()
  :PlatformInterface("Webots", 40),
  key(0)
{
  wb_robot_init();

  theBasicTimeStep = (int)wb_robot_get_basic_time_step(); // simulation-step starts every 40ms

  get_Devices();

  cout << "Constructor finished" << endl;
}

WebotsController::~WebotsController()
{
  wb_robot_cleanup();
}

//get all devices of the robot and enable them with the basic update frequency
void WebotsController::get_Devices()
{
  
  camera = wb_robot_get_device("camera");
  wb_camera_enable(camera,theBasicTimeStep);

  cameraSelect = wb_robot_get_device("CameraSelect");
  wb_servo_enable_position(cameraSelect,theBasicTimeStep);

  for(int i=0;i<FSRData::numOfFSR;i++)
  {
    fsr[(FSRData::FSRID) i] = wb_robot_get_device(FSRData::getFSRName((FSRData::FSRID) i).c_str());
    wb_touch_sensor_enable(fsr[i],theBasicTimeStep);
  }//end for

  for(int i=0;i<JointData::numOfJoint;i++)
  {  
    joint[(JointData::JointID) i] = wb_robot_get_device(JointData::getJointName((JointData::JointID)i).c_str());
    wb_servo_enable_position(joint[i], theBasicTimeStep);
  }//end for

  accelerometer = wb_robot_get_device("accelerometer");
  wb_accelerometer_enable(accelerometer, theBasicTimeStep);

  gyrometer = wb_robot_get_device("gyro");
  wb_gyro_enable(gyrometer, theBasicTimeStep);

  gps = wb_robot_get_device("gps");
  wb_gps_enable(gps, theBasicTimeStep);

  leds[EarLeft] = wb_robot_get_device("Ears/Led/Left");
  leds[EarRight] = wb_robot_get_device("Ears/Led/Right");
  leds[FaceLeft] = wb_robot_get_device("Face/Led/Left");
  leds[FaceRight] = wb_robot_get_device("Face/Led/Right");
  leds[ChestButton] = wb_robot_get_device("ChestBoard/Led");
  leds[FootRight] = wb_robot_get_device("RFoot/Led");
  leds[FootLeft] = wb_robot_get_device("LFoot/Led");

  bumper[BumperData::LeftBumperLeft] = wb_robot_get_device("LFoot/Bumper/Left");
  bumper[BumperData::LeftBumperRight] = wb_robot_get_device("LFoot/Bumper/Right");
  bumper[BumperData::RightBumperLeft] = wb_robot_get_device("RFoot/Bumper/Left");
  bumper[BumperData::RightBumperRight] = wb_robot_get_device("RFoot/Bumper/Right");
  
  for(int i=0;i<BumperData::numOfBumper;i++)
  {
    wb_touch_sensor_enable(bumper[i], theBasicTimeStep);
  }

  wb_robot_keyboard_enable(theBasicTimeStep);

}//end get_Devices

void WebotsController::init()
{
//  PlayerInfo::TeamColor teamColor = PlayerInfo::red;
  bool isBlue = false;
  unsigned char playerNumber = 0;


  // get player-info from webots
  std::string robotName(wb_robot_get_name());

  if(robotName.substr(0,3) = "blue")
    isBlue = true;

  if(isBlue)
  {
    if(robotName != "blue goal keeper")
    {
      int n = 1;
      sscanf(robotName.c_str(), "blue player %d", &n);
      playerNumber = n;
    }
  }
  else
  {
    if(robotName != "red goal keeper")
    {
      int n = 1;
      sscanf(robotName.c_str(), "red player %d", &n);
      playerNumber = n;
    }
  }

  // calculate port
  unsigned short port = 5401;
  if(isBlue)
  {
    port = 5500 + playerNumber;
  }
  else
  {
    port = 5400 + playerNumber;
  }

  Platform::getInstance().init(this, new WebotsCommunicationCollection(port));

//  Cognition::getInstance().init();
//  Motion::getInstance().init();

}

void WebotsController::main()
{
  cout << "Run WebotsController" << endl;

  while(wb_robot_step(theBasicTimeStep) != -1)
  {
    //cout << "Step WebotsController" << endl;
    key = wb_robot_keyboard_get_key();
    if(key >= '0' && key <='9')
    {
      key-='0';
    }

    ((WebotsCommunicationCollection&)(*Platform::getInstance().commCollection)).receiveMessages();
    
//    Cognition::getInstance().main();
//
//    Motion::getInstance().main();
  }//end while

  cout << "Run finished" << endl;
}//end main

void WebotsController::get(FrameInfo& data)
{
  data.frameNumber++;
  data.time += theBasicTimeStep;
}

void WebotsController::get(SensorJointData& data)
{
  double dt = theBasicTimeStep / 1000.0;
  for (int i = 0; i < JointData::numOfJoint; i++) {
    double p = wb_servo_get_position(joint[i]);
    double dp = (p - data.position[i]) / dt;
    data.ddp[i] = (dp - data.dp[i]) / dt;
{'fraction_non_alphanumeric': 0.14929242735227397, 'fraction_numerical': 0.00993842497569407, 'mean_word_length': 4.7431761786600495, 'pattern_counts': {'":': 0, '<': 49, '<?xml version=': 0, '>': 9, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '365579', 'n_tokens_mistral': 3597, 'n_tokens_neox': 3419, 'n_words': 547}