Diff: PROJ515.cpp

Revision:

10:7d954fba5e7a

Parent:

9:2d9a0c9e5456

Child:

11:955c2ed70de2

--- a/PROJ515.cpp	Wed May 08 10:29:11 2019 +0000
+++ b/PROJ515.cpp	Wed May 08 13:55:17 2019 +0000
@@ -13,7 +13,7 @@
 float wPeriod = 10;    // Wait period(ms) for ROS publisher to finish publishing
 
 char gps_c = 0;                                    // GPS serial character
-int sats_n = 0;                                    // GPS Satelite count
+int  sats_n = 0;                                    // GPS Satelite count
 
 int main()
 {
@@ -23,6 +23,9 @@
     advRosPub();                            // Adverstise Ultrasound Topic
     nh.advertise(gps_odom_pub);             // Adverstise Odometry topic
     
+    nh.subscribe(AudioStatus);
+    nh.subscribe(Velocity);
+    
     setupRosMsg();                          // Setup the message constant values
     gps_odom_msg.header.frame_id = frameID_g;        // Pack ROS frame ID
     gps_odom_msg.status.status = UNAGUMENTED;      // Location fix method
@@ -40,6 +43,11 @@
     gps_odom_msg.position_covariance_type = UNNWN; // Covariance type
     gpsSer.baud(GPS_Baud);                         // Set Baud rate for GPS Serial
     
+    int LEDBrightness = 128;
+    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(!nh.connected()) {                // While node handler is !connected
         nh.spinOnce();                      // Attempt to connect & synchronise
@@ -71,6 +79,51 @@
         if (sats_n > 0){
             leds = LEDS_ON;
         }
+        
+        nh.spinOnce();
+        if (((abs(vel) || abs(ang)) > 0.1f) && (audio_state == "Playing")){
+            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++){
+                colors[i] = led_colour;
+            }
+            led_colour = (rgb_color) {LEDBrightness, 0, 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;
+            }
+        } else if((abs(vel) || abs(ang)) > 0.1f) {     //If Not In DeadZone (Robot Is Moving)
+            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;
+            }
+        } 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++){
+                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++){
+                colors[i] = led_colour;
+            }
+        }
+             
+        ledStripFront.write(colors, LED_COUNT);
+        ledStripBack.write(colors, LED_COUNT);
+        wait_ms(5);
     }
 }
 
@@ -246,3 +299,16 @@
     ultrasound_pub_cb4.publish(&ultrasound_msg_cb4);
     wait_ms(wPeriod);
 }
+
+void AudioStatusCB(const std_msgs::String &status) {
+    audio_state = status.data;
+}
+
+oid VelocityCB(const nav_msgs::Odometry &odom) {
+    vel = odom.twist.twist.linear.x;
+    ang = odom.twist.twist.angular.z;
+}
+
+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;
+}
\ No newline at end of file