Fawkes API  Fawkes Development Version
sensor_thread.cpp
00001 
00002 /***************************************************************************
00003  *  sensor_thread.cpp - Robotino sensor thread
00004  *
00005  *  Created: Sun Nov 13 15:35:24 2011
00006  *  Copyright  2011-2012  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 "sensor_thread.h"
00024 #include <rec/robotino/com/Com.h>
00025 #include <rec/sharedmemory/sharedmemory.h>
00026 #include <rec/iocontrol/remotestate/SensorState.h>
00027 #include <rec/iocontrol/robotstate/State.h>
00028 #include <baseapp/run.h>
00029 
00030 #include <interfaces/BatteryInterface.h>
00031 #include <interfaces/RobotinoSensorInterface.h>
00032 
00033 using namespace fawkes;
00034 
00035 /** @class RobotinoSensorThread "sensor_thread.h"
00036  * Robotino sensor hook integration thread.
00037  * This thread integrates into the Fawkes main loop at the SENSOR hook and
00038  * writes new sensor data.
00039  * @author Tim Niemueller
00040  */
00041 
00042 /** Constructor. */
00043 RobotinoSensorThread::RobotinoSensorThread()
00044   : Thread("RobotinoSensorThread", Thread::OPMODE_WAITFORWAKEUP),
00045     BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_ACQUIRE)
00046 {
00047 }
00048 
00049 
00050 void
00051 RobotinoSensorThread::init()
00052 {
00053   cfg_hostname_ = config->get_string("/hardware/robotino/hostname");
00054   cfg_quit_on_disconnect_ = config->get_bool("/hardware/robotino/quit_on_disconnect");
00055 
00056   com_ = new rec::robotino::com::Com();
00057   com_->setAddress(cfg_hostname_.c_str());
00058   com_->connect(/* blocking */ false);
00059 
00060   last_seqnum_ = 0;
00061 
00062   batt_if_ = blackboard->open_for_writing<BatteryInterface>("Robotino");
00063   sens_if_ = blackboard->open_for_writing<RobotinoSensorInterface>("Robotino");
00064 
00065   statemem_ =  new rec::sharedmemory::SharedMemory<rec::iocontrol::robotstate::State>
00066     (rec::iocontrol::robotstate::State::sharedMemoryKey);
00067   state_ = statemem_->getData();
00068 
00069   // taken from Robotino API2 DistanceSensorImpl.hpp
00070   voltage_to_dist_dps_.push_back(std::make_pair(0.3 , 0.41));
00071   voltage_to_dist_dps_.push_back(std::make_pair(0.39, 0.35));
00072   voltage_to_dist_dps_.push_back(std::make_pair(0.41, 0.30));
00073   voltage_to_dist_dps_.push_back(std::make_pair(0.5 , 0.25));
00074   voltage_to_dist_dps_.push_back(std::make_pair(0.75, 0.18));
00075   voltage_to_dist_dps_.push_back(std::make_pair(0.8 , 0.16));
00076   voltage_to_dist_dps_.push_back(std::make_pair(0.95, 0.14));
00077   voltage_to_dist_dps_.push_back(std::make_pair(1.05, 0.12));
00078   voltage_to_dist_dps_.push_back(std::make_pair(1.3 , 0.10));
00079   voltage_to_dist_dps_.push_back(std::make_pair(1.4 , 0.09));
00080   voltage_to_dist_dps_.push_back(std::make_pair(1.55, 0.08));
00081   voltage_to_dist_dps_.push_back(std::make_pair(1.8 , 0.07));
00082   voltage_to_dist_dps_.push_back(std::make_pair(2.35, 0.05));
00083   voltage_to_dist_dps_.push_back(std::make_pair(2.55, 0.04));
00084 }
00085 
00086 
00087 void
00088 RobotinoSensorThread::finalize()
00089 {
00090   blackboard->close(sens_if_);
00091   blackboard->close(batt_if_);
00092   delete com_;
00093 }
00094 
00095 void
00096 RobotinoSensorThread::loop()
00097 {
00098   if (com_->isConnected()) {
00099     rec::iocontrol::remotestate::SensorState sensor_state = com_->sensorState();
00100     if (sensor_state.sequenceNumber != last_seqnum_) {
00101       last_seqnum_ = sensor_state.sequenceNumber;
00102 
00103       // update sensor values in interface
00104       sens_if_->set_mot_velocity(sensor_state.actualVelocity);
00105       sens_if_->set_mot_position(sensor_state.actualPosition);
00106       sens_if_->set_mot_current(sensor_state.motorCurrent);
00107       sens_if_->set_bumper(sensor_state.bumper);
00108       sens_if_->set_digital_in(sensor_state.dIn);
00109       sens_if_->set_analog_in(sensor_state.aIn);
00110       if (state_->gyro.port == rec::serialport::UNDEFINED) {
00111         if (sens_if_->is_gyro_available()) {
00112           sens_if_->set_gyro_available(false);
00113           sens_if_->set_gyro_angle(0.);
00114           sens_if_->set_gyro_rate(0.);
00115         }
00116       } else {
00117         sens_if_->set_gyro_available(true);
00118         sens_if_->set_gyro_angle(state_->gyro.angle);
00119         sens_if_->set_gyro_rate(state_->gyro.rate);
00120       }
00121 
00122       update_distances(sensor_state.distanceSensor);
00123 
00124       sens_if_->write();
00125 
00126       batt_if_->set_voltage(roundf(sensor_state.voltage * 1000.));
00127       batt_if_->set_current(roundf(sensor_state.current));
00128 
00129       // 21.0V is empty, 26.0V is empty, from OpenRobotino lcdd
00130       float soc = (sensor_state.voltage - 21.0f) / 5.f;
00131       soc = std::min(1.f, std::max(0.f, soc));
00132 
00133       batt_if_->set_absolute_soc(soc);
00134       batt_if_->write();
00135 
00136     }
00137   } else if (com_->connectionState() == rec::robotino::com::Com::NotConnected) {
00138     // retry connection
00139     if (cfg_quit_on_disconnect_) {
00140       logger->log_warn(name(), "Connection lost, quitting (as per config)");
00141       fawkes::runtime::quit();
00142     } else {
00143       com_->connect(/* blocking */ false);
00144     }
00145   }
00146 }
00147 
00148 
00149 void
00150 RobotinoSensorThread::update_distances(float *voltages)
00151 {
00152   float dist_m[NUM_IR_SENSORS];
00153   const size_t num_dps = voltage_to_dist_dps_.size();
00154 
00155   for (int i = 0; i < NUM_IR_SENSORS; ++i) {
00156     dist_m[i] = 0.;
00157     // find the two enclosing data points
00158 
00159     for (size_t j = 0; j < num_dps - 1; ++j) {
00160       // This determines two points, l(eft) and r(ight) that are
00161       // defined by voltage (x coord) and distance (y coord). We
00162       // assume a linear progression between two adjacent points,
00163       // i.e. between l and r. We then do the following:
00164       // 1. Find two adjacent voltage values lv and rv where
00165       //    the voltage lies inbetween
00166       // 2. Interpolate by calculating the line parameters
00167       //    m = dd/dv, x = voltage - lv and b = ld.
00168       // cf. http://www.acroname.com/robotics/info/articles/irlinear/irlinear.html
00169 
00170       const double lv = voltage_to_dist_dps_[j  ].first;
00171       const double rv = voltage_to_dist_dps_[j+1].first;
00172 
00173       if ((voltages[i] >= lv) && (voltages[i] < rv)) {
00174         const double ld = voltage_to_dist_dps_[j  ].second;
00175         const double rd = voltage_to_dist_dps_[j+1].second;
00176 
00177         double dv = rv - lv;
00178         double dd = rd - ld;
00179 
00180         // Linear interpolation between 
00181         dist_m[i] = (dd / dv) * (voltages[i] - lv) + ld;
00182         break;
00183       }
00184     }
00185   }
00186 
00187   sens_if_->set_distance(dist_m);
00188 }