Publishing values from multiple hall effect encoders

[jackmcnamara107]

• Hardware description: Raspberry Pi Pico to Ubuntu 22.04 with ROS2 Humble
• Version or commit hash: humble 32aa731

Steps to reproduce the issue

Have two DC motors with Hall Effect encoders. I'm attempting to read the encoder data using interrupts and publish to seperate ROS messages. Each sensor has two signal wires with 10k ohm pullup resistors.

Expected behavior

Move motor and see respective encoder message change. This functionality works with the standard Pico SDK library using serial monitor when printing the two encoder values.

Actual behavior

Messages are not published by the device. The final setup steps are to enable the IRQ handlers and then spin. Not enabling the secondary IRQ with the handler allows for the other encoder value to be published.

Additional information

#include <stdio.h>

#include <rcl/rcl.h>
#include <rcl/error_handling.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
#include <std_msgs/msg/int32.h>
#include <rmw_microros/rmw_microros.h>

#include "pico/stdlib.h"
#include "pico_uart_transports.h"
#include "hardware/gpio.h"

const uint LED_PIN = 25;

// Right Motor Encoder pins
const uint ENC1_A = 8; // orange wire
const uint ENC1_B = 9; // white wire

// Left Motor Encoder pins
const uint ENC2_A = 0; // orange wire
const uint ENC2_B = 1; // white wire

// Initialise variables
int right_pos = 0; // Encoder 1 position
int left_pos = 0; // Encoder 2 position

#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){printf("Failed status on line %d: %d. Aborting.\n",__LINE__,(int)temp_rc); return 1;}}
#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){printf("Failed status on line %d: %d. Continuing.\n",__LINE__,(int)temp_rc);}}

rcl_publisher_t publisher_right;
rcl_publisher_t publisher_left;
std_msgs__msg__Int32 send_right_msg;
std_msgs__msg__Int32 send_left_msg;


void timer_callback(rcl_timer_t *timer, int64_t last_call_time)
{
    RCSOFTCHECK(rcl_publish(&publisher_right, &send_right_msg, NULL));
	RCSOFTCHECK(rcl_publish(&publisher_left, &send_left_msg, NULL));
}

void gpio_callback(uint gpio, uint32_t events) {
    // Read the encoder b wire and compare
    int b = gpio_get(ENC1_B);
    if (b > 0){
        right_pos++;
    } else {
        right_pos--;
    }
    send_right_msg.data = right_pos;
}

void gpio1_callback(void) {
    if (gpio_get_irq_event_mask(ENC2_A) & GPIO_IRQ_EDGE_RISE){
        gpio_acknowledge_irq(ENC2_A, GPIO_IRQ_EDGE_RISE);
        // Read the encoder b wire and compare
        int b = gpio_get(ENC2_B);
		if (b > 0){
			left_pos++;
		} else {
			left_pos--;
		}
		send_left_msg.data = left_pos;
    }
}

int main()
{	
    rmw_uros_set_custom_transport(
		true,
		NULL,
		pico_serial_transport_open,
		pico_serial_transport_close,
		pico_serial_transport_write,
		pico_serial_transport_read
	);

    gpio_init(LED_PIN);
    gpio_set_dir(LED_PIN, GPIO_OUT);

	gpio_init(ENC1_B);
	gpio_init(ENC2_B);
	gpio_set_dir(ENC1_B, GPIO_IN);
	gpio_set_dir(ENC2_B, GPIO_IN);

    rcl_timer_t timer;
    rcl_node_t node;
    rcl_allocator_t allocator;
    rclc_support_t support;
    rclc_executor_t executor;

    allocator = rcl_get_default_allocator();

    // Wait for agent successful ping for 2 minutes.
    const int timeout_ms = 1000; 
    const uint8_t attempts = 120;

    rcl_ret_t ret = rmw_uros_ping_agent(timeout_ms, attempts);

    if (ret != RCL_RET_OK)
    {
        // Unreachable agent, exiting program.
        return ret;
    }

    rclc_support_init(&support, 0, NULL, &allocator);

    rclc_node_init_default(&node, "pico_node", "", &support);

	// create publisher_right
    rclc_publisher_init_default(
        &publisher_right,
        &node,
        ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
        "wheel_pos_right");

	// create publisher_right
    rclc_publisher_init_default(
        &publisher_left,
        &node,
        ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
        "wheel_pos_left");

    rclc_timer_init_default(
        &timer,
        &support,
        RCL_MS_TO_NS(10),
        timer_callback);

    rclc_executor_init(&executor, &support.context, 1, &allocator);
    rclc_executor_add_timer(&executor, &timer);

    send_right_msg.data = 0;
	send_left_msg.data = 0;

	// Set interrupt and interrupt handler
    gpio_set_irq_enabled_with_callback(ENC1_A, GPIO_IRQ_EDGE_RISE, true, &gpio_callback);
    gpio_set_irq_enabled(ENC2_A, GPIO_IRQ_EDGE_RISE, true);
    gpio_add_raw_irq_handler(ENC2_A, gpio1_callback);

	gpio_put(LED_PIN, 1);

    while (true)
    {
        RCSOFTCHECK(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(10)));
    }
    return 0;
}

[jackmcnamara107]

Working Pico SDK code

/**
 * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
 *
 * SPDX-License-Identifier: BSD-3-Clause
 */

#include <time.h>
#include <math.h>
#include <stdio.h>
#include "pico/stdlib.h"
#include "hardware/gpio.h"

// Right Motor Encoder pins
const uint ENC1_A = 8; // orange wire
const uint ENC1_B = 9; // white wire

// Left Motor Encoder pins
const uint ENC2_A = 0; // orange wire
const uint ENC2_B = 1; // white wire

int pos = 0; // Encoder 1 position
int pos1 = 0;

void gpio_callback(uint gpio, uint32_t events) {
    // Read the encoder b wire and compare
    int b = gpio_get(ENC1_B);
    if (b > 0){
        pos++;
    } else {
        pos--;
    }
}

void gpio1_callback(void) {
    if (gpio_get_irq_event_mask(ENC2_A) & GPIO_IRQ_EDGE_RISE){
        gpio_acknowledge_irq(ENC2_A, GPIO_IRQ_EDGE_RISE);
        // Read the encoder b wire and compare
        int b = gpio_get(ENC2_B);
        if (b > 0){
            pos1++;
        } else {
            pos1--;
        }
    }
}

int main() {
    stdio_init_all();

    gpio_init(ENC1_B);
    gpio_init(ENC2_B);

    gpio_set_dir(ENC1_B, GPIO_IN);
    gpio_set_dir(ENC2_B, GPIO_IN);

    // Set interrupt and interrupt handler
    gpio_set_irq_enabled(ENC2_A, GPIO_IRQ_EDGE_RISE, true);
    gpio_set_irq_enabled_with_callback(ENC1_A, GPIO_IRQ_EDGE_RISE, true, &gpio_callback);
    gpio_add_raw_irq_handler(ENC2_A, gpio1_callback);
    
        
    while (1)
    {
        printf("Pos: %d Pos1: %d\n", pos, pos1);
        sleep_ms(10);
    }

    return 0;
}

[pablogs9]

Is your micro-ROS node connection properly to the micro-ROS Agent?

[jackmcnamara107]

Yes I believe so, if I remove the lines

gpio_set_irq_enabled(ENC2_A, GPIO_IRQ_EDGE_RISE, true);
    gpio_add_raw_irq_handler(ENC2_A, gpio1_callback);

Then the messages for publisher_right are published and can be seen through the relevant ROS2 topic.

But including the two lines above results in no messages being published. The topic still exists (with no messages being published) so it is making a connection to the ROS network

[pablogs9]

I guess that, if this is the behavior, this issue is related to your IRQ priorities or configuration.

[holmbuar]

@jackmcnamara107 have you tested the encoder values in an independent runtime, that is without micro-ROS? Just printf() to Minicom?

[holmbuar]

@jackmcnamara107 I also tried having two separate interrupt callbacks, which did not work. I had to use only one callback. Regardless of that, I have now implemented an encoder counter with PIO state machines, based on this example:

https://github.com/raspberrypi/pico-examples/tree/master/pio/quadrature_encoder

I recommend you give it a try, being able to handle encoders without using processor cycles.