main.cpp
Go to the documentation of this file.
00001 
00002 // Author: Antons Rebguns
00003 
00004 #include <stdint.h>
00005 
00006 #include <sstream>
00007 #include <string>
00008 #include <vector>
00009 
00010 #include <dynamixel_hardware_interface/dynamixel_io.h>
00011 #include <dynamixel_hardware_interface/serial_proxy.h>
00012 
00013 #include <ros/ros.h>
00014 
00015 int main (int argc, char **argv)
00016 {
00017     ros::init(argc, argv, "dynamixel_hardware_interface");
00018 /*    dynamixel_hardware_interface::SerialProxy sp("/dev/ttyUSB0", "ttyUSB0", "1000000");
00019     sp.connect();
00020     ros::spin();
00021     exit(1);*/
00022     
00023     
00024     dynamixel_hardware_interface::DynamixelIO dxl_io("/dev/ttyUSB0", "1000000");
00025 
00026     int num_motors = 6;
00027     int motors[] = { 4, 5, 6, 13, 15, 16 };
00028 
00029     for (int i = 0; i < num_motors; ++i)
00030     {
00031         int id = motors[i];
00032 
00033         if (dxl_io.ping(id))
00034         {
00035             ROS_INFO("Pinging motor %d", id);
00036             
00037             uint16_t model_number, cw_angle, ccw_angle, position;
00038             uint8_t firmware_version, return_delay_time;
00039             int16_t velocity;
00040 
00041             dxl_io.getModelNumber(id, model_number);
00042             dxl_io.getFirmwareVersion(id, firmware_version);
00043             dxl_io.getReturnDelayTime(id, return_delay_time);
00044             dxl_io.getCWAngleLimit(id, cw_angle);
00045             dxl_io.getCCWAngleLimit(id, ccw_angle);
00046             dxl_io.getPosition(id, position);
00047             dxl_io.getVelocity(id, velocity);
00048 
00049             ROS_INFO("Pinging motor ID %d successfull, model is %d", id, model_number);
00050             ROS_INFO("ID %d: fw %d, delay %d, cw %d, ccw %d, pos %d, vel %d", id, firmware_version, return_delay_time, cw_angle, ccw_angle, position, velocity);
00051 
00052             //dxl_io.setVelocity(id, 64);
00053             //dxl_io.setPosition(id, 256);
00054         }
00055     }
00056 
00057     int position = 512;
00058     int velocity = 32;
00059     int cw_compliance_margin = 1;
00060     int ccw_compliance_margin = cw_compliance_margin;
00061     int cw_compliance_slope = 32;
00062     int ccw_compliance_slope = cw_compliance_slope;
00063     
00064 //     std::vector<std::vector<int> > value_pairs;
00065 // 
00066 //     std::vector<int> pair1;
00067 //     pair1.push_back(4);
00068 //     pair1.push_back(position);
00069 //     pair1.push_back(velocity);
00070 // 
00071 //     std::vector<int> pair2;
00072 //     pair2.push_back(5);
00073 //     pair2.push_back(position);
00074 //     pair2.push_back(velocity);
00075 // 
00076 //     std::vector<int> pair3;
00077 //     pair3.push_back(6);
00078 //     pair3.push_back(position);
00079 //     pair3.push_back(velocity);
00080 // 
00081 //     std::vector<int> pair4;
00082 //     pair4.push_back(13);
00083 //     pair4.push_back(position);
00084 //     pair4.push_back(velocity);
00085 // 
00086 //     value_pairs.push_back(pair1);
00087 //     value_pairs.push_back(pair2);
00088 //     value_pairs.push_back(pair3);
00089 //     value_pairs.push_back(pair4);
00090 // 
00091 //     dxl_io.setMultiPositionVelocity(value_pairs);
00092 
00093     std::vector<std::map<std::string, int> > vm;
00094     
00095     std::map<std::string, int> m;
00096     m["id"] = 15;
00097     m["target_position"] = position;
00098     m["target_velocity"] = velocity;
00099     m["cw_compliance_margin"] = cw_compliance_margin;
00100     m["ccw_compliance_margin"] = ccw_compliance_margin;
00101     m["cw_compliance_slope"] = cw_compliance_slope;
00102     m["ccw_compliance_slope"] = ccw_compliance_slope;
00103     vm.push_back(m);
00104     
00105     m.clear();
00106     m["id"] = 16;
00107     m["target_position"] = position;
00108     m["target_velocity"] = velocity;
00109     m["cw_compliance_margin"] = cw_compliance_margin;
00110     m["ccw_compliance_margin"] = ccw_compliance_margin;
00111     m["cw_compliance_slope"] = cw_compliance_slope;
00112     m["ccw_compliance_slope"] = ccw_compliance_slope;
00113     vm.push_back(m);
00114     
00115     dxl_io.setMultiValues(vm);
00116 
00117     return 0;
00118 }


dynamixel_hardware_interface
Author(s): Antons Rebguns
autogenerated on Sun Oct 5 2014 23:33:10