Gitiles
Code Review Sign In
realtimeroboticsgroup.org / RealtimeRoboticsGroup / test / 4b81d3014e62032c63ff45fefc190fe002faa2bb / . / wpilibc / src / main / native / cpp / AddressableLED.cpp
blob: b0c3a4506ab75b963525b51857cfabcae1021ce6 [file] [log] [blame]
James Kuszmaul4b81d302019-12-14 20:53:14 -0800[diff] [blame^]1/*----------------------------------------------------------------------------*/
2/* Copyright (c) 2019 FIRST. All Rights Reserved. */
3/* Open Source Software - may be modified and shared by FRC teams. The code */
4/* must be accompanied by the FIRST BSD license file in the root directory of */
5/* the project. */
6/*----------------------------------------------------------------------------*/
7
8#include "frc/AddressableLED.h"
9
10#include <hal/AddressableLED.h>
11#include <hal/FRCUsageReporting.h>
12#include <hal/HALBase.h>
13#include <hal/PWM.h>
14#include <hal/Ports.h>
15
16#include "frc/WPIErrors.h"
17
18using namespace frc;
19
20AddressableLED::AddressableLED(int port) {
21 int32_t status = 0;
22
23 m_pwmHandle = HAL_InitializePWMPort(HAL_GetPort(port), &status);
24 wpi_setHALErrorWithRange(status, 0, HAL_GetNumPWMChannels(), port);
25 if (m_pwmHandle == HAL_kInvalidHandle) {
26 return;
27 }
28
29 m_handle = HAL_InitializeAddressableLED(m_pwmHandle, &status);
30 wpi_setHALError(status);
31 if (m_handle == HAL_kInvalidHandle) {
32 HAL_FreePWMPort(m_pwmHandle, &status);
33 }
34
35 HAL_Report(HALUsageReporting::kResourceType_AddressableLEDs, port + 1);
36}
37
38AddressableLED::~AddressableLED() {
39 HAL_FreeAddressableLED(m_handle);
40 int32_t status = 0;
41 HAL_FreePWMPort(m_pwmHandle, &status);
42}
43
44void AddressableLED::SetLength(int length) {
45 int32_t status = 0;
46 HAL_SetAddressableLEDLength(m_handle, length, &status);
47 wpi_setHALError(status);
48}
49
50static_assert(sizeof(AddressableLED::LEDData) == sizeof(HAL_AddressableLEDData),
51 "LED Structs MUST be the same size");
52
53void AddressableLED::SetData(wpi::ArrayRef<LEDData> ledData) {
54 int32_t status = 0;
55 HAL_WriteAddressableLEDData(m_handle, ledData.begin(), ledData.size(),
56 &status);
57 wpi_setHALError(status);
58}
59
60void AddressableLED::SetData(std::initializer_list<LEDData> ledData) {
61 int32_t status = 0;
62 HAL_WriteAddressableLEDData(m_handle, ledData.begin(), ledData.size(),
63 &status);
64 wpi_setHALError(status);
65}
66
67void AddressableLED::SetBitTiming(units::nanosecond_t lowTime0,
68 units::nanosecond_t highTime0,
69 units::nanosecond_t lowTime1,
70 units::nanosecond_t highTime1) {
71 int32_t status = 0;
72 HAL_SetAddressableLEDBitTiming(
73 m_handle, lowTime0.to<int32_t>(), highTime0.to<int32_t>(),
74 lowTime1.to<int32_t>(), highTime1.to<int32_t>(), &status);
75 wpi_setHALError(status);
76}
77
78void AddressableLED::SetSyncTime(units::microsecond_t syncTime) {
79 int32_t status = 0;
80 HAL_SetAddressableLEDSyncTime(m_handle, syncTime.to<int32_t>(), &status);
81 wpi_setHALError(status);
82}
83
84void AddressableLED::Start() {
85 int32_t status = 0;
86 HAL_StartAddressableLEDOutput(m_handle, &status);
87 wpi_setHALError(status);
88}
89
90void AddressableLED::Stop() {
91 int32_t status = 0;
92 HAL_StopAddressableLEDOutput(m_handle, &status);
93 wpi_setHALError(status);
94}
95
96void AddressableLED::LEDData::SetHSV(int h, int s, int v) {
97 if (s == 0) {
98 SetRGB(v, v, v);
99 return;
100 }
101
102 int region = h / 30;
103 int remainder = (h - (region * 30)) * 6;
104
105 int p = (v * (255 - s)) >> 8;
106 int q = (v * (255 - ((s * remainder) >> 8))) >> 8;
107 int t = (v * (255 - ((s * (255 - remainder)) >> 8))) >> 8;
108
109 switch (region) {
110 case 0:
111 SetRGB(v, t, p);
112 break;
113 case 1:
114 SetRGB(q, v, p);
115 break;
116 case 2:
117 SetRGB(p, v, t);
118 break;
119 case 3:
120 SetRGB(p, q, v);
121 break;
122 case 4:
123 SetRGB(t, p, v);
124 break;
125 default:
126 SetRGB(v, p, q);
127 break;
128 }
129}