Skip to content

Commit

Permalink
Merge pull request #58 from LexxPluss/HWDEV-2398-feat-improve-encoder…
Browse files Browse the repository at this point in the history 
…-count-precision

Hwdev 2398 feat improve encoder count precision
  • Loading branch information
@ar90n
ar90n authored Sep 25, 2024
2 parents c9671fb + aad70c2 commit 30ae63b
Showing 1 changed file with 47 additions and 37 deletions.
84 changes: 47 additions & 37 deletions lexxpluss_apps/src/actuator_controller.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,8 @@
* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

#include <optional>

#include <zephyr/kernel.h>
#include <zephyr/device.h>
#include <zephyr/drivers/gpio.h>
Expand Down Expand Up @@ -150,12 +152,12 @@ class encoder {
dev = GPIO_DT_SPEC_GET(DT_NODELABEL(encoder_center_actuator_ch1), gpios);
// PD12 CH1, PD13 CH2
if (gpio_is_ready_dt(&dev)) {
gpio_pin_configure_dt(&dev, GPIO_INPUT | GPIO_ACTIVE_HIGH);
gpio_pin_configure_dt(&dev, GPIO_INPUT | GPIO_PULL_UP | GPIO_ACTIVE_HIGH);
}

dev = GPIO_DT_SPEC_GET(DT_NODELABEL(encoder_center_actuator_ch2), gpios);
if (gpio_is_ready_dt(&dev)) {
gpio_pin_configure_dt(&dev, GPIO_INPUT | GPIO_ACTIVE_HIGH);
gpio_pin_configure_dt(&dev, GPIO_INPUT | GPIO_PULL_UP | GPIO_ACTIVE_HIGH);
}

// set callback function for center
Expand All @@ -168,12 +170,12 @@ class encoder {
dev = GPIO_DT_SPEC_GET(DT_NODELABEL(encoder_left_actuator_ch1), gpios);
// PD12 CH1, PD13 CH2
if (gpio_is_ready_dt(&dev)) {
gpio_pin_configure_dt(&dev, GPIO_INPUT | GPIO_ACTIVE_HIGH);
gpio_pin_configure_dt(&dev, GPIO_INPUT | GPIO_PULL_UP | GPIO_ACTIVE_HIGH);
}

dev = GPIO_DT_SPEC_GET(DT_NODELABEL(encoder_left_actuator_ch2), gpios);
if (gpio_is_ready_dt(&dev)) {
gpio_pin_configure_dt(&dev, GPIO_INPUT | GPIO_ACTIVE_HIGH);
gpio_pin_configure_dt(&dev, GPIO_INPUT | GPIO_PULL_UP | GPIO_ACTIVE_HIGH);
}

// set callback function for LEFT
Expand All @@ -186,12 +188,12 @@ class encoder {
dev = GPIO_DT_SPEC_GET(DT_NODELABEL(encoder_right_actuator_ch1), gpios);
// PD12 CH1, PD13 CH2
if (gpio_is_ready_dt(&dev)) {
gpio_pin_configure_dt(&dev, GPIO_INPUT | GPIO_ACTIVE_HIGH);
gpio_pin_configure_dt(&dev, GPIO_INPUT | GPIO_PULL_UP | GPIO_ACTIVE_HIGH);
}

dev = GPIO_DT_SPEC_GET(DT_NODELABEL(encoder_right_actuator_ch2), gpios);
if (gpio_is_ready_dt(&dev)) {
gpio_pin_configure_dt(&dev, GPIO_INPUT | GPIO_ACTIVE_HIGH);
gpio_pin_configure_dt(&dev, GPIO_INPUT | GPIO_PULL_UP | GPIO_ACTIVE_HIGH);
}

// set callback function for RIGHT
Expand Down Expand Up @@ -227,16 +229,9 @@ class encoder {
ch2 = gpio_pin_get_dt(&dev_ch2);
}

// catch rise edge
if (instance->ch1_prev == 0 && ch1 == 1) {
if (ch2 == 0) {
instance->count++;
} else {
instance->count--;
}
}

instance->count += instance->calc_delta_count(instance->ch1_prev, instance->ch2_prev, ch1, ch2);
instance->ch1_prev = ch1;
instance->ch2_prev = ch2;
}
}
static void static_center_left_callback(struct k_timer *timer_id) {
Expand All @@ -254,16 +249,9 @@ class encoder {
ch2 = gpio_pin_get_dt(&dev_ch2);
}

// catch rise edge
if (instance->ch1_prev == 0 && ch1 == 1) {
if (ch2 == 0) {
instance->count++;
} else {
instance->count--;
}
}

instance->count += instance->calc_delta_count(instance->ch1_prev, instance->ch2_prev, ch1, ch2);
instance->ch1_prev = ch1;
instance->ch2_prev = ch2;
}
}
static void static_center_right_callback(struct k_timer *timer_id) {
Expand All @@ -281,18 +269,41 @@ class encoder {
ch2 = gpio_pin_get_dt(&dev_ch2);
}

// catch rise edge
if (instance->ch1_prev == 0 && ch1 == 1) {
if (ch2 == 0) {
instance->count++;
} else {
instance->count--;
}
}

instance->count += instance->calc_delta_count(instance->ch1_prev, instance->ch2_prev, ch1, ch2);
instance->ch1_prev = ch1;
instance->ch2_prev = ch2;
}
}
inline int16_t calc_delta_count(const int prev_ch1, const int prev_ch2, const int ch1, const int ch2) const {
// index is prev_ch1,prev_ch2,ch1,ch2
static constexpr std::optional<int16_t> delta_table[16]{
0, // 00 -> 00 no change
-1, // 00 -> 01
1, // 00 -> 10
std::nullopt, // 00 -> 11 error(ignore)
1, // 01 -> 00
0, // 01 -> 01 no change
std::nullopt, // 01 -> 10 error(ignore)
-1, // 01 -> 11
-1, // 10 -> 00
std::nullopt, // 10 -> 01 error(ignore)
0, // 10 -> 10 no change
1, // 10 -> 11
std::nullopt, // 11 -> 00 error(ignore)
1, // 11 -> 01
-1, // 11 -> 10
0 // 11 -> 11 no change
};

const int index = (prev_ch1 << 3) | (prev_ch2 << 2) | (ch1 << 1) | (ch2 << 0);
const auto ret{delta_table[index]};
if (!ret.has_value()) {
LOG_WRN("Wrong encoder state: %d%d -> %d%d", prev_ch1, prev_ch2, ch1, ch2);
}

return ret.value_or(0); // return 0 if error
}

k_timer encoder_count_c, encoder_count_l, encoder_count_r;
int ch1_prev{0}, ch2_prev{0};
int16_t count{0};
Expand All @@ -307,16 +318,15 @@ class counter {
case POS::CENTER:
// result = enc.init(TIM4); // for HW counter
result = enc.init(pos);
// mm_per_pulse = 50.0f / 1054.0f; // for HW counter
mm_per_pulse = 50.0f / 144.0f;
mm_per_pulse = 50.0f / 1054.0f;
break;
case POS::LEFT:
result = enc.init(pos);
mm_per_pulse = 50.0f / 144.0f;
mm_per_pulse = 50.0f / 1054.0f;
break;
case POS::RIGHT:
result = enc.init(pos);
mm_per_pulse = 50.0f / 144.0f;
mm_per_pulse = 50.0f / 1054.0f;
break;
}
reset_pulse();
Expand Down

0 comments on commit 30ae63b

Please sign in to comment.