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 }