Fawkes API
Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * gripper_thread.cpp - Katana gripper one-time thread 00004 * 00005 * Created: Thu Jun 11 11:59:38 2009 00006 * Copyright 2006-2009 Tim Niemueller [www.niemueller.de] 00007 * 00008 ****************************************************************************/ 00009 00010 /* This program is free software; you can redistribute it and/or modify 00011 * it under the terms of the GNU General Public License as published by 00012 * the Free Software Foundation; either version 2 of the License, or 00013 * (at your option) any later version. 00014 * 00015 * This program is distributed in the hope that it will be useful, 00016 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00017 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00018 * GNU Library General Public License for more details. 00019 * 00020 * Read the full text in the LICENSE.GPL file in the doc directory. 00021 */ 00022 00023 #include "gripper_thread.h" 00024 00025 #include <kniBase.h> 00026 00027 /** @class KatanaGripperThread "gripper_thread.h" 00028 * Katana gripper thread. 00029 * This thread opens or closes the gripper when started. 00030 * @author Tim Niemueller 00031 */ 00032 00033 /** Constructor. 00034 * @param katana katana linear motion base class 00035 * @param logger logger 00036 * @param poll_interval_ms interval in ms between two checks if the 00037 * final position has been reached 00038 */ 00039 KatanaGripperThread::KatanaGripperThread(fawkes::RefPtr<CLMBase> katana, 00040 fawkes::Logger *logger, 00041 unsigned int poll_interval_ms) 00042 : KatanaMotionThread("KatanaGripperThread", katana, logger) 00043 { 00044 __mode = OPEN_GRIPPER; 00045 __poll_interval_usec = poll_interval_ms * 1000; 00046 } 00047 00048 00049 /** Set mode. 00050 * @param mode open, either open or close 00051 */ 00052 void 00053 KatanaGripperThread::set_mode(gripper_mode_t mode) 00054 { 00055 __mode = mode; 00056 } 00057 00058 00059 void 00060 KatanaGripperThread::once() 00061 { 00062 try { 00063 // non-blocking call to KNI 00064 if (__mode == CLOSE_GRIPPER) { 00065 _katana->closeGripper(/* wait */ false); 00066 } else { 00067 _katana->openGripper(/* wait */ false); 00068 } 00069 00070 } catch (/*KNI*/::Exception &e) { 00071 _logger->log_warn("KatanaGripperThread", "Starting gripper motion failed (ignoring): %s", e.what()); 00072 _finished = true; 00073 _error_code = fawkes::KatanaInterface::ERROR_CMD_START_FAILED; 00074 return; 00075 } 00076 00077 CKatBase *base = _katana->GetBase(); 00078 const TKatMOT *motors = base->GetMOT(); 00079 CMotBase *gripmot = &motors->arr[motors->cnt - 1]; 00080 const TMotPVP *grippvp = gripmot->GetPVP(); 00081 bool final = false; 00082 short last_pos = 0; 00083 short num_samepos = 0; 00084 short num_errors = 0; 00085 while (! final) { 00086 try { 00087 base->GetSCT()->arr[0].recvDAT(); // update sensor values 00088 gripmot->recvPVP(); 00089 } catch (/*KNI*/::Exception &e) { 00090 if (++num_errors <= 10) { 00091 _logger->log_warn("KatanaGripperThread", "Receiving PVP failed, retrying"); 00092 continue; 00093 } else { 00094 _logger->log_warn("KatanaGripperThread", "Receiving PVP failed too often, aborting"); 00095 _error_code = fawkes::KatanaInterface::ERROR_COMMUNICATION; 00096 break; 00097 } 00098 } 00099 if (grippvp->pos == last_pos) { 00100 if (++num_samepos >= 3) { 00101 _logger->log_debug(name(), "Gripper did not move for 3 cycles, considering as final"); 00102 final = true; 00103 } 00104 } else { 00105 //_logger->log_debug(name(), "Gripper still moving %i != %i", last_pos, grippvp->pos); 00106 last_pos = grippvp->pos; 00107 num_samepos = 0; 00108 usleep(__poll_interval_usec); 00109 } 00110 } 00111 00112 _logger->log_debug("KatanaGripperThread", "Gripper motion finished"); 00113 _finished = true; 00114 }