Пример #1
0
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);
}
Пример #2
0
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;
}
Пример #3
0
/**
 * 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;
}
Пример #4
0
/**
 * 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;
}
Пример #6
0
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_);
  }
}
Пример #7
0
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";

}
Пример #8
0
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;
}