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