atlasmv_base: /home/laradmin/lar/bases/atlasmv_base/include/c

00001 /**************************************************************************************************
00002  Software License Agreement (BSD License)
00003 
00004  Copyright (c) 2011-2013, LAR toolkit developers - University of Aveiro - http://lars.mec.ua.pt
00005  All rights reserved.
00006 
00007  Redistribution and use in source and binary forms, with or without modification, are permitted
00008  provided that the following conditions are met:
00009 
00010   *Redistributions of source code must retain the above copyright notice, this list of
00011    conditions and the following disclaimer.
00012   *Redistributions in binary form must reproduce the above copyright notice, this list of
00013    conditions and the following disclaimer in the documentation and/or other materials provided
00014    with the distribution.
00015   *Neither the name of the University of Aveiro nor the names of its contributors may be used to
00016    endorse or promote products derived from this software without specific prior written permission.
00017  
00018  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR
00019  IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
00020  FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
00021  CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
00022  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
00023  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
00024  IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
00025  OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00026 ***************************************************************************************************/
00031 #ifndef _CLASS_ATLASMV_H_
00032 #define _CLASS_ATLASMV_H_
00033 
00034 #define _DO_NOT_USE_OPENCV_ 0
00035 #define _DO_NOT_USE_CARMEN_ 0
00036 
00037 //####################################################################
00038 // Includes: no need to include headers from carmen modules carmen.h #
00039 // already does                 #####################################
00040 //####################################################################
00041 
00042 #include <pthread.h>
00043 #include <string.h>
00044 #include <termios.h>
00045 #include <sys/ioctl.h>
00046 #include <stdlib.h>
00047 #include <stdio.h>
00048 #include <sys/types.h>
00049 #include <sys/stat.h>
00050 #include <fcntl.h>
00051 #include <unistd.h>
00052 #include <ros/ros.h>
00053 #include <termios.h>
00054 #include <sys/ioctl.h>
00055 #include <signal.h>
00056 #include <timer.h>
00057 // #include <class_hitec5980gs.h>
00058 #include <hitec5980sg/hitec5980sg.h>
00059 #include <class_dioc.h>
00060 #include <des70_10.h>
00061 #include <iostream>
00062 
00063 #include <atlasmv_base/AtlasmvLightsCommand.h>
00064 #include <atlasmv_base/AtlasmvMotionCommand.h>
00065 #include <atlasmv_base/AtlasmvVertSignsCommand.h>
00066 #include <atlasmv_base/AtlasmvStatus.h>
00067 
00068 using namespace std;
00069 
00070 typedef enum
00071 {
00072         ENUM_ERR_NONE = 0,                              
00073         ENUM_ERR_READ_DIR = 3,                  
00074         ENUM_ERR_OVER_CURRENT = 4,              
00075         ENUM_ERR_OVER_VOLTAGE = 5,              
00076         ENUM_ERR_OVER_SPEED = 6,                
00077         ENUM_ERR_SUPPLY_VOLT_LOW = 7,   
00078         ENUM_ERR_ANG_DET = 8,                   
00079         ENUM_ERR_ENC_RES = 9                    
00080 }ENUM_ERR;
00081 
00082 typedef enum
00083 {
00084         MANDATORY_MEDIUM_LIGTHS = 1,
00085         MANDATORY_BUS_LANE = 2,
00086         WARNING_ROAD_NARROWS = 3,
00087         WARNING_DIP_AHEAD = 4,
00088         INFORMATION_HOSPITAL = 5,
00089         INFORMATION_REC_SPEED_60 = 6,
00090         NO_VERT_SIGN=0,
00091 }TYPE_ENUM_SIGNS;
00092 
00096 typedef struct details{
00097         int cycle_freq;
00098         double sleep_time;
00099 
00100         double max_forward_speed;
00101         double max_backward_speed;
00102         double brake_speed_dif;
00103         //servo device ids
00104         int servo_direction_id;
00105         int servo_brake_id;
00106         //device paths
00107         std::string com_servos;
00108         std::string com_dioc;
00109         std::string com_des;
00110         //phisical aspects
00111         double length;
00112         double width;
00113         double back_wheel_diam;
00114         double wheelaxisdistance;
00115         double maximum_dir;
00116         double minimum_dir;
00117         double maximum_brk;
00118         double minimum_brk;
00119         double transmission_relation;
00120         int min_pulse_brk;
00121         int max_pulse_brk;
00122         int min_pulse_dir;
00123         int max_pulse_dir;
00124         double brk_time_no_maxon;
00125         //servoamplifier aspects
00126         TYPE_des_sysparam sDES_sysparam;
00127         int des_frq_chk_errors;
00128         
00129         friend ostream& operator<< (ostream &o, const details &i)
00130         {
00131                 return o
00132                 << "cycle_freq: "<< i.cycle_freq <<endl
00133                 << "sleep_time: "<< i.sleep_time<<endl
00134                 << "com_servos: "<< i.com_servos<<endl
00135                 << "com_dioc: "<< i.com_dioc<<endl
00136                 << "com_des: "<< i.com_des<<endl;
00137         }
00138 }TYPE_atlasmv_public_params;
00139 
00143 class c_atlasmv{
00144         private:
00145                 //variables used that are connected with class running
00146 
00147                 pthread_mutex_t c_atlasmv_robot_details_mutex;
00148                 TYPE_atlasmv_public_params *robot_details;
00149                 pthread_mutex_t flags_mutex;
00150                 bool debug_mode;
00151 //              TYPE_executionflags* flags;
00152                 char get_executionflags_debug();
00153                 c_timer *timer;
00154                 pthread_mutex_t errors_mutex;
00155                 int get_errors();
00156                 int set_errors(int);
00157                 int errors;
00158 
00159 
00160                 //variables that are connected with des
00161                 pthread_mutex_t max_forward_speed_mutex;
00162                 double max_forward_speed;
00163                 pthread_mutex_t max_backward_speed_mutex;
00164                 double max_backward_speed;
00165                 pthread_mutex_t brake_speed_dif_mutex;
00166                 double brake_speed_dif;
00167                 //device paths
00168                 pthread_mutex_t com_des_mutex;
00169                 std::string com_des;
00170                 pthread_mutex_t porthandler_des_mutex;
00171                 int* porthandler_des;
00172                 //phisical aspects
00173                 pthread_mutex_t length_mutex;
00174                 double length;
00175                 pthread_mutex_t width_mutex;
00176                 double width;
00177                 pthread_mutex_t back_wheel_diam_mutex;
00178                 double back_wheel_diam;
00179                 pthread_mutex_t wheelaxisdistance_mutex;
00180                 double wheelaxisdistance;
00181                 pthread_mutex_t transmission_relation_mutex;
00182                 double transmission_relation;
00183                 pthread_mutex_t sDES_sysparam_mutex;
00184                 TYPE_des_sysparam sDES_sysparam;
00185                 pthread_mutex_t rpm_mutex;
00186                 int actual_rpm;
00187                 int req_rpm;
00188                 pthread_mutex_t linear_speed_mutex;
00189                 double actual_speed;
00190                 double req_speed;
00191                 pthread_mutex_t des_initialized_mutex;
00192                 char des_initialized;
00193                 pthread_mutex_t brk_time_no_maxon_mutex;
00194                 double brk_time_no_maxon;
00195 
00196                 /************************************************************
00197                 *                               related with servo control                                      *
00198                 ************************************************************/
00199                 //--- srv_var
00200                 pthread_mutex_t maximum_dir_mutex;
00201                 double maximum_dir;
00202                 pthread_mutex_t minimum_dir_mutex;
00203                 double minimum_dir;
00204                 pthread_mutex_t maximum_brk_mutex;
00205                 double maximum_brk;
00206                 pthread_mutex_t minimum_brk_mutex;
00207                 double minimum_brk;
00208                 pthread_mutex_t com_servos_mutex;
00209                 std::string com_servos;
00210                 pthread_mutex_t actual_dir_mutex;
00211                 double actual_dir;
00212                 pthread_mutex_t actual_brk_mutex;
00213                 double actual_brk;
00214                 pthread_mutex_t servo_initialized_mutex;
00215                 char servo_initialized;
00216                 pthread_mutex_t min_pulse_brk_mutex;
00217                 int min_pulse_brk;
00218                 pthread_mutex_t max_pulse_brk_mutex;
00219                 int max_pulse_brk;
00220                 pthread_mutex_t min_pulse_dir_mutex;
00221                 int min_pulse_dir;
00222                 pthread_mutex_t max_pulse_dir_mutex;
00223                 int max_pulse_dir;      
00224                 //servo device ids
00225                 pthread_mutex_t servo_direction_id_mutex;
00226                 int servo_direction_id;
00227                 pthread_mutex_t servo_brake_id_mutex;
00228                 int servo_brake_id;
00229 
00230 
00231                 float slope_rad2pulse;
00232                 float bias_rad2pulse;
00233                 float slope_pulse2rad;
00234                 float bias_pulse2rad;
00235                 hitec_5980SG *servo;
00236                 //float direction_ang;
00237                 //float brake_ang;
00238                 
00239                 //--- srv_func
00240                 int init_com_servo();
00241 
00242                 /************************************************************
00243                 *               related with discrete input and ouptup board            *
00244                 ************************************************************/
00245                 //--- sens_var
00246                 pthread_mutex_t com_dioc_mutex;
00247                 std::string com_dioc;
00248                 class_dioc *dioc;
00249                 pthread_mutex_t dioc_initialized_mutex;
00250                 char dioc_initialized;
00251                 pthread_mutex_t sign_mutex;
00252                 int prev_sign;
00253                 pthread_mutex_t lights_mutex;
00254                 int prev_lights;
00255                 pthread_mutex_t cross_sensor_mutex;
00256                 int cross_sensor;
00257                 //--- sens_func
00258                 int init_com_dioc();
00259                 pthread_mutex_t robot_status_mutex;
00260                 int turnright;          
00261                 int turnleft;           
00262                 int headlights;         
00263                 int taillights;         
00264                 int reverselights;      
00265                 int vert_sign;          
00266 
00267                 /************************************************************
00268                 *                               related with DES servoamplifier                         *
00269                 ************************************************************/
00270                 //--- des_var
00271                 bool init;
00272                 //char* com_des;
00273                 double velocity;
00274 
00275                 //--- des_func
00276                 int init_com_des();
00277                 
00278                 int rad2pulses(double rad);
00279                 double pulses2rad(int pulses);
00280 
00281         public:
00282                 c_atlasmv(bool debug_);//Y
00283                 int set_atlasmv_details(TYPE_atlasmv_public_params *);
00284                 int initialize_robot(void);// not totally
00285                 int new_robot_state(atlasmv_base::AtlasmvMotionCommand&,atlasmv_base::AtlasmvVertSignsCommand&,atlasmv_base::AtlasmvLightsCommand&);
00286                 int update_robot_status(atlasmv_base::AtlasmvStatus&);
00287 
00289                 //-------------- des_func  ---------------//
00291                 int check_des(void);
00292 
00293                 //functions
00294 
00295 
00296                 double map_linear(double in,std::vector<std::pair<double,double> >& cp)
00297                 {
00298                         //first está nas coordenadas de in
00299                         //second está nas coordenadas de out
00300                         
00301                         double out=NAN;
00302                         
00303                         for(uint i=0;i<cp.size()-1;i++)
00304                         {
00305                                 if( in >= cp[i].first && in <= cp[i+1].first)
00306                                 {
00307                                         //encontrei o intervalo certo
00308                                         double m = (cp[i+1].second - cp[i].second)/(cp[i+1].first - cp[i].first);
00309                                         double b = cp[i+1].second -m*cp[i+1].first;
00310                                         
00311                                         out=m*in+b;
00312                                         break;
00313                                 }
00314                         }
00315                         
00316                         if(isnan(out)){
00317                                 cout<<"Warning: Input argument: "<<in<<" is out of bounds, ["<<cp[0].first<<","<< cp[cp.size()-1].first<<"]"<<endl;
00318                                 return 0;
00319                         }
00320                         else{
00321                           return out;
00322                         }
00323                 }
00324 
00325 
00326                 //char *com_des;
00327                 int restart_des();
00328                 int set_speed(double *);
00329                 int read_speed(int);
00330 
00331                 void set_com_des(std::string);//Y
00332                 std::string get_com_des();
00333                 void set_porthandler_des(int );
00334                 int get_porthandler_des();
00335                 void set_linear_speed(double*, double *);
00336                 void get_linear_speed(double *, double *);
00337                 void set_rpm(int *, int *);
00338                 void get_rpm(int *, int *);
00339                 void set_max_forward_speed(double *);
00340                 double get_max_forward_speed();
00341                 void set_max_backward_speed(double *);
00342                 double get_max_backward_speed();
00343                 void set_brake_speed_dif(double *);
00344                 double get_brake_speed_dif();
00345                 void set_length(double *);
00346                 double get_length();
00347                 void set_width(double *);
00348                 double get_width();
00349                 void set_back_wheel_diam(double *);
00350                 double get_back_wheel_diam();
00351                 void set_wheelaxisdistance(double *);
00352                 double get_wheelaxisdistance();
00353                 void set_transmission_relation(double *);
00354                 double get_transmission_relation();
00355                 void set_sDES_sysparam(TYPE_des_sysparam *);
00356                 void get_sDES_sysparam(TYPE_des_sysparam *);
00357                 void set_brk_time_no_maxon(double *);
00358                 double get_brk_time_no_maxon();
00359                 //void set_actual_rpm(int *);
00360                 //void get_actual_rpm(int *);
00361                 //void set_req_rpm(int *);
00362                 //void get_req_rpm(int *);
00363                 //void set_actual_speed(double *);
00364                 //void get_actual_speed(double *);
00365                 //void set_req_speed(double *);
00366                 //void get_req_speed(double *);
00367                 void set_des_initialized(char);
00368                 char get_des_initialized();
00369 
00370                 //--- srv_func
00371                 int set_direction(double *);
00372                 int read_direction(double *);
00373                 double get_actual_dir();
00374                 void set_actual_dir(double *);
00375                 double get_actual_brk();
00376                 void set_actual_brk(double *);
00377                 int set_brake(double *);
00378                 //int read_brake(double *brake, int *)=NUL;
00379                 int read_brake(double *brk, int *num_pulses=NULL);
00380 
00381 
00382                 void set_com_servo(std::string);//Y
00383                 std::string get_com_servo();
00384                 void set_dir_srv_id(unsigned char);
00385                 char get_dir_srv_id();
00386                 void set_brk_srv_id(unsigned char);
00387                 char get_brk_srv_id();
00388                 void set_maximum_brk_angle(double *);
00389                 double get_maximum_brk_angle();
00390                 void set_minimum_brk_angle(double *);
00391                 double get_minimum_brk_angle();
00392                 void set_maximum_dir_angle(double *);
00393                 double get_maximum_dir_angle();
00394                 void set_minimum_dir_angle(double *);
00395                 double get_minimum_dir_angle();
00396                 void set_min_pulse_brk(int *);
00397                 int get_min_pulse_brk();
00398                 void set_max_pulse_brk(int *);
00399                 int get_max_pulse_brk();
00400                 void set_min_pulse_dir(int *);
00401                 int get_min_pulse_dir();
00402                 void set_max_pulse_dir(int *);
00403                 int get_max_pulse_dir();
00404                 void set_servo_initialized(char);
00405                 char get_servo_initialized();
00406 
00407                 //--- sens_func
00408                 int set_lights(atlasmv_base::AtlasmvVertSignsCommand&,atlasmv_base::AtlasmvLightsCommand&);
00409                 int read_cross_sensor();
00410                 //char *com_dioc;
00411                 void set_com_dioc(std::string);
00412                 std::string get_com_dioc();
00413                 void set_dioc_initialized(char);
00414                 char get_dioc_initialized();
00415                 void set_cross_sensor(int);
00416                 int get_cross_sensor();
00417                 int get_lights(atlasmv_base::AtlasmvStatus&);
00418                 
00419                 
00420                 ~c_atlasmv();//Y
00421 };
00422 
00423 #endif
/home/laradmin/lar/bases/atlasmv_base/include/c_atlasmv.h
