md49_base_controller_class.h
Go to the documentation of this file.
00001 #ifndef MD49_BASE_CONTROLLER_CLASS_H_
00002 #define MD49_BASE_CONTROLLER_CLASS_H_
00003 
00004 #define TIMEOUT 1000                                                                            
00006 class BaseController
00007 {
00008 public:
00009 
00010     ros::NodeHandle n;
00011 
00015     BaseController()
00016       {
00017         // Topics to publish
00018         md49_encoders_pub = n.advertise<md49_messages::md49_encoders>("md49_encoders",10);
00019         md49_data_pub = n.advertise<md49_messages::md49_data>("md49_data",10);
00020         // Topic to subscribe
00021         sub_cmd_vel = n.subscribe("/cmd_vel", 1, &BaseController::cmd_vel_callback, this);
00022         // Read initial parameters from parameter service
00023         n.param<std::string>("serialport/name", serialport, "/dev/ttyS0");                      // Get serialportname from ROS Parameter sevice, default is ttyS0 (pcDuinos GPIO UART)
00024         n.param("serialport/bps", serialport_bps, 38400);                                       // Get serialport bps from ROS Parameter sevice, default is 38400Bps
00025         n.param("md49/mode", initial_md49_mode, 0);                                             // Get MD49 Mode from ROS Parameter sevice, default is Mode=0
00026         n.param("md49/acceleration", initial_md49_acceleration, 5);                             // Get MD49 Acceleration from ROS Parameter sevice, default is Acceleration=0
00027         n.param("md49/regulator", initial_md49_regulator, true);                                // Get MD49 Regulator from ROS Parameter sevice, default is Regulator=ON
00028         n.param("md49/timeout", initial_md49_timeout, true);                                    // Get MD49 Timeout from ROS Parameter sevice, default is Timeout=ON
00029         n.param("md49/speed_l", requested_speed_l, 128);                                        // Get MD49 speed_l from ROS Parameter sevice, default is speed_l=128
00030         n.param("md49/speed_r",  requested_speed_r, 128);                                       // Get MD49 speed_r from ROS Parameter sevice, default is speed_r=128
00031         actual_speed_l=requested_speed_l;
00032         actual_speed_r=requested_speed_r;
00033       }
00034 
00039     void cmd_vel_callback(const geometry_msgs::Twist& vel_cmd){
00040         // Drive For- or Backward:
00041         if (vel_cmd.linear.x != 0){
00042             requested_speed_l = 128+(635*vel_cmd.linear.x);
00043             requested_speed_r = 128+(635*vel_cmd.linear.x);
00044         }
00045         // Drive stopped:
00046         if (vel_cmd.linear.x==0 && vel_cmd.angular.z==0){
00047             requested_speed_l = 128;
00048             requested_speed_r = 128;
00049         }
00050         // Turn clock- or counterclockwise:
00051         if (vel_cmd.angular.z != 0){
00052             requested_speed_l = 128 - (127*vel_cmd.angular.z);
00053             requested_speed_r = 128 + (127*vel_cmd.angular.z);
00054         }
00055 
00056         ROS_INFO("base_controller: Received /cmd_vel message. Requested speed_l=%i, speed_r=%i",requested_speed_l,requested_speed_r);
00057     }
00058 
00062     void open_serialport()
00063     {
00064         try{ device.open(serialport.c_str(), serialport_bps); }
00065         catch(cereal::Exception& e)
00066         {
00067             ROS_FATAL("base_controller: Failed to open serialport %s!",serialport.c_str());
00068             ROS_BREAK();
00069         }
00070         ROS_INFO("base_controller: Opened Serialport at %s with %i bps.",serialport.c_str(),serialport_bps);
00071     }
00075     void publish_encoders()
00076     {
00077         get_encoders();
00078         md49_encoders_pub.publish(md49_encoders);
00079     }
00080 
00084     void publish_md49_data()
00085     {
00086         md49_data.speed_l=get_speed_l();
00087         md49_data.speed_r=get_speed_r();
00088         md49_data.volts=get_volts();
00089         md49_data.current_l=get_current_l();
00090         md49_data.current_r=get_current_r();
00091         md49_data.acceleration=get_acceleration();
00092         md49_data.mode=get_mode();
00093         md49_data.error=get_error();
00094         md49_data_pub.publish(md49_data);
00095     }
00096 
00106     void init_md49(int speed_l, int speed_r, int mode, int acceleration, bool timeout, bool regulator)
00107     {
00108         set_speed(speed_l,speed_r);
00109         set_mode(mode);
00110         set_acceleration(acceleration);
00111         if (timeout==true)
00112         {
00113             enable_timeout();
00114         }
00115         else if (timeout==false)
00116         {
00117             disable_timeout();
00118         }
00119         if (regulator==true)
00120         {
00121             enable_regulator();
00122         }
00123         else if (regulator==false)
00124         {
00125             disable_regulator();
00126         }
00127     }
00133     void set_speed(int speed_l, int speed_r)
00134     {
00135         // set and send serial command for speed_l
00136         const char md49_set_speed_l[]={0x00,0x31,speed_l};
00137         device.write(md49_set_speed_l,3);
00138         // set and send serial command for speed_r
00139         const char md49_set_speed_r[]={0x00,0x32,speed_r};
00140         device.write(md49_set_speed_r,3);
00141 
00142     }
00147     void set_mode(int mode)
00148     {
00149         const char md49_set_mode[]={0x00,0x34,mode};
00150         device.write(md49_set_mode,3);
00151         ROS_INFO("base_controller: Set mode=%i on MD49", mode);
00152     }
00157     void set_acceleration(int acceleration)
00158     {
00159         const char md49_set_acceleration[]={0x00,0x33,acceleration};
00160         device.write(md49_set_acceleration,3);
00161         ROS_INFO("base_controller: Set acceleration=%i on MD49", acceleration);
00162     }
00166     void enable_timeout(void){
00167         const char md49_enable_timeout[] = {0x00,0x39};             // put together command to enable md49 timeout
00168         device.write(md49_enable_timeout,2);
00169         md49_data.timeout=1;
00170         ROS_INFO("base_controller: Enabled timeout on MD49");
00171     }
00175     void enable_regulator(void){
00176         const char md49_enable_regulator[] = {0x00,0x37};           // put together command to enable md49 regulator
00177         device.write(md49_enable_regulator,2);
00178         md49_data.regulator=1;
00179         ROS_INFO("base_controller: Enabled regulator on MD49");
00180     }
00184     void disable_timeout(void){
00185         const char md49_disable_timeout[] = {0x00,0x38};            // put together command to enable md49 regulator
00186         device.write(md49_disable_timeout,2);
00187         md49_data.timeout=0;
00188         ROS_INFO("base_controller: Disabled timeout on MD49");
00189     }
00193     void disable_regulator(void){
00194         const char md49_disable_regulator[] = {0x00,0x36};          // put together command to enable md49 timeout
00195         device.write(md49_disable_regulator,2);
00196         md49_data.regulator=0;
00197         ROS_INFO("base_controller: Disabled regulator on MD49");
00198     }
00203     int get_acceleration(){
00204         const char md49_get_acceleration[] = {0x00,0x2A};           // put together command to read md49 set acceleration
00205         device.write(md49_get_acceleration,2);
00206         try{ device.read(reply, 1, TIMEOUT); }                      // get answer, acceleration
00207         catch(cereal::TimeoutException& e){
00208             ROS_ERROR("base_controller: Timeout reading MD49 acceleration!");
00209         }
00210         //ROS_INFO("base_controller: MD49 Acceleration= %i", reply[0]);
00211         return reply[0];
00212     }
00217     int get_mode(){
00218         const char md49_get_mode[] = {0x00,0x2B};                   // put together command to read md49 set acceleration
00219         device.write(md49_get_mode,2);
00220         try{ device.read(reply, 1, TIMEOUT); }                      // get answer, acceleration
00221         catch(cereal::TimeoutException& e){
00222             ROS_ERROR("base_controller: Timeout reading MD49 Mode!");
00223         }
00224         //ROS_INFO("base_controller: MD49 Mode= %i", reply[0]);
00225         return reply[0];
00226     }
00231     int get_speed_l(){
00232         const char md49_get_speed_l[] = {0x00,0x21};                // put together command to read md49 set acceleration
00233         device.write(md49_get_speed_l,2);
00234         try{ device.read(reply, 1, TIMEOUT); }                      // get answer, acceleration
00235         catch(cereal::TimeoutException& e){
00236             ROS_ERROR("base_controller: Timeout reading MD49 speed_l!");
00237         }
00238         return reply[0];
00239         //ROS_INFO("base_controller: MD49 speed_l= %i, speed_r= %i", speed_l,speed_r);
00240     }
00245     int get_speed_r(){
00246         const char md49_get_speed_r[] = {0x00,0x22};                // put together command to read md49 set acceleration
00247         device.write(md49_get_speed_r,2);
00248         try{ device.read(reply, 1, TIMEOUT); }                      // get answer, acceleration
00249         catch(cereal::TimeoutException& e){
00250             ROS_ERROR("base_controller: Timeout reading MD49 speed_r!");
00251         }
00252         return reply[0];
00253         //ROS_INFO("base_controller: MD49 speed_l= %i, speed_r= %i", speed_l,speed_r);
00254     }
00258     void get_encoders(void){
00259         const char md49_get_encoders[] = {0x00,0x25};               // put together command to read md49 encoders
00260         device.write(md49_get_encoders,2);
00261         // ******************************************************
00262         // * Get the reply, the last value is the timeout in ms *
00263         // ******************************************************
00264         try{ device.read(reply, 8, TIMEOUT); }
00265         catch(cereal::TimeoutException& e){
00266             ROS_ERROR("base_controller: Timeout reading MD49 encodervalues!");
00267         }
00268         // ***************************************************
00269         // * Set all values of custom message /md49_encoders *
00270         // ***************************************************
00271         md49_encoders.encoder_l = reply[0] << 24;                   // put together first encoder value
00272         md49_encoders.encoder_l |= (reply[1] << 16);
00273         md49_encoders.encoder_l |= (reply[2] << 8);
00274         md49_encoders.encoder_l |= (reply[3]);
00275         md49_encoders.encoder_r = reply[4] << 24;                   // put together second encoder value
00276         md49_encoders.encoder_r |= (reply[5] << 16);
00277         md49_encoders.encoder_r |= (reply[6] << 8);
00278         md49_encoders.encoder_r |= (reply[7]);
00279         md49_encoders.encoderbyte1l=reply[0];
00280         md49_encoders.encoderbyte2l=reply[1];
00281         md49_encoders.encoderbyte3l=reply[2];
00282         md49_encoders.encoderbyte4l=reply[3];
00283         md49_encoders.encoderbyte1r=reply[4];
00284         md49_encoders.encoderbyte2r=reply[5];
00285         md49_encoders.encoderbyte3r=reply[6];
00286         md49_encoders.encoderbyte4r=reply[7];
00287         //ROS_INFO("Got this reply: %i,%i,%i,%i,%i,%i,%i,%i", reply[0], reply[1], reply[2],reply[3], reply[4], reply[5], reply[6], reply[7]);
00288     }
00293     int get_volts(){
00294         const char md49_get_volts[] = {0x00,0x26};                  // put together command to read md49 volts
00295         device.write(md49_get_volts,2);
00296         try{ device.read(reply, 1, TIMEOUT); }                      // get answer, volts
00297         catch(cereal::TimeoutException& e){
00298             ROS_ERROR("base_controller: Timeout reading MD49 volts!");
00299         }
00300         //ROS_INFO("Got this reply (volts): %i", reply[0]);
00301         return reply[0];
00302     }
00307     int get_current_l(){
00308         const char md49_get_current_l[] = {0x00,0x27};              // put together command to read md49 current_l
00309         device.write(md49_get_current_l,2);
00310         try{ device.read(reply, 1, TIMEOUT); }                      // get answer, current_l
00311         catch(cereal::TimeoutException& e){
00312             ROS_ERROR("base_controller: Timeout reading MD49 current_l!");
00313         }
00314         //ROS_INFO("Got this reply (current_l): %i", reply[0]);
00315         return reply[0];
00316     }
00321     int get_current_r(){
00322         const char md49_get_current_r[] = {0x00,0x28};              // put together command to read md49 current_r
00323         device.write(md49_get_current_r,2);
00324         try{ device.read(reply, 1, TIMEOUT); }                      // get answer, current_r
00325         catch(cereal::TimeoutException& e){
00326             ROS_ERROR("base_controller: Timeout reading MD49 current_r!");
00327         }
00328         //ROS_INFO("Got this reply (current_r): %i", reply[0]);
00329         return reply[0];
00330     }
00335     int get_error(){
00336         // ************************
00337         const char md49_get_error[] = {0x00,0x2D};                  // put together command to read md49 error
00338         device.write(md49_get_error,2);
00339         try{ device.read(reply, 1, TIMEOUT); }                      // get answer, error
00340         catch(cereal::TimeoutException& e){
00341             ROS_ERROR("base_controller: Timeout reading MD49 errorbyte!");
00342         }
00343         //ROS_INFO("Got this reply (error): %i", reply[0]);
00344         return reply[0];
00345     }
00349     void reset_encoders(void){
00350         const char md49_reset_encoders[] = {0x00,0x35};             // put together command to reset md49 encoders
00351         device.write(md49_reset_encoders,2);
00352         ROS_INFO("base_controller: Reset encoders on MD49");
00353     }
00354 
00359     int get_requested_speed_l()
00360     {
00361         return requested_speed_l;
00362     }
00367     int get_requested_speed_r()
00368     {
00369         return requested_speed_r;
00370     }
00375     void set_requested_speed_l(int speed_l)
00376     {
00377         requested_speed_l=speed_l;
00378     }
00383     void set_requested_speed_r(int speed_r)
00384     {
00385         requested_speed_r=speed_r;
00386     }
00391     int get_actual_speed_l()
00392     {
00393         return actual_speed_l;
00394     }
00399     int get_actual_speed_r()
00400     {
00401         return actual_speed_r;
00402     }
00407     void set_actual_speed_l(int speed_l)
00408     {
00409         actual_speed_l=speed_l;
00410     }
00415     void set_actual_speed_r(int speed_r)
00416     {
00417         actual_speed_r=speed_r;
00418     }
00423     int get_initial_md49_mode()
00424     {
00425         return initial_md49_mode;
00426     }
00430     int get_initial_md49_acceleration()
00431     {
00432         return initial_md49_acceleration;
00433     }
00438     int get_initial_md49_timeout()
00439     {
00440         return initial_md49_timeout;
00441     }
00446     int get_initial_md49_regulator()
00447     {
00448         return initial_md49_regulator;
00449     }
00450 
00451 private:
00452 
00453     cereal::CerealPort device;                                                                      
00454     char reply[8];
00455     int requested_speed_l, requested_speed_r;                                                       
00456     int actual_speed_l, actual_speed_r;                                                             
00457     int initial_md49_mode;                                                                          
00458     int initial_md49_acceleration;                                                                  
00459     bool initial_md49_timeout;                                                                      
00460     bool initial_md49_regulator;                                                                    
00461     std::string serialport;                                                                         
00462     int serialport_bps;                                                                             
00463     //ros::NodeHandle n;
00464     ros::Subscriber sub_cmd_vel;
00465     md49_messages::md49_data md49_data;                                                           
00466     md49_messages::md49_encoders md49_encoders;                                                   
00467     ros::Publisher md49_encoders_pub;
00468     ros::Publisher md49_data_pub;
00469 }; //End of class BaseController
00470 
00471 #endif


md49_base_controller
Author(s): Fabian Prinzing
autogenerated on Thu Feb 11 2016 23:25:32