Luka Danilovic / Mbed 2 deprecated LED_test

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?

UserRevisionLine numberNew 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 }