Skip to content

Commit

Permalink
Merge pull request #59 from LexxPluss/HWDEV-2399-feat-use-correct-mm-…
Browse files Browse the repository at this point in the history 
…per-pulse-when-tug-connected

Hwdev 2399 feat use correct mm per pulse when tug connected
  • Loading branch information
@ar90n
ar90n committed Sep 30, 2024
2 parents 30ae63b + abbfae7 commit 1b91ad6
Show file tree
Hide file tree
Showing 3 changed files with 79 additions and 43 deletions.
60 changes: 37 additions & 23 deletions lexxpluss_apps/src/actuator_controller.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,6 +39,7 @@
#include "adc_reader.hpp"
#include "board_controller.hpp"
#include "common.hpp"
#include "tug_encoder_controller.hpp"

// for HW counter
// extern "C" void HAL_TIM_Encoder_MspInit(TIM_HandleTypeDef *htim_encoder)
Expand Down Expand Up @@ -313,22 +314,8 @@ class encoder {
class counter {
public:
int init(POS pos) {
int result{0};
switch (pos) {
case POS::CENTER:
// result = enc.init(TIM4); // for HW counter
result = enc.init(pos);
mm_per_pulse = 50.0f / 1054.0f;
break;
case POS::LEFT:
result = enc.init(pos);
mm_per_pulse = 50.0f / 1054.0f;
break;
case POS::RIGHT:
result = enc.init(pos);
mm_per_pulse = 50.0f / 1054.0f;
break;
}
const int result{enc.init(pos)};
mm_per_pulse = get_mm_per_pulse(pos);
reset_pulse();
return result;
}
Expand All @@ -351,6 +338,28 @@ class counter {
prev_pulse_value = pulse_value;
return value;
}
static float get_mm_per_pulse(POS pos) {
switch (pos) {
case POS::CENTER:
return 50.0f / 1054.0f;
case POS::LEFT:
if (tug_encoder_controller::is_tug_connected()) {
return 0.0033f;
} else {
return 50.0f / 1054.0f;
}
case POS::RIGHT:
if (tug_encoder_controller::is_tug_connected()) {
return 0.0033f;
} else {
return 50.0f / 1054.0f;
}
}

// Not reach here
return 0.0f;
}

private:
void reset_pulse() {
pulse_value = prev_pulse_value = 0;
Expand Down Expand Up @@ -604,14 +613,18 @@ class actuator_controller_impl {
int init() {
k_msgq_init(&msgq, msgq_buffer, sizeof (msg), 8);
k_msgq_init(&msgq_control, msgq_control_buffer, sizeof (msg_control), 8);
if (act[0].init(POS::CENTER) != 0 ||
act[1].init(POS::LEFT) != 0 ||
act[2].init(POS::RIGHT) != 0)
return -1;
return 0;
}

void run() {
if (act[0].init(POS::CENTER) != 0 ||
act[1].init(POS::LEFT) != 0 ||
act[2].init(POS::RIGHT) != 0)
{
LOG_ERR("actuator init failed.");
return;
}

// Enable Pin, Reset Functions
gpio_dt_spec dev_enable = GET_GPIO(ps_lift_actuator);
if (gpio_is_ready_dt(&dev_enable))
Expand Down Expand Up @@ -734,9 +747,10 @@ class actuator_controller_impl {
shell_print(shell, "[notice] parameter order [Center] [Left] [Right]");
for (uint32_t i{0}; i < ACTUATOR_NUM; ++i) {
auto [pulse, current, fail, direction, duty]{act[i].get_info()};
const double mm_per_pulse{counter::get_mm_per_pulse(static_cast<POS>(i))};
shell_print(shell,
"actuator: %d encoder: %d pulse current: %d mV fail: %d dir: %d duty: %u",
i, pulse, current, fail, direction, duty);
"actuator: %d encoder: %d pulse current: %d mV fail: %d dir: %d duty: %u mm/pulse: %f",
i, pulse, current, fail, direction, duty, mm_per_pulse);
}
}
void pwm_trampoline(int index, int direction, uint8_t pwm_duty = 0) const {
Expand Down Expand Up @@ -937,4 +951,4 @@ k_msgq msgq, msgq_control;

}

// vim: set expandtab shiftwidth=4:
// vim: set expandtab shiftwidth=4:
61 changes: 41 additions & 20 deletions lexxpluss_apps/src/tug_encoder_controller.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,7 @@
* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

#include <atomic>
#include <optional>
#include <string>

Expand All @@ -36,6 +37,10 @@

namespace {
std::optional<std::string> active_token{std::nullopt};
template<typename T>
int value_or_minus_one(std::optional<T> value) {
return value.has_value() ? static_cast<int>(value.value()) : -1;
}
}

namespace lexxhard::tug_encoder_controller {
Expand Down Expand Up @@ -64,8 +69,8 @@ class tug_encoder_controller_impl {
return;
}

is_tug_connected = detect_tug();
if (!is_tug_connected) {
detect_tug();
if (!is_tug_connected()) {
LOG_INF("TUG Encoder not found");
return;
}
Expand All @@ -81,14 +86,14 @@ class tug_encoder_controller_impl {
}
}

void tug_encoder_info(const shell *shell) const {
void tug_encoder_info(const shell *shell) {
float const angle_deg{message.angle * 360.0f / 4096.0f};
int const burn_count{get_burn_count().value_or(-1)};
int const raw_angle{get_raw_angle().value_or(-1)};
int const zpos{get_zero_angle().value_or(-1)};
int const mpos{get_max_angle().value_or(-1)};
shell_print(shell, "Angle: %f[deg]", angle_deg);
shell_print(shell, "TUG connected: %d", is_tug_connected);
int const burn_count{value_or_minus_one(get_burn_count())};
int const raw_angle{value_or_minus_one(get_raw_angle())};
int const zpos{value_or_minus_one(get_zero_angle())};
int const mpos{value_or_minus_one(get_max_angle())};
shell_print(shell, "Angle: %f[deg]", static_cast<double>(angle_deg));
shell_print(shell, "TUG connected: %d", is_tug_connected());
shell_print(shell, "Magnet detected: %d", is_magnet_detected());
shell_print(shell, "Raw Angle: %d", raw_angle);
shell_print(shell, "Burn count: %d", burn_count);
Expand All @@ -105,7 +110,7 @@ class tug_encoder_controller_impl {
}

bool burn_angle() {
if (!is_tug_connected) {
if (!is_tug_connected()) {
LOG_ERR("TUG Encoder not found");
return false;
}
Expand Down Expand Up @@ -141,18 +146,30 @@ class tug_encoder_controller_impl {
return true;
}

bool is_tug_connected() {
while (true) {
auto status = is_tug_connected_status.load();
if (status.has_value()) {
return status.value();
}

k_msleep(10);
}
}

private:
bool detect_tug() {
void detect_tug() {
for (uint32_t i = 0; i < DETECTION_RETRY_COUNT; ++i) {
uint8_t buf;
if(i2c_reg_read_byte(dev, AS5600_ADDR, AS5600_REG_ANGLE_H, &buf) == 0) {
return true;
is_tug_connected_status.store(true);
return;
}

k_msleep(100);
}

return false;
is_tug_connected_status.store(false);
}

bool fetch_encoder_value() {
Expand Down Expand Up @@ -195,13 +212,13 @@ class tug_encoder_controller_impl {
uint8_t angle_h;
if (i2c_reg_read_byte(dev, AS5600_ADDR, AS5600_REG_ZPOS_H, &angle_h)) {
LOG_WRN("Failed to read AS5600_REG_ZPOS_H");
return 0;
return std::nullopt;
}

uint8_t angle_l;
if (i2c_reg_read_byte(dev, AS5600_ADDR, AS5600_REG_ZPOS_L, &angle_l)) {
LOG_WRN("Failed to read AS5600_REG_ZPOS_L");
return 0;
return std::nullopt;
}

return (angle_h <<8) | angle_l;
Expand All @@ -211,13 +228,13 @@ class tug_encoder_controller_impl {
uint8_t angle_h;
if (i2c_reg_read_byte(dev, AS5600_ADDR, AS5600_REG_MPOS_H, &angle_h)) {
LOG_WRN("Failed to read AS5600_REG_MPOS_H");
return 0;
return std::nullopt;
}

uint8_t angle_l;
if (i2c_reg_read_byte(dev, AS5600_ADDR, AS5600_REG_MPOS_L, &angle_l)) {
LOG_WRN("Failed to read AS5600_REG_MPOS_L");
return 0;
return std::nullopt;
}

return (angle_h <<8) | angle_l;
Expand All @@ -227,13 +244,13 @@ class tug_encoder_controller_impl {
uint8_t angle_h;
if (i2c_reg_read_byte(dev, AS5600_ADDR, AS5600_REG_RAW_ANGLE_H, &angle_h)) {
LOG_WRN("Failed to read AS5600_REG_RAW_ANGLE_H");
return 0;
return std::nullopt;
}

uint8_t angle_l;
if (i2c_reg_read_byte(dev, AS5600_ADDR, AS5600_REG_RAW_ANGLE_L, &angle_l)) {
LOG_WRN("Failed to read AS5600_REG_RAW_ANGLE_L");
return 0;
return std::nullopt;
}

return (angle_h <<8) | angle_l;
Expand Down Expand Up @@ -296,7 +313,7 @@ class tug_encoder_controller_impl {

msg message;
const device *dev{nullptr};
bool is_tug_connected{false};
std::atomic<std::optional<bool>> is_tug_connected_status{std::nullopt};

static constexpr uint8_t AS5600_ADDR{0x36};
static constexpr uint8_t AS5600_REG_ZMCO{0x00};
Expand Down Expand Up @@ -382,6 +399,10 @@ void run(void *p1, void *p2, void *p3)
impl.run();
}

bool is_tug_connected() {
return impl.is_tug_connected();
}

k_thread thread;
k_msgq msgq;

Expand Down
1 change: 1 addition & 0 deletions lexxpluss_apps/src/tug_encoder_controller.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,6 +36,7 @@ struct msg {

void init();
void run(void *p1, void *p2, void *p3);
bool is_tug_connected();
extern k_thread thread;
extern k_msgq msgq;
}
Expand Down

0 comments on commit 1b91ad6

Please sign in to comment.