Fawkes API
Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * motor_control_thread.cpp - Katana direct motor encoder/value control thread 00004 * 00005 * Created: Sun Mar 13 14:44:24 2011 00006 * Copyright 2011 Bahram Maleki-Fard 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 "motor_control_thread.h" 00024 #include "controller.h" 00025 #include "exception.h" 00026 00027 #include <cstdlib> 00028 00029 /** @class KatanaMotorControlThread "goto_thread.h" 00030 * Katana motor control thread. 00031 * This thread moves a single motor to/by a given value in encoders or angle. 00032 * @author Bahram Maleki-Fard 00033 */ 00034 00035 /** Constructor. 00036 * @param katana katana controller base class 00037 * @param logger logger 00038 * @param poll_interval_ms interval in ms between two checks if the 00039 * final position has been reached 00040 */ 00041 KatanaMotorControlThread::KatanaMotorControlThread(fawkes::RefPtr<fawkes::KatanaController> katana, 00042 fawkes::Logger *logger, 00043 unsigned int poll_interval_ms) 00044 : KatanaMotionThread("KatanaMotorControlThread", katana, logger) 00045 { 00046 __poll_interval_usec = poll_interval_ms * 1000; 00047 } 00048 00049 00050 /** Set target encoder value 00051 * @param nr number of motor 00052 * @param value encoder value 00053 * @param inc is value incremental? deault:false -> absolute 00054 */ 00055 void 00056 KatanaMotorControlThread::set_encoder(unsigned int nr, int value, bool inc) 00057 { 00058 __nr = nr; 00059 __encoder = value; 00060 00061 __is_encoder = true; 00062 __is_inc = inc; 00063 } 00064 00065 /** Set target angle value 00066 * @param nr number of motor 00067 * @param value angle value 00068 * @param inc is value incremental? deault:false -> absolute 00069 */ 00070 void 00071 KatanaMotorControlThread::set_angle(unsigned int nr, float value, bool inc) 00072 { 00073 __nr = nr; 00074 __angle = value; 00075 00076 __is_encoder = false; 00077 __is_inc = inc; 00078 } 00079 00080 00081 void 00082 KatanaMotorControlThread::once() 00083 { 00084 try { 00085 // non-blocking call to KNI 00086 if( __is_encoder ) { 00087 if( __is_inc ) 00088 00089 _katana->move_motor_by(__nr, __encoder); 00090 else 00091 _katana->move_motor_to(__nr, __encoder); 00092 } else { 00093 if( __is_inc ) 00094 _katana->move_motor_by(__nr, __angle); 00095 else 00096 _katana->move_motor_to(__nr, __angle); 00097 } 00098 } catch (fawkes::KatanaOutOfRangeException &e) { 00099 _logger->log_warn("KatanaMotorControlThread", "Motor %u out of range. Ex%s", __nr, e.what()); 00100 _finished = true; 00101 _error_code = fawkes::KatanaInterface::ERROR_UNSPECIFIC; 00102 return; 00103 } catch (fawkes::Exception &e) { 00104 _logger->log_warn("KatanaMotorControlThread", "Moving motor %u failed (ignoring): %s", __nr, e.what()); 00105 _finished = true; 00106 _error_code = fawkes::KatanaInterface::ERROR_CMD_START_FAILED; 00107 return; 00108 } 00109 00110 // check if final 00111 bool final = false; 00112 short num_errors = 0; 00113 while ( !final ) { 00114 usleep(__poll_interval_usec); 00115 try { 00116 _katana->read_sensor_data(); 00117 _katana->read_motor_data(); 00118 } catch (fawkes::Exception &e) { 00119 if (++num_errors <= 10) { 00120 _logger->log_warn("KatanaMotorControlThread", "Reading sensor/motor data failed, retrying"); 00121 continue; 00122 } else { 00123 _logger->log_warn("KatanaMotorControlThread", "Receiving sensor/motor data failed too often, aborting"); 00124 _error_code = fawkes::KatanaInterface::ERROR_COMMUNICATION; 00125 break; 00126 } 00127 } 00128 00129 try { 00130 final = _katana->final(); 00131 } catch (fawkes::KatanaMotorCrashedException &e) { 00132 _logger->log_warn("KatanaMotorControlTrhead", e.what()); 00133 _error_code = fawkes::KatanaInterface::ERROR_MOTOR_CRASHED; 00134 break; 00135 } 00136 } 00137 00138 _logger->log_debug(name(), "Successfully moved motor %u", __nr); 00139 00140 _finished = true; 00141 }