///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
}
