x52_joyext.cpp
Go to the documentation of this file.
00001 /*
00002  * x52_joyext Node
00003  * by Christian Holl (www.rad-lab.net)
00004  * License: BSD
00005  *
00006  * Have Fun! :-)
00007  */
00008 
00009 /*
00010  * Modified by Murilo FM (muhrix@gmail.com)
00011  * 12 Dec 2013
00012  *
00013  */
00014 
00015 
00016 #include "x52_joyext.h"
00017 
00018 X52_JoyExt::X52_JoyExt(ros::NodeHandle &n):nh(n)
00019 {
00020    //bzero(updateLED,10);
00021    memset(updateLED,1,10);
00022    updateLED_b=false;
00023    memset(updateMFD,true,3);
00024 
00025    updateDate=false;
00026    updateTime=false;
00027    memset(updateOffset,false,2);
00028    updateBrightnessMFD=false;
00029    updateBrightnessLED=false;
00030    bzero(LED,19);
00031    //memset(LED,1,19);
00032 
00033    bzero(Date,3);
00034    bzero(Time,2);
00035    bzero(Offset,2);
00036    brightnessMFD=255;
00037    brightnessLED=255;
00038 
00039    updateBrightnessLED=true;
00040    updateBrightnessMFD=true;
00041 
00042    //MFD content write
00043    mfd_content[0]="x52_joyext Node";
00044    mfd_content[1]=" Joystick under ";
00045    mfd_content[2]="   ROS Control  ";
00046 
00047 
00048    int rate;
00049    n.param("loop_rate", rate, 100);
00050    loop_rate=new ros::Rate(rate);
00051 
00052    subleds=nh.subscribe("leds",1000, &X52_JoyExt::cb_leds,this);
00053    submfd_text=nh.subscribe("mfd_text",1000, &X52_JoyExt::cb_mfd_text,this);
00054    subdate=nh.subscribe("mfd_date",1000, &X52_JoyExt::cb_date,this);
00055    subtime=nh.subscribe("mfd_time",1000, &X52_JoyExt::cb_time,this);
00056    subbrightnessMFD=nh.subscribe("mfd_brightness",1000, &X52_JoyExt::cb_brighnessMFD,this);
00057    subbrightnessLED=nh.subscribe("leds_brightness",1000, &X52_JoyExt::cb_brighnessLED,this);
00058 
00059    //Move to handling
00060    send_to_joystick();
00061 }
00062 
00063 
00064 X52_JoyExt::~X52_JoyExt()
00065 {
00066         delete loop_rate;
00067 }
00068 
00069 void X52_JoyExt::cb_leds(const x52_joyext::x52_led_colorConstPtr &msg)
00070 {
00071         if(msg->color_leds[x52_joyext::x52_led_color::LED_FIRE])
00072         {
00073                 if( msg->color_leds[ x52_joyext::x52_led_color::LED_FIRE ] >2)
00074                         ROS_WARN("WRONG VALUE (%i) FOR LED FOUND! Value must be in the range of 0-2",msg->color_leds[x52_joyext::x52_led_color::LED_FIRE]);
00075                 LED[0]=(msg->color_leds[x52_joyext::x52_led_color::LED_FIRE]>1);
00076                 updateLED[0]=true;
00077         }
00078         //  void setLEDs(uint8_t inValue, uint8_t *red, uint8_t *green, bool *update)
00079         setLEDs(msg->color_leds[x52_joyext::x52_led_color::LED_A]       ,&LED[X52PRO_LED_ARED-1] ,&LED[X52PRO_LED_AGREEN-1] ,&updateLED[1]);
00080         setLEDs(msg->color_leds[x52_joyext::x52_led_color::LED_B]       ,&LED[X52PRO_LED_BRED-1] ,&LED[X52PRO_LED_BGREEN-1] ,&updateLED[2]);
00081         setLEDs(msg->color_leds[x52_joyext::x52_led_color::LED_D]       ,&LED[X52PRO_LED_DRED-1] ,&LED[X52PRO_LED_DGREEN-1] ,&updateLED[3]);
00082         setLEDs(msg->color_leds[x52_joyext::x52_led_color::LED_E]       ,&LED[X52PRO_LED_ERED-1] ,&LED[X52PRO_LED_EGREEN-1] ,&updateLED[4]);
00083         setLEDs(msg->color_leds[x52_joyext::x52_led_color::LED_T12]     ,&LED[X52PRO_LED_T1RED-1],&LED[X52PRO_LED_T1GREEN-1],&updateLED[5]);
00084         setLEDs(msg->color_leds[x52_joyext::x52_led_color::LED_T34]     ,&LED[X52PRO_LED_T2RED-1],&LED[X52PRO_LED_T2GREEN-1],&updateLED[6]);
00085         setLEDs(msg->color_leds[x52_joyext::x52_led_color::LED_T56]     ,&LED[X52PRO_LED_T3RED-1],&LED[X52PRO_LED_T3GREEN-1],&updateLED[7]);
00086         setLEDs(msg->color_leds[x52_joyext::x52_led_color::LED_POV2],&LED[X52PRO_LED_CORED-1],&LED[X52PRO_LED_COGREEN-1],&updateLED[8]);
00087         setLEDs(msg->color_leds[x52_joyext::x52_led_color::LED_I]       ,&LED[X52PRO_LED_IRED-1] ,&LED[X52PRO_LED_IGREEN-1] ,&updateLED[9]);
00088 }
00089 
00090 void  X52_JoyExt::cb_mfd_text(const x52_joyext::x52_mfdConstPtr &msg)
00091 {
00092         int curPos=msg->pos+16*msg->line;
00093 
00094         if(msg->clearDisplay)
00095         {
00096                 mfd_content[0]="                ";
00097                 mfd_content[1]="                ";
00098                 mfd_content[2]="                ";
00099                 memset(updateMFD,true,3);
00100         }
00101 
00102         for (std::string::const_iterator it=msg->data.begin(); it != msg->data.end(); ++it)
00103         {
00104                 char curChr=*it;
00105 
00106                 if(curPos<3*16)
00107                 {
00108                         uint8_t curLine=curPos/16;
00109                         uint8_t posInLine=curPos-curLine*16;
00110                         if(curChr=='\n')
00111                         {
00112                                 //Fill the rest of the line with spaces
00113                                 for (curPos = 0; curPos < (curLine+1)*16; ++curPos)
00114                                 {
00115                                         posInLine=curPos-curLine*16;
00116                                         mfd_content[curLine][posInLine]=curChr;
00117                                 }
00118                                 continue;
00119                         }
00120                         else
00121                         {
00122                                 mfd_content[curLine][posInLine]=curChr;
00123                                 updateMFD[curLine]=true;
00124                                 curPos++;
00125                         }
00126                 }
00127                 else
00128                 {
00129                         ROS_WARN("MFD text exceeds display bounds!");
00130                         break;
00131                 }
00132         }
00133         ROS_DEBUG("Line 0: %s", mfd_content[0].c_str());
00134         ROS_DEBUG("Line 1: %s", mfd_content[1].c_str());
00135         ROS_DEBUG("Line 2: %s", mfd_content[2].c_str());
00136 }
00137 
00138 void  X52_JoyExt::cb_date(const x52_joyext::x52_dateConstPtr &msg)
00139 {
00140                 Date[2]=msg->date_field_left;
00141                 Date[1]=msg->date_field_center;
00142                 Date[0]=msg->date_field_right;
00143                 updateDate=true;
00144 }
00145 
00146 void  X52_JoyExt::cb_time(const x52_joyext::x52_timeConstPtr &msg)
00147 {
00148         if(msg->set_time)
00149         {
00150                 updateTime=true;
00151                 Time_24=msg->time_24;
00152                 Time[0]=msg->time_hours;
00153                 Time[1]=msg->time_minutes;
00154         }
00155 
00156         if(msg->set_offset_0)
00157         {
00158                 updateOffset[0]=true;
00159                 Offset_24[0]=msg->offset_0_24;
00160                 Offset_Inv[0]=msg->offset_0_inv;
00161                 Offset[0]=msg->offset_0;
00162         }
00163 
00164         if(msg->set_offset_1)
00165         {
00166                 updateOffset[1]=true;
00167                 Offset_24[1]=msg->offset_1_24;
00168                 Offset_Inv[1]=msg->offset_1_inv;
00169                 Offset[1]=msg->offset_1;
00170         }
00171 }
00172 
00173 void  X52_JoyExt::cb_brighnessMFD(const std_msgs::UInt8ConstPtr &msg)
00174 {
00175                 brightnessMFD=msg->data;
00176                 updateBrightnessMFD=true;
00177 }
00178 
00179 void  X52_JoyExt::cb_brighnessLED(const std_msgs::UInt8ConstPtr &msg)
00180 {
00181                 brightnessLED=msg->data;
00182                 updateBrightnessLED=true;
00183 }
00184 
00185 
00186 #define SLEEP_AFTER_COMMAND usleep(5000)
00187 
00188 void X52_JoyExt::send_to_joystick()
00189 {
00190         /*Get Joystick */
00191         struct x52 *hdl=0;
00192         while(hdl==0 && ros::ok())
00193         {
00194                 ROS_INFO("Trying to find joystick...");
00195                 hdl=x52_init();
00196                 loop_rate->sleep();
00197         }
00198         ROS_INFO("Joystick found...");
00199         x52_debug(hdl, 1);
00200 
00201 
00202         x52_setled(hdl,0,0);
00203 
00204         while(ros::ok())
00205         {
00206                 //Update Joystick LED
00207                         for (int lednum = 0; lednum < 10; ++lednum)
00208                         {
00209                                 if(updateLED[lednum])
00210                                 {
00211                                         ROS_DEBUG("Setting LED: %i",lednum);
00212                                         if(lednum != 0)
00213                                         {
00214                                                 int led=(lednum)*2;
00215                                                 x52_setled(hdl,led,LED[led-1]);
00216                                                 x52_setled(hdl,led+1,LED[led]);
00217                                         }
00218                                         else
00219                                         {
00220                                                 x52_setled(hdl,1,LED[0]);
00221                                         }
00222                                         updateLED[lednum]=false;
00223                                         usleep(50);
00224                                 }
00225                         }
00226 
00227                 //Update Brightness MFD
00228                         if(updateBrightnessMFD)
00229                         {
00230                                 x52_setbri(hdl,1,brightnessMFD);
00231                                 updateBrightnessMFD=false;
00232                                 usleep(50);
00233                         }
00234 
00235                 //Update Brightness LED
00236 
00237                         if(updateBrightnessLED)
00238                         {
00239                                 x52_setbri(hdl,0,brightnessLED);
00240                                 updateBrightnessLED=false;
00241                                 SLEEP_AFTER_COMMAND;
00242                         }
00243 
00244                 //Update MFD Text
00245                         for (int line = 0; line < 3; ++line)
00246                         {
00247                                 if(updateMFD[line])
00248                                 {
00249                                         x52_settext(hdl,line,(char*)mfd_content[line].c_str(),mfd_content[line].length());
00250                                         updateMFD[line]=false;
00251                                 }
00252                                 SLEEP_AFTER_COMMAND;
00253                         }
00254 
00255                 //Update Date
00256                         if(updateDate)
00257                         {
00258                                 x52_setdate(hdl,Date[0],Date[1],Date[2]);
00259                                 updateDate=false;
00260                                 SLEEP_AFTER_COMMAND;
00261                         }
00262 
00263                 //Update  Time
00264                         if(updateTime)
00265                         {
00266                                 x52_settime(hdl,Time_24,Time[0], Time[1]);
00267                                 updateTime=false;
00268                                 SLEEP_AFTER_COMMAND;
00269                         }
00270 
00271                 //Offset  Time
00272                         for (int o = 0; o < 2; ++ o)
00273                         {
00274                                 if(updateOffset[o])
00275                                 {
00276                                         x52_setoffs(hdl,o,Offset_24[o],Offset_Inv[o],Offset[o]);
00277                                         updateOffset[o]=false;
00278                                 }
00279                                 SLEEP_AFTER_COMMAND;
00280                         }
00281 
00282                 ros::spinOnce();
00283                 loop_rate->sleep();
00284         }
00285 }


x52_joyext
Author(s): Christian Holl
autogenerated on Mon Oct 6 2014 09:02:17