Luka Danilovic
WS2812B with ROS
Dependencies: mbed ros_lib_kinetic_led PololuLedStrip
Revision 1:29541cb65b8a, committed 2019-05-14
- Comitter:
- Luka_Danilovic
- Date:
- Tue May 14 12:19:07 2019 +0000
- Parent:
- 0:2a3a1e2ab55e
- Child:
- 2:f3d47d1e19c3
- Commit message:
- working;
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/WS2812.lib Tue May 14 12:19:07 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/bridadan/code/WS2812/#6e647820f587
--- a/main.cpp Thu May 09 10:03:01 2019 +0000
+++ b/main.cpp Tue May 14 12:19:07 2019 +0000
@@ -5,151 +5,166 @@
#include <std_msgs/String.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Twist.h>
-#include "std_msgs/Float64.h"
-PololuLedStrip ledStripFront(PC_10);
-PololuLedStrip ledStripBack(PC_11);
+PololuLedStrip ledStripFront(PC_9);
+PololuLedStrip ledStripBack(PC_8);
#define LED_COUNT 30
#define MaxVelocity 0.5f
rgb_color colors[LED_COUNT];
-BusOut myleds(LED1, LED2, LED3);
+
+
+ros::NodeHandle nh;
//ROS Audio Status Callback
string audio_state = "NothingSpecial";
-void AudioStatusCB(const std_msgs::String &status) {
+void AudioStatusCB(const std_msgs::String &status)
+{
audio_state = status.data;
}
float vel = 0.0f;
float ang = 0.0f;
-std_msgs::Float64 number;
-
-// Uncomment this For Realsies
+/* 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");
+//}
-void VelocityCB(const nav_msgs::Odometry &odom) {
- vel = odom.twist.twist.linear.x;
- ang = odom.twist.twist.angular.z;
- number.data += 1;
+/* 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");
}
-/*
-// Uncomment this For Testing With Controller
-void VelocityCB(const geometry_msgs::Twist &odom) {
- vel = odom.linear.x;
- ang = odom.angular.z;
-}
-*/
-float Map(float x, float in_min, float in_max, float out_min, float out_max){
+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::NodeHandle nh;
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 realsies */
+//ros::Subscriber<nav_msgs::Odometry> Velocity("wheel_odom/exact_odom", &VelocityCB);
-ros::Publisher odom_pub("odom_listener", &number);
-
-//Uncomment this for testing with controller
-//ros::Subscriber<geometry_msgs::Twist> Velocity("mtr_ctrl/cmd_vel", &VelocityCB);
+/* Uncomment this for testing with controller */
+ros::Subscriber<geometry_msgs::Twist> cmd_vel_sub("cmd_vel", &cmdVelCB);
-int main() {
-
- number.data = 0;
+int main()
+{
+
int LEDBrightness = 128;
- float led_tmp = LED_COUNT;
+ float led_tmp = LED_COUNT/4;
int led_num = floor(led_tmp);
- int k = 0;
- rgb_color led_colour = (rgb_color){0, 0, 0};
-
-// while(1)
-// {
-// LEDBrightness = 128;
-// led_colour = (rgb_color) {255, 0, 255};
-// for(int i = 0; i < led_num; i++){
-// colors[i] = led_colour;
-// }
-// ledStripFront.write(colors, LED_COUNT);
-// ledStripBack.write(colors, LED_COUNT);
-// wait_ms(200);
-// LEDBrightness = 128;
-// led_colour = (rgb_color) {0, 0, 255};
-// for(int i = 0; i < led_num; i++){
-// colors[i] = led_colour;
-// }
-// ledStripFront.write(colors, LED_COUNT);
-// ledStripBack.write(colors, LED_COUNT);
-// wait_ms(200);
-// }
-
- nh.getHardware()->setBaud(57600); //set ROSSERIAL baud rate
+ nh.getHardware()->setBaud(460800); //set ROSSERIAL baud rate
nh.initNode();
+ nh.advertise(status_pub);
nh.subscribe(AudioStatus);
- nh.subscribe(Velocity);
- nh.advertise(odom_pub);
-
- while(!nh.connected()){
- nh.spinOnce();
- }
-
- while (true) {
- if (!nh.connected()) {
+// 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 {
- // Main control loop. Only runs if the ROS serial node is connected
- while (nh.connected()) {
+ while(nh.connected()) {
nh.spinOnce();
- if (((abs(vel) || abs(ang)) > 0.1f) && (audio_state == "Playing")){
+
+ /* 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));
}
- led_colour = (rgb_color) {0, 0, 255};
- for(int i = 0; i < led_num; i++){
+
+ /* 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) {LEDBrightness, 0, 0};
- for(int i = led_num; i < ((3*led_num)+1); i++){
+
+ 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++){
+ led_colour = (rgb_color) {
+ 0, 0, 255
+ };
+ for(int i = ((3*led_num)+1); i < LED_COUNT; i++) {
colors[i] = led_colour;
}
- } else if((abs(vel) || abs(ang)) > 0.1f) { //If Not In DeadZone (Robot Is Moving)
+ }
+
+ /* 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++){
+ led_colour = (rgb_color) {
+ 0, LEDBrightness, 0
+ };
+ for(int i = 0; i < LED_COUNT; i++) {
colors[i] = led_colour;
}
- } else if (audio_state == "Playing") { //Else If Audio Is Playing
- led_colour = (rgb_color) {0, 0, 255};
- for(int i = 0; i < LED_COUNT; i++){
+ }
+
+ /* 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;
}
- } else { //Robot Is Neither Moving Nor Playing Audio
- led_colour = (rgb_color) {255, 0, 0};
- for(int i = 0; i < LED_COUNT; i++){
+ }
+
+ /* 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);
- odom_pub.publish(&number);
- wait_ms(500);
+ status_pub.publish(&status_msg);
+ wait_ms(200);
}
}
}
--- a/ros_lib_kinetic.lib Thu May 09 10:03:01 2019 +0000 +++ b/ros_lib_kinetic.lib Tue May 14 12:19:07 2019 +0000 @@ -1,1 +1,1 @@ -https://os.mbed.com/users/garyservin/code/ros_lib_kinetic/#6dc1aca70277 +https://os.mbed.com/users/garyservin/code/ros_lib_kinetic/#7bc03f1f0d03