From 47cf3605ad409585aee23a9e7aa1e290471247fd Mon Sep 17 00:00:00 2001
From: Fumio KANEHIRO <fkanehiro@gmail.com>
Date: Sat, 18 Apr 2015 14:31:09 +0900
Subject: [PATCH] adds the third argument to read_power of iob.h

---
 lib/io/iob.cpp                                  | 3 ++-
 lib/io/iob.h                                    | 3 ++-
 rtc/RobotHardware/RobotHardwareService_impl.cpp | 2 +-
 rtc/RobotHardware/robot.cpp                     | 4 ++--
 rtc/RobotHardware/robot.h                       | 3 ++-
 5 files changed, 9 insertions(+), 6 deletions(-)

diff --git a/lib/io/iob.cpp b/lib/io/iob.cpp
index 6d475904a32..a8c3e4c14a5 100644
--- a/lib/io/iob.cpp
+++ b/lib/io/iob.cpp
@@ -562,10 +562,11 @@ int number_of_substeps()
     return 5;
 }
 
-int read_power(double *voltage, double *current)
+int read_power(double *voltage, double *current, double *battery)
 {
     *voltage = ((double)random()-RAND_MAX/2)/(RAND_MAX/2)*1+48;
     *current = ((double)random()-RAND_MAX/2)/(RAND_MAX/2)*0.5+1;
+    *battery = ((double)random()-RAND_MAX/2)/(RAND_MAX/2)*0.5+50;
     return TRUE;
 }
 
diff --git a/lib/io/iob.h b/lib/io/iob.h
index f15f1f2a443..1167b8f8f6f 100644
--- a/lib/io/iob.h
+++ b/lib/io/iob.h
@@ -572,9 +572,10 @@ extern "C"{
      * @brief		read status of power source
      * @param v		voltage[V]
      * @param a		current[A]
+     * @param b		remaining battery level[%]
      * @return		TRUE or FALSE
      */
-    int read_power(double *v, double *a);
+    int read_power(double *v, double *a, double *b);
     //@}
 
     /**
diff --git a/rtc/RobotHardware/RobotHardwareService_impl.cpp b/rtc/RobotHardware/RobotHardwareService_impl.cpp
index 7ccda82c74b..e90de29d057 100644
--- a/rtc/RobotHardware/RobotHardwareService_impl.cpp
+++ b/rtc/RobotHardware/RobotHardwareService_impl.cpp
@@ -68,7 +68,7 @@ void RobotHardwareService_impl::getStatus(OpenHRP::RobotHardwareService::RobotSt
         m_robot->readForceSensor(i, rs->force[i].get_buffer());
     }
 
-    m_robot->readPowerStatus(rs->voltage, rs->current);
+    m_robot->readPowerStatus(rs->voltage, rs->current, rs->battery);
 }
 
 CORBA::Boolean RobotHardwareService_impl::power(const char* jname, OpenHRP::RobotHardwareService::SwitchStatus ss)
diff --git a/rtc/RobotHardware/robot.cpp b/rtc/RobotHardware/robot.cpp
index f1275cecf17..659cc7ba115 100644
--- a/rtc/RobotHardware/robot.cpp
+++ b/rtc/RobotHardware/robot.cpp
@@ -459,9 +459,9 @@ void robot::writeVelocityCommands(const double *i_commands)
     write_command_velocities(i_commands);
 }
 
-void robot::readPowerStatus(double &o_voltage, double &o_current)
+void robot::readPowerStatus(double &o_voltage, double &o_current, double &o_battery)
 {
-    read_power(&o_voltage, &o_current);
+    read_power(&o_voltage, &o_current, &o_battery);
 }
 
 int robot::readCalibState(int i)
diff --git a/rtc/RobotHardware/robot.h b/rtc/RobotHardware/robot.h
index 7aa89f22330..1bcdfc37d6a 100644
--- a/rtc/RobotHardware/robot.h
+++ b/rtc/RobotHardware/robot.h
@@ -127,8 +127,9 @@ class robot : public hrp::Body
        \brief read voltage and current of the robot power source
        \param o_voltage voltage
        \param o_current current
+       \param o_battery remaining battery level
      */
-    void readPowerStatus(double &o_voltage, double &o_current);
+    void readPowerStatus(double &o_voltage, double &o_current, double &o_battery);
 
     /**
        \brief read array of all joint angles[rad]