Code for testing the smart charger module

In terminal:
 -Default period of the smart charger and new period
 -Smart charger detected, not detected or detected and enabled
 -Start motion manager (if smart charger detected and enabled, it will execute the read operations)
 -Battery status: connected, disconnected or connected and charging
 -Shows average time to full and to empty in min (65535 it means -1)

src/examples/CMakeLists.txt

@@ -38,13 +38,19 @@ ADD_EXECUTABLE(darwin_head_tracking_test darwin_head_tracking_test.cpp)
 # link necessary libraries
 TARGET_LINK_LIBRARIES(darwin_head_tracking_test darwin_robot)
 
-# create an example application
-ADD_EXECUTABLE(darwin_arm_kin darwin_arm_kin.cpp)
-# link necessary libraries
-TARGET_LINK_LIBRARIES(darwin_arm_kin darwin_arm_kinematics)
+IF(KDL_FOUND)
+  # create an example application
+  ADD_EXECUTABLE(darwin_arm_kin darwin_arm_kin.cpp)
+  # link necessary libraries
+  TARGET_LINK_LIBRARIES(darwin_arm_kin darwin_arm_kinematics)
+
+  # create an example application
+  ADD_EXECUTABLE(darwin_leg_kin darwin_leg_kin.cpp)
+  # link necessary libraries
+  TARGET_LINK_LIBRARIES(darwin_leg_kin darwin_leg_kinematics)
+ENDIF(KDL_FOUND)
 
 # create an example application
-ADD_EXECUTABLE(darwin_leg_kin darwin_leg_kin.cpp)
+ADD_EXECUTABLE(darwin_smart_charger_test darwin_smart_charger_test.cpp)
 # link necessary libraries
-TARGET_LINK_LIBRARIES(darwin_leg_kin darwin_leg_kinematics)
-
+TARGET_LINK_LIBRARIES(darwin_smart_charger_test darwin_robot)
\ No newline at end of file

src/examples/darwin_smart_charger_test.cpp

+
+/* DRIVER of the DARWIN ROBOT firmware
+ * 
+ * Test with the robot darwin with smart charger connected to dynamixel v2, baudrate 1M
+ * 
+ * In terminal:
+ * 	Default period of the smart charger (read smart charger data period)
+ *	Smart charger detected, not detected or detected and enabled 
+ *	Start motion manager (if smart charger detected and enabled, it will execute the read operations)
+ * 	Battery status: connected, disconnected or connected and charging
+ * 	Shows average time to full and to empty in min (65535 it means -1)
+ * 
+ NOTES: First download darwin_stm32_fw with cm730_fw main code (MakeFile)
+ * 
+ * 
+ */
+
+
+
+#include "darwin_robot.h"
+#include "darwin_robot_exceptions.h"
+
+#include <iostream>
+
+//thread
+std::string robot_device="A603LOBS";
+//std::string robot_device="A4008atn";
+
+int main(int argc, char *argv[])
+{
+  unsigned int i=0;
+  unsigned short int period;
+  int control, status; //1=detected, 3=detected and enabled
+  double data[6];
+  double d;
+  TChargerData charger_data;
+
+  try{
+    CDarwinRobot darwin("Darwin",robot_device,1000000,0x02);
+    std::cout << "found darwin controller" << std::endl;
+
+  darwin.gpio_clear_led(LED_TX);
+  darwin.gpio_clear_led(LED_4);
+  darwin.gpio_clear_led(LED_3);
+  
+//Read/write smart charger period    
+    period=darwin.smart_charger_get_period();
+    std::cout << "smart charger period (ms): " << period << std::endl;
+    
+/*    darwin.smart_charger_set_period(1600);
+    std::cout << "smart charger period changed to 1600ms" << std::endl;
+    std::cout << "smart charger period (ms): " << darwin.smart_charger_get_period() << std::endl;
+*/  
+    control=darwin.smart_charger_read_control();
+    if(control==1)
+      std::cout << "smart charger detected" << std::endl;
+    else if(control==0)
+      std::cout << "smart charger not detected" << std::endl;
+    
+    darwin.smart_charger_enable();
+    control=darwin.smart_charger_read_control();
+    if(control==3)
+      std::cout << "smart charger detected and enabled" << std::endl;
+
+ /*   charger_data=darwin.smart_charger_read_data();
+    std::cout << "battery status: " << charger_data.bat_status << std::endl;
+    std::cout << "Avg time to empty: " << charger_data.avg_time_empty << std::endl;
+    std::cout << "Avg time to full: " << charger_data.avg_time_full << std::endl;
+  */
+    darwin.mm_start();
+    if(darwin.mm_is_running())
+    std::cout << "Motion manager is running" << std::endl;
+    charger_data=darwin.smart_charger_read_data();
+    if(charger_data.bat_status==0)
+      std::cout << "battery status: disconnected " << std::endl;
+    else if(charger_data.bat_status==192)
+      std::cout << "battery status: connected" << std::endl;
+    else if(charger_data.bat_status==128)
+      std::cout << "battery status: connected and charging " << std::endl;
+ //y totalmente cargado o descargado???
+    
+    //charger status --> batteries detected, AC detected, etc
+    status=darwin.smart_charger_read_status();
+    std::cout << "charger status: " << status << std::endl;
+
+
+
+    while(1){
+    std::cout << "Avg time to empty: " << charger_data.avg_time_empty << " min" << std::endl;
+    std::cout << "Avg time to full: " << charger_data.avg_time_full << " min" << std::endl;
+    std::cout << "---------------------- "  << std::endl;
+    
+    darwin.gpio_toggle_led(LED_RX); //
+      
+    usleep(500000);
+    }    
+    //std::cout << "Avg time to empty: " << darwin.smart_charger_read_time_empty() << " min" << std::endl;
+    
+    
+  //darwin.gpio_blink_led(LED_RX,1000);
+    
+  }catch(CException &e){
+    std::cout << e.what() << std::endl;
+  }
+}