Skip to content

Commit

Permalink
Merge pull request #63 from luxonis/speaker_led_fix
Browse files Browse the repository at this point in the history 
Small peripherals update
  • Loading branch information
@danilo-pejovic
danilo-pejovic authored Jan 16, 2024
2 parents 67fa27f + 774a863 commit ce3398e
Show file tree
Hide file tree
Showing 5 changed files with 49 additions and 31 deletions.
9 changes: 5 additions & 4 deletions rae_hw/include/rae_hw/peripherals/led.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,10 +39,11 @@ class LEDNode : public rclcpp::Node {
void fillBuffer(uint8_t color, float intensity = 1);
void LED_control();
uint8_t convertColor(float num);
void setSinglePixel(uint16_t pixel, uint8_t r, uint8_t g, uint8_t b, float frequency = 0.0);
void setAllPixels(uint8_t r, uint8_t g, uint8_t b, float frequency = 0.0);
void spinner(uint8_t r, uint8_t g, uint8_t b, uint8_t size = 5, uint8_t blades = 1, float frequency = 0.0);
void fan(uint8_t r, uint8_t g, uint8_t b, bool opening, uint8_t blades = 1, float frequency = 0.0);
float convertOpacity(float num);
void setSinglePixel(uint16_t pixel, uint8_t r, uint8_t g, uint8_t b, float a, float frequency = 0.0);
void setAllPixels(uint8_t r, uint8_t g, uint8_t b, float a, float frequency = 0.0);
void spinner(uint8_t r, uint8_t g, uint8_t b, float a, uint8_t size = 5, uint8_t blades = 1, float frequency = 0.0);
void fan(uint8_t r, uint8_t g, uint8_t b, float a, bool opening, uint8_t blades = 1, float frequency = 0.0);
void topic_callback(const rae_msgs::msg::LEDControl::SharedPtr msg);

rclcpp::Subscription<rae_msgs::msg::LEDControl>::SharedPtr subscription_;
Expand Down
54 changes: 35 additions & 19 deletions rae_hw/src/peripherals/led.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -54,28 +54,33 @@ void LEDNode::timer_callback() {
uint8_t r = convertColor(currentData_->data[0].color.r);
uint8_t g = convertColor(currentData_->data[0].color.g);
uint8_t b = convertColor(currentData_->data[0].color.b);
setAllPixels(r, g, b, currentData_->data[0].frequency);
float a = convertOpacity(currentData_->data[0].color.a);
setAllPixels(r, g, b, a, currentData_->data[0].frequency);
} else if(currentData_->control_type == currentData_->CTRL_TYPE_SINGLE) {
uint8_t r = convertColor(currentData_->data[0].color.r);
uint8_t g = convertColor(currentData_->data[0].color.g);
uint8_t b = convertColor(currentData_->data[0].color.b);
setSinglePixel(currentData_->single_led_n, r, g, b, currentData_->data[0].frequency);
float a = convertOpacity(currentData_->data[0].color.a);
setSinglePixel(currentData_->single_led_n, r, g, b, a, currentData_->data[0].frequency);
} else if(currentData_->control_type == currentData_->CTRL_TYPE_SPINNER) {
uint8_t r = convertColor(currentData_->data[0].color.r);
uint8_t g = convertColor(currentData_->data[0].color.g);
uint8_t b = convertColor(currentData_->data[0].color.b);
spinner(r, g, b, currentData_->animation_size, currentData_->animation_quantity, currentData_->data[0].frequency);
float a = convertOpacity(currentData_->data[0].color.a);
spinner(r, g, b, a, currentData_->animation_size, currentData_->animation_quantity, currentData_->data[0].frequency);
} else if(currentData_->control_type == currentData_->CTRL_TYPE_FAN) {
uint8_t r = convertColor(currentData_->data[0].color.r);
uint8_t g = convertColor(currentData_->data[0].color.g);
uint8_t b = convertColor(currentData_->data[0].color.b);
fan(r, g, b, true, currentData_->animation_quantity, currentData_->data[0].frequency);
float a = convertOpacity(currentData_->data[0].color.a);
fan(r, g, b, a, true, currentData_->animation_quantity, currentData_->data[0].frequency);
} else {
for(int i = 0; i < WS2812B_NUM_LEDS; i++) {
uint8_t r = convertColor(currentData_->data[i].color.r);
uint8_t g = convertColor(currentData_->data[i].color.g);
uint8_t b = convertColor(currentData_->data[i].color.b);
setSinglePixel(i, r, g, b, currentData_->data[i].frequency);
float a = convertOpacity(currentData_->data[i].color.a);
setSinglePixel(i, r, g, b, a, currentData_->data[i].frequency);
}
}
transmitSPI();
Expand All @@ -85,6 +90,17 @@ void LEDNode::timer_callback() {
uint8_t LEDNode::convertColor(float num) {
return static_cast<uint8_t>(round(num * 255.0));
}

float LEDNode::convertOpacity(float num) {
if (num < 0) {
num = 0;}
else if(num > 1) {
num = 1;
}

return num;
}

void LEDNode::transmitSPI() {
struct spi_ioc_transfer tr = spi_ioc_transfer();
tr.tx_buf = (unsigned long)ws2812b_buffer;
Expand All @@ -108,12 +124,12 @@ void LEDNode::fillBuffer(uint8_t color, float intensity) {
ptr++;
}
}
void LEDNode::setSinglePixel(uint16_t pixel, uint8_t r, uint8_t g, uint8_t b, float frequency) {
void LEDNode::setSinglePixel(uint16_t pixel, uint8_t r, uint8_t g, uint8_t b, float a, float frequency) {
auto mapping_pair = logicalToPhysicalMapping.find(pixel);
const float phaseOffset = 3.14 / 4; // Initial phase offset, if needed
const float amplitudeOffset = 1.0f; // Add 1 to sine wave to avoid negative values
const float scaler = 2.0f; // Scale the sine wave to 0-1
const float intensity = (std::sin(frame / (2 * M_PI) * frequency + phaseOffset) + amplitudeOffset) / scaler;
const float phaseOffset = (M_PI/ 4); // Initial phase offset, if needed
const float amplitudeOffset = 1.0f; // Add 1 to sine wave to avoid negative values
const float scaler = 2.0f; // Scale the sine wave to 0-1
const float intensity = ((std::sin(frame / (2 * M_PI) * frequency + phaseOffset) + amplitudeOffset) / scaler) * a;
if(mapping_pair != logicalToPhysicalMapping.end()) {
uint16_t physicalID = mapping_pair->second;
ptr = &ws2812b_buffer[24 * physicalID];
Expand All @@ -126,11 +142,11 @@ void LEDNode::setSinglePixel(uint16_t pixel, uint8_t r, uint8_t g, uint8_t b, fl
fillBuffer(b, intensity);
}

void LEDNode::setAllPixels(uint8_t r, uint8_t g, uint8_t b, float frequency) {
const float phaseOffset = 3.14 / 4; // Initial phase offset, if needed
const float amplitudeOffset = 1.0f; // Add 1 to sine wave to avoid negative values
const float scaler = 2.0f; // Scale the sine wave to 0-1
const float intensity = (std::sin(frame / (2 * M_PI) * frequency + phaseOffset) + amplitudeOffset) / scaler;
void LEDNode::setAllPixels(uint8_t r, uint8_t g, uint8_t b, float a, float frequency) {
const float phaseOffset = (M_PI / 4); // Initial phase offset, if needed
const float amplitudeOffset = 1.0f; // Add 1 to sine wave to avoid negative values
const float scaler = 2.0f; // Scale the sine wave to 0-1
const float intensity = ((std::sin(frame / (2 * M_PI) * frequency + phaseOffset) + amplitudeOffset) / scaler) * a;
ptr = ws2812b_buffer;

for(uint16_t i = 0; i < WS2812B_NUM_LEDS; ++i) {
Expand All @@ -140,22 +156,22 @@ void LEDNode::setAllPixels(uint8_t r, uint8_t g, uint8_t b, float frequency) {
}
}

void LEDNode::spinner(uint8_t r, uint8_t g, uint8_t b, uint8_t size, uint8_t blades, float frequency) {
void LEDNode::spinner(uint8_t r, uint8_t g, uint8_t b, float a, uint8_t size, uint8_t blades, float frequency) {
setAllPixels(0, 0, 0, 0);
for(uint8_t i = 0; i < blades; i++) {
for(uint32_t j = frame; j < frame + size; j++) {
setSinglePixel((j + i * (WS2812B_NUM_LEDS / blades)) % WS2812B_NUM_LEDS, r, g, b, frequency);
setSinglePixel((j + i * (WS2812B_NUM_LEDS / blades)) % WS2812B_NUM_LEDS, r, g, b, a, frequency);
};
};
};

void LEDNode::fan(uint8_t r, uint8_t g, uint8_t b, bool opening, uint8_t blades, float frequency) {
void LEDNode::fan(uint8_t r, uint8_t g, uint8_t b, float a, bool opening, uint8_t blades, float frequency) {
uint8_t blade_length = WS2812B_NUM_LEDS / blades;
uint8_t tip = frame % blade_length;
setAllPixels(0, 0, 0, 0);
for(uint32_t i = 0; i < blades; i++) {
for(uint32_t j = 0; j < (opening ? tip : blade_length - tip); j++) {
setSinglePixel((j + i * blade_length) % WS2812B_NUM_LEDS, r, g, b, frequency);
setSinglePixel((j + i * blade_length) % WS2812B_NUM_LEDS, r, g, b, a, frequency);
};
};
};
Expand Down
3 changes: 2 additions & 1 deletion rae_hw/src/speakers_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,6 +38,7 @@ void SpeakersNode::play_mp3(const char* mp3_file) {
// Initialize libmpg123
mpg123_init();
mh = mpg123_new(NULL, NULL);
int mic_rate = 44100;
long rate; // Set your desired sample rate here
int channels, encoding;
if(mpg123_open(mh, mp3_file) != MPG123_OK || mpg123_getformat(mh, &rate, &channels, &encoding) != MPG123_OK) {
Expand All @@ -53,7 +54,7 @@ void SpeakersNode::play_mp3(const char* mp3_file) {

// Set ALSA parameters

snd_pcm_set_params(alsaHandle, SND_PCM_FORMAT_S16_LE, SND_PCM_ACCESS_RW_INTERLEAVED, channels, rate * channels, 2, 50000);
snd_pcm_set_params(alsaHandle, SND_PCM_FORMAT_S16_LE, SND_PCM_ACCESS_RW_INTERLEAVED, channels, mic_rate * channels, 2, 50000);

// Read and play MP3 file
size_t buffer_size = mpg123_outblock(mh) * 4;
Expand Down
12 changes: 6 additions & 6 deletions rae_sdk/rae_sdk/robot/display.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -126,8 +126,8 @@ def display_imu_data(self, imu_data):
0, 0, axis_length], [0, 0, 0]]).reshape(-1, 3)

# Project 3D points to 2D
focal_length = self.screen_width
camera_matrix = np.array([[focal_length, 0, self.screen_width / 2],
focal_length = self._screen_width
camera_matrix = np.array([[focal_length, 0, self._screen_width / 2],
[0, focal_length, self.screen_height / 2],
[0, 0, 1]], dtype='double')
dist_coeffs = np.zeros((4, 1)) # Assuming no lens distortion
Expand All @@ -136,7 +136,7 @@ def display_imu_data(self, imu_data):

# Create image and draw the axes
image = np.zeros(
(self.screen_height, self.screen_width, 3), dtype=np.uint8)
(self.screen_height, self._screen_width, 3), dtype=np.uint8)
axes_2d = np.int32(axes_2d).reshape(-1, 2)
cv2.line(image, tuple(axes_2d[3]), tuple(
axes_2d[0]), (0, 0, 255), 3) # X-axis in red
Expand All @@ -156,10 +156,10 @@ def ball_callback(self):
self.ball_color = (255, 0, 255) # Red in BGR format

# Ball initial position and velocity
self.x, self.y = self.screen_width // 2, self.screen_height // 2
self.x, self.y = self._screen_width // 2, self.screen_height // 2
self.vx, self.vy = 2, 3 # Speed of the ball in x and y direction
image = np.zeros(
(self.screen_height, self.screen_width, 3), dtype=np.uint8)
(self.screen_height, self._screen_width, 3), dtype=np.uint8)

# Draw the ball
cv2.circle(image, (self.x, self.y),
Expand All @@ -170,7 +170,7 @@ def ball_callback(self):
self.y += self.vy

# Bounce off the walls
if self.x <= self.ball_size // 2 or self.x >= self.screen_width - self.ball_size // 2:
if self.x <= self.ball_size // 2 or self.x >= self._screen_width - self.ball_size // 2:
self.vx = -self.vx
if self.y <= self.ball_size // 2 or self.y >= self.screen_height - self.ball_size // 2:
self.vy = -self.vy
Expand Down
2 changes: 1 addition & 1 deletion rae_sdk/rae_sdk/robot/led.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -60,7 +60,7 @@ def set_leds_from_payload(self, payload: dict):
color_msg.frequency = float(payload['interval'])
else:
color_msg.frequency = 1.0
color_msg.color.a = 1.0
color_msg.color.a = float(payload['brightness']) / 100
color_msg.color.r = self.normalize(r)
color_msg.color.g = self.normalize(g)
color_msg.color.b = self.normalize(b)
Expand Down

0 comments on commit ce3398e

Please sign in to comment.