00001 #include "evarobot_eio/evarobot_rgb.h"
00002
00003 const int IMRGB::LEDOFF = 0;
00004 const int IMRGB::RED = 1;
00005 const int IMRGB::GREEN = 2;
00006 const int IMRGB::YELLOW = 3;
00007 const int IMRGB::BLUE = 4;
00008 const int IMRGB::PINK = 5;
00009 const int IMRGB::TURQUOISE = 6;
00010 const int IMRGB::WHITE = 7;
00011
00012 const int IMRGB::MANUAL = 0;
00013 const int IMRGB::ON = 1;
00014 const int IMRGB::AUTONOMOUS = 2;
00015 const int IMRGB::TELEOP = 3;
00016 const int IMRGB::WANDER = 4;
00017 const int IMRGB::LOW_BATTERY = 5;
00018 const int IMRGB::CHARGING = 6;
00019 const int IMRGB::CHARGED = 7;
00020 const int IMRGB::ERROR = 8;
00021
00022 int i_error_code;
00023
00024 IMRGB::IMRGB(IMEIO * eio)
00025 {
00026 this->eio = eio;
00027 this->b_new_data = false;
00028 this->i_mode = IMRGB::MANUAL;
00029 this->f_frequency = 10.0;
00030 this->i_times = 0;
00031 this->i_color = IMRGB::LEDOFF;
00032
00033 try{
00034 this->eio->SetPinADirection(IMEIO::RGB_LED1, IMEIO::OUTPUT);
00035 this->eio->SetPinADirection(IMEIO::RGB_LED2, IMEIO::OUTPUT);
00036 this->eio->SetPinADirection(IMEIO::RGB_LED3, IMEIO::OUTPUT);
00037 }catch(int e){
00038 ROS_INFO(GetErrorDescription(e).c_str());
00039 i_error_code = e;
00040 }
00041 this->TurnLedOn(0);
00042 }
00043
00044 IMRGB::~IMRGB()
00045 {
00046 this->TurnLedOff();
00047 }
00048
00049 void IMRGB::TurnLedOff()
00050 {
00051 this->TurnLedOn(0);
00052 this->f_frequency = 10.0;
00053 }
00054
00055 void IMRGB::TurnLedOn(int i_color)
00056 {
00057 int i_dummies[3] = {1, 2, 4};
00058 bool b_rgb[3];
00059
00060 if(i_color > 7)
00061 {
00062 ROS_INFO(GetErrorDescription(-92).c_str());
00063 i_error_code = -92;
00064 }
00065
00066 for(int i = 0; i < 3; i++)
00067 {
00068 b_rgb[i] = (bool)(i_color & i_dummies[i]);
00069 }
00070 try{
00071 this->eio->SetPinAValue(IMEIO::RGB_LED1, b_rgb[0]);
00072 this->eio->SetPinAValue(IMEIO::RGB_LED2, b_rgb[1]);
00073 this->eio->SetPinAValue(IMEIO::RGB_LED3, b_rgb[2]);
00074 }catch(int e){
00075 ROS_INFO(GetErrorDescription(e).c_str());
00076 i_error_code = e;
00077 }
00078 }
00079
00080 void IMRGB::RunAutonomousMode()
00081 {
00082 int i_colors[]={IMRGB::RED, 0,
00083 IMRGB::RED, 0,
00084 IMRGB::RED, 0,
00085 IMRGB::RED, 0,
00086 -1,
00087 IMRGB::BLUE, 0,
00088 IMRGB::BLUE, 0,
00089 IMRGB::BLUE, 0,
00090 IMRGB::BLUE, 0,
00091 -1};
00092
00093 for(int i = 0; i < (sizeof(i_colors)/sizeof(*i_colors)); i++)
00094 {
00095 if(i_colors[i] < 0)
00096 {
00097 usleep(200000);
00098 ++i;
00099 if(i >= (sizeof(i_colors)/sizeof(*i_colors)))
00100 break;
00101 }
00102
00103 this->TurnLedOn(i_colors[i]);
00104 usleep(50000);
00105 }
00106
00107 this->DecreaseTimesValue();
00108 }
00109
00110 void IMRGB::FlipFlip(int i_color)
00111 {
00112 int i_colors[]={i_color, 0,
00113 i_color, 0,
00114 i_color, 0,
00115 i_color, 0,
00116 i_color, 0,
00117 i_color, 0,
00118 i_color, 0,
00119 -1};
00120
00121 for(int i = 0; i < (sizeof(i_colors)/sizeof(*i_colors)); i++)
00122 {
00123 if(i_colors[i] < 0)
00124 {
00125 usleep(500000);
00126 ++i;
00127 if(i >= (sizeof(i_colors)/sizeof(*i_colors)))
00128 break;
00129 }
00130 this->TurnLedOn(i_colors[i]);
00131
00132 usleep(50000);
00133 }
00134 }
00135
00136 void IMRGB::RunTeleopMode()
00137 {
00138 this->FlipFlip(IMRGB::BLUE);
00139 this->DecreaseTimesValue();
00140 }
00141
00142 void IMRGB::RunWanderMode()
00143 {
00144 this->FlipFlip(IMRGB::TURQUOISE);
00145 this->DecreaseTimesValue();
00146 }
00147
00148 void IMRGB::Flash(int i_color, float f_frequency)
00149 {
00150 int i_colors[]={i_color, 0};
00151 int i_sleep_time;
00152
00153 if(f_frequency == 0.0)
00154 {
00155 ROS_INFO(GetErrorDescription(-93).c_str());
00156 i_error_code = -93;
00157 return;
00158 }
00159
00160 if(f_frequency < 0)
00161 {
00162 this->TurnLedOn(i_color);
00163 }
00164 else
00165 {
00166 for(int i = 0; i < (sizeof(i_colors)/sizeof(*i_colors)); i++)
00167 {
00168 this->TurnLedOn(i_colors[i]);
00169 i_sleep_time = (int)(1000000.0 / (2.0 * f_frequency));
00170 usleep(i_sleep_time);
00171 }
00172 }
00173 }
00174
00175 void IMRGB::RunManualMode()
00176 {
00177 this->Flash(this->i_color, this->f_frequency);
00178 this->DecreaseTimesValue();
00179 }
00180
00181 void IMRGB::RunChargingMode()
00182 {
00183 this->Flash(IMRGB::GREEN, 1.0);
00184 this->DecreaseTimesValue();
00185 }
00186
00187 void IMRGB::RunChargedMode()
00188 {
00189 this->TurnLedOn(IMRGB::GREEN);
00190 this->DecreaseTimesValue();
00191 }
00192
00193 void IMRGB::RunErrorMode()
00194 {
00195 this->Flash(IMRGB::RED, 1.0);
00196 this->DecreaseTimesValue();
00197 }
00198
00199 void IMRGB::RunLowBatteryMode()
00200 {
00201 this->Flash(IMRGB::YELLOW, 1.0);
00202 this->DecreaseTimesValue();
00203 }
00204 void IMRGB::RunOpeningMode()
00205 {
00206 int i_colors[]={IMRGB::RED, IMRGB::GREEN, IMRGB::YELLOW, IMRGB::BLUE,
00207 IMRGB::PINK, IMRGB::TURQUOISE, IMRGB::WHITE, IMRGB::TURQUOISE,
00208 IMRGB::PINK, IMRGB::BLUE, IMRGB::YELLOW, IMRGB::GREEN,
00209 IMRGB::RED, IMRGB::LEDOFF};
00210
00211 for(int i = 0; i < (sizeof(i_colors)/sizeof(*i_colors)); i++)
00212 {
00213 this->TurnLedOn(i_colors[i]);
00214 usleep(500000);
00215 }
00216
00217 this->DecreaseTimesValue();
00218 }
00219
00220 bool IMRGB::DecreaseTimesValue()
00221 {
00222 this->i_times--;
00223 this->b_new_data = false;
00224
00225 if(this->i_times < 0)
00226 {
00227 this->i_times == 0;
00228 return false;
00229 }
00230
00231 return true;
00232 }
00233
00234 bool IMRGB::isNewRequest() const
00235 {
00236 return this->b_new_data;
00237 }
00238
00239 int IMRGB::GetMode() const
00240 {
00241 return this->i_mode;
00242 }
00243
00244 float IMRGB::GetFrequency() const
00245 {
00246 return this->f_frequency;
00247 }
00248
00249 int IMRGB::GetTimesValue() const
00250 {
00251 return this->i_times;
00252 }
00253
00254 bool IMRGB::callbackSetRGB(im_msgs::SetRGB::Request& request, im_msgs::SetRGB::Response& response)
00255 {
00256
00257 this->b_new_data = true;
00258 response.ret = 1;
00259
00260
00261 this->i_times = request.times;
00262
00263
00264 if(request.mode < 0 || request.mode > 8)
00265 {
00266 response.ret = 0;
00267 ROS_INFO(GetErrorDescription(-94).c_str());
00268 i_error_code = -94;
00269 }
00270 else
00271 {
00272 this->i_mode = request.mode;
00273 }
00274
00275 if(request.frequency == 0)
00276 {
00277 response.ret = 0;
00278 ROS_INFO(GetErrorDescription(-95).c_str());
00279 i_error_code = -95;
00280 }
00281 else
00282 {
00283 this->f_frequency = request.frequency;
00284 }
00285
00286 if(request.color < 0 || request.color > 7)
00287 {
00288 response.ret = 0;
00289 ROS_INFO(GetErrorDescription(-96).c_str());
00290 i_error_code = -96;
00291 }
00292 else
00293 {
00294 this->i_color = request.color;
00295 }
00296
00297 return true;
00298 }
00299
00300 void ProduceDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat)
00301 {
00302 if(i_error_code<0)
00303 {
00304 stat.summaryf(diagnostic_msgs::DiagnosticStatus::ERROR, "%s", GetErrorDescription(i_error_code).c_str());
00305 i_error_code = 0;
00306 }
00307 else
00308 {
00309 stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "EvarobotRGB: No problem.");
00310 }
00311 }
00312
00313 int main(int argc, char **argv)
00314 {
00315
00316 key_t key;
00317 sem_t *mutex;
00318 FILE * fd;
00319
00320 key = 1005;
00321
00322 mutex = sem_open(SEM_NAME,O_CREAT,0644,1);
00323 if(mutex == SEM_FAILED)
00324 {
00325 ROS_INFO(GetErrorDescription(-97).c_str());
00326 i_error_code = -97;
00327 sem_unlink(SEM_NAME);
00328
00329 return(-1);
00330 }
00331
00332
00333 string str_i2c_path;
00334
00335
00336 ros::init(argc, argv, "/evarobot_rgb");
00337 ros::NodeHandle n;
00338
00339 n.param<string>("evarobot_eio/i2c_path", str_i2c_path, "/dev/i2c-1");
00340
00341 IMEIO * eio;
00342 try{
00343 eio = new IMEIO(0b00100000, string("/dev/i2c-1"), mutex);
00344 }catch(int e){
00345 ROS_INFO(GetErrorDescription(e).c_str());
00346 i_error_code = e;
00347 }
00348
00349 IMRGB * rgb = new IMRGB(eio);
00350
00351 ros::ServiceServer service = n.advertiseService("evarobot_rgb/SetRGB", &IMRGB::callbackSetRGB, rgb);
00352
00353 ros::Time start_time, end_time;
00354 ros::Duration sleep_duration, loop_duration;
00355 ros::Rate loop_rate(10.0);
00356
00357
00358
00359 diagnostic_updater::Updater updater;
00360 updater.setHardwareID("EvarobotRGB");
00361 updater.add("rgb", &ProduceDiagnostics);
00362
00363 while(ros::ok())
00364 {
00365 start_time = ros::Time::now();
00366
00367 if(rgb->GetTimesValue() > 0 || rgb->GetTimesValue() < 0)
00368 {
00369 switch(rgb->GetMode())
00370 {
00371 case IMRGB::MANUAL:
00372 {
00373 rgb->RunManualMode();
00374 break;
00375 }
00376
00377 case IMRGB::ON:
00378 {
00379 rgb->RunOpeningMode();
00380 break;
00381 }
00382
00383 case IMRGB::AUTONOMOUS:
00384 {
00385 rgb->RunAutonomousMode();
00386 break;
00387 }
00388
00389 case IMRGB::TELEOP:
00390 {
00391 rgb->RunTeleopMode();
00392 break;
00393 }
00394
00395 case IMRGB::WANDER:
00396 {
00397 rgb->RunWanderMode();
00398 break;
00399 }
00400
00401 case IMRGB::LOW_BATTERY:
00402 {
00403 rgb->RunLowBatteryMode();
00404 break;
00405 }
00406
00407 case IMRGB::CHARGING:
00408 {
00409 rgb->RunChargingMode();
00410 break;
00411 }
00412
00413 case IMRGB::CHARGED:
00414 {
00415 rgb->RunChargedMode();
00416 break;
00417 }
00418
00419 case IMRGB::ERROR:
00420 {
00421 rgb->RunErrorMode();
00422 break;
00423 }
00424
00425 }
00426 }
00427 else
00428 {
00429 rgb->TurnLedOff();
00430 }
00431
00432 updater.update();
00433 ros::spinOnce();
00434
00435 end_time = ros::Time::now();
00436
00437 if(rgb->GetFrequency() > 0.0 && !(rgb->GetMode() == IMRGB::TELEOP || rgb->GetMode() == IMRGB::WANDER))
00438 {
00439 loop_duration = end_time - start_time;
00440 sleep_duration = ros::Duration(1.0/rgb->GetFrequency()) - loop_duration;
00441 ros::Duration(sleep_duration).sleep();
00442 }
00443 else if(rgb->GetFrequency() < 0.0 && rgb->GetMode() == IMRGB::MANUAL)
00444 {
00445 loop_rate.sleep();
00446 }
00447 }
00448
00449 return 0;
00450 }