<commit_before>/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008-2016. All Rights Reserved.                        */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

#include "DriverStation.h"
#include "AnalogInput.h"
#include "Timer.h"
#include "NetworkCommunication/FRCComm.h"
#include "MotorSafetyHelper.h"
#include "Utility.h"
#include "WPIErrors.h"
#include <string.h>
#include "Log.hpp"

// set the logging level
TLogLevel dsLogLevel = logDEBUG;
const double JOYSTICK_UNPLUGGED_MESSAGE_INTERVAL = 1.0;

#define DS_LOG(level)     \
  if (level > dsLogLevel) \
    ;                     \
  else                    \
  Log().Get(level)

const uint32_t DriverStation::kJoystickPorts;

/**
 * DriverStation constructor.
 *
 * This is only called once the first time GetInstance() is called
 */
DriverStation::DriverStation() {
  // All joysticks should default to having zero axes, povs and buttons, so
  // uninitialized memory doesn't get sent to speed controllers.
  for (unsigned int i = 0; i < kJoystickPorts; i++) {
    m_joystickAxes[i].count = 0;
    m_joystickPOVs[i].count = 0;
    m_joystickButtons[i].count = 0;
    m_joystickDescriptor[i].isXbox = 0;
    m_joystickDescriptor[i].type = -1;
    m_joystickDescriptor[i].name[0] = '\0';
  }
  // Register that semaphore with the network communications task.
  // It will signal when new packet data is available.
  HALSetNewDataSem(&m_packetDataAvailableCond);

  AddToSingletonList();

  m_task = Task("DriverStation", &DriverStation::Run, this);
}

DriverStation::~DriverStation() {
  m_isRunning = false;
  m_task.join();

  // Unregister our semaphore.
  HALSetNewDataSem(nullptr);
}

void DriverStation::Run() {
  m_isRunning = true;
  int period = 0;
  while (m_isRunning) {
    {
      std::unique_lock<priority_mutex> lock(m_packetDataAvailableMutex);
      m_packetDataAvailableCond.wait(lock);
    }
    GetData();
    m_waitForDataCond.notify_all();

    if (++period >= 4) {
      MotorSafetyHelper::CheckMotors();
      period = 0;
    }
    if (m_userInDisabled) HALNetworkCommunicationObserveUserProgramDisabled();
    if (m_userInAutonomous)
      HALNetworkCommunicationObserveUserProgramAutonomous();
    if (m_userInTeleop) HALNetworkCommunicationObserveUserProgramTeleop();
    if (m_userInTest) HALNetworkCommunicationObserveUserProgramTest();
  }
}

/**
 * Return a pointer to the singleton DriverStation.
 * @return Pointer to the DS instance
 */
DriverStation &DriverStation::GetInstance() {
  static DriverStation *instance = new DriverStation();
  return *instance;
}

/**
 * Copy data from the DS task for the user.
 * If no new data exists, it will just be returned, otherwise
 * the data will be copied from the DS polling loop.
 */
void DriverStation::GetData() {
  // Get the status of all of the joysticks
  for (uint8_t stick = 0; stick < kJoystickPorts; stick++) {
    HALGetJoystickAxes(stick, &m_joystickAxes[stick]);
    HALGetJoystickPOVs(stick, &m_joystickPOVs[stick]);
    HALGetJoystickButtons(stick, &m_joystickButtons[stick]);
    HALGetJoystickDescriptor(stick, &m_joystickDescriptor[stick]);
  }
  m_newControlData.give();
}

/**
 * Read the battery voltage.
 *
 * @return The battery voltage in Volts.
 */
float DriverStation::GetBatteryVoltage() const {
  int32_t status = 0;
  float voltage = getVinVoltage(&status);
  wpi_setErrorWithContext(status, "getVinVoltage");

  return voltage;
}

/**
 * Reports errors related to unplugged joysticks
 * Throttles the errors so that they don't overwhelm the DS
 */
void DriverStation::ReportJoystickUnpluggedError(std::string message) {
  double currentTime = Timer::GetFPGATimestamp();
  if (currentTime > m_nextMessageTime) {
    ReportError(message);
    m_nextMessageTime = currentTime + JOYSTICK_UNPLUGGED_MESSAGE_INTERVAL;
  }
}

/**
 * Returns the number of axes on a given joystick port
 *
 * @param stick The joystick port number
 * @return The number of axes on the indicated joystick
 */
int DriverStation::GetStickAxisCount(uint32_t stick) const {
  if (stick >= kJoystickPorts) {
    wpi_setWPIError(BadJoystickIndex);
    return 0;
  }
  HALJoystickAxes joystickAxes;
  HALGetJoystickAxes(stick, &joystickAxes);
  return joystickAxes.count;
}

/**
 * Returns the name of the joystick at the given port
 *
 * @param stick The joystick port number
 * @return The name of the joystick at the given port
 */
std::string DriverStation::GetJoystickName(uint32_t stick) const {
  if (stick >= kJoystickPorts) {
    wpi_setWPIError(BadJoystickIndex);
  }
  std::string retVal(m_joystickDescriptor[0].name);
  return retVal;
}

/**
 * Returns the type of joystick at a given port
 *
 * @param stick The joystick port number
 * @return The HID type of joystick at the given port
 */
int DriverStation::GetJoystickType(uint32_t stick) const {
  if (stick >= kJoystickPorts) {
    wpi_setWPIError(BadJoystickIndex);
    return -1;
  }
  return (int)m_joystickDescriptor[stick].type;
}

/**
 * Returns a boolean indicating if the controller is an xbox controller.
 *
 * @param stick The joystick port number
 * @return A boolean that is true if the controller is an xbox controller.
 */
bool DriverStation::GetJoystickIsXbox(uint32_t stick) const {
  if (stick >= kJoystickPorts) {
    wpi_setWPIError(BadJoystickIndex);<commit_msg>DriverStation::GetJoystickName(): Make work for stick>0.<commit_after>/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008-2016. All Rights Reserved.                        */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

#include "DriverStation.h"
#include "AnalogInput.h"
#include "Timer.h"
#include "NetworkCommunication/FRCComm.h"
#include "MotorSafetyHelper.h"
#include "Utility.h"
#include "WPIErrors.h"
#include <string.h>
#include "Log.hpp"

// set the logging level
TLogLevel dsLogLevel = logDEBUG;
const double JOYSTICK_UNPLUGGED_MESSAGE_INTERVAL = 1.0;

#define DS_LOG(level)     \
  if (level > dsLogLevel) \
    ;                     \
  else                    \
  Log().Get(level)

const uint32_t DriverStation::kJoystickPorts;

/**
 * DriverStation constructor.
 *
 * This is only called once the first time GetInstance() is called
 */
DriverStation::DriverStation() {
  // All joysticks should default to having zero axes, povs and buttons, so
  // uninitialized memory doesn't get sent to speed controllers.
  for (unsigned int i = 0; i < kJoystickPorts; i++) {
    m_joystickAxes[i].count = 0;
    m_joystickPOVs[i].count = 0;
    m_joystickButtons[i].count = 0;
    m_joystickDescriptor[i].isXbox = 0;
    m_joystickDescriptor[i].type = -1;
    m_joystickDescriptor[i].name[0] = '\0';
  }
  // Register that semaphore with the network communications task.
  // It will signal when new packet data is available.
  HALSetNewDataSem(&m_packetDataAvailableCond);

  AddToSingletonList();

  m_task = Task("DriverStation", &DriverStation::Run, this);
}

DriverStation::~DriverStation() {
  m_isRunning = false;
  m_task.join();

  // Unregister our semaphore.
  HALSetNewDataSem(nullptr);
}

void DriverStation::Run() {
  m_isRunning = true;
  int period = 0;
  while (m_isRunning) {
    {
      std::unique_lock<priority_mutex> lock(m_packetDataAvailableMutex);
      m_packetDataAvailableCond.wait(lock);
    }
    GetData();
    m_waitForDataCond.notify_all();

    if (++period >= 4) {
      MotorSafetyHelper::CheckMotors();
      period = 0;
    }
    if (m_userInDisabled) HALNetworkCommunicationObserveUserProgramDisabled();
    if (m_userInAutonomous)
      HALNetworkCommunicationObserveUserProgramAutonomous();
    if (m_userInTeleop) HALNetworkCommunicationObserveUserProgramTeleop();
    if (m_userInTest) HALNetworkCommunicationObserveUserProgramTest();
  }
}

/**
 * Return a pointer to the singleton DriverStation.
 * @return Pointer to the DS instance
 */
DriverStation &DriverStation::GetInstance() {
  static DriverStation *instance = new DriverStation();
  return *instance;
}

/**
 * Copy data from the DS task for the user.
 * If no new data exists, it will just be returned, otherwise
 * the data will be copied from the DS polling loop.
 */
void DriverStation::GetData() {
  // Get the status of all of the joysticks
  for (uint8_t stick = 0; stick < kJoystickPorts; stick++) {
    HALGetJoystickAxes(stick, &m_joystickAxes[stick]);
    HALGetJoystickPOVs(stick, &m_joystickPOVs[stick]);
    HALGetJoystickButtons(stick, &m_joystickButtons[stick]);
    HALGetJoystickDescriptor(stick, &m_joystickDescriptor[stick]);
  }
  m_newControlData.give();
}

/**
 * Read the battery voltage.
 *
 * @return The battery voltage in Volts.
 */
float DriverStation::GetBatteryVoltage() const {
  int32_t status = 0;
  float voltage = getVinVoltage(&status);
  wpi_setErrorWithContext(status, "getVinVoltage");

  return voltage;
}

/**
 * Reports errors related to unplugged joysticks
 * Throttles the errors so that they don't overwhelm the DS
 */
void DriverStation::ReportJoystickUnpluggedError(std::string message) {
  double currentTime = Timer::GetFPGATimestamp();
  if (currentTime > m_nextMessageTime) {
    ReportError(message);
    m_nextMessageTime = currentTime + JOYSTICK_UNPLUGGED_MESSAGE_INTERVAL;
  }
}

/**
 * Returns the number of axes on a given joystick port
 *
 * @param stick The joystick port number
 * @return The number of axes on the indicated joystick
 */
int DriverStation::GetStickAxisCount(uint32_t stick) const {
  if (stick >= kJoystickPorts) {
    wpi_setWPIError(BadJoystickIndex);
    return 0;
  }
  HALJoystickAxes joystickAxes;
  HALGetJoystickAxes(stick, &joystickAxes);
  return joystickAxes.count;
}

/**
 * Returns the name of the joystick at the given port
 *
 * @param stick The joystick port number
 * @return The name of the joystick at the given port
 */
std::string DriverStation::GetJoystickName(uint32_t stick) const {
  if (stick >= kJoystickPorts) {
    wpi_setWPIError(BadJoystickIndex);
  }
  std::string retVal(m_joystickDescriptor[stick].name);
  return retVal;
}

/**
 * Returns the type of joystick at a given port
 *
 * @param stick The joystick port number
 * @return The HID type of joystick at the given port
 */
int DriverStation::GetJoystickType(uint32_t stick) const {
  if (stick >= kJoystickPorts) {
    wpi_setWPIError(BadJoystickIndex);
    return -1;
  }
  return (int)m_joystickDescriptor[stick].type;
}

/**
 * Returns a boolean indicating if the controller is an xbox controller.
 *
 * @param stick The joystick port number
 * @return A boolean that is true if the controller is an xbox controller.
 */
bool DriverStation::GetJoystickIsXbox(uint32_t stick) const {
  if (stick >= kJoystickPorts) {
    wpi_setWPIError(BadJoystickIndex);
{'fraction_non_alphanumeric': 0.12709237445753255, 'fraction_numerical': 0.00690815693915508, 'mean_word_length': 3.3767441860465115, 'pattern_counts': {'":': 0, '<': 11, '<?xml version=': 0, '>': 22, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 4, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '5200040', 'n_tokens_mistral': 3661, 'n_tokens_neox': 3453, 'n_words': 957}