Fawkes API  Fawkes Development Version
sensor_thread.cpp
1 
2 /***************************************************************************
3  * sensor_thread.cpp - Robotino sensor thread
4  *
5  * Created: Sun Nov 13 15:35:24 2011
6  * Copyright 2011-2012 Tim Niemueller [www.niemueller.de]
7  *
8  ****************************************************************************/
9 
10 /* This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version.
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU Library General Public License for more details.
19  *
20  * Read the full text in the LICENSE.GPL file in the doc directory.
21  */
22 
23 #include "sensor_thread.h"
24 #include <rec/robotino/com/Com.h>
25 #include <rec/sharedmemory/sharedmemory.h>
26 #include <rec/iocontrol/remotestate/SensorState.h>
27 #include <rec/iocontrol/robotstate/State.h>
28 #include <baseapp/run.h>
29 
30 #include <interfaces/BatteryInterface.h>
31 #include <interfaces/RobotinoSensorInterface.h>
32 
33 using namespace fawkes;
34 
35 /** @class RobotinoSensorThread "sensor_thread.h"
36  * Robotino sensor hook integration thread.
37  * This thread integrates into the Fawkes main loop at the SENSOR hook and
38  * writes new sensor data.
39  * @author Tim Niemueller
40  */
41 
42 /** Constructor. */
44  : Thread("RobotinoSensorThread", Thread::OPMODE_WAITFORWAKEUP),
45  BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_ACQUIRE)
46 {
47 }
48 
49 
50 void
52 {
53  cfg_hostname_ = config->get_string("/hardware/robotino/hostname");
54  cfg_quit_on_disconnect_ = config->get_bool("/hardware/robotino/quit_on_disconnect");
55 
56  com_ = new rec::robotino::com::Com();
57  com_->setAddress(cfg_hostname_.c_str());
58  com_->connect(/* blocking */ false);
59 
60  last_seqnum_ = 0;
61 
62  batt_if_ = blackboard->open_for_writing<BatteryInterface>("Robotino");
63  sens_if_ = blackboard->open_for_writing<RobotinoSensorInterface>("Robotino");
64 
66  (rec::iocontrol::robotstate::State::sharedMemoryKey);
67  state_ = statemem_->getData();
68 
69  // taken from Robotino API2 DistanceSensorImpl.hpp
70  voltage_to_dist_dps_.push_back(std::make_pair(0.3 , 0.41));
71  voltage_to_dist_dps_.push_back(std::make_pair(0.39, 0.35));
72  voltage_to_dist_dps_.push_back(std::make_pair(0.41, 0.30));
73  voltage_to_dist_dps_.push_back(std::make_pair(0.5 , 0.25));
74  voltage_to_dist_dps_.push_back(std::make_pair(0.75, 0.18));
75  voltage_to_dist_dps_.push_back(std::make_pair(0.8 , 0.16));
76  voltage_to_dist_dps_.push_back(std::make_pair(0.95, 0.14));
77  voltage_to_dist_dps_.push_back(std::make_pair(1.05, 0.12));
78  voltage_to_dist_dps_.push_back(std::make_pair(1.3 , 0.10));
79  voltage_to_dist_dps_.push_back(std::make_pair(1.4 , 0.09));
80  voltage_to_dist_dps_.push_back(std::make_pair(1.55, 0.08));
81  voltage_to_dist_dps_.push_back(std::make_pair(1.8 , 0.07));
82  voltage_to_dist_dps_.push_back(std::make_pair(2.35, 0.05));
83  voltage_to_dist_dps_.push_back(std::make_pair(2.55, 0.04));
84 }
85 
86 
87 void
89 {
90  blackboard->close(sens_if_);
91  blackboard->close(batt_if_);
92  delete com_;
93 }
94 
95 void
97 {
98  if (com_->isConnected()) {
99  rec::iocontrol::remotestate::SensorState sensor_state = com_->sensorState();
100  if (sensor_state.sequenceNumber != last_seqnum_) {
101  last_seqnum_ = sensor_state.sequenceNumber;
102 
103  // update sensor values in interface
104  sens_if_->set_mot_velocity(sensor_state.actualVelocity);
105  sens_if_->set_mot_position(sensor_state.actualPosition);
106  sens_if_->set_mot_current(sensor_state.motorCurrent);
107  sens_if_->set_bumper(sensor_state.bumper);
108  sens_if_->set_digital_in(sensor_state.dIn);
109  sens_if_->set_analog_in(sensor_state.aIn);
110  if (state_->gyro.port == rec::serialport::UNDEFINED) {
111  if (sens_if_->is_gyro_available()) {
112  sens_if_->set_gyro_available(false);
113  sens_if_->set_gyro_angle(0.);
114  sens_if_->set_gyro_rate(0.);
115  }
116  } else {
117  sens_if_->set_gyro_available(true);
118  sens_if_->set_gyro_angle(state_->gyro.angle);
119  sens_if_->set_gyro_rate(state_->gyro.rate);
120  }
121 
122  update_distances(sensor_state.distanceSensor);
123 
124  sens_if_->write();
125 
126  batt_if_->set_voltage(roundf(sensor_state.voltage * 1000.));
127  batt_if_->set_current(roundf(sensor_state.current));
128 
129  // 21.0V is empty, 26.0V is empty, from OpenRobotino lcdd
130  float soc = (sensor_state.voltage - 21.0f) / 5.f;
131  soc = std::min(1.f, std::max(0.f, soc));
132 
133  batt_if_->set_absolute_soc(soc);
134  batt_if_->write();
135 
136  }
137  } else if (com_->connectionState() == rec::robotino::com::Com::NotConnected) {
138  // retry connection
139  if (cfg_quit_on_disconnect_) {
140  logger->log_warn(name(), "Connection lost, quitting (as per config)");
141  fawkes::runtime::quit();
142  } else {
143  com_->connect(/* blocking */ false);
144  }
145  }
146 }
147 
148 
149 void
150 RobotinoSensorThread::update_distances(float *voltages)
151 {
152  float dist_m[NUM_IR_SENSORS];
153  const size_t num_dps = voltage_to_dist_dps_.size();
154 
155  for (int i = 0; i < NUM_IR_SENSORS; ++i) {
156  dist_m[i] = 0.;
157  // find the two enclosing data points
158 
159  for (size_t j = 0; j < num_dps - 1; ++j) {
160  // This determines two points, l(eft) and r(ight) that are
161  // defined by voltage (x coord) and distance (y coord). We
162  // assume a linear progression between two adjacent points,
163  // i.e. between l and r. We then do the following:
164  // 1. Find two adjacent voltage values lv and rv where
165  // the voltage lies inbetween
166  // 2. Interpolate by calculating the line parameters
167  // m = dd/dv, x = voltage - lv and b = ld.
168  // cf. http://www.acroname.com/robotics/info/articles/irlinear/irlinear.html
169 
170  const double lv = voltage_to_dist_dps_[j ].first;
171  const double rv = voltage_to_dist_dps_[j+1].first;
172 
173  if ((voltages[i] >= lv) && (voltages[i] < rv)) {
174  const double ld = voltage_to_dist_dps_[j ].second;
175  const double rd = voltage_to_dist_dps_[j+1].second;
176 
177  double dv = rv - lv;
178  double dd = rd - ld;
179 
180  // Linear interpolation between
181  dist_m[i] = (dd / dv) * (voltages[i] - lv) + ld;
182  break;
183  }
184  }
185  }
186 
187  sens_if_->set_distance(dist_m);
188 }
void set_gyro_rate(const float new_gyro_rate)
Set gyro_rate value.
void set_absolute_soc(const float new_absolute_soc)
Set absolute_soc value.
Fawkes library namespace.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
virtual void finalize()
Finalize the thread.
BatteryInterface Fawkes BlackBoard Interface.
void set_gyro_angle(const float new_gyro_angle)
Set gyro_angle value.
RobotinoSensorThread()
Constructor.
void set_voltage(const uint32_t new_voltage)
Set voltage value.
Thread class encapsulation of pthreads.
Definition: thread.h:42
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:495
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:44
virtual Interface * open_for_writing(const char *interface_type, const char *identifier)=0
Open interface for writing.
void set_analog_in(unsigned int index, const float new_analog_in)
Set analog_in value at given index.
RobotinoSensorInterface Fawkes BlackBoard Interface.
Thread aspect to use blocked timing.
virtual void init()
Initialize the thread.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
void set_mot_velocity(unsigned int index, const float new_mot_velocity)
Set mot_velocity value at given index.
const char * name() const
Get name of thread.
Definition: thread.h:95
bool is_gyro_available() const
Get gyro_available value.
void set_digital_in(unsigned int index, const bool new_digital_in)
Set digital_in value at given index.
void set_current(const uint32_t new_current)
Set current value.
void set_distance(unsigned int index, const float new_distance)
Set distance value at given index.
void set_mot_current(unsigned int index, const float new_mot_current)
Set mot_current value at given index.
void set_mot_position(unsigned int index, const int32_t new_mot_position)
Set mot_position value at given index.
virtual void loop()
Code to execute in the thread.
void set_bumper(const bool new_bumper)
Set bumper value.
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:44
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:43
void set_gyro_available(const bool new_gyro_available)
Set gyro_available value.
virtual void close(Interface *interface)=0
Close interface.