allwpilib/wpilibc/src/main/native/cpp/AddressableLED.cpp

// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.

#include "frc/AddressableLED.h"

#include <hal/AddressableLED.h>
#include <hal/FRCUsageReporting.h>
#include <hal/HALBase.h>
#include <hal/PWM.h>
#include <hal/Ports.h>
#include <wpi/StackTrace.h>

#include "frc/Errors.h"

using namespace frc;

AddressableLED::AddressableLED(int port) {
  int32_t status = 0;

  auto stack = wpi::GetStackTrace(1);
  m_pwmHandle =
      HAL_InitializePWMPort(HAL_GetPort(port), stack.c_str(), &status);
  FRC_CheckErrorStatus(status, "Port " + wpi::Twine{port});
  if (m_pwmHandle == HAL_kInvalidHandle) {
    return;
  }

  m_handle = HAL_InitializeAddressableLED(m_pwmHandle, &status);
  FRC_CheckErrorStatus(status, "Port " + wpi::Twine{port});
  if (m_handle == HAL_kInvalidHandle) {
    HAL_FreePWMPort(m_pwmHandle, &status);
  }

  HAL_Report(HALUsageReporting::kResourceType_AddressableLEDs, port + 1);
}

AddressableLED::~AddressableLED() {
  HAL_FreeAddressableLED(m_handle);
  int32_t status = 0;
  HAL_FreePWMPort(m_pwmHandle, &status);
  FRC_ReportError(status, "FreePWM");
}

void AddressableLED::SetLength(int length) {
  int32_t status = 0;
  HAL_SetAddressableLEDLength(m_handle, length, &status);
  FRC_CheckErrorStatus(status, "length " + wpi::Twine{length});
}

static_assert(sizeof(AddressableLED::LEDData) == sizeof(HAL_AddressableLEDData),
              "LED Structs MUST be the same size");

void AddressableLED::SetData(wpi::ArrayRef<LEDData> ledData) {
  int32_t status = 0;
  HAL_WriteAddressableLEDData(m_handle, ledData.begin(), ledData.size(),
                              &status);
  FRC_CheckErrorStatus(status, "SetData");
}

void AddressableLED::SetData(std::initializer_list<LEDData> ledData) {
  int32_t status = 0;
  HAL_WriteAddressableLEDData(m_handle, ledData.begin(), ledData.size(),
                              &status);
  FRC_CheckErrorStatus(status, "SetData");
}

void AddressableLED::SetBitTiming(units::nanosecond_t lowTime0,
                                  units::nanosecond_t highTime0,
                                  units::nanosecond_t lowTime1,
                                  units::nanosecond_t highTime1) {
  int32_t status = 0;
  HAL_SetAddressableLEDBitTiming(
      m_handle, lowTime0.to<int32_t>(), highTime0.to<int32_t>(),
      lowTime1.to<int32_t>(), highTime1.to<int32_t>(), &status);
  FRC_CheckErrorStatus(status, "SetBitTiming");
}

void AddressableLED::SetSyncTime(units::microsecond_t syncTime) {
  int32_t status = 0;
  HAL_SetAddressableLEDSyncTime(m_handle, syncTime.to<int32_t>(), &status);
  FRC_CheckErrorStatus(status, "SetSyncTime");
}

void AddressableLED::Start() {
  int32_t status = 0;
  HAL_StartAddressableLEDOutput(m_handle, &status);
  FRC_CheckErrorStatus(status, "Start");
}

void AddressableLED::Stop() {
  int32_t status = 0;
  HAL_StopAddressableLEDOutput(m_handle, &status);
  FRC_CheckErrorStatus(status, "Stop");
}

void AddressableLED::LEDData::SetHSV(int h, int s, int v) {
  SetLED(Color::FromHSV(h, s, v));
}