0
$\begingroup$

I am wanting to link to this encoder code with another ROS2 C++ program, in the ROS2_Controller group. I believe I can include the header file from this encoder code in compilation and use a public variable to share the "pos(ition)" when the Ros2 program wants it. However, I'm not a C++ Guru and don't know the proper technique. Can anyone here give pointers? This will work on the Raspberry Pi with Ubuntu Jammy (server) and ROS2 Humble. The encoder program works very well. I'll have to do one for left and one for right side of the diff-drive rover. They will be tied together in the controller code, of course. Thanks!

#include <iostream>
#include <pigpiod_if2.h>
#include "rotary_encoder.hpp"

/*

"test_rotary_encoder.cpp"

REQUIRES

A rotary encoder contacts A and B connected to separate gpios and
the common contact connected to Pi ground.

TO BUILD

g++ -o rot_enc test_rotary_encoder.cpp rotary_encoder.cpp -lpigpiod_if2

TO RUN

./rot_enc

*/

void callback(int way)
{
   static int pos = 0;

   pos += way;
   // Final output number above. Don't write to screen in final copy

   std::cout << "pos=" << pos << std::endl;
}

int main(int argc, char *argv[])
{
   int pi;

   pi = pigpio_start(NULL, NULL);

   if (pi < 0)
   {
      std::cout << "can't connect to pigpio daemon" << std::endl;
      return 1;
   }

   re_decoder dec(pi, 7, 8, callback);
   // Gpio pins numbered above. Change as necessary

   time_sleep(3000);

   dec.re_cancel();

   pigpio_stop(pi);
}
#include <iostream>
#include <pigpiod_if2.h>
#include "rotary_encoder.hpp"

/*

"rotary_encoder.cpp"

             +---------+         +---------+      0
             |         |         |         |
   A         |         |         |         |
             |         |         |         |
   +---------+         +---------+         +----- 1

       +---------+         +---------+            0
       |         |         |         |
   B   |         |         |         |
       |         |         |         |
   ----+         +---------+         +---------+  1

*/

void re_decoder::_pulse(int gpio, int level, uint32_t tick)
{
   if (gpio == mygpioA) levA = level; else levB = level;

   if (gpio != lastGpio) /* debounce */
   {
      lastGpio = gpio;

      if ((gpio == mygpioA) && (level == 1))
      {
         if (levB) (mycallback)(1);
      }
      else if ((gpio == mygpioB) && (level == 1))
      {
         if (levA) (mycallback)(-1);
      }
   }
}

void re_decoder::_pulseEx(int pi, unsigned gpio, unsigned level, uint32_t tick, void *user)
{
   /*
      Need a static callback to link with C.
   */

   re_decoder *mySelf = (re_decoder *) user;

   mySelf->_pulse(gpio, level, tick); /* Call the instance callback. */
}

re_decoder::re_decoder(int pi, int gpioA, int gpioB, re_decoderCB_t callback)
{
   mypi = pi;
   mygpioA = gpioA;
   mygpioB = gpioB;

   mycallback = callback;

   levA=0;
   levB=0;

   lastGpio = -1;

   set_mode(pi, gpioA, PI_INPUT);
   set_mode(pi, gpioB, PI_INPUT);

   /* pull up is needed as encoder common is grounded */
   // New comment: Works fine without any pull up. Just power encoders with 3.3V from Raspi.
   // Ground encoders with Raspi's ground.

   set_pull_up_down(pi, gpioA, PI_PUD_UP);
   set_pull_up_down(pi, gpioB, PI_PUD_UP);

   /* monitor encoder level changes */

   cba_id = callback_ex(pi, gpioA, EITHER_EDGE, _pulseEx, this);
   cbb_id = callback_ex(pi, gpioB, EITHER_EDGE, _pulseEx, this);
}

void re_decoder::re_cancel(void)
{
   callback_cancel(cba_id);
   callback_cancel(cbb_id);
}
#ifndef ROTARY_ENCODER_HPP
#define ROTARY_ENCODER_HPP

#include <stdint.h>

typedef void (*re_decoderCB_t)(int);

class re_decoder
{
   int cba_id, cbb_id, mypi, mygpioA, mygpioB, levA, levB, lastGpio;

   re_decoderCB_t mycallback;

   void _pulse(int gpio, int level, uint32_t tick);

   /* Need a static callback to link with C. */
   static void _pulseEx(int pi, unsigned gpio, unsigned level, uint32_t tick, void *user);


   public:

   re_decoder(int pi, int gpioA, int gpioB, re_decoderCB_t callback);
   /*
      This function establishes a rotary encoder on gpioA and gpioB.

      When the encoder is turned the callback function is called.
   */

   void re_cancel(void);
   /*
      This function releases the resources used by the decoder.
   */
};
#endif
$\endgroup$

1 Answer 1

0
$\begingroup$

Are you familiar with the ros2_control hardware components? This example is a good way to start.

Then you have to include the header file rotary_encoder.hpp and add the cpp file to the library here

add_library(
  ros2_control_demo_example_2
  SHARED
  hardware/diffbot_system.cpp
  hardware/rotary_encoder.cpp
)
$\endgroup$
2
  • $\begingroup$ Thank you, that is helpful. I'm having trouble getting the encoder values out of the callback function in the test_rotary_encoder.cpp. "pos" there holds the values I want, but they are not accessible outside this function. Apparently this callback style is a mixture of C and C++? The desired values only print to screen in the callback as is. $\endgroup$
    – Russ76
    Dec 29, 2023 at 3:41
  • $\begingroup$ Make the callback a class method of your hardware_component and update a class member variable pos, which you can then use in the read() method. $\endgroup$ Dec 29, 2023 at 9:21

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.