WS2812B with ROS
Dependencies: mbed ros_lib_kinetic_led PololuLedStrip
led.cpp
- Committer:
- Luka_Danilovic
- Date:
- 2019-05-19
- Revision:
- 2:f3d47d1e19c3
File content as of revision 2:f3d47d1e19c3:
#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); } } } }