@@ -25,15 +25,15 @@ sudo useradd $USER dialout
2525``` C++
2626#include < cstdlib>
2727#include < iostream>
28- #include < dynamixel++/Dynamixel ++.h>
28+ #include < dynamixel++/dynamixel ++.h>
2929
3030using namespace dynamixelplusplus ;
3131
3232static uint16_t const MX28_ControlTable_Torque_Enable = 64 ;
3333static uint16_t const MX28_ControlTable_GoalPosition = 116 ;
3434static uint16_t const MX28_ControlTable_PresentPosition = 132 ;
3535
36- int main (int argc, char ** argv)
36+ int main (int argc, char ** argv) try
3737{
3838 Dynamixel dynamixel_ctrl("/dev/serial/by-id/usb-FTDI_USB__ -__ Serial_Converter_FT4NNZ55-if00-port0",
3939 Dynamixel::Protocol::V2_0,
@@ -52,16 +52,21 @@ int main(int argc, char **argv)
5252 dynamixel_ctrl.syncWrite(MX28_ControlTable_Torque_Enable, goal_position_data_map);
5353
5454 /* Read current position. * /
55- std::map<Dynamixel::Id, uint32_t> position_map;
56- auto err_read = dynamixel_ctrl.syncRead(MX28_ControlTable_PresentPosition, id_vect, position_map);
57- if (err_read != Dynamixel::Error::None) {
58- std::cerr << "'syncRead' failed with error code " << static_cast<int >(err_read) << std::endl;
59- return EXIT_FAILURE;
60- }
61-
55+ std::map<Dynamixel::Id, uint32_t> const position_map = dynamixel_ctrl.syncRead<uint32_t>(MX28_ControlTable_PresentPosition, id_vect);
56+
6257 for (auto [ id, position_raw] : position_map)
6358 std::cout << "Dynamixel MX28 servo #" << static_cast<int >(id) << ": " << position_raw << std::endl;
6459
6560 return EXIT_SUCCESS;
6661}
62+ catch (dynamixelplusplus::CommunicationError const & e)
63+ {
64+ std::cerr << "CommunicationError caught: " << e.what() << std::endl;
65+ return EXIT_FAILURE;
66+ }
67+ catch (dynamixelplusplus::StatusError const & e)
68+ {
69+ std::cerr << "StatusError caught: " << e.what() << std::endl;
70+ return EXIT_FAILURE;
71+ }
6772```
0 commit comments