00001 /* 00002 * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) 00003 * 00004 * Licensed under the Apache License, Version 2.0 (the "License"); 00005 * you may not use this file except in compliance with the License. 00006 * You may obtain a copy of the License at 00007 * 00008 * http://www.apache.org/licenses/LICENSE-2.0 00009 00010 * Unless required by applicable law or agreed to in writing, software 00011 * distributed under the License is distributed on an "AS IS" BASIS, 00012 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 00013 * See the License for the specific language governing permissions and 00014 * limitations under the License. 00015 */ 00016 00017 00018 //ROS typedefs 00019 #include "ros/ros.h" 00020 #include <cob_msgs/PowerState.h> 00021 #include <cob_msgs/EmergencyStopState.h> 00022 #include <std_msgs/Float64.h> 00023 #include <algorithm> 00024 00025 class cob_voltage_control_config 00026 { 00027 public: 00028 double min_voltage; 00029 double max_voltage; 00030 double max_voltage_res; 00031 int num_voltage_port; 00032 int num_em_stop_port; 00033 int num_scanner_em_port; 00034 }; 00035 00036 class cob_voltage_control_data 00037 { 00038 // autogenerated: don't touch this class 00039 public: 00040 //configuration data 00041 00042 //input data 00043 int in_phidget_voltage; 00044 int in_phidget_current; 00045 00046 //output data 00047 cob_msgs::PowerState out_pub_power_state_; 00048 cob_msgs::EmergencyStopState out_pub_em_stop_state_; 00049 std_msgs::Float64 out_pub_voltage_; 00050 std_msgs::Float64 out_pub_current_; 00051 }; 00052 00053 //document how this class has to look 00054 //never change after first generation 00055 class cob_voltage_control_impl 00056 { 00057 00058 public: 00059 00060 //CPhidgetInterfaceKitHandle IFK; 00061 cob_voltage_control_impl() 00062 { 00063 //user specific code 00064 } 00065 void configure() 00066 { 00067 } 00068 void update(cob_voltage_control_data &data, cob_voltage_control_config config) 00069 { 00070 //user specific code 00071 //Get Battery Voltage 00072 double voltage_raw = 0; 00073 voltage_raw = data.in_phidget_voltage; 00074 //Get Battery Current 00075 double current_raw = 0; 00076 current_raw = data.in_phidget_current; 00077 00078 ROS_DEBUG("voltage_raw: %f", voltage_raw); 00079 ROS_DEBUG("current_raw: %f", current_raw); 00080 00081 //Calculation of real voltage 00082 //max_voltage = 50V ; max_counts = 999 00083 //from measurements: 49.1 V => 486 counts 00084 double max_counts = 693.0; // 3v => max 00085 double voltage = voltage_raw * config.max_voltage_res/max_counts; 00086 data.out_pub_voltage_.data = voltage; 00087 ROS_DEBUG("voltage %f", voltage); 00088 00089 //current calculation 00090 //500 counts => 0 A 00091 //999 counts => 2.5 A 00092 //000 counts => -2.5 A 00093 double count_min = 611; 00094 double count_max = 450; 00095 double v_min = -3.0; 00096 double v_max = 1.6; 00097 00098 double current = v_min+(v_max - v_min)*(current_raw-count_min)/(count_max - count_min); 00099 data.out_pub_current_.data = current; 00100 ROS_DEBUG("current %f", current); 00101 00102 bool charging; 00103 if (current > 0){charging = true;} 00104 else {charging = false;} 00105 00106 //Linear calculation for percentage 00107 double percentage = (voltage - config.min_voltage) * 100/(config.max_voltage - config.min_voltage); 00108 percentage = std::min(percentage, 100.0); 00109 00110 data.out_pub_power_state_.header.stamp = ros::Time::now(); 00111 data.out_pub_power_state_.voltage = voltage; 00112 data.out_pub_power_state_.current = current; 00113 data.out_pub_power_state_.power_consumption = voltage * current; 00114 data.out_pub_power_state_.relative_remaining_capacity = percentage; 00115 data.out_pub_power_state_.charging = charging; 00116 00117 } 00118 00119 void exit() 00120 { 00121 } 00122 00123 };