WS2812B with ROS
Dependencies: mbed ros_lib_kinetic_led PololuLedStrip
led.cpp@2:f3d47d1e19c3, 2019-05-19 (annotated)
- Committer:
- Luka_Danilovic
- Date:
- Sun May 19 21:38:11 2019 +0000
- Revision:
- 2:f3d47d1e19c3
Final
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Luka_Danilovic | 2:f3d47d1e19c3 | 1 | #include "mbed.h" |
Luka_Danilovic | 2:f3d47d1e19c3 | 2 | #include "PololuLedStrip.h" |
Luka_Danilovic | 2:f3d47d1e19c3 | 3 | #include <string> |
Luka_Danilovic | 2:f3d47d1e19c3 | 4 | #include <ros.h> |
Luka_Danilovic | 2:f3d47d1e19c3 | 5 | #include <std_msgs/String.h> |
Luka_Danilovic | 2:f3d47d1e19c3 | 6 | //#include <nav_msgs/Odometry.h> |
Luka_Danilovic | 2:f3d47d1e19c3 | 7 | #include <geometry_msgs/Twist.h> |
Luka_Danilovic | 2:f3d47d1e19c3 | 8 | |
Luka_Danilovic | 2:f3d47d1e19c3 | 9 | PololuLedStrip ledStripFront(PC_9); |
Luka_Danilovic | 2:f3d47d1e19c3 | 10 | PololuLedStrip ledStripBack(PC_8); |
Luka_Danilovic | 2:f3d47d1e19c3 | 11 | |
Luka_Danilovic | 2:f3d47d1e19c3 | 12 | #define LED_COUNT 30 |
Luka_Danilovic | 2:f3d47d1e19c3 | 13 | #define MaxVelocity 0.5f |
Luka_Danilovic | 2:f3d47d1e19c3 | 14 | rgb_color colors[LED_COUNT]; |
Luka_Danilovic | 2:f3d47d1e19c3 | 15 | |
Luka_Danilovic | 2:f3d47d1e19c3 | 16 | |
Luka_Danilovic | 2:f3d47d1e19c3 | 17 | ros::NodeHandle nh; |
Luka_Danilovic | 2:f3d47d1e19c3 | 18 | |
Luka_Danilovic | 2:f3d47d1e19c3 | 19 | //ROS Audio Status Callback |
Luka_Danilovic | 2:f3d47d1e19c3 | 20 | string audio_state = "NothingSpecial"; |
Luka_Danilovic | 2:f3d47d1e19c3 | 21 | |
Luka_Danilovic | 2:f3d47d1e19c3 | 22 | void AudioStatusCB(const std_msgs::String &status) |
Luka_Danilovic | 2:f3d47d1e19c3 | 23 | { |
Luka_Danilovic | 2:f3d47d1e19c3 | 24 | audio_state = status.data; |
Luka_Danilovic | 2:f3d47d1e19c3 | 25 | } |
Luka_Danilovic | 2:f3d47d1e19c3 | 26 | |
Luka_Danilovic | 2:f3d47d1e19c3 | 27 | float vel = 0.0f; |
Luka_Danilovic | 2:f3d47d1e19c3 | 28 | float ang = 0.0f; |
Luka_Danilovic | 2:f3d47d1e19c3 | 29 | |
Luka_Danilovic | 2:f3d47d1e19c3 | 30 | |
Luka_Danilovic | 2:f3d47d1e19c3 | 31 | /* Uncomment this For Testing With Controller */ |
Luka_Danilovic | 2:f3d47d1e19c3 | 32 | void cmdVelCB(const geometry_msgs::Twist &twist) { |
Luka_Danilovic | 2:f3d47d1e19c3 | 33 | vel = twist.linear.x; |
Luka_Danilovic | 2:f3d47d1e19c3 | 34 | ang = twist.angular.z; |
Luka_Danilovic | 2:f3d47d1e19c3 | 35 | nh.loginfo("received twist"); |
Luka_Danilovic | 2:f3d47d1e19c3 | 36 | } |
Luka_Danilovic | 2:f3d47d1e19c3 | 37 | |
Luka_Danilovic | 2:f3d47d1e19c3 | 38 | float Map(float x, float in_min, float in_max, float out_min, float out_max) |
Luka_Danilovic | 2:f3d47d1e19c3 | 39 | { |
Luka_Danilovic | 2:f3d47d1e19c3 | 40 | return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; |
Luka_Danilovic | 2:f3d47d1e19c3 | 41 | } |
Luka_Danilovic | 2:f3d47d1e19c3 | 42 | |
Luka_Danilovic | 2:f3d47d1e19c3 | 43 | std_msgs::String status_msg; |
Luka_Danilovic | 2:f3d47d1e19c3 | 44 | ros::Publisher status_pub("status", &status_msg); // Instance for ROS publisher (String Message) |
Luka_Danilovic | 2:f3d47d1e19c3 | 45 | |
Luka_Danilovic | 2:f3d47d1e19c3 | 46 | ros::Subscriber<std_msgs::String> AudioStatus("audio_status", &AudioStatusCB); |
Luka_Danilovic | 2:f3d47d1e19c3 | 47 | |
Luka_Danilovic | 2:f3d47d1e19c3 | 48 | /* Uncomment this for testing with controller */ |
Luka_Danilovic | 2:f3d47d1e19c3 | 49 | ros::Subscriber<geometry_msgs::Twist> cmd_vel_sub("cmd_vel", &cmdVelCB); |
Luka_Danilovic | 2:f3d47d1e19c3 | 50 | |
Luka_Danilovic | 2:f3d47d1e19c3 | 51 | int main() |
Luka_Danilovic | 2:f3d47d1e19c3 | 52 | { |
Luka_Danilovic | 2:f3d47d1e19c3 | 53 | |
Luka_Danilovic | 2:f3d47d1e19c3 | 54 | int LEDBrightness = 128; |
Luka_Danilovic | 2:f3d47d1e19c3 | 55 | float led_tmp = LED_COUNT/4; |
Luka_Danilovic | 2:f3d47d1e19c3 | 56 | int led_num = floor(led_tmp); |
Luka_Danilovic | 2:f3d47d1e19c3 | 57 | nh.getHardware()->setBaud(460800); //set ROSSERIAL baud rate |
Luka_Danilovic | 2:f3d47d1e19c3 | 58 | nh.initNode(); |
Luka_Danilovic | 2:f3d47d1e19c3 | 59 | nh.advertise(status_pub); |
Luka_Danilovic | 2:f3d47d1e19c3 | 60 | nh.subscribe(AudioStatus); |
Luka_Danilovic | 2:f3d47d1e19c3 | 61 | // nh.subscribe(Velocity); |
Luka_Danilovic | 2:f3d47d1e19c3 | 62 | nh.subscribe(cmd_vel_sub); |
Luka_Danilovic | 2:f3d47d1e19c3 | 63 | |
Luka_Danilovic | 2:f3d47d1e19c3 | 64 | rgb_color led_colour = (rgb_color) { |
Luka_Danilovic | 2:f3d47d1e19c3 | 65 | 0, 0, 0 |
Luka_Danilovic | 2:f3d47d1e19c3 | 66 | }; |
Luka_Danilovic | 2:f3d47d1e19c3 | 67 | |
Luka_Danilovic | 2:f3d47d1e19c3 | 68 | while(true) { |
Luka_Danilovic | 2:f3d47d1e19c3 | 69 | |
Luka_Danilovic | 2:f3d47d1e19c3 | 70 | if(!nh.connected()) { |
Luka_Danilovic | 2:f3d47d1e19c3 | 71 | nh.spinOnce(); |
Luka_Danilovic | 2:f3d47d1e19c3 | 72 | } else { |
Luka_Danilovic | 2:f3d47d1e19c3 | 73 | |
Luka_Danilovic | 2:f3d47d1e19c3 | 74 | while(nh.connected()) { |
Luka_Danilovic | 2:f3d47d1e19c3 | 75 | nh.spinOnce(); |
Luka_Danilovic | 2:f3d47d1e19c3 | 76 | |
Luka_Danilovic | 2:f3d47d1e19c3 | 77 | /* Moving and Playing */ |
Luka_Danilovic | 2:f3d47d1e19c3 | 78 | if (((abs(vel) || abs(ang)) > 0.01f) && (audio_state == "Playing")) { |
Luka_Danilovic | 2:f3d47d1e19c3 | 79 | status_msg.data = "M & A"; |
Luka_Danilovic | 2:f3d47d1e19c3 | 80 | |
Luka_Danilovic | 2:f3d47d1e19c3 | 81 | /* Take the bigger of the two (angular or linear) */ |
Luka_Danilovic | 2:f3d47d1e19c3 | 82 | if(abs(vel) >= abs(ang)) { |
Luka_Danilovic | 2:f3d47d1e19c3 | 83 | LEDBrightness = floor(Map(abs(vel), 0, MaxVelocity, 0, 255)); |
Luka_Danilovic | 2:f3d47d1e19c3 | 84 | } else { |
Luka_Danilovic | 2:f3d47d1e19c3 | 85 | LEDBrightness = floor(Map(abs(ang), 0, MaxVelocity, 0, 255)); |
Luka_Danilovic | 2:f3d47d1e19c3 | 86 | } |
Luka_Danilovic | 2:f3d47d1e19c3 | 87 | |
Luka_Danilovic | 2:f3d47d1e19c3 | 88 | /* Set colour */ |
Luka_Danilovic | 2:f3d47d1e19c3 | 89 | led_colour = (rgb_color) { |
Luka_Danilovic | 2:f3d47d1e19c3 | 90 | 0, 0, 255 |
Luka_Danilovic | 2:f3d47d1e19c3 | 91 | }; |
Luka_Danilovic | 2:f3d47d1e19c3 | 92 | |
Luka_Danilovic | 2:f3d47d1e19c3 | 93 | /* Populate colours for each led */ |
Luka_Danilovic | 2:f3d47d1e19c3 | 94 | for(int i = 0; i < led_num; i++) { |
Luka_Danilovic | 2:f3d47d1e19c3 | 95 | colors[i] = led_colour; |
Luka_Danilovic | 2:f3d47d1e19c3 | 96 | } |
Luka_Danilovic | 2:f3d47d1e19c3 | 97 | |
Luka_Danilovic | 2:f3d47d1e19c3 | 98 | led_colour = (rgb_color) { |
Luka_Danilovic | 2:f3d47d1e19c3 | 99 | 0, LEDBrightness, 0 |
Luka_Danilovic | 2:f3d47d1e19c3 | 100 | }; |
Luka_Danilovic | 2:f3d47d1e19c3 | 101 | for(int i = led_num; i < ((3*led_num)+1); i++) { |
Luka_Danilovic | 2:f3d47d1e19c3 | 102 | colors[i] = led_colour; |
Luka_Danilovic | 2:f3d47d1e19c3 | 103 | } |
Luka_Danilovic | 2:f3d47d1e19c3 | 104 | led_colour = (rgb_color) { |
Luka_Danilovic | 2:f3d47d1e19c3 | 105 | 0, 0, 255 |
Luka_Danilovic | 2:f3d47d1e19c3 | 106 | }; |
Luka_Danilovic | 2:f3d47d1e19c3 | 107 | for(int i = ((3*led_num)+1); i < LED_COUNT; i++) { |
Luka_Danilovic | 2:f3d47d1e19c3 | 108 | colors[i] = led_colour; |
Luka_Danilovic | 2:f3d47d1e19c3 | 109 | } |
Luka_Danilovic | 2:f3d47d1e19c3 | 110 | } |
Luka_Danilovic | 2:f3d47d1e19c3 | 111 | |
Luka_Danilovic | 2:f3d47d1e19c3 | 112 | /* Moing only */ |
Luka_Danilovic | 2:f3d47d1e19c3 | 113 | else if((abs(vel) || abs(ang)) > 0.01f) { //If Not In DeadZone (Robot Is Moving) |
Luka_Danilovic | 2:f3d47d1e19c3 | 114 | status_msg.data = "M."; |
Luka_Danilovic | 2:f3d47d1e19c3 | 115 | |
Luka_Danilovic | 2:f3d47d1e19c3 | 116 | if(abs(vel) >= abs(ang)) { |
Luka_Danilovic | 2:f3d47d1e19c3 | 117 | LEDBrightness = floor(Map(abs(vel), 0, MaxVelocity, 0, 255)); |
Luka_Danilovic | 2:f3d47d1e19c3 | 118 | } else { |
Luka_Danilovic | 2:f3d47d1e19c3 | 119 | LEDBrightness = floor(Map(abs(ang), 0, MaxVelocity, 0, 255)); |
Luka_Danilovic | 2:f3d47d1e19c3 | 120 | } |
Luka_Danilovic | 2:f3d47d1e19c3 | 121 | led_colour = (rgb_color) { |
Luka_Danilovic | 2:f3d47d1e19c3 | 122 | 0, LEDBrightness, 0 |
Luka_Danilovic | 2:f3d47d1e19c3 | 123 | }; |
Luka_Danilovic | 2:f3d47d1e19c3 | 124 | for(int i = 0; i < LED_COUNT; i++) { |
Luka_Danilovic | 2:f3d47d1e19c3 | 125 | colors[i] = led_colour; |
Luka_Danilovic | 2:f3d47d1e19c3 | 126 | } |
Luka_Danilovic | 2:f3d47d1e19c3 | 127 | } |
Luka_Danilovic | 2:f3d47d1e19c3 | 128 | |
Luka_Danilovic | 2:f3d47d1e19c3 | 129 | /* Playing only */ |
Luka_Danilovic | 2:f3d47d1e19c3 | 130 | else if (audio_state == "Playing") { //Else If Audio Is Playing |
Luka_Danilovic | 2:f3d47d1e19c3 | 131 | |
Luka_Danilovic | 2:f3d47d1e19c3 | 132 | status_msg.data = "A."; |
Luka_Danilovic | 2:f3d47d1e19c3 | 133 | led_colour = (rgb_color) { |
Luka_Danilovic | 2:f3d47d1e19c3 | 134 | 0, 0, 255 |
Luka_Danilovic | 2:f3d47d1e19c3 | 135 | }; |
Luka_Danilovic | 2:f3d47d1e19c3 | 136 | for(int i = 0; i < LED_COUNT; i++) { |
Luka_Danilovic | 2:f3d47d1e19c3 | 137 | colors[i] = led_colour; |
Luka_Danilovic | 2:f3d47d1e19c3 | 138 | } |
Luka_Danilovic | 2:f3d47d1e19c3 | 139 | } |
Luka_Danilovic | 2:f3d47d1e19c3 | 140 | |
Luka_Danilovic | 2:f3d47d1e19c3 | 141 | /* Not moing and not playing */ |
Luka_Danilovic | 2:f3d47d1e19c3 | 142 | else { |
Luka_Danilovic | 2:f3d47d1e19c3 | 143 | status_msg.data = "N."; //Robot Is Neither Moving Nor Playing Audio |
Luka_Danilovic | 2:f3d47d1e19c3 | 144 | led_colour = (rgb_color) { |
Luka_Danilovic | 2:f3d47d1e19c3 | 145 | 255, 0, 0 |
Luka_Danilovic | 2:f3d47d1e19c3 | 146 | }; |
Luka_Danilovic | 2:f3d47d1e19c3 | 147 | for(int i = 0; i < LED_COUNT; i++) { |
Luka_Danilovic | 2:f3d47d1e19c3 | 148 | colors[i] = led_colour; |
Luka_Danilovic | 2:f3d47d1e19c3 | 149 | } |
Luka_Danilovic | 2:f3d47d1e19c3 | 150 | } |
Luka_Danilovic | 2:f3d47d1e19c3 | 151 | |
Luka_Danilovic | 2:f3d47d1e19c3 | 152 | |
Luka_Danilovic | 2:f3d47d1e19c3 | 153 | /* Write to leds */ |
Luka_Danilovic | 2:f3d47d1e19c3 | 154 | ledStripFront.write(colors, LED_COUNT); |
Luka_Danilovic | 2:f3d47d1e19c3 | 155 | ledStripBack.write(colors, LED_COUNT); |
Luka_Danilovic | 2:f3d47d1e19c3 | 156 | status_pub.publish(&status_msg); |
Luka_Danilovic | 2:f3d47d1e19c3 | 157 | wait_ms(200); |
Luka_Danilovic | 2:f3d47d1e19c3 | 158 | } |
Luka_Danilovic | 2:f3d47d1e19c3 | 159 | } |
Luka_Danilovic | 2:f3d47d1e19c3 | 160 | } |
Luka_Danilovic | 2:f3d47d1e19c3 | 161 | } |