Diff: PROJ515.cpp

Revision:

12:1a3272d67500

Parent:

11:955c2ed70de2

Child:

14:b972d0d0f63d

--- a/PROJ515.cpp	Wed May 08 14:50:32 2019 +0000
+++ b/PROJ515.cpp	Thu May 09 18:37:25 2019 +0000
@@ -13,20 +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
-
-void AudioStatusCB(const std_msgs::String &status) {
-    audio_state = status.data;
-}
-
-void VelocityCB(const nav_msgs::Odometry &odom) {
-    vel = odom.twist.twist.linear.x;
-    ang = odom.twist.twist.angular.z;
-}
-
-// Instance for ROS subscribers
-ros::Subscriber<std_msgs::String> AudioStatus("audio_status", &AudioStatusCB);
-ros::Subscriber<nav_msgs::Odometry> Velocity("wheel_odom/exact_odom", &VelocityCB);
+int sats_n = 0;                                    // GPS Satelite count
 
 int main()
 {
@@ -36,9 +23,6 @@
     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
@@ -56,11 +40,6 @@
     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
@@ -92,51 +71,6 @@
         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);
     }
 }
 
@@ -312,9 +246,3 @@
     ultrasound_pub_cb4.publish(&ultrasound_msg_cb4);
     wait_ms(wPeriod);
 }
-
-
-
-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