From 45e24e8f08df8ebb1461c9846d56e283ecbfcddf Mon Sep 17 00:00:00 2001
From: Irene Garcia Camacho <igarcia@iri.upc.edu>
Date: Mon, 3 Apr 2017 12:45:57 +0200
Subject: [PATCH] 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 | 20 ++--
src/examples/darwin_smart_charger_test.cpp | 105 +++++++++++++++++++++
2 files changed, 118 insertions(+), 7 deletions(-)
create mode 100644 src/examples/darwin_smart_charger_test.cpp
diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt
index a21e19c..f505e95 100644
--- a/src/examples/CMakeLists.txt
+++ b/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
diff --git a/src/examples/darwin_smart_charger_test.cpp b/src/examples/darwin_smart_charger_test.cpp
new file mode 100644
index 0000000..9e575f3
--- /dev/null
+++ b/src/examples/darwin_smart_charger_test.cpp
@@ -0,0 +1,105 @@
+
+/* 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;
+ }
+}
--
GitLab