diff --git a/rae_hw/include/rae_hw/peripherals/led.hpp b/rae_hw/include/rae_hw/peripherals/led.hpp index 3a0acc8..1747c6b 100644 --- a/rae_hw/include/rae_hw/peripherals/led.hpp +++ b/rae_hw/include/rae_hw/peripherals/led.hpp @@ -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::SharedPtr subscription_; diff --git a/rae_hw/src/peripherals/led.cpp b/rae_hw/src/peripherals/led.cpp index 5a412fe..c6074e6 100644 --- a/rae_hw/src/peripherals/led.cpp +++ b/rae_hw/src/peripherals/led.cpp @@ -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(); @@ -85,6 +90,17 @@ void LEDNode::timer_callback() { uint8_t LEDNode::convertColor(float num) { return static_cast(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; @@ -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]; @@ -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) { @@ -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); }; }; }; diff --git a/rae_hw/src/speakers_node.cpp b/rae_hw/src/speakers_node.cpp index 24d0a16..3f72f92 100644 --- a/rae_hw/src/speakers_node.cpp +++ b/rae_hw/src/speakers_node.cpp @@ -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) { @@ -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; diff --git a/rae_sdk/rae_sdk/robot/display.py b/rae_sdk/rae_sdk/robot/display.py index 084cbd5..96a8bce 100644 --- a/rae_sdk/rae_sdk/robot/display.py +++ b/rae_sdk/rae_sdk/robot/display.py @@ -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 @@ -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 @@ -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), @@ -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 diff --git a/rae_sdk/rae_sdk/robot/led.py b/rae_sdk/rae_sdk/robot/led.py index 5e5dee1..79fc30f 100644 --- a/rae_sdk/rae_sdk/robot/led.py +++ b/rae_sdk/rae_sdk/robot/led.py @@ -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)