Skip to content

Commit

Permalink
mecanum: add position control and move PIDs to main file
Browse files Browse the repository at this point in the history
  • Loading branch information
@chfriedrich98
chfriedrich98 committed Aug 26, 2024
1 parent 4628a0a commit 315f29e
Show file tree
Hide file tree
Showing 4 changed files with 109 additions and 90 deletions.
89 changes: 87 additions & 2 deletions src/modules/rover_mecanum/RoverMecanum.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,6 +42,9 @@ RoverMecanum::RoverMecanum() :
updateParams();
_rover_mecanum_status_pub.advertise();
pid_init(&_pid_yaw_rate, PID_MODE_DERIVATIV_NONE, 0.001f);
pid_init(&_pid_throttle, PID_MODE_DERIVATIV_NONE, 0.001f);
pid_init(&_pid_lateral_throttle, PID_MODE_DERIVATIV_NONE, 0.001f);
pid_init(&_pid_heading, PID_MODE_DERIVATIV_NONE, 0.001f);
}

bool RoverMecanum::init()
Expand All @@ -62,7 +65,24 @@ void RoverMecanum::updateParams()
0.f, // Derivative gain
1.f, // Integral limit
1.f); // Output limit

pid_set_parameters(&_pid_throttle,
_param_rm_p_gain_speed.get(), // Proportional gain
_param_rm_i_gain_speed.get(), // Integral gain
0.f, // Derivative gain
1.f, // Integral limit
1.f); // Output limit
pid_set_parameters(&_pid_lateral_throttle,
_param_rm_p_gain_speed.get(), // Proportional gain
_param_rm_i_gain_speed.get(), // Integral gain
0.f, // Derivative gain
1.f, // Integral limit
1.f); // Output limit
pid_set_parameters(&_pid_heading,
_param_rm_p_gain_heading.get(), // Proportional gain
_param_rm_i_gain_heading.get(), // Integral gain
0.f, // Derivative gain
_max_yaw_rate, // Integral limit
_max_yaw_rate); // Output limit
}

void RoverMecanum::Run()
Expand Down Expand Up @@ -93,6 +113,7 @@ void RoverMecanum::Run()
}

_mecanum_setpoint.closed_loop_yaw_rate = false;
_mecanum_setpoint.closed_loop_speed = false;
} break;

case vehicle_status_s::NAVIGATION_STATE_ACRO: {
Expand All @@ -106,6 +127,29 @@ void RoverMecanum::Run()
}

_mecanum_setpoint.closed_loop_yaw_rate = true;
_mecanum_setpoint.closed_loop_speed = false;
} break;

case vehicle_status_s::NAVIGATION_STATE_POSCTL: {
manual_control_setpoint_s manual_control_setpoint{};

if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
_mecanum_setpoint.forward_throttle = manual_control_setpoint.throttle;
_mecanum_setpoint.lateral_throttle = manual_control_setpoint.roll;
_mecanum_setpoint.yaw_rate = math::interpolate<float>(manual_control_setpoint.yaw,
-1.f, 1.f, -_max_yaw_rate, _max_yaw_rate);

if (_mecanum_setpoint.yaw_rate < FLT_EPSILON && _mecanum_setpoint.closed_loop_heading == false) {
_mecanum_setpoint.heading = _vehicle_yaw;
_mecanum_setpoint.closed_loop_heading = true;

} else {
_mecanum_setpoint.closed_loop_heading = false;
}
}

_mecanum_setpoint.closed_loop_yaw_rate = true;
_mecanum_setpoint.closed_loop_speed = true;
} break;

case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
Expand All @@ -118,12 +162,19 @@ void RoverMecanum::Run()
default: // Unimplemented nav states will stop the rover
_mecanum_setpoint.forward_throttle = 0.f;
_mecanum_setpoint.lateral_throttle = 0.f;
_mecanum_setpoint.yaw_rate = 0.f;
_mecanum_setpoint.closed_loop_yaw_rate = false;
_mecanum_setpoint.closed_loop_speed = false;
break;
}

float speed_diff_normalized = _mecanum_setpoint.yaw_rate;

// Closed loop heading control
if (_mecanum_setpoint.closed_loop_heading) {
_mecanum_setpoint.yaw_rate = pid_calculate(&_pid_heading, _mecanum_setpoint.heading, _vehicle_yaw, 0, dt);
}

// Closed loop yaw rate control
if (_mecanum_setpoint.closed_loop_yaw_rate) {
if (fabsf(_mecanum_setpoint.yaw_rate - _vehicle_body_yaw_rate) < YAW_RATE_ERROR_THRESHOLD
Expand All @@ -144,6 +195,40 @@ void RoverMecanum::Run()
pid_reset_integral(&_pid_yaw_rate);
}

// Closed loop speed control
float forward_throttle = _mecanum_setpoint.forward_throttle;
float lateral_throttle = _mecanum_setpoint.lateral_throttle;

if (_mecanum_setpoint.closed_loop_speed) {
if (fabsf(_mecanum_setpoint.forward_throttle) < FLT_EPSILON) {
pid_reset_integral(&_pid_throttle);

} else {
forward_throttle = pid_calculate(&_pid_throttle, _mecanum_setpoint.forward_throttle, _vehicle_forward_speed, 0,
dt);

if (_param_rm_max_speed.get() > FLT_EPSILON) { // Feed-forward term
forward_throttle += math::interpolate<float>(_mecanum_setpoint.forward_throttle,
0.f, _param_rm_max_speed.get(),
0.f, 1.f);
}
}

if (fabsf(_mecanum_setpoint.lateral_throttle) < FLT_EPSILON) {
pid_reset_integral(&_pid_lateral_throttle);

} else {
lateral_throttle = pid_calculate(&_pid_lateral_throttle, _mecanum_setpoint.lateral_throttle, _vehicle_lateral_speed, 0,
dt);

if (_param_rm_max_speed.get() > FLT_EPSILON) { // Feed-forward term
lateral_throttle += math::interpolate<float>(_mecanum_setpoint.lateral_throttle,
0.f, _param_rm_max_speed.get(),
0.f, 1.f);
}
}
}

// Publish rover mecanum status (logging)
rover_mecanum_status_s rover_mecanum_status{};
rover_mecanum_status.timestamp = _timestamp;
Expand All @@ -157,7 +242,7 @@ void RoverMecanum::Run()
// Publish to motors
actuator_motors_s actuator_motors{};
actuator_motors.reversible_flags = _param_r_rev.get();
computeMotorCommands(_mecanum_setpoint.forward_throttle, _mecanum_setpoint.lateral_throttle,
computeMotorCommands(forward_throttle, lateral_throttle,
speed_diff_normalized).copyTo(actuator_motors.control);
actuator_motors.timestamp = _timestamp;
_actuator_motors_pub.publish(actuator_motors);
Expand Down
7 changes: 7 additions & 0 deletions src/modules/rover_mecanum/RoverMecanum.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -127,6 +127,9 @@ class RoverMecanum : public ModuleBase<RoverMecanum>, public ModuleParams,
hrt_abstime _timestamp{0};
PID_t _pid_yaw_rate; // The PID controller for yaw rate
RoverMecanumGuidance::mecanum_setpoint _mecanum_setpoint;
PID_t _pid_throttle; // The PID controller for velocity
PID_t _pid_lateral_throttle; // The PID controller for velocity
PID_t _pid_heading; // The PID controller for the heading

// Constants
static constexpr float YAW_RATE_ERROR_THRESHOLD = 1.f; // [rad/s] Error threshold for the closed loop yaw rate control
Expand All @@ -138,6 +141,10 @@ class RoverMecanum : public ModuleBase<RoverMecanum>, public ModuleParams,
(ParamFloat<px4::params::RM_MAX_YAW_RATE>) _param_rm_max_yaw_rate,
(ParamFloat<px4::params::RM_YAW_RATE_P>) _param_rm_yaw_rate_p,
(ParamFloat<px4::params::RM_YAW_RATE_I>) _param_rm_yaw_rate_i,
(ParamFloat<px4::params::RM_SPEED_P>) _param_rm_p_gain_speed,
(ParamFloat<px4::params::RM_SPEED_I>) _param_rm_i_gain_speed,
(ParamFloat<px4::params::RM_HEADING_P>) _param_rm_p_gain_heading,
(ParamFloat<px4::params::RM_HEADING_I>) _param_rm_i_gain_heading,
(ParamInt<px4::params::CA_R_REV>) _param_r_rev
)
};
77 changes: 11 additions & 66 deletions src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,34 +42,13 @@ RoverMecanumGuidance::RoverMecanumGuidance(ModuleParams *parent) : ModuleParams(
{
updateParams();
_rover_mecanum_guidance_status_pub.advertise();
pid_init(&_pid_heading, PID_MODE_DERIVATIV_NONE, 0.001f);
pid_init(&_pid_throttle, PID_MODE_DERIVATIV_NONE, 0.001f);
pid_init(&_pid_lateral_throttle, PID_MODE_DERIVATIV_NONE, 0.001f);
}

void RoverMecanumGuidance::updateParams()
{
ModuleParams::updateParams();

_max_yaw_rate = _param_rm_max_yaw_rate.get() * M_DEG_TO_RAD_F;
pid_set_parameters(&_pid_heading,
_param_rm_p_gain_heading.get(), // Proportional gain
_param_rm_i_gain_heading.get(), // Integral gain
0.f, // Derivative gain
_max_yaw_rate, // Integral limit
_max_yaw_rate); // Output limit
pid_set_parameters(&_pid_throttle,
_param_rm_p_gain_speed.get(), // Proportional gain
_param_rm_i_gain_speed.get(), // Integral gain
0.f, // Derivative gain
1.f, // Integral limit
1.f); // Output limit
pid_set_parameters(&_pid_lateral_throttle,
_param_rm_p_gain_speed.get(), // Proportional gain
_param_rm_i_gain_speed.get(), // Integral gain
0.f, // Derivative gain
1.f, // Integral limit
1.f); // Output limit
}

RoverMecanumGuidance::mecanum_setpoint RoverMecanumGuidance::computeGuidance(const float yaw,
Expand All @@ -78,11 +57,10 @@ RoverMecanumGuidance::mecanum_setpoint RoverMecanumGuidance::computeGuidance(con
// Initializations
bool mission_finished{false};
float desired_speed{0.f};
float desired_yaw{0.f}; // TODO: Make this a parameter
Vector2f desired_velocity(0.f, 0.f);
hrt_abstime timestamp_prev = _timestamp;
_timestamp = hrt_absolute_time();
const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
// hrt_abstime timestamp_prev = _timestamp;
// _timestamp = hrt_absolute_time();
// const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;

// uORB subscriber updates
if (_vehicle_global_position_sub.updated()) {
Expand Down Expand Up @@ -148,64 +126,31 @@ RoverMecanumGuidance::mecanum_setpoint RoverMecanumGuidance::computeGuidance(con
mission_finished = true;
}

float forward_throttle{0.f};
float lateral_throttle{0.f};
float desired_yaw_rate{0.f};

if (!mission_finished) {
desired_velocity = desired_speed * Vector2f(cosf(heading_error), sinf(heading_error));

// Closed loop heading control
desired_yaw_rate = pid_calculate(&_pid_heading, desired_yaw, yaw, 0, dt);

// Closed loop speed control
if (fabsf(desired_velocity(0)) < FLT_EPSILON) {
pid_reset_integral(&_pid_throttle);

} else {
forward_throttle = pid_calculate(&_pid_throttle, desired_velocity(0), forward_speed, 0,
dt);

if (_param_rm_max_speed.get() > FLT_EPSILON) { // Feed-forward term
forward_throttle += math::interpolate<float>(desired_velocity(0),
0.f, _param_rm_max_speed.get(),
0.f, 1.f);
}
}

if (fabsf(desired_velocity(1)) < FLT_EPSILON) {
pid_reset_integral(&_pid_lateral_throttle);

} else {
lateral_throttle = pid_calculate(&_pid_lateral_throttle, desired_velocity(1), lateral_speed, 0,
dt);

if (_param_rm_max_speed.get() > FLT_EPSILON) { // Feed-forward term
lateral_throttle += math::interpolate<float>(desired_velocity(1),
0.f, _param_rm_max_speed.get(),
0.f, 1.f);
}
}
}

// Publish mecanum controller status (logging)
_rover_mecanum_guidance_status.timestamp = _timestamp;
_rover_mecanum_guidance_status.desired_forward_speed = desired_velocity(0);
_rover_mecanum_guidance_status.desired_lateral_speed = desired_velocity(1);
_rover_mecanum_guidance_status.pid_throttle_integral = _pid_throttle.integral;
_rover_mecanum_guidance_status.pid_throttle_integral = 0.f;
_rover_mecanum_guidance_status.lookahead_distance = _pure_pursuit.getLookaheadDistance();
_rover_mecanum_guidance_status.pid_heading_integral = _pid_heading.integral;
_rover_mecanum_guidance_status.pid_heading_integral = 0.f;
_rover_mecanum_guidance_status.heading_error_deg = M_RAD_TO_DEG_F * heading_error;
_rover_mecanum_guidance_status.state_machine = (uint8_t) _currentState;
_rover_mecanum_guidance_status_pub.publish(_rover_mecanum_guidance_status);


// Return setpoints
mecanum_setpoint mecanum_setpoint_temp;
mecanum_setpoint_temp.forward_throttle = math::constrain(forward_throttle, -1.f, 1.f);
mecanum_setpoint_temp.lateral_throttle = math::constrain(lateral_throttle, -1.f, 1.f);
mecanum_setpoint_temp.yaw_rate = desired_yaw_rate;
mecanum_setpoint_temp.forward_throttle = desired_velocity(0);
mecanum_setpoint_temp.lateral_throttle = desired_velocity(1);
mecanum_setpoint_temp.heading = 0.f; // TODO: Make this variable
mecanum_setpoint_temp.yaw_rate = 0.f;
mecanum_setpoint_temp.closed_loop_yaw_rate = true;
mecanum_setpoint_temp.closed_loop_speed = true;
mecanum_setpoint_temp.closed_loop_heading = true;
return mecanum_setpoint_temp;

}
Expand Down
26 changes: 4 additions & 22 deletions src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -58,15 +58,6 @@

using namespace matrix;

/**
* @brief Enum class for the different states of guidance.
*/
enum class GuidanceState {
SPOT_TURNING, // The vehicle is currently turning on the spot.
DRIVING, // The vehicle is currently driving.
STOPPED // The vehicle is stopped.
};

/**
* @brief Class for mecanum rover guidance.
*/
Expand All @@ -84,7 +75,10 @@ class RoverMecanumGuidance : public ModuleParams
float forward_throttle{0.f};
float lateral_throttle{0.f};
float yaw_rate{0.f};
float heading{0.f};
bool closed_loop_yaw_rate{false};
bool closed_loop_speed{false};
bool closed_loop_heading{false};
};

/**
Expand Down Expand Up @@ -139,29 +133,17 @@ class RoverMecanumGuidance : public ModuleParams
Vector2f _next_wp_ned{};
Vector2d _home_position{};

// Controllers
PID_t _pid_heading; // The PID controller for the heading
PID_t _pid_throttle; // The PID controller for velocity
PID_t _pid_lateral_throttle; // The PID controller for velocity

// Constants
static constexpr float SLOW_DOWN_THRESHOLD =
2.61799f; // Slow down threshold for the maximum angle between the prev-curr and curr-next waypoint segments.

// Parameters
DEFINE_PARAMETERS(
(ParamFloat<px4::params::RM_HEADING_P>) _param_rm_p_gain_heading,
(ParamFloat<px4::params::RM_HEADING_I>) _param_rm_i_gain_heading,
(ParamFloat<px4::params::RM_SPEED_P>) _param_rm_p_gain_speed,
(ParamFloat<px4::params::RM_SPEED_I>) _param_rm_i_gain_speed,
(ParamFloat<px4::params::RM_MAX_SPEED>) _param_rm_max_speed,
(ParamFloat<px4::params::NAV_ACC_RAD>) _param_nav_acc_rad,
(ParamFloat<px4::params::RM_MAX_JERK>) _param_rm_max_jerk,
(ParamFloat<px4::params::RM_MAX_ACCEL>) _param_rm_max_accel,
(ParamFloat<px4::params::RM_MISS_SPD_DEF>) _param_rm_miss_spd_def,
(ParamFloat<px4::params::RM_MAX_YAW_RATE>) _param_rm_max_yaw_rate,
(ParamFloat<px4::params::RM_TRANS_TRN_DRV>) _param_rm_trans_trn_drv,
(ParamFloat<px4::params::RM_TRANS_DRV_TRN>) _param_rm_trans_drv_trn

(ParamFloat<px4::params::RM_MAX_YAW_RATE>) _param_rm_max_yaw_rate
)
};

0 comments on commit 315f29e

Please sign in to comment.