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_;
282 RobotinoComThread::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_);