arduino_interface.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  *
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2012, Robert Bosch LLC.
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/or other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the Robert Bosch nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *
00035  *********************************************************************/
00036 
00037 //\Author Joshua Vasquez, Kai Franke, and Philip Roan, Robert Bosch LLC
00038 
00039 #include "arduino_interface/arduino_interface.hpp"
00040 
00041 /**********************************************************************/
00042 // Constructor
00043 /**********************************************************************/
00044 ArduinoInterface::ArduinoInterface( std::string port_name ) :
00045   port_name_( port_name ),
00046   baud_rate_( DEFAULT_BAUD_RATE ),
00047   connection_failure_( true ), // no connection established yet.
00048   timeout_( 0.5 ), // delay in seconds before we declare Serial communication lost.
00049   is_initialized_( false ),
00050   data_packet_( 0 ),
00051   _reference_voltage(5000)  // set adc reference voltage to 5V, this is Arduino Uno specific
00052 {
00053   // Create a serial port and assign it the class name: 
00054   serial_port_ = new uniserial();
00055 }
00056 
00057 
00058 /**********************************************************************/
00059 // Destructor
00060 /**********************************************************************/
00061 ArduinoInterface::~ArduinoInterface()
00062 {
00063   delete serial_port_;
00064 }
00065 
00066 
00067 /**********************************************************************/
00068 // Initialize:   
00069 /**********************************************************************/
00070 bool ArduinoInterface::initialize()
00071 {
00072   // Prevent Double-initialization!
00073   if( is_initialized_ == true )
00074   {
00075     ROS_INFO( "ArduinoInterface::initialize(): hardware already initialized." );
00076     return true;
00077   }
00078  
00079   // uniserial init args: [8-bit data], [no parity], [1 stop bit], [class baud rate]. 
00080   bool init_success = serial_port_->initialize( port_name_, 8 , 0, 1, baud_rate_ );
00081  
00082   // At least a 3-second delay while the Arduino resets. Note: We can remove this 3 second startup if we cut the solder bridge on the board's RESET EN trace. 
00083   sleep( 3 );
00084  
00085   // Error if connection failure:
00086   if( init_success != true )
00087   {
00088     ROS_ERROR( "Arduino Serial connection not initialized properly.  Make sure serial_port and baud_rate match on both ends!" );
00089     connection_failure_ = true;
00090     return false;
00091   }
00092  
00093   // successful Serial connection!
00094   connection_failure_ = false; 
00095   is_initialized_ = true;
00096   ROS_INFO( "ArduinoInterface::initialize(): hardware initialized." );
00097   return true;
00098 }
00099 
00100 
00101 /**********************************************************************/
00102 // Read
00103 /**********************************************************************/
00104 ssize_t ArduinoInterface::read( int device_address, interface_protocol protocol, int frequency, int* flags, uint8_t reg_address, uint8_t* data, size_t num_bytes )
00105 {
00106   int error_code = 0;
00107   
00108   // Check connection:
00109   if( connection_failure_ == true )
00110   {
00111     return -1;
00112   }
00113   
00114   // Specify data package as read command
00115   data_packet_ = READ;
00116 
00117         // Specify data package protocol
00118         data_packet_ |= (protocol << 1);
00119  
00120   switch( protocol )
00121   {
00122     case I2C:
00123     {
00124       error_code = arduinoI2cRead( device_address, frequency, reg_address, data, num_bytes );               
00125       break;
00126     }
00127     case SPI: 
00128     {
00129       error_code = arduinoSpiRead( (uint8_t)frequency, (uint8_t)flags[0], reg_address, data, num_bytes );
00130       break;
00131     } 
00132     case GPIO:
00133     {
00134       error_code = arduinoGpioRead( (uint8_t)flags[0], reg_address, data );
00135       break;
00136     }
00137     case ENCODER:
00138     {
00139       error_code = arduinoEncoderRead( flags, data );
00140       break;
00141     }
00142     case ADCONVERTER:
00143     {
00144       error_code = arduinoAdcRead( reg_address, data );
00145       break;
00146     }
00147     default:
00148     {
00149       ROS_ERROR("Arduino does not support reading through this protocol.");
00150       return -1;
00151     }
00152   }
00153   return error_code;
00154 }
00155 
00156 
00157 /**********************************************************************/
00158 // Write
00159 /**********************************************************************/
00160 ssize_t ArduinoInterface::write( int device_address, interface_protocol protocol, int frequency, int* flags, uint8_t reg_address, uint8_t* data, size_t num_bytes )
00161 { 
00162   int error_code = 0;
00163 
00164   // Check connection:
00165   if( connection_failure_ == true )
00166   {
00167                 std::cout << "connection_failure, hardware initialized?" << '\n';
00168     return -1;
00169   }
00170   
00171         // Specify data package as write command
00172   data_packet_ = WRITE;
00173 
00174         // Specify data package protocol
00175         data_packet_ |= (protocol << 1);
00176  
00177   switch( protocol )
00178   {
00179     case I2C:
00180           {
00181       error_code = arduinoI2cWrite( (uint8_t)device_address, (uint32_t)frequency, reg_address, data, num_bytes );
00182       break;
00183           }
00184     case SPI:
00185     {
00186       error_code = arduinoSpiWrite ( (uint8_t)frequency, (uint8_t)flags[0], reg_address, data, num_bytes );
00187       break;
00188     }
00189           case PWM:
00190           {
00191             // Arduino only accepts 8 Bit PWM, so pass MSB only
00192                   error_code = arduinoPwmWrite( (uint32_t)frequency, reg_address, data[0] );
00193                   break;
00194           }
00195           case GPIO:
00196           {
00197       error_code = arduinoGpioWrite( reg_address, (bool)data[0] );
00198       break;
00199     }
00200     case ENCODER:
00201     {
00202       error_code = arduinoEncoderWrite( flags, data );
00203       break;
00204     }
00205     case ADCONVERTER:
00206     {
00207       error_code = arduinoAdcWrite( data );
00208       break;
00209     }
00210     default:
00211     {
00212       ROS_ERROR( "Arduino does not support writing through this protocol." );
00213       error_code = -1;
00214     }
00215   }
00216  
00217   return error_code; // bytes written, or error. 
00218 }
00219 
00220 
00221 /**********************************************************************/
00222 // supportedProtocol
00223 /**********************************************************************/
00224 bool ArduinoInterface::supportedProtocol( interface_protocol protocol )
00225 {
00226   switch( protocol )
00227   {
00228   case SPI: 
00229     return true;
00230   case I2C: 
00231     return true;
00232   case PWM:
00233     return true;
00234   case GPIO:
00235     return true;
00236   case ENCODER:
00237     return true;
00238   case ADCONVERTER:
00239     return true;
00240   case RS232: {}
00241   case RS485: {}
00242   case ETHERNET: {}
00243   case ETHERCAT: {}
00244   default:
00245     return false;
00246   }
00247 }
00248 
00249 
00250 /**********************************************************************/
00251 std::string ArduinoInterface::getID()
00252 /**********************************************************************/
00253 {
00254   return port_name_;
00255 }
00256 
00257 
00258 /**********************************************************************/
00259 /**********************************************************************/
00260 bool ArduinoInterface::waitOnBytes( int num_bytes )
00261 {
00262   // Error checking variables 
00263   unsigned int sleep_interval_us = 25;
00264   unsigned int loop_counter = 0;
00265 
00266   while( serial_port_->Available() < num_bytes ) // wait for Arduino's response.
00267   {
00268     double time_lapsed = (double)(loop_counter * sleep_interval_us ) / 1e6; // [s]
00269     if( time_lapsed > timeout_ )
00270     {
00271       ROS_ERROR( "Serial communication timed out waiting for: %d byte(s).", num_bytes );
00272       connection_failure_ = true;
00273       return false;
00274     }
00275     loop_counter++;
00276     usleep( sleep_interval_us );
00277   }
00278   return true;   
00279 }
00280 
00281 
00282 /**********************************************************************/
00283 /**********************************************************************/
00284 ssize_t ArduinoInterface::arduinoI2cWrite( uint8_t device_address, uint32_t frequency, uint8_t reg_address, uint8_t* data, size_t num_bytes )
00285 {
00286         // the only flag for I2C is the frequency, so no external argument accepted
00287         uint8_t flags;
00288 
00289   // Encode i2c frequency data into flags
00290         // lower 3 Bits are reserved for frequency
00291   switch( frequency )
00292   {
00293                 case 100000:
00294                         flags = FREQ_STANDARD;
00295                   break;
00296                 case 400000:
00297                         // set Bit 2
00298                   flags = FREQ_FAST;
00299                   break;
00300                 default:
00301                   ROS_ERROR("Arduino cannot write at this frequency.");
00302                   return -1; // error code. 
00303   }
00304    
00305   uint8_t i2c_write_prompt[5] = { data_packet_, flags, (uint8_t)device_address, reg_address, num_bytes };
00306   serial_port_->Write_Bytes( 5, i2c_write_prompt );
00307    
00308   //Wait for verification:
00309   waitOnBytes( 1 );
00310    
00311   int verification = serial_port_->Read();
00312    
00313   // Flush the buffer before receiving the data:
00314   serial_port_->Flush();
00315   if( verification != VERIFY )
00316   {
00317     ROS_INFO("No response to i2c write prompt. Instead, 0x%x", verification);
00318     return -1; // error code.
00319   }
00320 
00321   serial_port_->Write_Bytes( num_bytes, data );
00322 
00323   // wait for Arduino's Verification code:
00324   waitOnBytes( 1 );
00325    
00326   // Verify:
00327   verification = serial_port_->Read();
00328   if( verification != SUCCESS )
00329   {
00330     ROS_ERROR("arduinoI2cWrite error.  No verification.");
00331     return -1;
00332   }
00333    
00334   return num_bytes;
00335 }
00336 
00337 
00338 /**********************************************************************/
00339 /**********************************************************************/
00340 ssize_t ArduinoInterface::arduinoSpiWrite( uint8_t frequency, uint8_t flags, uint8_t reg_address, uint8_t* data, size_t num_bytes )
00341 {
00342   // construct array to send to Arduino:
00343   uint8_t write_packet[num_bytes + 5];
00344   // load it with setup parameters and data:
00345   write_packet[0] = data_packet_;
00346   write_packet[1] = frequency;
00347   write_packet[2] = flags;
00348   write_packet[3] = reg_address;
00349   write_packet[4] = num_bytes;
00350 
00351   for( uint8_t i = 0; i < num_bytes; i++ )
00352   {
00353     write_packet[i+5] = data[i];
00354   }
00355    
00356   // send the data:
00357   serial_port_->Write_Bytes( (num_bytes + 5), write_packet );
00358    
00359   usleep( 5000 );  
00360     
00361   //Wait for verification:
00362   waitOnBytes( 1 );
00363    
00364   uint8_t verification = serial_port_->Read();
00365    
00366   //serial_port_->Flush();
00367   if( verification != VERIFY )
00368   {
00369     ROS_INFO("No response to spi write prompt. Instead, 0x%x", verification);
00370     return -1; // error code.
00371   }
00372    
00373   return num_bytes;
00374 }
00375 
00376 
00377 /**********************************************************************/
00378 /**********************************************************************/
00379 ssize_t ArduinoInterface::arduinoSpiRead( uint8_t frequency, uint8_t flags, uint8_t reg_address, uint8_t* data, size_t num_bytes ) 
00380 {
00381   uint8_t spi_read_prompt[5] = { data_packet_, frequency, flags, reg_address, num_bytes };
00382                 
00383   serial_port_->Write_Bytes( 5, spi_read_prompt );
00384    
00385   // wait for the Arduino to verify these commands:
00386   while( serial_port_->Available() < 1 )
00387     ; // do nothing until verification arrives.
00388    
00389   // Verify:
00390   int verification = serial_port_->Read();
00391 
00392   if( verification != VERIFY )
00393   {
00394     ROS_INFO( "No response to SPI read prompt. Instead:  0x%x", verification );
00395     return -1; // error code.
00396   }
00397 
00398   // Timeout Check Routine: 
00399   bool error = waitOnBytes( num_bytes );
00400   if( error == false )
00401   {
00402     ROS_ERROR( "Read broke: Arduino did not return SPI data." );
00403     return -1; // error code.
00404   }
00405 
00406   // Read num_bytes bytes off the serial line:
00407   if( serial_port_->Read_Bytes( num_bytes, data ) == false )
00408         {
00409                 ROS_ERROR("Read_Bytes Error");
00410     return -1; // error code.
00411         }
00412   else
00413     return num_bytes;
00414 }
00415 
00416 
00417 
00418 /**********************************************************************/
00419 ssize_t ArduinoInterface::arduinoI2cRead(uint8_t device_address, uint32_t frequency, uint8_t reg_address, uint8_t* data, size_t num_bytes ) 
00420 {
00421 
00422 // the only flag for I2C is the frequency, so no external argument accepted
00423         uint8_t flags;
00424 
00425   // Encode i2c frequency data into flags
00426         // lower 3 Bits are reserved for frequency
00427   switch( frequency )
00428   {
00429                 case 100000:
00430                         flags = FREQ_STANDARD;
00431                   break;
00432                 case 400000:
00433                         // set Bit 2
00434                   flags = FREQ_FAST;
00435                   break;
00436                 default:
00437                   ROS_ERROR("Arduino cannot read at this frequency.");
00438                   return -1; // error code. 
00439   }
00440 
00441   uint8_t i2c_read_prompt[5] = { data_packet_, flags, (uint8_t)device_address, reg_address, num_bytes };
00442                 
00443   serial_port_->Write_Bytes( 5, i2c_read_prompt );  
00444    
00445   // wait for the Arduino to verify these commands:
00446   while( serial_port_->Available() < 1 )
00447     ;
00448 
00449   // Verify:
00450   int verification = serial_port_->Read();
00451 
00452   if( verification != VERIFY )
00453   {
00454     ROS_ERROR("ArduinoInterface::arduinoI2cRead(): No response to I2C read prompt. Instead:  0x%x", verification);
00455     return -1; // error code.
00456   }
00457    
00458   // Wait for data to arrive from sensor:
00459   //Timeout Check Routine:   
00460   bool error = waitOnBytes( num_bytes );
00461   if( error == false )
00462   {
00463     ROS_ERROR("ArduinoInterface::arduinoI2cRead(): Read broke: did not receive data back.");
00464     return -1; // error code.
00465   } 
00466    
00467   // Read num_bytes bytes off the serial line:
00468   if( serial_port_->Read_Bytes( num_bytes, data) == false )
00469     return -1; // error code.
00470   else
00471     return num_bytes;
00472 }
00473 
00474 /**********************************************************************/
00475 /**********************************************************************/
00476 ssize_t ArduinoInterface::arduinoPwmWrite( uint32_t frequency, uint8_t reg_address, uint8_t data )
00477 {
00478         if( frequency != 490)
00479         {
00480                 ROS_ERROR("Only frequency 490Hz suppported for Arduino");
00481                 return -1;
00482         }
00483         /* 
00484          * the following block is Arduino Uno specific
00485          */
00486         switch (reg_address)
00487         {
00488           case 3:
00489           case 5:
00490           case 6:
00491           case 9:
00492           case 10:
00493           case 11: break;
00494           default:
00495           {
00496             ROS_ERROR("The selected Pin number (reg_address) is not available for PWM");
00497             ROS_ERROR("Select Pins 3,5,6,9,10,11 instead");
00498             return -1;
00499           }
00500         }
00501         
00502   // construct array to send to Arduino:
00503   uint8_t write_packet[3];
00504   // load it with setup parameters and data:
00505   write_packet[0] = data_packet_;
00506   write_packet[1] = reg_address;
00507         write_packet[2] = data;
00508    
00509   // send the data:
00510   serial_port_->Write_Bytes( 3, write_packet );
00511    
00512   usleep( 5000 );    
00513   //Wait for verification:
00514   waitOnBytes( 1 );
00515    
00516   uint8_t verification = serial_port_->Read();
00517    
00518   if( verification != SUCCESS )
00519   {
00520     ROS_INFO("No SUCCESS response to PWM write prompt. Instead, 0x%x", verification);
00521     return -1; // error code.
00522   }
00523    
00524   return 1; // wrote one byte
00525 }
00526 
00527 /**********************************************************************/
00528 /**********************************************************************/
00529 ssize_t ArduinoInterface::arduinoGpioWrite( uint8_t pin, bool value )
00530 {
00531   /* 
00532          * the following block is Arduino Uno specific
00533          */
00534   switch (pin)
00535   {
00536     case 0:
00537     case 1:
00538     case 2:
00539     case 3:
00540     case 4:
00541     case 5:
00542     case 6:
00543     case 7:
00544     case 8:
00545     case 9:
00546     case 10:
00547     case 11:
00548     case 12:
00549     case 13: break;
00550     default:
00551     {
00552             ROS_ERROR("The selected Pin number is not available for GPIO");
00553             ROS_ERROR("Select Pins 0 through 13 instead");
00554             return -1;
00555           }
00556   }
00557   // construct array to send to Arduino:
00558   uint8_t write_packet[3];
00559   // load it with setup parameters and data:
00560   write_packet[0] = data_packet_;
00561   write_packet[1] = pin;
00562         write_packet[2] = value;
00563         
00564   // send the data:
00565   serial_port_->Write_Bytes( 3, write_packet );
00566   usleep( 5000 );    
00567   //Wait for verification:
00568   if(!( waitOnBytes( 1 )))
00569     return -1;
00570    
00571   uint8_t verification = serial_port_->Read();
00572    
00573   if( verification != SUCCESS )
00574   {
00575     ROS_INFO("No SUCCESS response to GPIO write prompt. Instead, 0x%x", verification);
00576     return -1; // error code.
00577   }
00578   return 1;
00579 }
00580 
00581 /**********************************************************************/
00582 /**********************************************************************/
00583 ssize_t ArduinoInterface::arduinoGpioRead( uint8_t flags, uint8_t pin, uint8_t* value )
00584 {
00585   /* 
00586          * the following block is Arduino Uno specific
00587          */
00588   switch (pin)
00589   {
00590     case 0:
00591     case 1:
00592     case 2:
00593     case 3:
00594     case 4:
00595     case 5:
00596     case 6:
00597     case 7:
00598     case 8:
00599     case 9:
00600     case 10:
00601     case 11:
00602     case 12:
00603     case 13: break;
00604     default:
00605     {
00606             ROS_ERROR("The selected Pin number is not available for GPIO");
00607             ROS_ERROR("Select Pins 0 through 13 instead");
00608             return -1;
00609           }
00610   }
00611   
00612   switch ((gpio_input_mode) flags )
00613   {
00614     /* 
00615            * PULLUP will only work with Arduino version 1.0.1 or greater!!!
00616            */
00617     case FLOATING: 
00618     case PULLUP: break;
00619     case PULLDOWN:
00620     {
00621       ROS_ERROR("The selected input mode is not available for Arduino");
00622             ROS_ERROR("Select FLOATING instead");
00623             return -1;
00624     }
00625     default:
00626     {
00627       ROS_ERROR("ArduinoInterface::arduinoGpioRead The selected input mode is not known");
00628       return -1;
00629     }
00630   }
00631   // construct array to send to Arduino:
00632   uint8_t write_packet[3];
00633   // load it with setup parameters and data:
00634   write_packet[0] = data_packet_;
00635   write_packet[1] = flags;
00636         write_packet[2] = pin;
00637    
00638   // send the data:
00639   if(! serial_port_->Write_Bytes( 3, write_packet ))
00640   {
00641     ROS_ERROR("ArduinoInterface::arduinoGpioRead(): Could not send data to Arduino");
00642     return -1;
00643   }
00644    
00645   usleep( 5000 );    
00646    
00647   // Wait for data to arrive from sensor:
00648   //Timeout Check Routine:   
00649   bool error = waitOnBytes( 1 );
00650   if( error == false )
00651   {
00652     ROS_ERROR("ArduinoInterface::arduinoGpioRead(): Read broke: did not receive data back.");
00653     return -1; // error code.
00654   } 
00655   // Read value off the serial line:
00656   if( serial_port_->Read_Bytes( 1, value) == false )
00657     return -1; // error code.
00658   else
00659     return 1;
00660 }
00661 
00662 /**********************************************************************/
00663 /**********************************************************************/
00664 ssize_t ArduinoInterface::arduinoEncoderRead( int* flags, uint8_t* data )
00665 {
00666   static const int num_bytes_encoder_transmission = 4;
00667   // construct array to send to Arduino:
00668   uint8_t write_packet[2];
00669   // load it with setup parameters and data:
00670   write_packet[0] = data_packet_;
00671   write_packet[1] = flags[0];
00672   
00673   // send the data:
00674   if(! serial_port_->Write_Bytes( 2, write_packet ))
00675   {
00676     ROS_ERROR("ArduinoInterface::arduinoEncoderRead(): Could not send data to Arduino");
00677     return -1;
00678   }
00679    
00680   usleep( 5000 );    
00681    
00682   // Wait for data to arrive from hardware:
00683   //Timeout Check Routine:   
00684   bool error = waitOnBytes( num_bytes_encoder_transmission );
00685   if( error == false )
00686   {
00687     ROS_ERROR("ArduinoInterface::arduinoEncoderRead(): Did not receive data back.");
00688     return -1; // error code.
00689   } 
00690   // Read value off the serial line:
00691   if( serial_port_->Read_Bytes( num_bytes_encoder_transmission, data ) == false )
00692     return -1; // error code.
00693   else
00694     return num_bytes_encoder_transmission;
00695 }
00696 
00697 /**********************************************************************/
00698 /**********************************************************************/
00699 ssize_t ArduinoInterface::arduinoEncoderWrite( int* flags, uint8_t* data )
00700 {  
00701   static const int num_bytes_encoder_transmission = 4;
00702   // construct array to send to Arduino:
00703   uint8_t write_packet[6];
00704   // load it with setup parameters and data:
00705   write_packet[0] = data_packet_;
00706   write_packet[1] = flags[0];  // command byte containing object_id in upper 4 bits and command in lower 4 bits
00707   
00708   switch( flags[0] & 0x0F) // read lower 4 bits to decrypt command
00709   {
00710     case CREATE:  // creates new object on arduino
00711     {
00712       /* 
00713              * the following block is Arduino Uno specific
00714              */
00715             //check for correct input
00716       switch(flags[1])  // first encoder pin
00717       {
00718         case 0:
00719         case 1:
00720         case 2:
00721         case 3:
00722         case 4:
00723         case 5:
00724         case 6:
00725         case 7:
00726         case 8:
00727         case 9:
00728         case 10:
00729         case 11:
00730         case 12:
00731         case 13: break;
00732         default:
00733         {
00734                 ROS_ERROR("The selected Pin number is not available for Encoder");
00735                 ROS_ERROR("Select Pins 0 through 13 instead");
00736                 return -1;
00737               }
00738       }
00739             switch(flags[2])  // second encoder pin
00740       {
00741         case 0:
00742         case 1:
00743         case 2:
00744         case 3:
00745         case 4:
00746         case 5:
00747         case 6:
00748         case 7:
00749         case 8:
00750         case 9:
00751         case 10:
00752         case 11:
00753         case 12:
00754         case 13: break;
00755         default:
00756         {
00757                 ROS_ERROR("The selected Pin number is not available for Encoder");
00758                 ROS_ERROR("Select Pins 0 through 13 instead");
00759                 return -1;
00760               }
00761       } 
00762       
00763       write_packet[2] = flags[1];  // encoder pin 1
00764       write_packet[3] = flags[2];  // encoder pin 2
00765       // send the data:
00766       if(! serial_port_->Write_Bytes( 4, write_packet ))
00767       {
00768         ROS_ERROR("ArduinoInterface::arduinoEncoderWrite(): Could not send data to Arduino");
00769         return -1;
00770       }
00771     }break;
00772     
00773     
00774     case DESTROY: // destroys object on hardware device
00775     {
00776       // send the data:
00777       if(! serial_port_->Write_Bytes( 2, write_packet ))
00778       {
00779         ROS_ERROR("ArduinoInterface::arduinoEncoderWrite(): Could not send data to Arduino");
00780         return -1;
00781       }
00782     }break;
00783     
00784     
00785     case SET_POSITION:  // sets new position for encoder object
00786     {
00787       write_packet[2] = data[0];  // position MSB
00788       write_packet[3] = data[1];
00789       write_packet[4] = data[2];
00790       write_packet[5] = data[3];  // position LSB
00791       // send the data:
00792       if(! serial_port_->Write_Bytes( 6, write_packet ))
00793       {
00794         ROS_ERROR("ArduinoInterface::arduinoEncoderWrite(): Could not send data to Arduino");
00795         return -1;
00796       }
00797     }break;
00798   }
00799   
00800   //Wait for verification:
00801   if(!( waitOnBytes( 1 )))
00802     return -1;
00803   uint8_t verification = serial_port_->Read();
00804    
00805   if( verification != SUCCESS )
00806   {
00807     ROS_ERROR("No SUCCESS response to GPIO write prompt. Instead, 0x%x", verification);
00808     return -1; // error code.
00809   }
00810   return num_bytes_encoder_transmission;
00811 }
00812 
00813 ssize_t ArduinoInterface::arduinoAdcRead( uint8_t pin, uint8_t* data )
00814 {
00815 /* 
00816          * the following block is Arduino Uno specific
00817          */
00818   switch (pin)
00819   {
00820     case 0:
00821     case 1:
00822     case 2:
00823     case 3:
00824     case 4:
00825     case 5: break;
00826     default:
00827     {
00828             ROS_ERROR("The selected Pin number is not available for ADC");
00829             ROS_ERROR("Select Pins 0 through 5 instead");
00830             return -1;
00831           }
00832   }
00833   
00834   // construct array to send to Arduino:
00835   uint8_t write_packet[2];
00836   // load it with setup parameters and data:
00837   write_packet[0] = data_packet_;
00838         write_packet[1] = pin;
00839    
00840   // send the data:
00841   if(! serial_port_->Write_Bytes( 2, write_packet ))
00842   {
00843     ROS_ERROR("ArduinoInterface::arduinoAdcRead(): Could not send data to Arduino");
00844     return -1;
00845   }
00846    
00847   usleep( 5000 );    
00848    
00849   // Wait for data to arrive from sensor:
00850   //Timeout Check Routine:   
00851   bool error = waitOnBytes( 2 );
00852   if( error == false )
00853   {
00854     ROS_ERROR("ArduinoInterface::arduinoAdcRead(): Read broke: did not receive data back.");
00855     return -1; // error code.
00856   } 
00857   // Read value off the serial line:
00858   uint16_t value = serial_port_->Read(); // writes the MSB byte of the received value
00859   value = value << 8; // shifts the MSB to the proper position
00860   value |= serial_port_->Read(); // writes the LSB
00861 
00862   /* the following code would allow returning the voltage as a float but is most likely not very portable because of the reinterpret cast
00863   
00864   // voltage[V] = value(0..1023) * reference_voltage_in_mV / 1000 / max_value
00865   float voltage = (float)( (double)value * (double)_reference_voltage / 1000.0 / (double)MAX_ADC_VALUE );
00866   uint32_t transport_helper = *reinterpret_cast<uint32_t *>( &voltage ); // interprets float into uint32_t for transmission
00867   */
00868   
00869   // converts the read value into micro volts
00870   // uint64_t initialization only to not loose accuracy during calculation
00871   uint64_t voltage = (uint64_t)( (uint64_t)value * (uint64_t)_reference_voltage * 1000 ) / (uint64_t)MAX_ADC_VALUE;
00872   
00873   // write expects an uint_8 array, chopping voltage to four uint8_t MSB first
00874   // maximum voltage is 5000000 micro volts which fits into four byte 
00875   uint32_t temp;
00876   temp = (voltage & (0xFF << 24)) >> 24;
00877   data[0] = (uint8_t)temp;
00878   temp = (voltage & (0xFF << 16)) >> 16;
00879   data[1] = (uint8_t)temp;
00880   temp = (voltage & (0xFF << 8)) >> 8;
00881   data[2] = (uint8_t)temp;
00882   temp = (voltage & (0xFF << 0));
00883   data[3] = (uint8_t)temp;
00884   
00885   return 4; // data array filled with 4 bytes
00886 }
00887 
00888 ssize_t ArduinoInterface::arduinoAdcWrite( uint8_t* voltage )
00889 {
00890   uint32_t temp[4];
00891   temp[0] = voltage[0];
00892   temp[1] = voltage[1];
00893   temp[2] = voltage[2];
00894   temp[3] = voltage[3];
00895   _reference_voltage  = temp[0] << 24;
00896   _reference_voltage |= temp[1] << 16;
00897   _reference_voltage |= temp[2] << 8;
00898   _reference_voltage |= temp[3] << 0;
00899   
00900   /* 
00901          * the following block is Arduino Uno specific
00902          */
00903   switch (_reference_voltage)
00904   {
00905   case 0:
00906   case 1:  
00907   case 1100:
00908   case 2560:
00909   case 5000:
00910     break;
00911   default:
00912     ROS_ERROR("The selected reference voltage is not available for ADC.");
00913     ROS_ERROR("Select 0, 1, 1100, 2560, or 5000 instead.");
00914     return -1;
00915   }
00916   // construct array to send to Arduino:
00917   uint8_t write_packet[3];
00918   // load it with setup parameters and data:
00919   write_packet[0] = data_packet_;
00920   write_packet[1] = voltage[2]; // transmitting only the two LSB is enough because Arduino has maximum reference value = 5000
00921   write_packet[2] = voltage[3];
00922         
00923   // send the data:
00924   serial_port_->Write_Bytes( 3, write_packet );
00925   usleep( 5000 );    //TODO remove?
00926   //Wait for verification:
00927   if(!( waitOnBytes( 1 )))
00928     return -1;
00929    
00930   uint8_t verification = serial_port_->Read();
00931    
00932   if( verification != SUCCESS )
00933   {
00934     ROS_INFO("No SUCCESS response to GPIO write prompt. Instead, 0x%x", verification);
00935     return -1; // error code.
00936   }
00937   return 4; // 4 bytes have been processed
00938 }


arduino_interface
Author(s): Joshua Vasquez and Philip Roan. Maintained by Philip Roan
autogenerated on Sat Dec 28 2013 16:49:06