22#include "com_thread.h"
24#include <core/threading/mutex.h>
25#include <core/threading/mutex_locker.h>
93 mot_velocity{0, 0, 0},
94 mot_position{0, 0, 0},
95 mot_current{0., 0., 0.},
97 bumper_estop_enabled(false),
98 digital_in{0, 0, 0, 0, 0, 0, 0, 0},
99 digital_out{0, 0, 0, 0, 0, 0, 0, 0},
100 analog_in{0., 0., 0., 0., 0., 0., 0., 0.},
104 imu_orientation{0., 0., 0., 0.},
105 imu_angular_velocity{0., 0., 0.},
106 imu_angular_velocity_covariance{0., 0., 0., 0., 0., 0., 0., 0., 0.},
107 ir_voltages{0., 0., 0., 0., 0., 0., 0., 0., 0.}
120 vel_mutex_ =
new Mutex();
121 vel_last_update_ =
new Time();
122 vel_last_zero_ =
false;
135 cfg_trans_accel_ = 0.;
136 cfg_trans_decel_ = 0.;
140#ifdef USE_VELOCITY_RECORDING
141 f_ = fopen(
"comdata.csv",
"w");
151 delete vel_last_update_;
152#ifdef USE_VELOCITY_RECORDING
202 cfg_trans_accel_ = trans_accel;
203 cfg_trans_decel_ = trans_decel;
204 cfg_rot_accel_ = rot_accel;
205 cfg_rot_decel_ = rot_decel;
228 bool set_speed =
false;
231 float diff_sec = now - vel_last_update_;
232 *vel_last_update_ = now;
234 set_vx_ = update_speed(des_vx_, set_vx_, cfg_trans_accel_, cfg_trans_decel_, diff_sec);
235 set_vy_ = update_speed(des_vy_, set_vy_, cfg_trans_accel_, cfg_trans_decel_, diff_sec);
236 set_omega_ = update_speed(des_omega_, set_omega_, cfg_rot_accel_, cfg_rot_decel_, diff_sec);
245 if (set_vx_ == 0.0 && set_vy_ == 0.0 && set_omega_ == 0.0) {
246 if (!vel_last_zero_) {
248 vel_last_zero_ =
true;
252 vel_last_zero_ =
false;
256 float s1 = 0., s2 = 0., s3 = 0.;
257 project(&s1, &s2, &s3, set_vx_, set_vy_, set_omega_);
260#ifdef USE_VELOCITY_RECORDING
263 float time_diff = now - start_;
266 "%f\t%f\t%f\t%f\t%f\t%f\t%f\n",
278 return !vel_last_zero_;
282RobotinoComThread::update_speed(
float des,
float set,
float accel,
float decel,
float diff_sec)
284 if (des >= 0 && set < 0) {
285 const float decrement = std::copysign(decel, set) * diff_sec;
286 if (des > set - decrement) {
294 }
else if (des <= 0 && set > 0) {
295 const float decrement = std::copysign(decel, set) * diff_sec;
296 if (des < set - decrement) {
304 }
else if (fabs(des) > fabs(set)) {
305 const float increment = std::copysign(accel, des) * diff_sec;
306 if (fabs(des) > fabs(set + increment)) {
313 }
else if (fabs(des) < fabs(set)) {
314 const float decrement = std::copysign(decel, des) * diff_sec;
315 if (fabs(des) < fabs(set - decrement)) {
364 static const double v0[2] = {-0.5 * sqrt(3.0), 0.5};
365 static const double v1[2] = {0.0, -1.0};
366 static const double v2[2] = {0.5 * sqrt(3.0), 0.5};
369 double vOmegaScaled = cfg_rb_ * (double)omega;
372 const double k = 60.0 * cfg_gear_ / (2.0 * M_PI * cfg_rw_);
375 *m1 =
static_cast<float>((v0[0] * (double)vx + v0[1] * (
double)vy + vOmegaScaled) * k);
376 *m2 =
static_cast<float>((v1[0] * (double)vx + v1[1] * (
double)vy + vOmegaScaled) * k);
377 *m3 =
static_cast<float>((v2[0] * (double)vx + v2[1] * (
double)vy + vOmegaScaled) * k);
397 const double k = 60.0 * cfg_gear_ / (2.0 * M_PI * cfg_rw_);
399 *vx =
static_cast<float>(((double)m3 - (
double)m1) / sqrt(3.0) / k);
401 static_cast<float>(2.0 / 3.0 * ((double)m1 + 0.5 * ((
double)m3 - (double)m1) - (double)m2) / k);
403 double vw = (double)*vy + (
double)m2 / k;
405 *omega =
static_cast<float>(vw / cfg_rb_);
void set_drive_limits(float trans_accel, float trans_decel, float rot_accel, float rot_decel)
Set the omni drive limits.
virtual void set_speed_points(float s1, float s2, float s3)=0
Set speed points for wheels.
fawkes::Mutex * data_mutex_
Mutex to protect data_.
void set_drive_layout(float rb, float rw, float gear)
Set omni drive layout parameters.
RobotinoComThread(const char *thread_name)
Constructor.
virtual void set_desired_vel(float vx, float vy, float omega)
Set desired velocities.
void project(float *m1, float *m2, float *m3, float vx, float vy, float omega) const
Project the velocity of the robot in cartesian coordinates to single motor speeds.
virtual bool get_data(SensorData &sensor_data)
Get all current sensor data.
void unproject(float *vx, float *vy, float *omega, float m1, float m2, float m3) const
Project single motor speeds to velocity in cartesian coordinates.
virtual ~RobotinoComThread()
Destructor.
bool update_velocities()
Update velocity values.
SensorData data_
Data struct that must be updated whenever new data is available.
bool new_data_
Flag to indicate new data, set to true if data_ is modified.
Clock * clock
By means of this member access to the clock is given.
Mutex mutual exclusion lock.
Thread class encapsulation of pthreads.
@ OPMODE_CONTINUOUS
operate in continuous mode (default)
A class for handling time.
Fawkes library namespace.
Struct to exchange data between com and sensor thread.