00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039 #include "arduino_interface/arduino_interface.hpp"
00040
00041
00042
00043
00044 ArduinoInterface::ArduinoInterface( std::string port_name ) :
00045 port_name_( port_name ),
00046 baud_rate_( DEFAULT_BAUD_RATE ),
00047 connection_failure_( true ),
00048 timeout_( 0.5 ),
00049 is_initialized_( false ),
00050 data_packet_( 0 ),
00051 _reference_voltage(5000)
00052 {
00053
00054 serial_port_ = new uniserial();
00055 }
00056
00057
00058
00059
00060
00061 ArduinoInterface::~ArduinoInterface()
00062 {
00063 delete serial_port_;
00064 }
00065
00066
00067
00068
00069
00070 bool ArduinoInterface::initialize()
00071 {
00072
00073 if( is_initialized_ == true )
00074 {
00075 ROS_INFO( "ArduinoInterface::initialize(): hardware already initialized." );
00076 return true;
00077 }
00078
00079
00080 bool init_success = serial_port_->initialize( port_name_, 8 , 0, 1, baud_rate_ );
00081
00082
00083 sleep( 3 );
00084
00085
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
00094 connection_failure_ = false;
00095 is_initialized_ = true;
00096 ROS_INFO( "ArduinoInterface::initialize(): hardware initialized." );
00097 return true;
00098 }
00099
00100
00101
00102
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
00109 if( connection_failure_ == true )
00110 {
00111 return -1;
00112 }
00113
00114
00115 data_packet_ = READ;
00116
00117
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
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
00165 if( connection_failure_ == true )
00166 {
00167 std::cout << "connection_failure, hardware initialized?" << '\n';
00168 return -1;
00169 }
00170
00171
00172 data_packet_ = WRITE;
00173
00174
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
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;
00218 }
00219
00220
00221
00222
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
00263 unsigned int sleep_interval_us = 25;
00264 unsigned int loop_counter = 0;
00265
00266 while( serial_port_->Available() < num_bytes )
00267 {
00268 double time_lapsed = (double)(loop_counter * sleep_interval_us ) / 1e6;
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
00287 uint8_t flags;
00288
00289
00290
00291 switch( frequency )
00292 {
00293 case 100000:
00294 flags = FREQ_STANDARD;
00295 break;
00296 case 400000:
00297
00298 flags = FREQ_FAST;
00299 break;
00300 default:
00301 ROS_ERROR("Arduino cannot write at this frequency.");
00302 return -1;
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
00309 waitOnBytes( 1 );
00310
00311 int verification = serial_port_->Read();
00312
00313
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;
00319 }
00320
00321 serial_port_->Write_Bytes( num_bytes, data );
00322
00323
00324 waitOnBytes( 1 );
00325
00326
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
00343 uint8_t write_packet[num_bytes + 5];
00344
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
00357 serial_port_->Write_Bytes( (num_bytes + 5), write_packet );
00358
00359 usleep( 5000 );
00360
00361
00362 waitOnBytes( 1 );
00363
00364 uint8_t verification = serial_port_->Read();
00365
00366
00367 if( verification != VERIFY )
00368 {
00369 ROS_INFO("No response to spi write prompt. Instead, 0x%x", verification);
00370 return -1;
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
00386 while( serial_port_->Available() < 1 )
00387 ;
00388
00389
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;
00396 }
00397
00398
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;
00404 }
00405
00406
00407 if( serial_port_->Read_Bytes( num_bytes, data ) == false )
00408 {
00409 ROS_ERROR("Read_Bytes Error");
00410 return -1;
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
00423 uint8_t flags;
00424
00425
00426
00427 switch( frequency )
00428 {
00429 case 100000:
00430 flags = FREQ_STANDARD;
00431 break;
00432 case 400000:
00433
00434 flags = FREQ_FAST;
00435 break;
00436 default:
00437 ROS_ERROR("Arduino cannot read at this frequency.");
00438 return -1;
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
00446 while( serial_port_->Available() < 1 )
00447 ;
00448
00449
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;
00456 }
00457
00458
00459
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;
00465 }
00466
00467
00468 if( serial_port_->Read_Bytes( num_bytes, data) == false )
00469 return -1;
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
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
00503 uint8_t write_packet[3];
00504
00505 write_packet[0] = data_packet_;
00506 write_packet[1] = reg_address;
00507 write_packet[2] = data;
00508
00509
00510 serial_port_->Write_Bytes( 3, write_packet );
00511
00512 usleep( 5000 );
00513
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;
00522 }
00523
00524 return 1;
00525 }
00526
00527
00528
00529 ssize_t ArduinoInterface::arduinoGpioWrite( uint8_t pin, bool value )
00530 {
00531
00532
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
00558 uint8_t write_packet[3];
00559
00560 write_packet[0] = data_packet_;
00561 write_packet[1] = pin;
00562 write_packet[2] = value;
00563
00564
00565 serial_port_->Write_Bytes( 3, write_packet );
00566 usleep( 5000 );
00567
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;
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
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
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
00632 uint8_t write_packet[3];
00633
00634 write_packet[0] = data_packet_;
00635 write_packet[1] = flags;
00636 write_packet[2] = pin;
00637
00638
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
00648
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;
00654 }
00655
00656 if( serial_port_->Read_Bytes( 1, value) == false )
00657 return -1;
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
00668 uint8_t write_packet[2];
00669
00670 write_packet[0] = data_packet_;
00671 write_packet[1] = flags[0];
00672
00673
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
00683
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;
00689 }
00690
00691 if( serial_port_->Read_Bytes( num_bytes_encoder_transmission, data ) == false )
00692 return -1;
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
00703 uint8_t write_packet[6];
00704
00705 write_packet[0] = data_packet_;
00706 write_packet[1] = flags[0];
00707
00708 switch( flags[0] & 0x0F)
00709 {
00710 case CREATE:
00711 {
00712
00713
00714
00715
00716 switch(flags[1])
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])
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];
00764 write_packet[3] = flags[2];
00765
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:
00775 {
00776
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:
00786 {
00787 write_packet[2] = data[0];
00788 write_packet[3] = data[1];
00789 write_packet[4] = data[2];
00790 write_packet[5] = data[3];
00791
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
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;
00809 }
00810 return num_bytes_encoder_transmission;
00811 }
00812
00813 ssize_t ArduinoInterface::arduinoAdcRead( uint8_t pin, uint8_t* data )
00814 {
00815
00816
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
00835 uint8_t write_packet[2];
00836
00837 write_packet[0] = data_packet_;
00838 write_packet[1] = pin;
00839
00840
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
00850
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;
00856 }
00857
00858 uint16_t value = serial_port_->Read();
00859 value = value << 8;
00860 value |= serial_port_->Read();
00861
00862
00863
00864
00865
00866
00867
00868
00869
00870
00871 uint64_t voltage = (uint64_t)( (uint64_t)value * (uint64_t)_reference_voltage * 1000 ) / (uint64_t)MAX_ADC_VALUE;
00872
00873
00874
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;
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
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
00917 uint8_t write_packet[3];
00918
00919 write_packet[0] = data_packet_;
00920 write_packet[1] = voltage[2];
00921 write_packet[2] = voltage[3];
00922
00923
00924 serial_port_->Write_Bytes( 3, write_packet );
00925 usleep( 5000 );
00926
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;
00936 }
00937 return 4;
00938 }