gripper_thread.cpp

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 }

Generated on Tue Feb 22 13:32:30 2011 for Fawkes API by  doxygen 1.4.7