Open jackmcnamara107 opened 1 year ago
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;
}
Is your micro-ROS node connection properly to the micro-ROS Agent?
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
I guess that, if this is the behavior, this issue is related to your IRQ priorities or configuration.
@jackmcnamara107 have you tested the encoder values in an independent runtime, that is without micro-ROS? Just printf()
to Minicom?
@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.
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