$search
00001 //ROS typedefs 00002 #include "ros/ros.h" 00003 #include <pr2_msgs/PowerBoardState.h> 00004 #include <pr2_msgs/PowerState.h> 00005 #include <cob_relayboard/EmergencyStopState.h> 00006 00007 #include <libphidgets/phidget21.h> 00008 00009 00010 class cob_voltage_control_config 00011 { 00012 public: 00013 double min_voltage; 00014 double max_voltage; 00015 double max_voltage_res; 00016 int num_voltage_port; 00017 int num_em_stop_port; 00018 int num_scanner_em_port; 00019 00020 }; 00021 00022 class cob_voltage_control_data 00023 { 00024 // autogenerated: don't touch this class 00025 public: 00026 //configuration data 00027 00028 //input data 00029 00030 //output data 00031 pr2_msgs::PowerBoardState out_pub_em_stop_state_; 00032 pr2_msgs::PowerState out_pub_powerstate_; 00033 cob_relayboard::EmergencyStopState out_pub_relayboard_state; 00034 }; 00035 00036 //document how this class has to look 00037 //never change after first generation 00038 00039 00040 class cob_voltage_control_impl 00041 { 00042 00043 public: 00044 00045 CPhidgetInterfaceKitHandle IFK; 00046 00047 00048 00049 00050 cob_voltage_control_impl() 00051 { 00052 //user specific code 00053 } 00054 void configure() 00055 { 00056 //user specific code 00057 //init and open phidget 00058 int numInputs, numOutputs, numSensors, numAnalog; 00059 int err; 00060 00061 IFK = 0; 00062 CPhidget_enableLogging(PHIDGET_LOG_VERBOSE, NULL); 00063 CPhidgetInterfaceKit_create(&IFK); 00064 00065 //CPhidgetInterfaceKit_set_OnSensorChange_Handler(IFK, IFK_SensorChangeHandler, NULL); 00066 00067 /*CPhidgetInterfaceKit_set_OnInputChange_Handler(IFK, IFK_InputChangeHandler, NULL); 00068 CPhidgetInterfaceKit_set_OnOutputChange_Handler(IFK, IFK_OutputChangeHandler, NULL); 00069 CPhidgetInterfaceKit_set_OnSensorChange_Handler(IFK, IFK_SensorChangeHandler, NULL); 00070 CPhidget_set_OnAttach_Handler((CPhidgetHandle)IFK, IFK_AttachHandler, NULL); 00071 CPhidget_set_OnDetach_Handler((CPhidgetHandle)IFK, IFK_DetachHandler, NULL); 00072 CPhidget_set_OnError_Handler((CPhidgetHandle)IFK, IFK_ErrorHandler, NULL);*/ 00073 00074 //opening phidget 00075 CPhidget_open((CPhidgetHandle)IFK, -1); 00076 00077 //wait 5 seconds for attachment 00078 ROS_INFO("waiting for phidgets attachement..."); 00079 if((err = CPhidget_waitForAttachment((CPhidgetHandle)IFK, 10000)) != EPHIDGET_OK ) 00080 { 00081 const char *errStr; 00082 CPhidget_getErrorDescription(err, &errStr); 00083 ROS_ERROR("Error waiting for attachment: (%d): %s",err,errStr); 00084 return; 00085 } 00086 ROS_INFO("... attached"); 00087 00088 int sernum, version; 00089 const char *deviceptr, *label; 00090 CPhidget_getDeviceType((CPhidgetHandle)IFK, &deviceptr); 00091 CPhidget_getSerialNumber((CPhidgetHandle)IFK, &sernum); 00092 CPhidget_getDeviceVersion((CPhidgetHandle)IFK, &version); 00093 CPhidget_getDeviceLabel((CPhidgetHandle)IFK, &label); 00094 00095 ROS_INFO("%s", deviceptr); 00096 ROS_INFO("Version: %8d SerialNumber: %10d", version, sernum); 00097 ROS_INFO("Label: %s", label); 00098 CPhidgetInterfaceKit_getOutputCount((CPhidgetInterfaceKitHandle)IFK, &numOutputs); 00099 CPhidgetInterfaceKit_getInputCount((CPhidgetInterfaceKitHandle)IFK, &numInputs); 00100 CPhidgetInterfaceKit_getSensorCount((CPhidgetInterfaceKitHandle)IFK, &numSensors); 00101 00102 ROS_INFO("Sensors:%d Inputs:%d Outputs:%d", numSensors, numInputs, numOutputs); 00103 00104 00105 } 00106 void update(cob_voltage_control_data &data, cob_voltage_control_config config) 00107 { 00108 //user specific code 00109 00110 //Check for EM Stop 00111 int em_stop_State = -1; 00112 int scanner_stop_State = -1; 00113 CPhidgetInterfaceKit_getInputState ((CPhidgetInterfaceKitHandle)IFK, config.num_em_stop_port, &em_stop_State); 00114 CPhidgetInterfaceKit_getInputState ((CPhidgetInterfaceKitHandle)IFK, config.num_scanner_em_port, &scanner_stop_State); 00115 ROS_DEBUG("DIO: %d %d", em_stop_State, scanner_stop_State); 00116 00117 data.out_pub_em_stop_state_.header.stamp = ros::Time::now(); 00118 // pr2 power_board_state 00119 if(em_stop_State == 1) 00120 { 00121 data.out_pub_em_stop_state_.run_stop = true; 00122 //for cob the wireless stop field is misused as laser stop field 00123 data.out_pub_relayboard_state.emergency_state = 2; 00124 if(scanner_stop_State == 1) 00125 data.out_pub_em_stop_state_.wireless_stop = true; 00126 else 00127 { 00128 data.out_pub_em_stop_state_.wireless_stop = false; 00129 data.out_pub_relayboard_state.emergency_state = 1; 00130 } 00131 00132 } 00133 else 00134 { 00135 data.out_pub_em_stop_state_.run_stop = false; 00136 data.out_pub_em_stop_state_.wireless_stop = true; 00137 data.out_pub_relayboard_state.emergency_state = 1; 00138 } 00139 00140 data.out_pub_relayboard_state.emergency_button_stop = data.out_pub_em_stop_state_.run_stop; 00141 data.out_pub_relayboard_state.scanner_stop = data.out_pub_em_stop_state_.wireless_stop; 00142 00143 00144 00145 00146 //Get Battery Voltage 00147 int voltageState = -1; 00148 CPhidgetInterfaceKit_getSensorValue((CPhidgetInterfaceKitHandle)IFK, config.num_voltage_port, &voltageState); 00149 ROS_DEBUG("Sensor: %d", voltageState); 00150 00151 //Calculation of real voltage 00152 //max_voltage = 70V ; max_counts = 999 00153 double max_counts = 999.0; // see phidgit analog io docu 00154 double voltage = voltageState * config.max_voltage_res/max_counts; 00155 00156 ROS_DEBUG("Current voltage %f", voltage); 00157 00158 //Linear calculation for percentage 00159 double percentage = (voltage - config.min_voltage) * 100/(config.max_voltage - config.min_voltage); 00160 00161 00162 data.out_pub_powerstate_.header.stamp = ros::Time::now(); 00163 data.out_pub_powerstate_.power_consumption = 0.0; 00164 data.out_pub_powerstate_.time_remaining = ros::Duration(1000); 00165 data.out_pub_powerstate_.relative_capacity = percentage; 00166 00167 } 00168 00169 void exit() 00170 { 00171 CPhidget_close((CPhidgetHandle)IFK); 00172 CPhidget_delete((CPhidgetHandle)IFK); 00173 } 00174 00175 };