Fawkes API  Fawkes Development Version
motor_control_thread.cpp
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 }