void DxlROSBaseNav::init(char *path)
{
CDxlConfig *config = new CDxlConfig();
if (path)
{
ROS_INFO("Using shared_serial");
motor1_ = new C3mxlROS(path);
motor2_ = new C3mxlROS(path);
}
else
{
ROS_INFO("Using direct connection");
motor1_ = new C3mxl();
motor2_ = new C3mxl();
serial_port_.port_open("/dev/ttyUSB0", LxSerial::RS485_FTDI);
serial_port_.set_speed(LxSerial::S921600);
motor1_->setSerialPort(&serial_port_);
motor2_->setSerialPort(&serial_port_);
}
motor1_->setConfig(config->setID(106));
motor1_->init(false);
motor2_->setConfig(config->setID(107));
motor2_->init(false);
motor1_->set3MxlMode(SPEED_MODE);
motor2_->set3MxlMode(SPEED_MODE);
motor1_->setPIDPosition(0.1,0,0,5);
motor2_->setPIDPosition(0.1,0,0,5);
motor1_->setAcceleration(2,false);
motor2_->setAcceleration(2,false);
x_ = 0;
y_ = 0;
th_ = 0;
prevLeft_ = 0;
prevRight_ = 0;
wheel_diameter_ = 0.26;
wheel_base_ = 0.3;
delete config;
/*
speed1_pub = n.advertise<std_msgs::Float64>("speed1", 1);
speed2_pub = n.advertise<std_msgs::Float64>("speed2", 1);
pos1_pub = n.advertise<std_msgs::Float64>("pos1", 1);
pos2_pub = n.advertise<std_msgs::Float64>("pos2", 1);
*/
odom_pub_ = nh_.advertise<nav_msgs::Odometry>("/odom", 10);
}
void DxlROSExample::init(char *path)
{
CDxlConfig *config = new CDxlConfig();
if (path)
{
ROS_INFO("Using shared_serial");
motor_ = new C3mxlROS(path);
}
else
{
ROS_INFO("Using direct connection");
motor_ = new C3mxl();
serial_port_.port_open("/dev/ttyUSB0", LxSerial::RS485_FTDI);
serial_port_.set_speed(LxSerial::S921600);
motor_->setSerialPort(&serial_port_);
}
motor_->setConfig(config->setID(100));
ROS_ASSERT(motor_->init(false) == DXL_SUCCESS);
ROS_ASSERT(motor_->set3MxlMode(TORQUE_MODE) == DXL_SUCCESS);
delete config;
}
/**
* Initialize for testing
* @param maxpwm maximal speed to climb to
*/
void PWMBoardTest::init(char *maxpwm)
{
CDxlConfig *config = new CDxlConfig();
maxPWM = boost::lexical_cast<double>(maxpwm);
ROS_INFO("Set PWM to: %f",maxPWM);
config->mDxlTypeStr = "3MXL";
motors = new CDxlGroup();
serial_port.port_open("/dev/ttyUSB0", LxSerial::RS485_FTDI);
serial_port.set_speed(LxSerial::S921600);
motors->setSerialPort(&serial_port);
motors->addNewDynamixel(config->setID(107));
motors->addNewDynamixel(config->setID(106));
motors->init();
if (MODE == 0)
{
motors->getDynamixel(0)->set3MxlMode(PWM_MODE);
motors->getDynamixel(1)->set3MxlMode(PWM_MODE);
}
else
{
motors->getDynamixel(0)->set3MxlMode(SPEED_MODE);
motors->getDynamixel(1)->set3MxlMode(SPEED_MODE);
}
delete config;
}
/**
* Initializes a motor
* @param id The 3mxl bus ID of the corresponding motor.
* @return Pointer to a C3mxlROS object, used to control the motor.
*/
C3mxlROS* initializeMotor(int id){
// Initialize 3mxl node and set config
C3mxlROS *motor = new C3mxlROS("/threemxl_com");
CDxlConfig *config = new CDxlConfig();
config->setID(id);
motor->setConfig(config);
motor->init(false);
return motor;
}
/**
* Initialize the motors interface and add the established connection
* to the interface.
* The motors are set to SPEED_MODE
*
* The first motor from the motors interface array is the left motor,
* The second motor from the motors interface array is the right motor.
*
* @return true if successful
*/
bool ThreeMxlBoard::init_motors()
{
ROS_DEBUG("Initialize motors with id 107 an 106");
//Check for connection
if (serial_port.is_port_open())
{
CDxlConfig *config = new CDxlConfig();
config->mDxlTypeStr = "3MXL";
//First motor, left
motors->addNewDynamixel(config->setID(107));
//Second motor, right
motors->addNewDynamixel(config->setID(106));
//Set connection
motors->setSerialPort(&serial_port);
//Initialize and set mode to SPEED
ROS_ASSERT(motors->init());
ROS_ASSERT(motors->getDynamixel(0)->set3MxlMode(SPEED_MODE) == DXL_SUCCESS);
ROS_ASSERT(motors->getDynamixel(1)->set3MxlMode(SPEED_MODE) == DXL_SUCCESS);
//TODO check if needed
//Make sure to set speed to 0
drive_angularSpeed(0, 0);
delete config;
}
else
{
ROS_ERROR("The serial port is not open");
return false;
}
return true;
}
void DxlROSConsole::setMotor(int id)
{
motor_ = NULL;
// See if this motor already exists
for (size_t ii=0; ii < motors_.size(); ++ii)
if (motors_[ii]->getID() == id)
motor_ = motors_[ii];
// If not, construct a new motor object
if (!motor_)
{
motor_ = createMotor();
CDxlConfig config;
motor_->setConfig(config.setID(id));
motors_.push_back(motor_);
}
}
void DxlROSGripper::init(char *path)
{
CDxlConfig *config = new CDxlConfig();
if (path)
{
ROS_INFO("Using shared_serial");
motor_ = new C3mxlROS(path);
}
else
{
ROS_INFO("Using direct connection");
motor_ = new C3mxl();
serial_port_.port_open("/dev/ttyUSB1", LxSerial::RS485_FTDI);
serial_port_.set_speed(LxSerial::S921600);
motor_->setSerialPort(&serial_port_);
}
motor_->setConfig(config->setID(109));
motor_->init(false);
delete config;
closed = false;
cur = 0;
count = 0;
ros::NodeHandle n;
//configure visual gripper
joint_pub = n.advertise<sensor_msgs::JointState>("joint_states", 1);
js.name.resize(7);
js.position.resize(7);
js.name[0] ="motor_to_base";
js.name[1] ="base_to_left_finger";
js.name[2] ="base_to_right_finger1";
js.name[3] ="base_to_right_finger2";
js.name[4] ="left_finger_to_left_fingertip";
js.name[5] ="right_finger1_to_right_fingertip1";
js.name[6] ="right_finger2_to_right_fingertip2";
}
bool DxlROSCommand::execute(ArgList args)
{
// Sanity check
if (args.size() != nargs_)
return false;
if (name_ == "exit")
{
ros::shutdown();
return true;
}
else if (name_ == "id")
{
if (args.size() == 1)
{
int id = atoi(args[0].c_str());
if (id >= 0 && id <= 255)
console_->setMotor(id);
else
return false;
}
else
{
CDxlGeneric *motor = console_->getMotor();
DxlROSConsole::MotorList &motors = console_->getMotors();
cout << "Available motors:" << endl;
for (size_t ii=0; ii < motors.size(); ++ii)
{
if (motors[ii] == motor)
cout << "* ";
else
cout << " ";
cout << "id " << motors[ii]->getID() << endl;
}
}
return true;
}
else if (name_ == "hb")
{
double freq = atof(args[0].c_str());
if (freq < 0)
return false;
if (freq > 0)
console_->setHeartbeatInterval(1/freq);
else
console_->setHeartbeatInterval(0);
return true;
}
else if (name_ == "scan")
{
CDxlGeneric *motor = console_->createMotor();
CDxlConfig config;
std::vector<int> motors;
for (int id=1; id < BROADCAST_ID && ros::ok(); ++id)
{
motor->setConfig(config.setID(id));
if (motor->ping() == DXL_SUCCESS)
motors.push_back(id);
std::cout << "Scanning [";
for (int ii=1; ii < BROADCAST_ID; ++ii)
if (!(ii%15))
{
if (ii < id)
cout << "=";
else
cout << ".";
}
cout << "]\r" << flush;
}
delete motor;
if (motors.empty())
{
cout << "No motors found. Check the connection and power." << endl;
}
else
{
cout << "Motor controllers found on bus:" << endl;
for (size_t ii=0; ii < motors.size(); ++ii)
cout << " id " << motors[ii] << endl;
}
return true;
}
CDxlGeneric *motor = console_->getMotor();
if (!motor)
{
cout << "Error:" << endl << " No motor specified" << endl;
return true;
}
if (name_ == "init")
DXLC_SAFE_CALL(motor->init(false));
else if (name_ == "mode")
{
if (args[0] == "speed")
DXLC_SAFE_CALL(motor->set3MxlMode(SPEED_MODE));
else if (args[0] == "pos")
DXLC_SAFE_CALL(motor->set3MxlMode(POSITION_MODE));
else if (args[0] == "current")
DXLC_SAFE_CALL(motor->set3MxlMode(CURRENT_MODE));
else if (args[0] == "torque")
DXLC_SAFE_CALL(motor->set3MxlMode(TORQUE_MODE));
else if (args[0] == "sine")
DXLC_SAFE_CALL(motor->set3MxlMode(SINUSOIDAL_POSITION_MODE));
else if (args[0] == "ext_init")
DXLC_SAFE_CALL(motor->set3MxlMode(EXTERNAL_INIT));
else if (args[0] == "hsi_init")
DXLC_SAFE_CALL(motor->set3MxlMode(HOME_SWITCH_AND_INDEX_INIT));
else if (args[0] == "pwm")
DXLC_SAFE_CALL(motor->set3MxlMode(PWM_MODE));
else if (args[0] == "stop")
DXLC_SAFE_CALL(motor->set3MxlMode(STOP_MODE));
else
{
cout << "Unknown mode" << endl;
return false;
}
}
else if (name_ == "config")
{
CXMLConfiguration config_xml;
if (!config_xml.loadFile(args[0]))
{
cout << "Couldn't read config from " << args[0] << endl;
return false;
}
CDxlConfig config;
if (args.size() == 2)
config.readConfig(config_xml.root().section(args[1]));
else
config.readConfig(config_xml.root());
if (config.mID.isSet() && config.mID != motor->getID())
cout << "Config has different ID. Ignoring" << std::endl;
config.setID(motor->getID());
DXLC_SAFE_CALL(config.configureDynamixel(motor));
}
else if (name_ == "pos")
{
switch (args.size())
{
case 0:
DXLC_SAFE_CALL(motor->getPos());
cout << "Position:" << endl << " " << motor->presentPos() << " rad" << endl;
break;
case 1:
DXLC_SAFE_CALL(motor->setPos(atof(args[0].c_str())));
break;
case 2:
DXLC_SAFE_CALL(motor->setPos(atof(args[0].c_str()), atof(args[1].c_str())));
break;
case 3:
DXLC_SAFE_CALL(motor->setPos(atof(args[0].c_str()), atof(args[1].c_str()), atof(args[2].c_str())));
break;
}
}
else if (name_ == "speed")
DXLC_SAFE_CALL(motor->setSpeed(atof(args[0].c_str())));
else if (name_ == "accel")
DXLC_SAFE_CALL(motor->setAcceleration(atof(args[0].c_str())));
else if (name_ == "moffset")
DXLC_SAFE_CALL(motor->setMotorOffset(atof(args[0].c_str())));
else if (name_ == "joffset")
DXLC_SAFE_CALL(motor->setJointOffset(atof(args[0].c_str())));
else if (name_ == "jlimits")
DXLC_SAFE_CALL(motor->setAngleLimits(atof(args[0].c_str()), atof(args[1].c_str())));
else if (name_ == "lpos")
{
switch (args.size())
{
case 0:
DXLC_SAFE_CALL(motor->getLinearPos());
cout << "Position:" << endl << " " << motor->presentLinearPos() << " m" << endl;
break;
case 1:
DXLC_SAFE_CALL(motor->setLinearPos(atof(args[0].c_str())));
break;
case 2:
DXLC_SAFE_CALL(motor->setLinearPos(atof(args[0].c_str()), atof(args[1].c_str())));
break;
case 3:
DXLC_SAFE_CALL(motor->setLinearPos(atof(args[0].c_str()), atof(args[1].c_str()), atof(args[2].c_str())));
break;
}
}
else if (name_ == "lspeed")
DXLC_SAFE_CALL(motor->setLinearSpeed(atof(args[0].c_str())));
else if (name_ == "laccel")
DXLC_SAFE_CALL(motor->setLinearAcceleration(atof(args[0].c_str())));
else if (name_ == "current")
DXLC_SAFE_CALL(motor->setCurrent(atof(args[0].c_str())));
else if (name_ == "torque")
DXLC_SAFE_CALL(motor->setTorque(atof(args[0].c_str())));
else if (name_ == "pwm")
DXLC_SAFE_CALL(motor->setPWM(atof(args[0].c_str())));
else if (name_ == "freq")
DXLC_SAFE_CALL(motor->setSineFrequency(atof(args[0].c_str())));
else if (name_ == "ampl")
DXLC_SAFE_CALL(motor->setSineAmplitude(atof(args[0].c_str())));
else if (name_ == "phase")
DXLC_SAFE_CALL(motor->setSinePhase(atof(args[0].c_str())));
else if (name_ == "accel")
DXLC_SAFE_CALL(motor->setAcceleration(atof(args[0].c_str())));
else if (name_ == "ppid")
{
if (args.empty())
{
double p, d, i, i_limit;
DXLC_SAFE_CALL(motor->getPIDPosition(p, d, i, i_limit));
cout << "Position gains:" << endl << " " << std::fixed
<< "P " << std::setw(8) << std::setprecision(3) << p << ", "
<< "D " << std::setw(8) << std::setprecision(3) << d << ", "
<< "I " << std::setw(8) << std::setprecision(3) << i << ", "
<< "IL " << std::setw(8) << std::setprecision(3) << i_limit << endl;
}
else
DXLC_SAFE_CALL(motor->setPIDPosition(atof(args[0].c_str()), atof(args[1].c_str()), atof(args[2].c_str()), atof(args[3].c_str())));
}
else if (name_ == "spid")
{
if (args.empty())
{
double p, d, i, i_limit;
DXLC_SAFE_CALL(motor->getPIDSpeed(p, d, i, i_limit));
cout << "Speed gains:" << endl << " " << std::fixed
<< "P " << std::setw(8) << std::setprecision(3) << p << ", "
<< "D " << std::setw(8) << std::setprecision(3) << d << ", "
<< "I " << std::setw(8) << std::setprecision(3) << i << ", "
<< "IL " << std::setw(8) << std::setprecision(3) << i_limit << endl;
}
else
DXLC_SAFE_CALL(motor->setPIDSpeed(atof(args[0].c_str()), atof(args[1].c_str()), atof(args[2].c_str()), atof(args[3].c_str())));
}
else if (name_ == "cpid")
{
if (args.empty())
{
double p, d, i, i_limit;
DXLC_SAFE_CALL(motor->getPIDCurrent(p, d, i, i_limit));
cout << "Current gains:" << endl << " " << std::fixed
<< "P " << std::setw(8) << std::setprecision(3) << p << ", "
<< "D " << std::setw(8) << std::setprecision(3) << d << ", "
<< "I " << std::setw(8) << std::setprecision(3) << i << ", "
<< "IL " << std::setw(8) << std::setprecision(3) << i_limit << endl;
}
else
DXLC_SAFE_CALL(motor->setPIDCurrent(atof(args[0].c_str()), atof(args[1].c_str()), atof(args[2].c_str()), atof(args[3].c_str())));
}
else if (name_ == "tpid")
{
if (args.empty())
{
double p, d, i, i_limit;
DXLC_SAFE_CALL(motor->getPIDTorque(p, d, i, i_limit));
cout << "Torque gains:" << endl << " " << std::fixed
<< "P " << std::setw(8) << std::setprecision(3) << p << ", "
<< "D " << std::setw(8) << std::setprecision(3) << d << ", "
<< "I " << std::setw(8) << std::setprecision(3) << i << ", "
<< "IL " << std::setw(8) << std::setprecision(3) << i_limit << endl;
}
else
DXLC_SAFE_CALL(motor->setPIDTorque(atof(args[0].c_str()), atof(args[1].c_str()), atof(args[2].c_str()), atof(args[3].c_str())));
}
else if (name_ == "state")
{
DxlROSConsole::MotorList &motors = console_->getMotors();
cout << "State:" << endl << std::fixed;
for (size_t ii=0; ii < motors.size(); ++ii)
{
if (motors[ii]->isInitialized() && motors[ii]->getID() != BROADCAST_ID)
{
if (motors[ii] == motor)
cout << "* ";
else
cout << " ";
DXLC_SAFE_CALL(motors[ii]->getState());
DXLC_SAFE_CALL(motors[ii]->getStatus());
cout << "id " << motors[ii]->getID() << " "
<< std::setw(8) << std::setprecision(3) << motors[ii]->presentVoltage() << " V, "
<< std::setw(8) << std::setprecision(3) << motors[ii]->presentCurrent() << " A, "
<< std::setw(8) << std::setprecision(3) << motors[ii]->presentTorque() << " Nm, "
<< std::setw(8) << std::setprecision(3) << motors[ii]->presentPos() << " rad, "
<< std::setw(8) << std::setprecision(3) << motors[ii]->presentSpeed() << " rad/s, "
<< C3mxl::translateErrorCode(motors[ii]->presentStatus()) << " (0x" << hex << motors[ii]->presentStatus() << dec << ")" << endl;
}
}
cout << std::resetiosflags(std::ios_base::floatfield);
}
else if (name_ == "bus")
{
DXLC_SAFE_CALL(motor->getBusVoltage());
cout << "Bus voltage:" << endl << " " << motor->presentBusVoltage() << " V" << endl;
}
else if (name_ == "sensors")
{
DXLC_SAFE_CALL(motor->getSensorVoltages());
cout << "Analog sensor voltages:" << endl << " " << std::fixed
<< "Bus " << std::setw(8) << std::setprecision(3) << motor->presentBusVoltage() << ", "
<< "Current ADC " << std::setw(8) << std::setprecision(3) << motor->presentCurrentADCVoltage() << ", "
<< "Analog 1 " << std::setw(8) << std::setprecision(3) << motor->presentAnalog1Voltage() << ", "
<< "Analog 2 " << std::setw(8) << std::setprecision(3) << motor->presentAnalog2Voltage() << endl;
}
else if (name_ == "log")
DXLC_SAFE_CALL(motor->setLogInterval(atoi(args[0].c_str())));
else if (name_ == "getlog")
{
DXLC_SAFE_CALL(motor->getLog());
TMxlLog log = motor->presentLog();
cout << " Time PWM Current Voltage Desired Actual" << endl;
for (TMxlLog::iterator ii = log.begin(); ii != log.end(); ++ii)
cout << (*ii) << endl;
}
else if (name_ == "savelog")
{
DXLC_SAFE_CALL(motor->getLog());
TMxlLog log = motor->presentLog();
std::ofstream logstream(args[0].c_str());
logstream << " Time PWM Current Voltage Desired Actual" << endl;
for (TMxlLog::iterator ii = log.begin(); ii != log.end(); ++ii)
logstream << (*ii) << endl;
logstream.close();
}
else if (name_ == "table")
{
BYTE data[256];
// Read control table in chunks to stay within maximum message size
for (int ii=0; ii < 256; ii += 64)
DXLC_SAFE_CALL(motor->readData(ii, 64, &data[ii]));
cout << "Control table:" << endl;
for (int rr=0; rr != 16; rr++)
{
cout << " " << hex << std::setfill('0');
for (int cc=0; cc != 16; ++cc)
{
cout << std::setw(2) << cc*16+rr << ": " << std::setw(2) << (int)data[cc*16+rr];
if (cc < 16-1) cout << ", ";
}
cout << dec << std::setfill(' ') << endl;
}
}
else
{
cout << "Error:" << endl << " Unknown command" << endl;
return false;
}
return true;
}