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>
30 #include <interfaces/BatteryInterface.h>
31 #include <interfaces/RobotinoSensorInterface.h>
33 using namespace fawkes;
44 :
Thread(
"RobotinoSensorThread",
Thread::OPMODE_WAITFORWAKEUP),
54 cfg_quit_on_disconnect_ =
config->
get_bool(
"/hardware/robotino/quit_on_disconnect");
56 com_ =
new rec::robotino::com::Com();
57 com_->setAddress(cfg_hostname_.c_str());
58 com_->connect(
false);
65 statemem_ =
new rec::sharedmemory::SharedMemory<rec::iocontrol::robotstate::State>
66 (rec::iocontrol::robotstate::State::sharedMemoryKey);
67 state_ = statemem_->getData();
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));
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;
110 if (state_->gyro.port == rec::serialport::UNDEFINED) {
122 update_distances(sensor_state.distanceSensor);
126 batt_if_->
set_voltage(roundf(sensor_state.voltage * 1000.));
127 batt_if_->
set_current(roundf(sensor_state.current));
130 float soc = (sensor_state.voltage - 21.0f) / 5.f;
131 soc = std::min(1.f, std::max(0.f, soc));
137 }
else if (com_->connectionState() == rec::robotino::com::Com::NotConnected) {
139 if (cfg_quit_on_disconnect_) {
141 fawkes::runtime::quit();
143 com_->connect(
false);
150 RobotinoSensorThread::update_distances(
float *voltages)
152 float dist_m[NUM_IR_SENSORS];
153 const size_t num_dps = voltage_to_dist_dps_.size();
155 for (
int i = 0; i < NUM_IR_SENSORS; ++i) {
159 for (
size_t j = 0; j < num_dps - 1; ++j) {
170 const double lv = voltage_to_dist_dps_[j ].first;
171 const double rv = voltage_to_dist_dps_[j+1].first;
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;
181 dist_m[i] = (dd / dv) * (voltages[i] - lv) + ld;