WS2812B with ROS
Dependencies: mbed ros_lib_kinetic_led PololuLedStrip
Diff: led.cpp
- Revision:
- 2:f3d47d1e19c3
diff -r 29541cb65b8a -r f3d47d1e19c3 led.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/led.cpp Sun May 19 21:38:11 2019 +0000 @@ -0,0 +1,161 @@ +#include "mbed.h" +#include "PololuLedStrip.h" +#include <string> +#include <ros.h> +#include <std_msgs/String.h> +//#include <nav_msgs/Odometry.h> +#include <geometry_msgs/Twist.h> + +PololuLedStrip ledStripFront(PC_9); +PololuLedStrip ledStripBack(PC_8); + +#define LED_COUNT 30 +#define MaxVelocity 0.5f +rgb_color colors[LED_COUNT]; + + +ros::NodeHandle nh; + +//ROS Audio Status Callback +string audio_state = "NothingSpecial"; + +void AudioStatusCB(const std_msgs::String &status) +{ + audio_state = status.data; +} + +float vel = 0.0f; +float ang = 0.0f; + + +/* Uncomment this For Testing With Controller */ +void cmdVelCB(const geometry_msgs::Twist &twist) { + vel = twist.linear.x; + ang = twist.angular.z; + nh.loginfo("received twist"); +} + +float Map(float x, float in_min, float in_max, float out_min, float out_max) +{ + return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; +} + +std_msgs::String status_msg; +ros::Publisher status_pub("status", &status_msg); // Instance for ROS publisher (String Message) + +ros::Subscriber<std_msgs::String> AudioStatus("audio_status", &AudioStatusCB); + +/* Uncomment this for testing with controller */ +ros::Subscriber<geometry_msgs::Twist> cmd_vel_sub("cmd_vel", &cmdVelCB); + +int main() +{ + + int LEDBrightness = 128; + float led_tmp = LED_COUNT/4; + int led_num = floor(led_tmp); + nh.getHardware()->setBaud(460800); //set ROSSERIAL baud rate + nh.initNode(); + nh.advertise(status_pub); + nh.subscribe(AudioStatus); +// nh.subscribe(Velocity); + nh.subscribe(cmd_vel_sub); + + rgb_color led_colour = (rgb_color) { + 0, 0, 0 + }; + + while(true) { + + if(!nh.connected()) { + nh.spinOnce(); + } else { + + while(nh.connected()) { + nh.spinOnce(); + + /* Moving and Playing */ + if (((abs(vel) || abs(ang)) > 0.01f) && (audio_state == "Playing")) { + status_msg.data = "M & A"; + + /* Take the bigger of the two (angular or linear) */ + if(abs(vel) >= abs(ang)) { + LEDBrightness = floor(Map(abs(vel), 0, MaxVelocity, 0, 255)); + } else { + LEDBrightness = floor(Map(abs(ang), 0, MaxVelocity, 0, 255)); + } + + /* Set colour */ + led_colour = (rgb_color) { + 0, 0, 255 + }; + + /* Populate colours for each led */ + for(int i = 0; i < led_num; i++) { + colors[i] = led_colour; + } + + led_colour = (rgb_color) { + 0, LEDBrightness, 0 + }; + for(int i = led_num; i < ((3*led_num)+1); i++) { + colors[i] = led_colour; + } + led_colour = (rgb_color) { + 0, 0, 255 + }; + for(int i = ((3*led_num)+1); i < LED_COUNT; i++) { + colors[i] = led_colour; + } + } + + /* Moing only */ + else if((abs(vel) || abs(ang)) > 0.01f) { //If Not In DeadZone (Robot Is Moving) + status_msg.data = "M."; + + if(abs(vel) >= abs(ang)) { + LEDBrightness = floor(Map(abs(vel), 0, MaxVelocity, 0, 255)); + } else { + LEDBrightness = floor(Map(abs(ang), 0, MaxVelocity, 0, 255)); + } + led_colour = (rgb_color) { + 0, LEDBrightness, 0 + }; + for(int i = 0; i < LED_COUNT; i++) { + colors[i] = led_colour; + } + } + + /* Playing only */ + else if (audio_state == "Playing") { //Else If Audio Is Playing + + status_msg.data = "A."; + led_colour = (rgb_color) { + 0, 0, 255 + }; + for(int i = 0; i < LED_COUNT; i++) { + colors[i] = led_colour; + } + } + + /* Not moing and not playing */ + else { + status_msg.data = "N."; //Robot Is Neither Moving Nor Playing Audio + led_colour = (rgb_color) { + 255, 0, 0 + }; + for(int i = 0; i < LED_COUNT; i++) { + colors[i] = led_colour; + } + } + + + /* Write to leds */ + ledStripFront.write(colors, LED_COUNT); + ledStripBack.write(colors, LED_COUNT); + status_pub.publish(&status_msg); + wait_ms(200); + } + } + } +}