Mirrors_Framework/ODrive
1
0
Fork 0
mirror of https://github.com/odriverobotics/ODrive.git synced 2026-09-23 09:03:40 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

Un-removed custom setter for input_pos_

Browse Source 
removed explicit casts where variables would be promoted anyway
This commit is contained in:
PAJohnson
2020-06-26 19:48:21 +01:00
parent 193dddad00
commit 744bfeb110
4 changed files with 11 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Firmware/MotorControl/controller.cpp
+4 -4
View File
@@ -85,8 +85,8 @@ void Controller::start_anticogging_calibration() {
*/
bool Controller::anticogging_calibration(float pos_estimate, float vel_estimate) {
float pos_err = input_pos_ - pos_estimate;
if (std::abs(pos_err) <= config_.anticogging.calib_pos_threshold * (2.0f * M_PI) / (float)axis_->encoder_.config_.cpr &&
std::abs(vel_estimate) < config_.anticogging.calib_vel_threshold * (2.0f * M_PI) / (float)axis_->encoder_.config_.cpr) {
if (std::abs(pos_err) <= config_.anticogging.calib_pos_threshold * (2.0f * M_PI) / axis_->encoder_.config_.cpr &&
std::abs(vel_estimate) < config_.anticogging.calib_vel_threshold * (2.0f * M_PI) / axis_->encoder_.config_.cpr) {
config_.anticogging.cogging_map[std::clamp<uint32_t>(config_.anticogging.index++, 0, 3600)] = vel_integrator_torque_;
}
if (config_.anticogging.index < 3600) {
@@ -140,7 +140,7 @@ bool Controller::update(float* torque_setpoint_output) {
// TODO also enable circular deltas for 2nd order filter, etc.
if (pos_wrap_src_) {
float cpr = *pos_wrap_src_ * 2.0f * M_PI / ((float)axis_->encoder_.config_.cpr);
float cpr = *pos_wrap_src_ * 2.0f * M_PI / axis_->encoder_.config_.cpr;
// Keep pos setpoint from drifting
input_pos_ = fmodf_pos(input_pos_, cpr);
}
@@ -235,7 +235,7 @@ bool Controller::update(float* torque_setpoint_output) {
}
if (pos_wrap_src_) {
float cpr = *pos_wrap_src_ * 2.0f * M_PI / ((float)axis_->encoder_.config_.cpr);
float cpr = *pos_wrap_src_ * 2.0f * M_PI / axis_->encoder_.config_.cpr;
// Keep pos setpoint from drifting
pos_setpoint_ = fmodf_pos(pos_setpoint_, cpr);
// Circular delta
Firmware/MotorControl/controller.hpp
+2
View File
@@ -95,6 +95,8 @@ public:
bool anticogging_valid_ = false;
// custom setters
void set_input_pos(float value) { input_pos_ = value; input_pos_updated();}
};
#endif // __CONTROLLER_HPP
Firmware/MotorControl/encoder.cpp
+4 -4
View File
@@ -510,7 +510,7 @@ bool Encoder::update() {
// discrete phase detector
float delta_pos = (float)(shadow_count_ - (int32_t)std::floor(pos_estimate_));
float delta_pos_cpr = (float)(count_in_cpr_ - (int32_t)std::floor(pos_cpr_));
delta_pos_cpr = wrap_pm(delta_pos_cpr, 0.5f * (float)(config_.cpr));
delta_pos_cpr = wrap_pm(delta_pos_cpr, 0.5f * config_.cpr);
// pll feedback
pos_estimate_ += current_meas_period * pll_kp_ * delta_pos;
pos_cpr_ += current_meas_period * pll_kp_ * delta_pos_cpr;
@@ -523,9 +523,9 @@ bool Encoder::update() {
}
//expose pos/vel estimates in radians for Controller
pos_est_rad_ = pos_estimate_ * 2.0f * M_PI / (float)config_.cpr;
vel_est_rad_ = vel_estimate_ * 2.0f * M_PI / (float)config_.cpr;
pos_cpr_rad_ = pos_cpr_ * 2.0f * M_PI / (float)config_.cpr;
pos_est_rad_ = pos_estimate_ * 2.0f * M_PI / config_.cpr;
vel_est_rad_ = vel_estimate_ * 2.0f * M_PI / config_.cpr;
pos_cpr_rad_ = pos_cpr_ * 2.0f * M_PI / config_.cpr;
//// run encoder count interpolation
int32_t corrected_enc = count_in_cpr_ - config_.offset;
Firmware/odrive-interface.yaml
+1
View File
@@ -596,6 +596,7 @@ interfaces:
input_pos:
type: float32
unit: rad
c_setter: set_input_pos
input_vel:
type: float32
unit: rad/s