gripper_thread.cpp
00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023 #include "gripper_thread.h"
00024
00025 #include <kniBase.h>
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
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
00050
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
00064 if (__mode == CLOSE_GRIPPER) {
00065 _katana->closeGripper( false);
00066 } else {
00067 _katana->openGripper( false);
00068 }
00069
00070 } catch (::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();
00088 gripmot->recvPVP();
00089 } catch (::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
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 }