with the tof code

Dependencies:   mbed Servo ros_lib_kinetic

Revision:
7:8248af58df5a
Parent:
6:2cc2aac35868
--- a/RGB_LED/LED.cpp	Tue Nov 12 12:56:02 2019 +0000
+++ b/RGB_LED/LED.cpp	Tue Nov 19 12:55:44 2019 +0000
@@ -24,6 +24,10 @@
 
     // Initial condition of output enables
     _DIAG_RED = 0;
+    
+    //Initialise battery level
+    _battery_level = 0;
+    
 }
 
 /*--------------------------------------------------------------------------------
@@ -79,6 +83,19 @@
 }
 
 /*--------------------------------------------------------------------------------
+Function name: ORANGE_LED ()
+Input Parameters: N/A
+Output Parameters: N/A
+Description: Turns on orange led
+----------------------------------------------------------------------------------*/
+void cRGB_LED::orange_led()
+{
+    _DIAG_RED = 1.0f;      //Enable Red
+    _DIAG_BLU = 0.0f;    //Disable Blue
+    _DIAG_GRN = 0.647f;   //Enable Green at 35% duty 
+}
+
+/*--------------------------------------------------------------------------------
 Function name: led_off()
 Input Parameters: N/A
 Output Parameters: N/A
@@ -89,4 +106,42 @@
     _DIAG_RED = 0;  //Disable Red
     _DIAG_BLU = 0;  //Disable Blue
     _DIAG_GRN = 0;  //Disable Green
-}
\ No newline at end of file
+}
+
+
+/*--------------------------------------------------------------------------------
+Function name: record_power()
+Input Parameters: vBatt - the battery voltage level
+Output Parameters: N/A
+Description: Stores an input battery level privately in the LED class
+----------------------------------------------------------------------------------*/
+void cRGB_LED::record_power(float vBatt)
+{
+    _battery_level = vBatt; //Record battery level for LED class
+}
+
+/*--------------------------------------------------------------------------------
+Function name: display_power()
+Input Parameters: N/A
+Output Parameters: N/A
+Description: Sets the LED colour based on the input voltage levels
+----------------------------------------------------------------------------------*/
+void cRGB_LED::display_power()
+{
+    if (_battery_level >= 4.0f)
+    {
+        green_led();  //Power above 70%      
+    }
+    else if (_battery_level < 4.0f){
+    
+        orange_led();  //Power above 40%
+    }
+    else if (_battery_level < 3.70f) 
+    {
+        red_led();  //Low power needs charging
+    }
+    else{
+        
+        //SOMETHING WENT WRONG!
+    }  
+}