How do I modify this DC motor encoder code to work with 4 DC motors?

Question

I have been loosely following Articulated Robotics' series on ROS and decided to use his ros_arduino_bridge code to drive my motors over serial to my Arduino Mega 2560 from ROS2. Anyways my robot uses 4-wheel mecanum drive, so the code does need modification.

Everything seems simple enough to change except the encoder code as it utilizes concepts of the Arduino I am not familiar with. I have learnt some since but not 100%. Below are all the main bits I believe to pertain to the encoder code.

In Setup:

void setup() {
...
    //set as inputs
    DDRD &= ~(1<<LEFT_ENC_PIN_A);
    DDRD &= ~(1<<LEFT_ENC_PIN_B);
    DDRC &= ~(1<<RIGHT_ENC_PIN_A);
    DDRC &= ~(1<<RIGHT_ENC_PIN_B);
    
    //enable pull up resistors
    PORTD |= (1<<LEFT_ENC_PIN_A);
    PORTD |= (1<<LEFT_ENC_PIN_B);
    PORTC |= (1<<RIGHT_ENC_PIN_A);
    PORTC |= (1<<RIGHT_ENC_PIN_B);
    
    // tell pin change mask to listen to left encoder pins
    PCMSK2 |= (1 << LEFT_ENC_PIN_A)|(1 << LEFT_ENC_PIN_B);
    // tell pin change mask to listen to right encoder pins
    PCMSK1 |= (1 << RIGHT_ENC_PIN_A)|(1 << RIGHT_ENC_PIN_B);
    
    // enable PCINT1 and PCINT2 interrupt in the general interrupt mask
    PCICR |= (1 << PCIE1) | (1 << PCIE2);
...
}

In econder_driver.h:

...
  //below can be changed, but should be PORTD pins; 
  //otherwise additional changes in the code are required
  #define LEFT_ENC_PIN_A PD2  //pin 2
  #define LEFT_ENC_PIN_B PD3  //pin 3
  
  //below can be changed, but should be PORTC pins
  #define RIGHT_ENC_PIN_A PC4  //pin A4
  #define RIGHT_ENC_PIN_B PC5   //pin A5
...

In encoder_driver.ino:

  volatile long left_enc_pos = 0L;
  volatile long right_enc_pos = 0L;
  static const int8_t ENC_STATES [] = {0,1,-1,0,-1,0,0,1,1,0,0,-1,0,-1,1,0};  //encoder lookup table
    
  /* Interrupt routine for LEFT encoder, taking care of actual counting */
  ISR (PCINT2_vect){
    static uint8_t enc_last=0;
        
    enc_last <<=2; //shift previous state two places
    enc_last |= (PIND & (3 << 2)) >> 2; //read the current state into lowest 2 bits
  
    left_enc_pos += ENC_STATES[(enc_last & 0x0f)];
  }
  
  /* Interrupt routine for RIGHT encoder, taking care of actual counting */
  ISR (PCINT1_vect){
        static uint8_t enc_last=0;
            
    enc_last <<=2; //shift previous state two places
    enc_last |= (PINC & (3 << 4)) >> 4; //read the current state into lowest 2 bits
  
    right_enc_pos += ENC_STATES[(enc_last & 0x0f)];
  }
  
  /* Wrap the encoder reading function */
  long readEncoder(int i) {
    if (i == LEFT) return left_enc_pos;
    else return right_enc_pos;
  }
...

I did the monkey brain tactic of just adding two more DDRB and DDRA, and PORTB and PORTA assignments for two other motors and adding two more ICR functions. I am not quite sure on how to do the bit mask and shifting and stuff tho for the functions. Although from what I gathered the Mega might not even have enough interrupt pins for 4 motors. Sadly I cannot actually test this with the actual motors which is an interesting problem right now, so I am hoping someone can help me out all the same.

Answer 1

The ros_arduino_bridge is mainly codded for the Arduino Uno Atmega328p and you can see that from the pins selected and their corresponding PCINT.

ATmega328P microcontroller has three Pin Change Interrupt vectors:

PCINT0_vect: For pins PCINT0-7 (Physical pins 23-30)
PCINT1_vect: For pins PCINT8-14 (Physical pins 1-7)
PCINT2_vect: For pins PCINT16-23 (Physical pins 8-14)

There are a total of 24 pins on the ATmega328P that can trigger pin change interrupts, but they are grouped into these three vectors. You should like the Arduino uno pin out to understand more.

You can always configure more than one pair of pins to trigger encoder changes. it doesn't have to be PD2 and PD3 for example only handled by PCINT2_vect. You can handle other pins in there.

So here is what you can do if you are using Arduino Uno. You can use PCINT0-3 which is grouped in PCINT0_vect and cross pond to pins PB0-4 (pins 8-11 on the uno headers) and connect your two news encoders. (ofc assuming they are not used - if they are you get the concept and can find others).

to set them up

// Set PB0-PB3 as input pins
DDRB &= ~((1 << PB0) | (1 << PB1) | (1 << PB2) | (1 << PB3));

// Enable pin change interrupts for PB0-PB3
PCMSK0 |= (1 << PCINT0) | (1 << PCINT1) | (1 << PCINT2) | (1 << PCINT3);

// Enable pin change interrupt for PCINT0_vect
PCICR |= (1 << PCIE0);

// Enable global interrupts
sei();

and the ISR can be something like this

volatile long left_enc_pos = 0L;
volatile long right_enc_pos = 0L;
// encoder lookup table
static const int8_t ENC_STATES[] = {0, 1, -1, 0, -1, 0, 0, 1, 1, 0, 0, -1, 0, -1, 1, 0}; 

ISR(PCINT0_vect) {
    static uint8_t prev_state = 0;
    uint8_t current_state = PINB & 0x0F; // Read PB0-PB3 pins
    // Update enc_last based on the changes in the state of PB0-PB3
    uint8_t change = current_state ^ prev_state;

    // Check for changes on PB0 and PB1 (Left encoder)
    if (change & ((1 << PB0) | (1 << PB1))) {
        uint8_t state = (current_state >> PB0) & 0x03; // Extract the state of PB0 and PB1
        left_enc_pos += ENC_STATES[state];
    }

    // Check for changes on PB2 and PB3 (Right encoder)
    if (change & ((1 << PB2) | (1 << PB3))) {
        uint8_t state = (current_state >> PB2) & 0x03; // Extract the state of PB2 and PB3
        right_enc_pos += ENC_STATES[state];
    }

    // Store the current state for the next iteration
    prev_state = current_state;
}

if you want to use the Arduino Mega you would have to configure the pins and the PINCT#_Vect used for all 4 encoders. existing ones and new ones as well. you can look at the mega pinout diagrams and easily figure out the numbers. The PCINTs are grouped in the Arduino Mega similar to the UNO just map to different PORTs and PIN numbers

Answer 2

You've probably realized this, but I will point it out anyway-- that since you are using ROS2, you do not need to put all four wheel controls on one board. Your Mega will work, no doubt, but in the context of his tutorials, you are already using the powerful Raspberry Pi4.

The entire point of ROS2 is to have the ability to easily modularize your robot. Just like his tutorial suggests, use small, inexpensive arduino boards (or similar), and put a pair of wheel controllers on each. In your case, use two identical arduino boards, for two sets of wheels. So you now have reliable PID control for each pair of wheels, and you didn't have to write specialized code. (you may need to add some library stuff if you are using a different motor driver, like me).

I am also loosely following Articulated Robotics' series, and I am doing a six wheeled version. I'm also trying to figure out the ros-ardiuno-bridge library on the Uno, with the intent on porting it over to use on the $5 raspberry pi pico boards. So I get 6 wheels of control for less than $15. My Arduino Mega will sit unused in a drawer.