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