///does the commutation of the arm joints with firmware 1.48 and below
void YouBotManipulator::commutationFirmware148() {
// Bouml preserved body begin 0010D971
InitializeJoint doInitialization;
bool isInitialized = false;
int noInitialization = 0;
std::string jointName;
ClearMotorControllerTimeoutFlag clearTimeoutFlag;
for (unsigned int i = 1; i <= numberArmJoints; i++) {
this->getArmJoint(i).setConfigurationParameter(clearTimeoutFlag);
}
for (unsigned int i = 1; i <= numberArmJoints; i++) {
doInitialization.setParameter(false);
this->getArmJoint(i).getConfigurationParameter(doInitialization);
doInitialization.getParameter(isInitialized);
if (!isInitialized) {
noInitialization++;
}
}
if (noInitialization != 0) {
LOG(info) << "Manipulator Joint Commutation";
doInitialization.setParameter(true);
ethercatMaster.AutomaticReceiveOn(false);
for (unsigned int i = 1; i <= numberArmJoints; i++)
this->getArmJoint(i).setConfigurationParameter(doInitialization);
ethercatMaster.AutomaticReceiveOn(true);
unsigned int statusFlags;
std::vector<bool> isCommutated;
isCommutated.assign(numberArmJoints, false);
unsigned int u = 0;
// check for the next 5 sec if the joints are commutated
for (u = 1; u <= 5000; u++) {
for (unsigned int i = 1; i <= numberArmJoints; i++) {
if(!ethercatMaster.isThreadActive()){
ethercatMaster.sendProcessData();
ethercatMaster.receiveProcessData();
}
this->getArmJoint(i).getStatus(statusFlags);
if (statusFlags & INITIALIZED) {
isCommutated[i - 1] = true;
}
}
if (isCommutated[0] && isCommutated[1] && isCommutated[2] && isCommutated[3] && isCommutated[4]) {
break;
}
SLEEP_MILLISEC(1);
}
SLEEP_MILLISEC(10); // the controller likes it
for (unsigned int i = 1; i <= numberArmJoints; i++) {
doInitialization.setParameter(false);
this->getArmJoint(i).getConfigurationParameter(doInitialization);
doInitialization.getParameter(isInitialized);
if (!isInitialized) {
std::stringstream jointNameStream;
jointNameStream << "manipulator joint " << i;
jointName = jointNameStream.str();
throw std::runtime_error("Could not commutate " + jointName);
}
}
}
// Bouml preserved body end 0010D971
}
///establishes the ethercat connection
void EthercatMaster::initializeEthercat() {
// Bouml preserved body begin 000410F1
/* initialise SOEM, bind socket to ifname */
if (ec_init(ethernetDevice.c_str())) {
LOG(info) << "Initializing EtherCAT on " << ethernetDevice;
/* find and auto-config slaves */
if (ec_config(TRUE, &IOmap_) > 0) {
LOG(trace) << ec_slavecount << " slaves found and configured.";
/* wait for all slaves to reach Pre OP state */
/*ec_statecheck(0, EC_STATE_PRE_OP, EC_TIMEOUTSTATE);
if (ec_slave[0].state != EC_STATE_PRE_OP ){
printf("Not all slaves reached pre operational state.\n");
ec_readstate();
//If not all slaves operational find out which one
for(int i = 1; i<=ec_slavecount ; i++){
if(ec_slave[i].state != EC_STATE_PRE_OP){
printf("Slave %d State=%2x StatusCode=%4x : %s\n",
i, ec_slave[i].state, ec_slave[i].ALstatuscode, ec_ALstatuscode2string(ec_slave[i].ALstatuscode));
}
}
}
*/
/* distributed clock is not working
//Configure distributed clock
if(!ec_configdc()){
LOG(warning) << "no distributed clock is available";
}else{
uint32 CyclTime = 4000000;
uint32 CyclShift = 0;
for (int i = 1; i <= ec_slavecount; i++) {
ec_dcsync0(i, true, CyclTime, CyclShift);
}
}
*/
/* wait for all slaves to reach SAFE_OP state */
ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE);
if (ec_slave[0].state != EC_STATE_SAFE_OP) {
LOG(warning) << "Not all slaves reached safe operational state.";
ec_readstate();
//If not all slaves operational find out which one
for (int i = 1; i <= ec_slavecount; i++) {
if (ec_slave[i].state != EC_STATE_SAFE_OP) {
LOG(info) << "Slave " << i << " State=" << ec_slave[i].state << " StatusCode=" << ec_slave[i].ALstatuscode << " : " << ec_ALstatuscode2string(ec_slave[i].ALstatuscode);
}
}
}
//Read the state of all slaves
//ec_readstate();
LOG(trace) << "Request operational state for all slaves";
ec_slave[0].state = EC_STATE_OPERATIONAL;
// request OP state for all slaves
/* send one valid process data to make outputs in slaves happy*/
ec_send_processdata();
ec_receive_processdata(EC_TIMEOUTRET);
/* request OP state for all slaves */
ec_writestate(0);
// wait for all slaves to reach OP state
ec_statecheck(0, EC_STATE_OPERATIONAL, EC_TIMEOUTSTATE);
if (ec_slave[0].state == EC_STATE_OPERATIONAL) {
LOG(trace) << "Operational state reached for all slaves.";
} else {
throw std::runtime_error("Not all slaves reached operational state.");
}
} else {
throw std::runtime_error("No slaves found!");
}
} else {
throw std::runtime_error("No socket connection on " + ethernetDevice + "\nExcecute as root");
}
std::string baseJointControllerName = "TMCM-174";
std::string manipulatorJointControllerName = "TMCM-174";
std::string actualSlaveName;
nrOfSlaves = 0;
YouBotSlaveMsg emptySlaveMsg;
quantity<si::current> maxContinuousCurrentBase = 3.54 * ampere;
quantity<si::time> thermalTimeConstantWindingBase = 16.6 * second;
quantity<si::time> thermalTimeConstantMotorBase = 212 * second;
quantity<si::current> maxContinuousCurrentJoint13 = 2.36 * ampere;
quantity<si::time> thermalTimeConstantWindingJoint13 = 16.6 * second;
quantity<si::time> thermalTimeConstantMotorJoint13 = 212 * second;
quantity<si::current> maxContinuousCurrentJoint4 = 1.07 * ampere;
quantity<si::time> thermalTimeConstantWindingJoint4 = 13.2 * second;
quantity<si::time> thermalTimeConstantMotorJoint4 = 186 * second;
quantity<si::current> maxContinuousCurrentJoint5 = 0.49 * ampere;
quantity<si::time> thermalTimeConstantWindingJoint5 = 8.1 * second;
quantity<si::time> thermalTimeConstantMotorJoint5 = 108 * second;
int manipulatorNo = 0;
configfile->readInto(baseJointControllerName, "BaseJointControllerName");
configfile->readInto(manipulatorJointControllerName, "ManipulatorJointControllerName");
//reserve memory for all slave with a input/output buffer
for (int cnt = 1; cnt <= ec_slavecount; cnt++) {
// printf("Slave:%d Name:%s Output size:%3dbits Input size:%3dbits State:%2d delay:%d.%d\n",
// cnt, ec_slave[cnt].name, ec_slave[cnt].Obits, ec_slave[cnt].Ibits,
// ec_slave[cnt].state, (int) ec_slave[cnt].pdelay, ec_slave[cnt].hasdc);
ethercatSlaveInfo.push_back(ec_slave[cnt]);
actualSlaveName = ec_slave[cnt].name;
if ((actualSlaveName == baseJointControllerName || actualSlaveName == manipulatorJointControllerName) && ec_slave[cnt].Obits > 0 && ec_slave[cnt].Ibits > 0) {
nrOfSlaves++;
// joints.push_back(YouBotJoint(nrOfSlaves));
firstBufferVector.push_back(emptySlaveMsg);
secondBufferVector.push_back(emptySlaveMsg);
ethercatOutputBufferVector.push_back((SlaveMessageOutput*) (ec_slave[cnt].outputs));
ethercatInputBufferVector.push_back((SlaveMessageInput*) (ec_slave[cnt].inputs));
YouBotSlaveMailboxMsg emptyMailboxSlaveMsg(cnt);
firstMailboxBufferVector.push_back(emptyMailboxSlaveMsg);
secondMailboxBufferVector.push_back(emptyMailboxSlaveMsg);
newOutputDataFlagOne.push_back(false);
newOutputDataFlagTwo.push_back(false);
newMailboxDataFlagOne.push_back(false);
newMailboxDataFlagTwo.push_back(false);
newMailboxInputDataFlagOne.push_back(false);
newMailboxInputDataFlagTwo.push_back(false);
pendingMailboxMsgsReply.push_back(false);
int i = 0;
bool b = false;
upperLimit.push_back(i);
lowerLimit.push_back(i);
limitActive.push_back(b);
jointLimitReached.push_back(b);
inverseMovementDirection.push_back(b);
if (actualSlaveName == baseJointControllerName) {
motorProtections.push_back(MotorProtection(maxContinuousCurrentBase,
thermalTimeConstantWindingBase,
thermalTimeConstantMotorBase));
}
if (actualSlaveName == manipulatorJointControllerName) {
manipulatorNo++;
if (manipulatorNo >= 1 && manipulatorNo <= 3) {
motorProtections.push_back(MotorProtection(maxContinuousCurrentJoint13,
thermalTimeConstantWindingJoint13,
thermalTimeConstantMotorJoint13));
}
if (manipulatorNo == 4) {
motorProtections.push_back(MotorProtection(maxContinuousCurrentJoint4,
thermalTimeConstantWindingJoint4,
thermalTimeConstantMotorJoint4));
}
if (manipulatorNo == 5) {
motorProtections.push_back(MotorProtection(maxContinuousCurrentJoint5,
thermalTimeConstantWindingJoint5,
thermalTimeConstantMotorJoint5));
manipulatorNo = 0;
}
}
}
}
if (nrOfSlaves != motorProtections.size()) {
throw std::runtime_error("Insufficient motor protections loaded");
}
if (nrOfSlaves > 0) {
LOG(info) << nrOfSlaves << " EtherCAT slaves found" ;
} else {
throw std::runtime_error("No EtherCAT slave could be found");
return;
}
stopThread = false;
threads.create_thread(boost::bind(&EthercatMaster::updateSensorActorValues, this));
SLEEP_MILLISEC(10); //needed to start up thread and EtherCAT communication
return;
// Bouml preserved body end 000410F1
}
///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
}
