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
}
bool parseJointLimits(JointLimits &jl, TiXmlElement* config)
{
jl.clear();
// Get lower joint limit
const char* lower_str = config->Attribute("lower");
if (lower_str == NULL){
CONSOLE_BRIDGE_logDebug("urdfdom.joint_limit: no lower, defaults to 0");
jl.lower = 0;
}
else
{
try
{
jl.lower = boost::lexical_cast<double>(lower_str);
}
catch (boost::bad_lexical_cast &e)
{
CONSOLE_BRIDGE_logError("lower value (%s) is not a float: %s", lower_str, e.what());
return false;
}
}
// Get upper joint limit
const char* upper_str = config->Attribute("upper");
if (upper_str == NULL){
CONSOLE_BRIDGE_logDebug("urdfdom.joint_limit: no upper, , defaults to 0");
jl.upper = 0;
}
else
{
try
{
jl.upper = boost::lexical_cast<double>(upper_str);
}
catch (boost::bad_lexical_cast &e)
{
CONSOLE_BRIDGE_logError("upper value (%s) is not a float: %s",upper_str, e.what());
return false;
}
}
// Get joint effort limit
const char* effort_str = config->Attribute("effort");
if (effort_str == NULL){
CONSOLE_BRIDGE_logError("joint limit: no effort");
return false;
}
else
{
try
{
jl.effort = boost::lexical_cast<double>(effort_str);
}
catch (boost::bad_lexical_cast &e)
{
CONSOLE_BRIDGE_logError("effort value (%s) is not a float: %s",effort_str, e.what());
return false;
}
}
// Get joint velocity limit
const char* velocity_str = config->Attribute("velocity");
if (velocity_str == NULL){
CONSOLE_BRIDGE_logError("joint limit: no velocity");
return false;
}
else
{
try
{
jl.velocity = boost::lexical_cast<double>(velocity_str);
}
catch (boost::bad_lexical_cast &e)
{
CONSOLE_BRIDGE_logError("velocity value (%s) is not a float: %s",velocity_str, e.what());
return false;
}
}
return true;
}
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
}
void YouBotDiagnostics::getAllTopologicalInformation()
{
bool reset = true;
unsigned jointNo = 0;
unsigned jointCfg = 0;
//joint konfiguration variables
JointName jName;
GearRatio gearRatio;
EncoderTicksPerRound ticksPerRound;
InverseMovementDirection inverseDir;
JointLimits jLimits;
FirmwareVersion firmwareTypeVersion;
ticksPerRound.setParameter(4000);
int controllerType;
string firmwareVersion;
*outputFile << "----- Topological Information of all slaves: -----" << endl;
for (int i = 0, slaveCtr = 1; i < etherCATSlaves.size(); i++, slaveCtr++)
{
//formatted output:
*outputFile << "slave no " << setw(2) << slaveCtr << ":\t" << slaveTypMap[etherCATSlaves[i].name];
// if( slaveTypMap[etherCATSlaves[i].name] == "BASE-Master")
// *outputFile << ": " << etherCATSlaves[i].name;
// else
*outputFile << ":\t" << etherCATSlaves[i].name;
if ( slaveTypMap[etherCATSlaves[i].name] == "BASE-Master" || slaveTypMap[etherCATSlaves[i].name] == "ARM-Master")
*outputFile << "\t\t--> parent: ";
else
*outputFile << "\t--> parent: ";
*outputFile << setw(2) << etherCATSlaves[i].parent << "\t--> connections: " << setw(2) << (int) etherCATSlaves[i].topology << endl;
//create the joint map
if (reset)
jointCfg = 1;
else
jointCfg++;
if ( !strcmp( etherCATSlaves[i].name, BASE_MASTER_BOARD) )
{
reset = true;
noOfBaseMasterBoards++;
motorActivity[slaveCtr] = true;
}
if ( !strcmp( etherCATSlaves[i].name, ARM_MASTER_BOARD ) )
{
reset = true;
noOfArmMasterBoards++;
motorActivity[slaveCtr] = true;
}
if ( !strcmp( etherCATSlaves[i].name, BASE_CONTROLLER ) )
{
noOfBaseSlaveControllers++;
jointNo++;
reset = false;
TopologyMap::iterator it = youBotJointMap.insert( pair<int, JointTopology>( slaveCtr, JointTopology() ) ).first;
it->second.relatedUnit = string("BASE");
it->second.relatedMasterBoard = string(BASE_MASTER_BOARD);
it->second.controllerTyp = string(BASE_CONTROLLER);
it->second.masterBoardNo = noOfBaseMasterBoards;
it->second.slaveNo = slaveCtr;
it->second.jointTopologyNo = jointNo;
it->second.jointConfigNo = jointCfg;
it->second.joint = new YouBotJoint(jointNo);
it->second.joint->getConfigurationParameter(firmwareTypeVersion);
firmwareTypeVersion.getParameter(controllerType, firmwareVersion);
it->second.firmwareVersion = atof(firmwareVersion.c_str());
stringstream name;
name << "joint_" << jointNo;
jName.setParameter( name.str() );
gearRatio.setParameter(364.0/9405.0);
inverseDir.setParameter(false);
it->second.joint->setConfigurationParameter(jName);
it->second.joint->setConfigurationParameter(gearRatio);
it->second.joint->setConfigurationParameter(ticksPerRound);
it->second.joint->setConfigurationParameter(inverseDir);
}
if( !strcmp( etherCATSlaves[i].name, ARM_CONTROLLER ) )
{
noOfArmSlaveControllers++;
jointNo++;
reset = false;
TopologyMap::iterator it = youBotJointMap.insert( pair<int, JointTopology>( slaveCtr, JointTopology() ) ).first;
it->second.relatedUnit = string("MANIPULATOR");
it->second.relatedMasterBoard = string(ARM_MASTER_BOARD);
it->second.controllerTyp = string(ARM_CONTROLLER);
it->second.masterBoardNo = noOfArmMasterBoards;
it->second.slaveNo = slaveCtr;
it->second.jointTopologyNo = jointNo;
it->second.jointConfigNo = jointCfg;
it->second.joint = new YouBotJoint(jointNo);
it->second.joint->getConfigurationParameter(firmwareTypeVersion);
firmwareTypeVersion.getParameter(controllerType, firmwareVersion);
it->second.firmwareVersion = atof(firmwareVersion.c_str());
stringstream name;
name << "joint_" << jointNo;
jName.setParameter( name.str() );
switch (jointCfg)
{
case 1:
gearRatio.setParameter(1.0/156.0);
inverseDir.setParameter(true);
jLimits.setParameter(-580000, -1000, true);
break;
case 2:
gearRatio.setParameter(1.0/156.0);
inverseDir.setParameter(true);
jLimits.setParameter(-260000, -1000, true);
break;
case 3:
gearRatio.setParameter(1.0/100.0);
inverseDir.setParameter(true);
jLimits.setParameter(-320000, -1000, true);
break;
case 4:
gearRatio.setParameter(1.0/ 71.0);
inverseDir.setParameter(true);
jLimits.setParameter(-155000, -1000, true);
break;
case 5:
gearRatio.setParameter(1.0/ 71.0);
inverseDir.setParameter(true);
jLimits.setParameter(-255000, -1000, true);
break;
}
it->second.joint->setConfigurationParameter(jName);
it->second.joint->setConfigurationParameter(ticksPerRound);
it->second.joint->setConfigurationParameter(inverseDir);
it->second.joint->setConfigurationParameter(jLimits);
it->second.joint->setConfigurationParameter(gearRatio);
}
}
*outputFile << separator;
*outputFile << "Number of ALL slaves: " << etherCATSlaves.size() << endl;
*outputFile << "Number of JOINT slaves: " << youBotJointMap.size() << endl;
*outputFile << "Number of BaseMasterBoards: " << noOfBaseMasterBoards << endl;
*outputFile << "Number of BaseSlaveControllers: " << noOfBaseSlaveControllers << endl;
*outputFile << "Number of ArmMasterBoards: " << noOfArmMasterBoards << endl;
*outputFile << "Number of ArmSlaveControllers: " << noOfArmSlaveControllers << endl;
*outputFile << separator;
}
///calibrate the reference position of the arm joints
void YouBotManipulator::calibrateManipulator(const bool forceCalibration) {
// Bouml preserved body begin 000A9C71
//Calibrate all manipulator joints
std::vector<JointRoundsPerMinuteSetpoint> calibrationVel;
JointRoundsPerMinuteSetpoint tempdummy;
tempdummy.rpm = 0;
calibrationVel.assign(numberArmJoints, tempdummy);
std::vector<quantity<si::current> > maxCurrent;
quantity<si::current> tempdummy2;
maxCurrent.assign(numberArmJoints, tempdummy2);
std::vector<bool> doCalibration;
doCalibration.assign(numberArmJoints, true);
std::string jointName;
double dummy = 0;
char index = 16; // Parameter 0 to 15 of bank 2 are password protected
YouBotSlaveMailboxMsg IsCalibratedReadMessage;
IsCalibratedReadMessage.stctOutput.moduleAddress = DRIVE;
IsCalibratedReadMessage.stctOutput.commandNumber = GGP;
IsCalibratedReadMessage.stctOutput.typeNumber = index;
IsCalibratedReadMessage.stctOutput.motorNumber = USER_VARIABLE_BANK;
IsCalibratedReadMessage.stctOutput.value = 0;
IsCalibratedReadMessage.stctInput.value = 0;
YouBotSlaveMailboxMsg IsCalibratedSetMessage;
IsCalibratedSetMessage.stctOutput.moduleAddress = DRIVE;
IsCalibratedSetMessage.stctOutput.commandNumber = SGP;
IsCalibratedSetMessage.stctOutput.typeNumber = index;
IsCalibratedSetMessage.stctOutput.motorNumber = USER_VARIABLE_BANK;
IsCalibratedSetMessage.stctOutput.value = 1;
//get parameters for calibration
for (unsigned int i = 0; i < numberArmJoints; i++) {
std::stringstream jointNameStream;
jointNameStream << "Joint_" << i + 1;
jointName = jointNameStream.str();
bool calib = true;
configfile->readInto(calib, jointName, "DoCalibration");
doCalibration[i] = calib;
joints[i].getConfigurationParameter(IsCalibratedReadMessage);
if (IsCalibratedReadMessage.stctInput.value == 1) {
doCalibration[i] = false;
}
if (forceCalibration) {
doCalibration[i] = true;
}
configfile->readInto(dummy, jointName, "CalibrationMaxCurrent_[ampere]");
maxCurrent[i] = dummy * ampere;
std::string direction;
configfile->readInto(direction, jointName, "CalibrationDirection");
GearRatio gearRatio;
joints[i].getConfigurationParameter(gearRatio);
double gearratio = 1;
gearRatio.getParameter(gearratio);
if (direction == "POSITIV") {
calibrationVel[i].rpm = 1 / gearratio;
} else if (direction == "NEGATIV") {
calibrationVel[i].rpm = -1 / gearratio;
} else {
throw std::runtime_error("Wrong calibration direction for " + jointName);
}
}
LOG(info) << "Calibrate Manipulator Joints ";
std::vector<bool> finished;
finished.assign(numberArmJoints, false);
int numberUnfinished = numberArmJoints;
JointSensedCurrent sensedCurrent;
//move the joints slowly in calibration direction
for (unsigned int i = 0; i < numberArmJoints; i++) {
if (doCalibration[i] == true) {
joints[i].setData(calibrationVel[i]);
if(!ethercatMaster.isThreadActive()){
ethercatMaster.sendProcessData();
ethercatMaster.receiveProcessData();
}
} else {
if (!finished[i]) {
finished[i] = true;
numberUnfinished--;
}
}
}
//monitor the current to find end stop
while (numberUnfinished > 0) {
for (unsigned int i = 0; i < numberArmJoints; i++) {
if(!ethercatMaster.isThreadActive()){
ethercatMaster.sendProcessData();
ethercatMaster.receiveProcessData();
}
joints[i].getData(sensedCurrent);
//turn till a max current is reached
if (abs(sensedCurrent.current) > abs(maxCurrent[i])) {
//stop movement
youbot::JointCurrentSetpoint currentStopMovement;
currentStopMovement.current = 0 * ampere;
joints[i].setData(currentStopMovement);
if(!ethercatMaster.isThreadActive()){
ethercatMaster.sendProcessData();
ethercatMaster.receiveProcessData();
}
if (!finished[i]) {
finished[i] = true;
numberUnfinished--;
}
}
}
SLEEP_MILLISEC(1);
}
// wait to let the joint stop the motion
SLEEP_MILLISEC(100);
for (unsigned int i = 0; i < numberArmJoints; i++) {
if (doCalibration[i] == true) {
//set encoder reference position
joints[i].setEncoderToZero();
if(!ethercatMaster.isThreadActive()){
ethercatMaster.sendProcessData();
ethercatMaster.receiveProcessData();
}
// set a flag in the user variable to remember that it is calibrated
joints[i].setConfigurationParameter(IsCalibratedSetMessage);
// LOG(info) << "Calibration finished for joint: " << this->jointName;
}
}
//setting joint Limits
JointLimits jLimits;
for (unsigned int i = 0; i < numberArmJoints; i++) {
long upperlimit = 0, lowerlimit = 0;
std::stringstream jointNameStream;
bool inverted = false;
jointNameStream << "Joint_" << i + 1;
jointName = jointNameStream.str();
JointEncoderSetpoint minEncoderValue;
configfile->readInto(lowerlimit, jointName, "LowerLimit_[encoderTicks]");
configfile->readInto(upperlimit, jointName, "UpperLimit_[encoderTicks]");
configfile->readInto(inverted, jointName, "InverseMovementDirection");
if(inverted){
minEncoderValue.encoderTicks = lowerlimit + 1000;
}else{
minEncoderValue.encoderTicks = upperlimit - 1000;
}
jLimits.setParameter(lowerlimit, upperlimit, true);
joints[i].setConfigurationParameter(jLimits);
// joints[i].setData(minEncoderValue);
}
// Bouml preserved body end 000A9C71
}
