void mtsIntuitiveResearchKitPSM::RunHomingPower(void)
{
const double timeToPower = 3.0 * cmn_s;
const double currentTime = this->StateTable.GetTic();
// first, request power to be turned on
if (!HomingPowerRequested) {
RobotIO.BiasEncoder();
HomingTimer = currentTime;
// make sure the PID is not sending currents
PID.Enable(false);
// pre-load the boards with zero current
RobotIO.SetMotorCurrent(vctDoubleVec(NumberOfJoints, 0.0));
// enable power and set a flags to move to next step
RobotIO.EnablePower();
HomingPowerRequested = true;
EventTriggers.RobotStatusMsg(this->GetName() + " power requested");
return;
}
// second, check status
if (HomingPowerRequested
&& ((currentTime - HomingTimer) > timeToPower)) {
// pre-load PID to make sure desired position has some reasonable values
PID.GetPositionJoint(JointCurrentParam);
// angles are radians but cisst uses mm. robManipulator uses SI, so we need meters
JointCurrentParam.Position().Element(2) /= 1000.0; // convert from mm to m
// assign to a more convenient vctDoubleVec
JointCurrent.Assign(JointCurrentParam.Position(), NumberOfJoints);
SetPositionJointLocal(JointCurrent);
// check power status
vctBoolVec amplifiersStatus(NumberOfJoints);
RobotIO.GetAmpStatus(amplifiersStatus);
if (amplifiersStatus.All()) {
EventTriggers.RobotStatusMsg(this->GetName() + " power on");
this->SetState(PSM_HOMING_CALIBRATING_ARM);
} else {
EventTriggers.RobotErrorMsg(this->GetName() + " failed to enable power.");
this->SetState(PSM_UNINITIALIZED);
}
}
}
void mtsIntuitiveResearchKitMTM::RunHomingPower(void)
{
const double timeToPower = 3.0 * cmn_s;
const double currentTime = this->StateTable.GetTic();
// first, request power to be turned on
if (!HomingPowerRequested) {
RobotIO.BiasEncoder();
{ // use pots for redundancy
vctDoubleVec potsToEncodersTolerance(this->NumberOfJoints + 1); // IO level treats the gripper as joint :-)
potsToEncodersTolerance.SetAll(10.0 * cmnPI_180); // 10 degrees for rotations
// pots on gripper rotation are not directly mapped to encoders
potsToEncodersTolerance.Element(6) = cmnTypeTraits<double>::PlusInfinityOrMax();
// last joint is gripper, encoders can be anything
potsToEncodersTolerance.Element(7) = cmnTypeTraits<double>::PlusInfinityOrMax();
RobotIO.SetPotsToEncodersTolerance(potsToEncodersTolerance);
RobotIO.UsePotsForSafetyCheck(true);
}
HomingTimer = currentTime;
// make sure the PID is not sending currents
PID.Enable(false);
// pre-load the boards with zero current
RobotIO.SetActuatorCurrent(vctDoubleVec(NumberOfJoints + 1, 0.0));
// enable power and set a flags to move to next step
RobotIO.EnablePower();
HomingPowerRequested = true;
HomingPowerCurrentBiasRequested = false;
EventTriggers.RobotStatusMsg(this->GetName() + " power requested");
return;
}
// second, check status
if (HomingPowerRequested && ((currentTime - HomingTimer) > timeToPower)) {
// check power status
vctBoolVec amplifiersStatus(NumberOfJoints + 1);
RobotIO.GetActuatorAmpStatus(amplifiersStatus);
if (amplifiersStatus.All()) {
EventTriggers.RobotStatusMsg(this->GetName() + " power on");
this->SetState(mtsIntuitiveResearchKitMTMTypes::MTM_HOMING_CALIBRATING_ARM);
} else {
EventTriggers.RobotErrorMsg(this->GetName() + " failed to enable power.");
this->SetState(mtsIntuitiveResearchKitMTMTypes::MTM_UNINITIALIZED);
}
}
}
