22 #include "com_thread.h" 24 #include <core/threading/mutex.h> 25 #include <core/threading/mutex_locker.h> 92 : seq(0), mot_velocity{0,0,0}, mot_position{0,0,0}, mot_current{0.,0.,0.},
93 bumper(
false), bumper_estop_enabled(
false), digital_in{0,0,0,0,0,0,0,0},
94 digital_out{0,0,0,0,0,0,0,0},
95 analog_in{0.,0.,0.,0.,0.,0.,0.,0.}, bat_voltage(0.), bat_current(0.),
96 imu_enabled(
false), imu_orientation{0.,0.,0.,0.}, imu_angular_velocity{0.,0.,0.},
97 imu_angular_velocity_covariance{0.,0.,0.,0.,0.,0.,0.,0.,0.},
98 ir_voltages{0.,0.,0.,0.,0.,0.,0.,0.,0.}
111 vel_mutex_ =
new Mutex();
112 vel_last_update_ =
new Time();
113 vel_last_zero_ =
false;
126 cfg_trans_accel_ = 0.;
127 cfg_trans_decel_ = 0.;
131 #ifdef USE_VELOCITY_RECORDING 132 f_ = fopen(
"comdata.csv",
"w");
142 #ifdef USE_VELOCITY_RECORDING 191 cfg_trans_accel_ = trans_accel;
192 cfg_trans_decel_ = trans_decel;
193 cfg_rot_accel_ = rot_accel;
194 cfg_rot_decel_ = rot_decel;
219 bool set_speed =
false;
222 float diff_sec = now - vel_last_update_;
223 *vel_last_update_ = now;
225 set_vx_ = update_speed(des_vx_, set_vx_, cfg_trans_accel_, cfg_trans_decel_, diff_sec);
226 set_vy_ = update_speed(des_vy_, set_vy_, cfg_trans_accel_, cfg_trans_decel_, diff_sec);
227 set_omega_ = update_speed(des_omega_, set_omega_, cfg_rot_accel_, cfg_rot_decel_, diff_sec);
236 if (set_vx_ == 0.0 && set_vy_ == 0.0 && set_omega_ == 0.0) {
237 if (! vel_last_zero_) {
239 vel_last_zero_ =
true;
243 vel_last_zero_ =
false;
247 float s1 = 0., s2 = 0., s3 = 0.;
248 project(&s1, &s2, &s3, set_vx_, set_vy_, set_omega_);
251 #ifdef USE_VELOCITY_RECORDING 254 float time_diff = now - start_;
256 fprintf(f_,
"%f\t%f\t%f\t%f\t%f\t%f\t%f\n", time_diff,
257 des_vx_, set_vx_, des_vy_, set_vy_, des_omega_, set_omega_);
263 return ! vel_last_zero_;
267 RobotinoComThread::update_speed(
float des,
float set,
float accel,
float decel,
float diff_sec)
269 if (des >= 0 &&
set < 0) {
270 const float decrement = std::copysign(decel,
set) * diff_sec;
271 if (des >
set - decrement) {
279 }
else if (des <= 0 && set > 0) {
280 const float decrement = std::copysign(decel,
set) * diff_sec;
281 if (des <
set - decrement ) {
289 }
else if (fabs(des) > fabs(
set)) {
290 const float increment = std::copysign(accel, des) * diff_sec;
291 if (fabs(des) > fabs(
set + increment)) {
298 }
else if (fabs(des) < fabs(
set)) {
299 const float decrement = std::copysign(decel, des) * diff_sec;
300 if (fabs(des) < fabs(
set - decrement)) {
349 static const double v0[2] = { -0.5 * sqrt( 3.0 ), 0.5 };
350 static const double v1[2] = { 0.0 , -1.0 };
351 static const double v2[2] = { 0.5 * sqrt( 3.0 ), 0.5 };
354 double vOmegaScaled = cfg_rb_ * (double)omega ;
357 const double k = 60.0 * cfg_gear_ / ( 2.0 * M_PI * cfg_rw_ );
360 *m1 =
static_cast<float>( ( v0[0] * (double)vx + v0[1] * (
double)vy + vOmegaScaled ) * k );
361 *m2 =
static_cast<float>( ( v1[0] * (double)vx + v1[1] * (
double)vy + vOmegaScaled ) * k );
362 *m3 =
static_cast<float>( ( v2[0] * (double)vx + v2[1] * (
double)vy + vOmegaScaled ) * k );
382 const double k = 60.0 * cfg_gear_ / ( 2.0 * M_PI * cfg_rw_ );
384 *vx =
static_cast<float>( ( (double)m3 - (
double)m1 ) / sqrt( 3.0 ) / k );
385 *vy =
static_cast<float>( 2.0 / 3.0 * ( (double)m1 + 0.5 * ( (
double)m3 - (double)m1 ) - (double)m2 ) / k );
387 double vw = (double)*vy + (
double)m2 / k;
389 *omega =
static_cast<float>( vw / cfg_rb_ );
SensorData data_
Data struct that must be updated whenever new data is available.
virtual ~RobotinoComThread()
Destructor.
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 void set_speed_points(float s1, float s2, float s3)=0
Set speed points for wheels.
Struct to exchange data between com and sensor thread.
bool new_data_
Flag to indicate new data, set to true if data_ is modified.
Fawkes library namespace.
bool update_velocities()
Update velocity values.
A class for handling time.
Thread class encapsulation of pthreads.
fawkes::Mutex * data_mutex_
Mutex to protect data_.
virtual void set_desired_vel(float vx, float vy, float omega)
Set desired velocities.
Clock * clock
By means of this member access to the clock is given.
void set_drive_layout(float rb, float rw, float gear)
Set omni drive layout parameters.
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.
operate in continuous mode (default)
virtual bool get_data(SensorData &sensor_data)
Get all current sensor data.
RobotinoComThread(const char *thread_name)
Constructor.
void set_drive_limits(float trans_accel, float trans_decel, float rot_accel, float rot_decel)
Set the omni drive limits.
Mutex mutual exclusion lock.