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

diff -r 29541cb65b8a -r f3d47d1e19c3 WS2812.lib
--- 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

diff -r 29541cb65b8a -r f3d47d1e19c3 led.cpp
--- /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);
+            }
+        }
+    }
+}

diff -r 29541cb65b8a -r f3d47d1e19c3 led.h
--- /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

diff -r 29541cb65b8a -r f3d47d1e19c3 main.cpp
--- 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);
-            }
-        }
-    }
-}

diff -r 29541cb65b8a -r f3d47d1e19c3 ros_lib_kinetic.lib
--- 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