Luka Danilovic
WS2812B with ROS
Dependencies: mbed ros_lib_kinetic_led PololuLedStrip
Revision 2:f3d47d1e19c3, committed 2019-05-19
- Comitter:
- Luka_Danilovic
- Date:
- Sun May 19 21:38:11 2019 +0000
- Parent:
- 1:29541cb65b8a
- Commit message:
- Final
Changed in this revision
--- a/WS2812.lib Tue May 14 12:19:07 2019 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -https://os.mbed.com/users/bridadan/code/WS2812/#6e647820f587
--- /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);
+ }
+ }
+ }
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/led.h Sun May 19 21:38:11 2019 +0000
@@ -0,0 +1,46 @@
+#ifndef __LED_H__ //Inclusion safeguards
+#define __LED_H__ //Definition of the inclusion
+/*============================================================================*/
+
+/* Libraries */
+#include "mbed.h"
+#include "PololuLedStrip.h"
+#include <string>
+#include <ros.h>
+#include <std_msgs/String.h>
+#include <geometry_msgs/Twist.h>
+
+/* Definitions */
+#define LED_COUNT 30
+#define LED_F_PIN PC_9
+#define LED_B_PIN PC_8
+#define MaxVelocity 0.5f
+
+
+/* Declarations */
+extern rgb_color colors[];
+int LEDBrightness;
+float led_tmp;
+int led_num;
+
+extern string audio_state;
+extern float vel;
+extern float ang;
+
+
+/* Instantiations */
+PololuLedStrip ledStripFront(LED_F_PIN);
+PololuLedStrip ledStripBack(LED_B_PIN);
+ros::NodeHandle nh;
+std_msgs::String status_msg;
+ros::Publisher status_pub("status", &status_msg);
+ros::Subscriber<std_msgs::String> AudioStatus("audio_status", &AudioStatusCB);
+ros::Subscriber<geometry_msgs::Twist> cmd_vel_sub("cmd_vel", &cmdVelCB);
+
+/* Function Prototypes */
+void AudioStatusCB(const std_msgs::String &status);
+void cmdVelCB(const geometry_msgs::Twist &twist);
+float Map(float x, float in_min, float in_max, float out_min, float out_max);
+
+/*============================================================================*/
+#endif // End of inclusion
\ No newline at end of file
--- a/main.cpp Tue May 14 12:19:07 2019 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,171 +0,0 @@
-#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 Realsies */
-//void VelocityCB(const nav_msgs::Odometry &odom)
-//{
-// vel = odom.twist.twist.linear.x;
-// ang = odom.twist.twist.angular.z;
-// nh.loginfo("received odom");
-//}
-
-/* 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 realsies */
-//ros::Subscriber<nav_msgs::Odometry> Velocity("wheel_odom/exact_odom", &VelocityCB);
-
-/* 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);
- }
- }
- }
-}
--- a/ros_lib_kinetic.lib Tue May 14 12:19:07 2019 +0000 +++ b/ros_lib_kinetic.lib Sun May 19 21:38:11 2019 +0000 @@ -1,1 +1,1 @@ -https://os.mbed.com/users/garyservin/code/ros_lib_kinetic/#7bc03f1f0d03 +https://os.mbed.com/users/Luka_Danilovic/code/ros_lib_kinetic_led/#7bc03f1f0d03