Ejemplo n.º 1
0
void DataTrace::stopTrace() {
  // Bouml preserved body begin 000C9471
    file.close();

    parametersEndTraceFile.open((path+"ParametersAfterTrace").c_str(), std::fstream::out | std::fstream::trunc);
    std::string parameterString;
    
    parametersEndTraceFile << "Name: " << this->name << std::endl;
    ptime today;
    today = second_clock::local_time();
    parametersEndTraceFile << "Date: " << boost::posix_time::to_simple_string(today) << std::endl;
    
        JointName jointName;
    joint.getConfigurationParameter(jointName);
    jointName.toString(parameterString);
    //   std::cout << parameterString << std::endl;
    parametersEndTraceFile << parameterString << std::endl;

    TorqueConstant torqueconst;
    joint.getConfigurationParameter(torqueconst);
    torqueconst.toString(parameterString);
    //   std::cout << parameterString << std::endl;
    parametersEndTraceFile << parameterString << std::endl;

    JointLimits jointLimits;
    joint.getConfigurationParameter(jointLimits);
    jointLimits.toString(parameterString);
    //   std::cout << parameterString << std::endl;
    parametersEndTraceFile << parameterString << std::endl;

    EncoderTicksPerRound encoderTicksPerRound;
    joint.getConfigurationParameter(encoderTicksPerRound);
    encoderTicksPerRound.toString(parameterString);
    //   std::cout << parameterString << std::endl;
    parametersEndTraceFile << parameterString << std::endl;

    GearRatio gearRatio;
    joint.getConfigurationParameter(gearRatio);
    gearRatio.toString(parameterString);
    //   std::cout << parameterString << std::endl;
    parametersEndTraceFile << parameterString << std::endl;
    
    InverseMovementDirection inverseMovementDirection;
    joint.getConfigurationParameter(inverseMovementDirection);
    inverseMovementDirection.toString(parameterString);
    //   std::cout << parameterString << std::endl;
    parametersEndTraceFile << parameterString << std::endl;
    
    for (int i = 0; i < parameterVector.size(); i++) {
      joint.getConfigurationParameter(*(parameterVector[i]));
      parameterVector[i]->toString(parameterString);
      parametersEndTraceFile << parameterString << std::endl;
      delete parameterVector[i];
    }
    
    
    parametersEndTraceFile.close();
  // Bouml preserved body end 000C9471
}
Ejemplo n.º 2
0
void DataTrace::startTrace()
{
  // Bouml preserved body begin 000C93F1

  timeDurationMicroSec = 0;

  file.open((path + "jointDataTrace").c_str(), std::fstream::out | std::fstream::trunc);

  ptime today;
  today = second_clock::local_time();

  file << "# Name: " << this->name << std::endl;

  file << "# Date: " << boost::posix_time::to_simple_string(today) << std::endl;

  JointName jointName;
  FirmwareVersion firmwareParameter;
  std::string parameterString;
  joint.getConfigurationParameter(firmwareParameter);
  joint.getConfigurationParameter(jointName);
  jointName.toString(parameterString);
  file << "# " << parameterString << std::endl;
  firmwareParameter.toString(parameterString);
  file << "# " << parameterString << std::endl;

  file << "# time [milliseconds]" << " " << "angle setpoint [rad]" << " " << "velocity setpoint [rad/s]" << " "
      << "RPM setpoint" << " " << "current setpoint [A]" << " " << "torque setpoint [Nm]" << " "
      << "ramp generator setpoint [rad/s]" << " " << "encoder setpoint"

      << " " << "sensed angle [rad]" << " " << "sensed encoder ticks" << " " << "sensed velocity [rad/s]" << " "
      << "sensed RPM" << " " << "sensed current [A]" << " " << "sensed torque [Nm]" << " " << "actual PWM"

      << " " << "OVER_CURRENT" << " " << "UNDER_VOLTAGE" << " " << "OVER_VOLTAGE" << " " << "OVER_TEMPERATURE" << " "
      << "MOTOR_HALTED" << " " << "HALL_SENSOR_ERROR" << " " << "PWM_MODE_ACTIVE" << " " << "VELOCITY_MODE" << " "
      << "POSITION_MODE" << " " << "TORQUE_MODE" << " " << "POSITION_REACHED" << " " << "INITIALIZED" << " "
      << "TIMEOUT" << " " << "I2T_EXCEEDED" << " " << std::endl;

  parametersBeginTraceFile.open((path + "ParametersAtBegin").c_str(), std::fstream::out | std::fstream::trunc);

  parameterVector.push_back(new ActualMotorVoltage);
  //   parameterVector.push_back(new ErrorAndStatus);
  parameterVector.push_back(new ActualMotorDriverTemperature);
  parameterVector.push_back(new I2tSum);
  parameterVector.push_back(new PositionError);
  parameterVector.push_back(new PositionErrorSum);
  parameterVector.push_back(new RampGeneratorSpeed);
  parameterVector.push_back(new VelocityError);
  parameterVector.push_back(new VelocityErrorSum);
  //   parameterVector.push_back(new CalibrateJoint);
  parameterVector.push_back(new DParameterFirstParametersPositionControl);
  parameterVector.push_back(new DParameterFirstParametersSpeedControl);
  parameterVector.push_back(new DParameterCurrentControl);
  parameterVector.push_back(new DParameterSecondParametersPositionControl);
  parameterVector.push_back(new DParameterSecondParametersSpeedControl);

  parameterVector.push_back(new IClippingParameterFirstParametersPositionControl);
  parameterVector.push_back(new IClippingParameterFirstParametersSpeedControl);
  parameterVector.push_back(new IClippingParameterCurrentControl);
  parameterVector.push_back(new IClippingParameterSecondParametersPositionControl);
  parameterVector.push_back(new IClippingParameterSecondParametersSpeedControl);
  //    parameterVector.push_back(new InitializeJoint);

  parameterVector.push_back(new IParameterFirstParametersPositionControl);
  parameterVector.push_back(new IParameterFirstParametersSpeedControl);
  parameterVector.push_back(new IParameterCurrentControl);
  parameterVector.push_back(new IParameterSecondParametersPositionControl);
  parameterVector.push_back(new IParameterSecondParametersSpeedControl);

  parameterVector.push_back(new MaximumPositioningVelocity);
  parameterVector.push_back(new MotorAcceleration);
  parameterVector.push_back(new PositionControlSwitchingThreshold);
  parameterVector.push_back(new PParameterFirstParametersPositionControl);
  parameterVector.push_back(new PParameterFirstParametersSpeedControl);
  parameterVector.push_back(new PParameterCurrentControl);
  parameterVector.push_back(new PParameterSecondParametersPositionControl);
  parameterVector.push_back(new PParameterSecondParametersSpeedControl);
  parameterVector.push_back(new RampGeneratorSpeedAndPositionControl);
  parameterVector.push_back(new SpeedControlSwitchingThreshold);

  parameterVector.push_back(new ActivateOvervoltageProtection);
  parameterVector.push_back(new ActualCommutationOffset);
  //   parameterVector.push_back(new ApproveProtectedParameters);
  parameterVector.push_back(new BEMFConstant);
  //   parameterVector.push_back(new ClearI2tExceededFlag);
  //   parameterVector.push_back(new ClearMotorControllerTimeoutFlag);
  parameterVector.push_back(new CommutationMode);
  parameterVector.push_back(new CommutationMotorCurrent);
  parameterVector.push_back(new CurrentControlLoopDelay);
  parameterVector.push_back(new EncoderResolution);
  parameterVector.push_back(new EncoderStopSwitch);
  parameterVector.push_back(new HallSensorPolarityReversal);
  parameterVector.push_back(new I2tExceedCounter);
  parameterVector.push_back(new I2tLimit);
  parameterVector.push_back(new InitializationMode);
  parameterVector.push_back(new InitSineDelay);
  parameterVector.push_back(new MassInertiaConstant);
  parameterVector.push_back(new MaximumMotorCurrent);
  parameterVector.push_back(new MaximumVelocityToSetPosition);
  parameterVector.push_back(new MotorCoilResistance);
  parameterVector.push_back(new MotorControllerTimeout);
  parameterVector.push_back(new MotorPoles);
  parameterVector.push_back(new OperationalTime);
  parameterVector.push_back(new PIDControlTime);
  parameterVector.push_back(new PositionTargetReachedDistance);
  parameterVector.push_back(new ReversingEncoderDirection);
  parameterVector.push_back(new SetEncoderCounterZeroAtNextNChannel);
  parameterVector.push_back(new SetEncoderCounterZeroAtNextSwitch);
  parameterVector.push_back(new SetEncoderCounterZeroOnlyOnce);
  parameterVector.push_back(new SineInitializationVelocity);
  parameterVector.push_back(new StopSwitchPolarity);
  parameterVector.push_back(new ThermalWindingTimeConstant);
  parameterVector.push_back(new VelocityThresholdForHallFX);
  parameterVector.push_back(new MotorHaltedVelocity);

//    apiParameterVector.push_back(new JointName);
//    apiParameterVector.push_back(new TorqueConstant);
//    apiParameterVector.push_back(new JointLimits);
//    apiParameterVector.push_back(new GearRatio);
//    apiParameterVector.push_back(new EncoderTicksPerRound);
//    apiParameterVector.push_back(new InverseMovementDirection);

//    for (int i = 0; i < apiParameterVector.size(); i++) {
//      joint.getConfigurationParameter(*(apiParameterVector[i]));
//      apiParameterVector[i]->toString(parameterString);
//      //   std::cout << parameterString << std::endl;
//      parametersBeginTraceFile << parameterString << std::endl;
//    }

  parametersBeginTraceFile << "Name: " << this->name << std::endl;
  parametersBeginTraceFile << "Date: " << boost::posix_time::to_simple_string(today) << std::endl;
  //  joint.getConfigurationParameter(jointName);
  jointName.toString(parameterString);
  //   std::cout << parameterString << std::endl;
  parametersBeginTraceFile << parameterString << std::endl;
  firmwareParameter.toString(parameterString);
  parametersBeginTraceFile << parameterString << std::endl;

  TorqueConstant torqueconst;
  joint.getConfigurationParameter(torqueconst);
  torqueconst.toString(parameterString);
  //   std::cout << parameterString << std::endl;
  parametersBeginTraceFile << parameterString << std::endl;

  JointLimits jointLimits;
  joint.getConfigurationParameter(jointLimits);
  jointLimits.toString(parameterString);
  //   std::cout << parameterString << std::endl;
  parametersBeginTraceFile << parameterString << std::endl;

  EncoderTicksPerRound encoderTicksPerRound;
  joint.getConfigurationParameter(encoderTicksPerRound);
  encoderTicksPerRound.toString(parameterString);
  //   std::cout << parameterString << std::endl;
  parametersBeginTraceFile << parameterString << std::endl;

  GearRatio gearRatio;
  joint.getConfigurationParameter(gearRatio);
  gearRatio.toString(parameterString);
  //   std::cout << parameterString << std::endl;
  parametersBeginTraceFile << parameterString << std::endl;

  InverseMovementDirection inverseMovementDirection;
  joint.getConfigurationParameter(inverseMovementDirection);
  inverseMovementDirection.toString(parameterString);
  //   std::cout << parameterString << std::endl;
  parametersBeginTraceFile << parameterString << std::endl;

  for (unsigned int i = 0; i < parameterVector.size(); i++)
  {
    joint.getConfigurationParameter(*(parameterVector[i]));
    parameterVector[i]->toString(parameterString);
    //   std::cout << parameterString << std::endl;
    parametersBeginTraceFile << parameterString << std::endl;
  }
  parametersBeginTraceFile.close();

  traceStartTime = microsec_clock::local_time();
  // Bouml preserved body end 000C93F1
}
Ejemplo n.º 3
0
void YouBotManipulator::initializeJoints() {
  // Bouml preserved body begin 00068071

 //   LOG(info) << "Initializing Joints";

    //enable overriding the number of joints
    if (configfile->keyExists("JointTopology", "NumberJoints"))
        configfile->readInto(numberArmJoints, "JointTopology", "NumberJoints");

    //get number of slaves
    unsigned int noSlaves = ethercatMaster.getNumberOfSlaves();

    if (noSlaves < numberArmJoints) {
      throw std::runtime_error("Not enough ethercat slaves were found to create a YouBotManipulator!");
    }

    unsigned int slaveNumber = 0;
    for (unsigned int i = 1; i <= numberArmJoints; i++) {
		std::stringstream jointConfigNameStream;
		jointConfigNameStream << "ManipulatorJoint" << i;
		std::string jointConfigName = jointConfigNameStream.str();

		configfile->readInto(slaveNumber, "JointTopology", jointConfigName);
		if (slaveNumber <= noSlaves) {
			joints.push_back(new YouBotJoint(slaveNumber));
		} else {
			throw std::out_of_range("The ethercat slave number is not available!");
		}
	}

    //Configure Joint Parameters
    std::string jointName;
    JointName jName;
    GearRatio gearRatio;
    EncoderTicksPerRound ticksPerRound;
    InverseMovementDirection inverseDir;
    double gearRatio_numerator = 0;
    double gearRatio_denominator = 1;
    FirmwareVersion firmwareTypeVersion;
    TorqueConstant torqueConst;
    double trajectory_p=0, trajectory_i=0, trajectory_d=0, trajectory_imax=0, trajectory_imin=0;

    for (unsigned int i = 0; i < numberArmJoints; i++) {
      std::stringstream jointNameStream;
      jointNameStream << "Joint_" << i + 1;
      jointName = jointNameStream.str();


      joints[i].getConfigurationParameter(firmwareTypeVersion);
      std::string version;
      int controllerType;
      std::string firmwareVersion;
      firmwareTypeVersion.getParameter(controllerType, firmwareVersion);

      string name;
      configfile->readInto(name, jointName, "JointName");
      jName.setParameter(name);

      LOG(info) << name << "\t Controller Type: " << controllerType << "  Firmware version: " << firmwareVersion;

      if (this->controllerType != controllerType && alternativeControllerType != controllerType) {
        std::stringstream ss;
        ss << "The youBot manipulator motor controller have to be of type: " << this->controllerType << " or " << alternativeControllerType;
        throw std::runtime_error(ss.str().c_str());
      }

      //check if firmware is supported
      bool isfirmwareSupported = false;
      for(unsigned int d = 0; d < supportedFirmwareVersions.size(); d++){
        if(this->supportedFirmwareVersions[d] == firmwareVersion){
          isfirmwareSupported = true;
          break;
        }
      }
      
      if(!isfirmwareSupported){
        throw std::runtime_error("Unsupported firmware version: " + firmwareVersion);
      }
      
      if(this->actualFirmwareVersionAllJoints == ""){
        this->actualFirmwareVersionAllJoints = firmwareVersion;
      }
      
      if(!(firmwareVersion == this->actualFirmwareVersionAllJoints)){
         throw std::runtime_error("All joints must have the same firmware version!");
      }

      configfile->readInto(gearRatio_numerator, jointName, "GearRatio_numerator");
      configfile->readInto(gearRatio_denominator, jointName, "GearRatio_denominator");
      gearRatio.setParameter(gearRatio_numerator / gearRatio_denominator);
      int ticks;
      configfile->readInto(ticks, jointName, "EncoderTicksPerRound");
      ticksPerRound.setParameter(ticks);
      
      double torqueConstant;
      configfile->readInto(torqueConstant, jointName, "TorqueConstant_[newton_meter_divided_by_ampere]");
      torqueConst.setParameter(torqueConstant);
      
      bool invdir = false;
      configfile->readInto(invdir, jointName, "InverseMovementDirection");
      inverseDir.setParameter(invdir);

      joints[i].setConfigurationParameter(jName);
      joints[i].setConfigurationParameter(gearRatio);
      joints[i].setConfigurationParameter(ticksPerRound);
      joints[i].setConfigurationParameter(torqueConst);
      joints[i].setConfigurationParameter(inverseDir);
      
      //Joint Trajectory Controller
      if(ethercatMaster.isThreadActive()){
				configfile->readInto(trajectory_p, jointName, "trajectory_controller_P");
        configfile->readInto(trajectory_i, jointName, "trajectory_controller_I");
        configfile->readInto(trajectory_d, jointName, "trajectory_controller_D");
        configfile->readInto(trajectory_imax, jointName, "trajectory_controller_I_max");
        configfile->readInto(trajectory_imin, jointName, "trajectory_controller_I_min");
        joints[i].trajectoryController.setConfigurationParameter(trajectory_p, trajectory_i, trajectory_d, trajectory_imax, trajectory_imin);
				joints[i].trajectoryController.setEncoderTicksPerRound(ticks);
        joints[i].trajectoryController.setGearRatio(gearRatio_numerator / gearRatio_denominator);
        joints[i].trajectoryController.setInverseMovementDirection(invdir);
        ethercatMasterWithThread->registerJointTrajectoryController(&(joints[i].trajectoryController), joints[i].getJointNumber());
			}
    }


		configfile->readInto(useGripper, "Gripper", "EnableGripper");
		
    if(useGripper){
    //Initializing Gripper
    configfile->readInto(slaveNumber, "JointTopology", "ManipulatorJoint5");
    this->gripper.reset(new YouBotGripper(slaveNumber));
    BarSpacingOffset barOffest;
    MaxTravelDistance maxDistance;
    MaxEncoderValue maxEncoder;
    GripperBarName BarName;
    double dummy = 0;
    int controllerType;
    double firmwareVersion;
    string barname;
    
    GripperFirmwareVersion gripperVersion;
    this->gripper->getConfigurationParameter(gripperVersion);
    gripperVersion.getParameter(controllerType, firmwareVersion);
    
    LOG(info) << "Gripper" << "\t\t Controller Type: " << controllerType << "  Firmware version: " << firmwareVersion;

    // Gripper Bar 1
    configfile->readInto(barname, "GripperBar1", "BarName");
    BarName.setParameter(barname);
    this->gripper->getGripperBar1().setConfigurationParameter(BarName);
    
    configfile->readInto(dummy, "GripperBar1", "BarSpacingOffset_[meter]");
    barOffest.setParameter(dummy * meter);
    this->gripper->getGripperBar1().setConfigurationParameter(barOffest);
    
    configfile->readInto(dummy, "GripperBar1", "MaxTravelDistance_[meter]");
    maxDistance.setParameter(dummy * meter);
    this->gripper->getGripperBar1().setConfigurationParameter(maxDistance);
    
    int maxenc = 0;
    configfile->readInto(maxenc, "GripperBar1", "MaxEncoderValue");
    maxEncoder.setParameter(maxenc);
    this->gripper->getGripperBar1().setConfigurationParameter(maxEncoder);
    
    int stallThreshold = 0;
    configfile->readInto(stallThreshold, "GripperBar1", "StallGuard2Threshold");
    StallGuard2Threshold threshold;
    threshold.setParameter(stallThreshold);
    this->gripper->getGripperBar1().setConfigurationParameter(threshold);

    bool stallGuardFilter = false;
    configfile->readInto(stallGuardFilter, "GripperBar1", "StallGuard2FilterEnable");
    StallGuard2FilterEnable filter;
    filter.setParameter(stallGuardFilter);
    this->gripper->getGripperBar1().setConfigurationParameter(filter);
    
    // Gripper Bar 2
    configfile->readInto(barname, "GripperBar2", "BarName");
    BarName.setParameter(barname);
    this->gripper->getGripperBar2().setConfigurationParameter(BarName);
    
    configfile->readInto(dummy, "GripperBar2", "BarSpacingOffset_[meter]");
    barOffest.setParameter(dummy * meter);
    this->gripper->getGripperBar2().setConfigurationParameter(barOffest);
    
    configfile->readInto(dummy, "GripperBar2", "MaxTravelDistance_[meter]");
    maxDistance.setParameter(dummy * meter);
    this->gripper->getGripperBar2().setConfigurationParameter(maxDistance);

    configfile->readInto(maxenc, "GripperBar2", "MaxEncoderValue");
    maxEncoder.setParameter(maxenc);
    this->gripper->getGripperBar2().setConfigurationParameter(maxEncoder);
    
    configfile->readInto(stallThreshold, "GripperBar2", "StallGuard2Threshold");
    threshold.setParameter(stallThreshold);
    this->gripper->getGripperBar2().setConfigurationParameter(threshold);

    configfile->readInto(stallGuardFilter, "GripperBar2", "StallGuard2FilterEnable");
    filter.setParameter(stallGuardFilter);
    this->gripper->getGripperBar2().setConfigurationParameter(filter);
    }
    

    return;
  // Bouml preserved body end 00068071
}