evarobot_battery.cpp
Go to the documentation of this file.
00001 #include "evarobot_battery/evarobot_battery.h"
00002 
00003 #include "IMI2C.h"
00004 #include "evarobot_battery/LTC2943.h"
00005 
00006 #define AUTOMATIC_MODE                                  0xC0
00007 #define SCAN_MODE                                               0x80
00008 #define MANUAL_MODE                                             0x40
00009 #define SLEEP_MODE                                              0x00
00010 
00011 #define PRESCALER_1                                             0x00
00012 #define PRESCALER_4                                             0x08
00013 #define PRESCALER_16                                    0x10
00014 #define PRESCALER_64                                    0x18
00015 #define PRESCALER_256                                   0x20
00016 #define PRESCALER_1024                                  0x28
00017 #define PRESCALER_4096                                  0x30
00018 #define PRESCALER_4096_2                                0x31
00019 
00020 #define ALERT_MODE                                              0x04
00021 #define CHARGE_COMPLETE_MODE                    0x02
00022 #define DISABLE_ALCC_PIN                                0x00
00023 
00024 #define SHUTDOWN_MODE                                   0x01
00025 
00026 using namespace std;
00027 
00028 int i_error_code = 0;
00029 
00030 
00031 void ProduceDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat)
00032 {
00033     if(i_error_code<0)
00034     {
00035         stat.summaryf(diagnostic_msgs::DiagnosticStatus::ERROR, "%s", GetErrorDescription(i_error_code).c_str());
00036         i_error_code = 0;
00037     }
00038     else
00039     {
00040         stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "No collision!");
00041     }
00042 }
00043 
00044 int main(int argc, char *argv[])
00045 {
00046         // Semaphore
00047         key_t key;
00048         sem_t *mutex;
00049         FILE *fd;
00050         
00051         im_msgs::Battery msg;
00052         double d_charge_last = -1.0;
00053         
00054         // ROS PARAMS
00055         double d_frequency;
00056         double d_min_freq, d_max_freq;
00057         double d_high_voltage_th, d_low_voltage_th;
00058         double d_max_current;
00059         double d_bat_capacity, d_low_capacity;
00060         bool b_always_on;
00061         string str_i2cDevice;
00062         
00063         //name the shared memory segment
00064         key = 1005;
00065 
00066         //create & initialize semaphore
00067         mutex = sem_open(SEM_NAME,O_CREAT,0644,1);
00068         if(mutex == SEM_FAILED)
00069         {
00070                 ROS_INFO(GetErrorDescription(-125).c_str());
00071         i_error_code = -125;
00072         
00073                 sem_unlink(SEM_NAME);
00074                 
00075                 return(-1);
00076         }
00077         
00078         ros::init(argc, argv, "evarobot_battery");
00079         ros::NodeHandle n;
00080 
00081         n.param("evarobot_battery/alwaysOn", b_always_on, false);
00082         n.param("evarobot_battery/minFreq", d_min_freq, 0.2);
00083         n.param("evarobot_battery/maxFreq", d_max_freq, 1.0);
00084         n.param("evarobot_battery/frequency", d_frequency, 1.0);
00085         n.param("evarobot_battery/highVTh", d_high_voltage_th, 13.0);
00086         n.param("evarobot_battery/lowVTh", d_low_voltage_th, 11.0);
00087         n.param("evarobot_battery/maxCurr", d_max_current, 7.0);
00088         n.param("evarobot_battery/batCapacity", d_bat_capacity, 13000.0);
00089         n.param("evarobot_battery/lowCapacity", d_low_capacity, 7000.0);
00090         n.param<std::string>("evarobot_minimu9/i2cDevice", str_i2cDevice, "/dev/i2c-1");
00091 
00092         ros::Publisher pub_bat = n.advertise<im_msgs::Battery>("battery", 10);
00093 
00094         LTC2943 batteryReader = LTC2943(0x64, "/dev/i2c-1", mutex);
00095 
00096         //sem_wait(mutex);
00097 
00098         try
00099         {
00100                 batteryReader.setControlRegister(AUTOMATIC_MODE,PRESCALER_4096, ALERT_MODE);
00101                 batteryReader.setVoltageThresholdHigh(d_high_voltage_th);
00102                 batteryReader.setVoltageThresholdLow(d_low_voltage_th);
00103         }catch(int e)
00104         {
00105                 ROS_INFO(GetErrorDescription(e).c_str());
00106                 i_error_code = e;
00107         }
00108         
00109         //sem_post(mutex);
00110 
00111         // Diagnostics
00112         diagnostic_updater::Updater updater;
00113         updater.setHardwareID("LTC2943");
00114         updater.add("evarobot_battery", &ProduceDiagnostics);
00115 
00116         diagnostic_updater::HeaderlessTopicDiagnostic pub_freq("battery", updater,
00117             diagnostic_updater::FrequencyStatusParam(&d_min_freq, &d_max_freq, 0.1, 10));
00118         
00119         // Define frequency
00120         ros::Rate loop_rate(10.0);      
00121         while(ros::ok())
00122         {
00123                 
00124                 //sem_wait(mutex);
00125 
00126                 int i_status_register = 0;
00127 
00128 //              i_status_register = batteryReader.readStatusRegister();
00129 //              ROS_INFO("Status Register: %d", i_status_register);
00130 //              i_status = batteryReader.resetAlertStatus();
00131 
00132                 try
00133                 {
00134                         batteryReader.readRegisters();
00135                 }catch(int e)
00136                 {
00137                         ROS_INFO(GetErrorDescription(e).c_str());
00138                         i_error_code = e;
00139                 }
00140 
00141                 msg.voltage = batteryReader.readVoltage();
00142                 msg.current = batteryReader.readCurrent();
00143                 msg.charge = batteryReader.readAccumulatedCharge();
00144                 msg.temperature = batteryReader.readTemperature();
00145                         
00146                 //sem_post(mutex);
00147                 
00148                 if(msg.voltage >= d_high_voltage_th)
00149                 {
00150                         // High Voltage
00151                         ROS_INFO(GetErrorDescription(-126).c_str());
00152                         i_error_code = -126;
00153                 }
00154                 else if(msg.voltage <= d_low_voltage_th)
00155                 {
00156                         // Low voltage
00157                         ROS_INFO(GetErrorDescription(-127).c_str());
00158                         i_error_code = -127;
00159                 }
00160                 
00161                 if(msg.charge <= d_low_capacity)
00162                 {
00163                         // Low battery
00164                         ROS_INFO(GetErrorDescription(-129).c_str());
00165                         i_error_code = -129;                    
00166                 }
00167                 
00168                 if(d_charge_last > 0 && d_charge_last > msg.charge)
00169                 {
00170                         // Charging
00171                         msg.charge_state = 1;
00172                 }
00173                 else if(d_charge_last > 0 && d_charge_last < msg.charge)
00174                 {
00175                         // Discharging
00176                         msg.charge_state = 0;
00177                 }
00178                 
00179                 if(msg.charge >= d_bat_capacity)
00180                 {
00181                         // Charged
00182                         msg.charge_state = 2;
00183                 }
00184                 
00185                 d_charge_last = msg.charge;             
00186                 
00187                 if(pub_bat.getNumSubscribers() > 0 || b_always_on)
00188                 {
00189                         pub_bat.publish(msg);
00190                         pub_freq.tick();
00191                 }
00192                 
00193                 updater.update();
00194                 ros::spinOnce();
00195                 loop_rate.sleep();
00196                 
00197         }
00198 
00199         return 0;
00200 }


evarobot_battery
Author(s): Mehmet Akcakoca
autogenerated on Fri Feb 12 2016 01:15:13