led.cpp

00001 #include "mbed.h"
00002 #include "PololuLedStrip.h"
00003 #include <string>
00004 #include <ros.h>
00005 #include <std_msgs/String.h>
00006 //#include <nav_msgs/Odometry.h>
00007 #include <geometry_msgs/Twist.h>
00008 
00009 PololuLedStrip ledStripFront(PC_9);
00010 PololuLedStrip ledStripBack(PC_8);
00011 
00012 #define LED_COUNT 30
00013 #define MaxVelocity 0.5f
00014 rgb_color colors[LED_COUNT];
00015 
00016 
00017 ros::NodeHandle nh;
00018 
00019 //ROS Audio Status Callback
00020 string audio_state = "NothingSpecial";
00021 
00022 void AudioStatusCB(const std_msgs::String &status)
00023 {
00024     audio_state = status.data;
00025 }
00026 
00027 float vel = 0.0f;
00028 float ang = 0.0f;
00029 
00030 
00031 /* Uncomment this For Testing With Controller */
00032 void cmdVelCB(const geometry_msgs::Twist &twist) {
00033     vel = twist.linear.x;
00034     ang = twist.angular.z;
00035     nh.loginfo("received twist");
00036 }
00037 
00038 float Map(float x, float in_min, float in_max, float out_min, float out_max)
00039 {
00040     return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
00041 }
00042 
00043 std_msgs::String status_msg;
00044 ros::Publisher status_pub("status", &status_msg); // Instance for ROS publisher (String Message)
00045 
00046 ros::Subscriber<std_msgs::String> AudioStatus("audio_status", &AudioStatusCB);
00047 
00048 /* Uncomment this for testing with controller */
00049 ros::Subscriber<geometry_msgs::Twist> cmd_vel_sub("cmd_vel", &cmdVelCB);
00050 
00051 int main()
00052 {
00053 
00054     int LEDBrightness = 128;
00055     float led_tmp = LED_COUNT/4;
00056     int led_num = floor(led_tmp);
00057     nh.getHardware()->setBaud(460800);      //set ROSSERIAL baud rate
00058     nh.initNode();
00059     nh.advertise(status_pub);                  
00060     nh.subscribe(AudioStatus);
00061 //    nh.subscribe(Velocity);
00062     nh.subscribe(cmd_vel_sub);
00063 
00064     rgb_color led_colour = (rgb_color) {
00065         0, 0, 0
00066     };
00067 
00068     while(true) {
00069 
00070         if(!nh.connected()) {
00071             nh.spinOnce();
00072         } else {
00073 
00074             while(nh.connected()) {
00075                 nh.spinOnce();
00076                 
00077                 /* Moving and Playing */
00078                 if (((abs(vel) || abs(ang)) > 0.01f) && (audio_state == "Playing")) {
00079                     status_msg.data = "M & A";
00080                     
00081                     /* Take the bigger of the two (angular or linear) */
00082                     if(abs(vel) >= abs(ang)) {
00083                         LEDBrightness = floor(Map(abs(vel), 0, MaxVelocity, 0, 255));
00084                     } else {
00085                         LEDBrightness = floor(Map(abs(ang), 0, MaxVelocity, 0, 255));
00086                     }
00087                     
00088                     /* Set colour */
00089                     led_colour = (rgb_color) {
00090                         0, 0, 255
00091                     };
00092                     
00093                     /* Populate colours for each led */
00094                     for(int i = 0; i < led_num; i++) {
00095                         colors[i] = led_colour;
00096                     }
00097                     
00098                     led_colour = (rgb_color) {
00099                         0, LEDBrightness, 0
00100                     };
00101                     for(int i = led_num; i < ((3*led_num)+1); i++) {
00102                         colors[i] = led_colour;
00103                     }
00104                     led_colour = (rgb_color) {
00105                         0, 0, 255
00106                     };
00107                     for(int i = ((3*led_num)+1); i < LED_COUNT; i++) {
00108                         colors[i] = led_colour;
00109                     }
00110                 } 
00111                 
00112                 /* Moing only */
00113                 else if((abs(vel) || abs(ang)) > 0.01f) {     //If Not In DeadZone (Robot Is Moving)
00114                     status_msg.data = "M.";
00115                     
00116                     if(abs(vel) >= abs(ang)) {
00117                         LEDBrightness = floor(Map(abs(vel), 0, MaxVelocity, 0, 255));
00118                     } else {
00119                         LEDBrightness = floor(Map(abs(ang), 0, MaxVelocity, 0, 255));
00120                     }
00121                     led_colour = (rgb_color) {
00122                         0, LEDBrightness, 0
00123                     };
00124                     for(int i = 0; i < LED_COUNT; i++) {
00125                         colors[i] = led_colour;
00126                     }
00127                 } 
00128                 
00129                 /* Playing only */
00130                 else if (audio_state == "Playing") {           //Else If Audio Is Playing
00131                 
00132                 status_msg.data = "A.";
00133                     led_colour = (rgb_color) {
00134                         0, 0, 255
00135                     };
00136                     for(int i = 0; i < LED_COUNT; i++) {
00137                         colors[i] = led_colour;
00138                     }
00139                 } 
00140                 
00141                 /* Not moing and not playing */
00142                 else {   
00143                 status_msg.data = "N.";                                     //Robot Is Neither Moving Nor Playing Audio
00144                     led_colour = (rgb_color) {
00145                         255, 0, 0
00146                     };
00147                     for(int i = 0; i < LED_COUNT; i++) {
00148                         colors[i] = led_colour;
00149                     }
00150                 }
00151 
00152 
00153                 /* Write to leds */
00154                 ledStripFront.write(colors, LED_COUNT);
00155                 ledStripBack.write(colors, LED_COUNT);
00156                 status_pub.publish(&status_msg);
00157                 wait_ms(200);
00158             }
00159         }
00160     }
00161 }