Arduino/Raspi control firmware for differential robot

I recently started working on a robot project, a small ROS2 robot that explores, finds objects, and can be used for my computer vision experiments. I'm using a Raspberry Pi 5 as the brain and an Arduino Nano as the muscle with a TB6612FNG motor controller to drive two motors. I took a course not long ago on writing Arduino "firmware" for motor control, but you needed to know the gear ratio of your motors, which I have never been able to find for these online. I also took some courses on differential drive, which was helpful. I gathered some bits together and made this script, and so far it seems to work ok. I'll know better when I actually get the bot on the floor moving around. It basically takes a facsimile of a "Twist" message from ROS2 running on the Pi and sends it through Serial to the Arduino where the values are processed into motor speeds using the standard PID controller library. One of the weird things about my code is that the angular velocity doesn't register unless multiplied by 10 (maybe my PID values? Not sure). Maybe this is the correct way to do it. Anyway, I thought I'd post the code here and see if there were any comments or suggestions. It probably has some bugs or needs corrections. Right now I've only tested through hard coding the Twist values. I think it's commented well enough. Let me know what you think if you have anything.

/*
 * BLUEBOT: A ROS2 Differential Drive Exploration Robot 
 * By Jeffrey Jene
 * Version 1.0
 * 2/27/2025
 * jeffreyjene@gmail.com
 * 
 */

#define LOOPTIME 10
#include <PID_v1.h>

//Motor driver TB6612FNG H-Bridge
//Motor Left
int pwmL = 5;
int l_in1 = 7;
int l_in2 = 8;

//Motor Right
int pwmR = 6;
int r_in1 = 9;
int r_in2 = 10;

//Wheel Encoders Connection PINs and counter vars
#define right_encoder_A 3  // Interrupt 
#define right_encoder_B 12  
#define left_encoder_A 2   // Interrupt
#define left_encoder_B 11
unsigned int right_encoder_counter = 0;
unsigned int left_encoder_counter = 0;
String right_wheel_sign = "p";  // 'p' = positive, 'n' = negative
String left_wheel_sign = "p";  // 'p' = positive, 'n' = negative

unsigned long currentMillis;
unsigned long prevMillis;

//PID values
double left_kp = 3.8 , left_ki = 0 , left_kd = 0.0;             // modify for optimal performance
double right_kp = 4 , right_ki = 0 , right_kd = 0.0;

double right_input = 0, right_output = 0, right_setpoint = 0;
PID rightPID(&right_input, &right_output, &right_setpoint, right_kp, right_ki, right_kd, DIRECT);  

double left_input = 0, left_output = 0, left_setpoint = 0;
PID leftPID(&left_input, &left_output, &left_setpoint, left_kp, left_ki, left_kd, DIRECT);  

//values come from message in Serial
float demandx = 0; //linear velocity
float demandz = 0; //angular velocity

//Vars for figuring out error for PID
float left_encoder_error;
float right_encoder_error;

float left_encoder_prev;
float right_encoder_prev;

//Wheel and hardware info. A couple of commented values are for my reference
float wheel_radius = 0.033;
//float wheel_diameter = 0.066;
//float wheel_circumference = wheel_diameter * PI;
int pulses_per_rotation = 492; //Encoder resolution
//float distance_per_pulse = wheel_circumference/pulses_per_rotation;
float wheelbase = 0.185; //distance between wheel centers in meters

void setup() {

  Serial.begin(115200);
  
  pinMode(pwmL, OUTPUT);
  pinMode(pwmR, OUTPUT);
  pinMode(l_in1, OUTPUT);
  pinMode(l_in2, OUTPUT);
  pinMode(r_in1, OUTPUT);
  pinMode(r_in2, OUTPUT);

  //Initialize motors
  digitalWrite(l_in1, HIGH);
  digitalWrite(l_in2, LOW);
  analogWrite(pwmL, 0);

  digitalWrite(r_in1, HIGH);
  digitalWrite(r_in2, LOW);
  analogWrite(pwmR, 0);

  //Init PID
  rightPID.SetMode(AUTOMATIC);
  rightPID.SetSampleTime(1);
  rightPID.SetOutputLimits(-100, 100);

  leftPID.SetMode(AUTOMATIC);
  leftPID.SetSampleTime(1);
  leftPID.SetOutputLimits(-100, 100);

  // Init encoders
  pinMode(right_encoder_B, INPUT);
  pinMode(left_encoder_B, INPUT);
  // Set Callback for Wheel Encoders Pulse
  attachInterrupt(digitalPinToInterrupt(right_encoder_A), rightEncoderCallback, RISING);
  attachInterrupt(digitalPinToInterrupt(left_encoder_A), leftEncoderCallback, RISING);
}

void loop() {

  //For testing, fake the TWIST input
  //demandx = 0.0; //linear velocity
  //demandz = 0.0; //angular velocity

  //Serial velocity message from Raspberry Pi
  //Format of Serial message for ROS "Twist" will be T + linear + , + angular + line break
  //Example: T0.843,-0.7\n makes linear=0.83 and angular=-0.7
  //The two values will always be between -1 and 1
  if (Serial.available()){
    String cmd = Serial.readStringUntil('\n');
    if (cmd.startsWith("T")){
      cmd.trim(); //remove line breaks and trailing spaces
      cmd.remove(0,1); // chop off the opening T
      //Split the values at the comma
      String dx = cmd.substring(0, cmd.indexOf(","));
      String dz = cmd.substring(cmd.indexOf(",")+1);
      demandx = dx.toFloat();
      demandz = dz.toFloat();
    }
    
  }

  //The loop (avoids using delay())
  currentMillis = millis();
  if (currentMillis - prevMillis >= LOOPTIME){
    
    prevMillis = currentMillis;
    
    //Calculate wheel velocities
    double right_vel = demandx + ((demandz*wheelbase)/2);
    double left_vel = demandx - ((demandz*wheelbase)/2);
  
    //Calculate rotational speeds
    double right_rot_spd = (right_vel/wheel_radius) * 10; //for some reason I have to multiply by 10 to get proper velocitiy
    double left_rot_spd = (left_vel/wheel_radius) * 10;
  
    //Calculate pulses per loop cycle
    int pulses_right = right_rot_spd * (pulses_per_rotation/(2*PI)) * 0.01; // multiply by 10ms LOOPTIME
    int pulses_left = left_rot_spd * (pulses_per_rotation/(2*PI)) * 0.01; // multiply by 10ms LOOPTIME

    //PID inputs
    left_setpoint = pulses_left;
    right_setpoint = pulses_right;

    left_encoder_error = left_encoder_counter - left_encoder_prev;
    right_encoder_error = right_encoder_counter - right_encoder_prev;
    
    //save the encoder values for next loop
    left_encoder_prev = left_encoder_counter;
    right_encoder_prev = right_encoder_counter;

    left_input = left_encoder_error;
    right_input = right_encoder_error;

    //TODO: Here I will write to Serial to send current encoder readings to ROS2 on the Raspberry Pi 5
    //Serial.println...

    //reset the encoders
    left_encoder_counter = 0;
    right_encoder_counter = 0;

    leftPID.Compute();
    rightPID.Compute();

    //Here we set the direction of the motors based on if the output values are negative or positive
    if (left_output < 0){
      digitalWrite(l_in1, LOW);
      digitalWrite(l_in2, HIGH);
    }
    else{
      digitalWrite(l_in1, HIGH);
      digitalWrite(l_in2, LOW);
    }

    if (right_output < 0){
      digitalWrite(r_in1, LOW);
      digitalWrite(r_in2, HIGH);
    }
    else{
      digitalWrite(r_in1, HIGH);
      digitalWrite(r_in2, LOW);
    }

    //Write out the motor speeds (absolute values)
    analogWrite(pwmL, abs(left_output));
    analogWrite(pwmR, abs(right_output));

    
  }

}

// New pulse from Right Wheel Encoder
void rightEncoderCallback()
{
  if(digitalRead(right_encoder_B) == HIGH)
  {
    right_wheel_sign = "n"; //Backward wheel direction indicator to send in Serial
    right_encoder_counter--;
  }
  else
  {
    right_wheel_sign = "p"; //Forward wheel direction indicator to send in Serial
    right_encoder_counter++;
  }
  
}

// New pulse from Left Wheel Encoder
void leftEncoderCallback()
{
  if(digitalRead(left_encoder_B) == HIGH)
  {
    left_wheel_sign = "n"; //Backward wheel direction indicator to send in Serial
    left_encoder_counter--;
  }
  else
  {
    left_wheel_sign = "p"; //Forward wheel direction indicator to send in Serial
    left_encoder_counter++;
  }
  
}