void mtsIntuitiveResearchKitPSM::RunArmSpecific(void)
{
    switch (RobotState) {
    case mtsIntuitiveResearchKitArmTypes::DVRK_ENGAGING_ADAPTER:
        RunEngagingAdapter();
        break;
    case mtsIntuitiveResearchKitArmTypes::DVRK_ADAPTER_ENGAGED:
        break;
    case mtsIntuitiveResearchKitArmTypes::DVRK_ENGAGING_TOOL:
        RunEngagingTool();
        break;
    case mtsIntuitiveResearchKitArmTypes::DVRK_CONSTRAINT_CONTROLLER_CARTESIAN:
        RunConstraintControllerCartesian();
        break;
    default:
        break;
    }
}
void mtsIntuitiveResearchKitPSM::Run(void)
{
    Counter++;

    ProcessQueuedEvents();
    GetRobotData();

    switch (RobotState) {
    case PSM_UNINITIALIZED:
        break;
    case PSM_HOMING_POWERING:
        RunHomingPower();
        break;
    case PSM_HOMING_CALIBRATING_ARM:
        RunHomingCalibrateArm();
        break;
    case PSM_ARM_CALIBRATED:
        break;
    case PSM_ENGAGING_ADAPTER:
        RunEngagingAdapter();
        break;
    case PSM_ADAPTER_ENGAGED:
        // choose next state
        break;
    case PSM_ENGAGING_TOOL:
        RunEngagingTool();
        break;
    case PSM_READY:
    case PSM_POSITION_CARTESIAN:
        RunPositionCartesian();
        break;
    case PSM_MANUAL:
        break;
    default:
        break;
    }

    RunEvent();
    ProcessQueuedCommands();
}