Speed control problem

We are designing a differential drive robot and we have 2 same model and brande dc motor. Pololu HP 12V 350RPM 37mm Metal Gearboxes DC Motor (30:1). These two motor are not turning with same speed at the same pulse width modulation. Also we implemented PID.
How can we control 2 dc motor with encoders same speed?

Motor driver : L298N

Motors of the same model and brand have slight differences, and will not rotate at the same RPM when the same voltage and load is applied.

To have them rotate at the same RPM, each motor will need its own individually calibrated speed control. If you have encoders, then that is straightforward.

If the goal is to drive the vehicle in a straight line, then it is easier to use differential speed control, and one PID loop. Simply measure the deviation from the straight line (a magnetic compass will work for this), and change one or both motor speeds to compensate.

Our aim is not only drive straight line also I can publish our arduino code. My motors have encoders and I applied PID however motors are not still same speed. I was able to equalize their speed in no-load state, but their speed changes when the load is loaded and the PID doesn’t work

    #include <Wire.h>
    #include <PID_v1.h>
    #include <ros.h>
    #include <std_msgs/String.h>
    #include <geometry_msgs/Vector3Stamped.h>
    #include <geometry_msgs/Twist.h>
    #include <geometry_msgs/Pose2D.h>
    #include <ros/time.h>
    #include <ArduinoHardware.h>
    #include <std_msgs/Float64.h>

    //initializing all the variables
    #define LOOPTIME                      100     //Looptime in millisecond
    const byte noCommLoopMax = 10;                //number of main loops the robot will execute without communication before stopping
    unsigned int noCommLoops = 0;                 //main loop without communication counter

    const int PIN_L_IN1 = 24;
    const int PIN_L_IN2 = 25;
    const int PIN_L_PWM = 6;
    const int PIN_L_ENCOD_A_MOTOR = 2;               //A channel for encoder of left motor
    const int PIN_L_ENCOD_B_MOTOR = 4;               //B channel for encoder of left motor

    const int PIN_R_IN1 = 30;
    const int PIN_R_IN2 = 31;
    const int PIN_R_PWM = 7;
    const int PIN_R_ENCOD_A_MOTOR = 3;              //A channel for encoder of right motor
    const int PIN_R_ENCOD_B_MOTOR = 18;              //B channel for encoder of right motor

    unsigned long lastMilli = 0;
    const double radius = 0.07;                   //Wheel radius, in m
    const double wheelbase = 0.325;               //Wheelbase, in m

    double speed_req = 0;                         //Desired linear speed for the robot, in m/s
    double angular_speed_req = 0;                 //Desired angular speed for the robot, in rad/s

    double speed_req_left = 0;                    //Desired speed for left wheel in m/s
    double speed_act_left = 0;                    //Actual speed for left wheel in m/s
    double speed_cmd_left = 0;                    //Command speed for left wheel in m/s

    double speed_req_right = 0;                   //Desired speed for right wheel in m/s
    double speed_act_right = 0;                   //Actual speed for right wheel in m/s
    double speed_cmd_right = 0;                   //Command speed for right wheel in m/s

    const double max_speed = 0.4;                 //Max speed in m/s

    int PWM_leftMotor = 0;                     //PWM command for left motor
    int PWM_rightMotor = 0;                    //PWM command for right motor

    unsigned int encoder_left = 0;
    unsigned int encoder_right = 0;

    // PID Parameters
    const double PID_left_param[] = { 0.29, 0, 0.0 }; //Respectively Kp, Ki and Kd for left motor PID
    const double PID_right_param[] = { 0.262, 0, 0.0 }; //Respectively Kp, Ki and Kd for right motor PID

    volatile float pos_left = 0;       //Left motor encoder position
    volatile float pos_right = 0;      //Right motor encoder position

    PID PID_leftMotor(&speed_act_left, &speed_cmd_left, &speed_req_left, PID_left_param[0], PID_left_param[1], PID_left_param[2], DIRECT);          //Setting up the PID for left motor
    PID PID_rightMotor(&speed_act_right, &speed_cmd_right, &speed_req_right, PID_right_param[0], PID_right_param[1], PID_right_param[2], DIRECT);   //Setting up the PID for right motor

    ros::NodeHandle nh;

    //function that will be called when receiving command from host
    void handle_cmd (const geometry_msgs::Twist& cmd_vel) {
    noCommLoops = 0;                                                  //Reset the counter for number of main loops without communication

    speed_req = cmd_vel.linear.x;                                     //Extract the commanded linear speed from the message

    angular_speed_req = cmd_vel.angular.z;                            //Extract the commanded angular speed from the message

    speed_req_left  = speed_req - angular_speed_req * (wheelbase / 2); //Calculate the required speed for the left motor to comply with commanded linear and angular speeds
    speed_req_right = speed_req + angular_speed_req * (wheelbase / 2); //Calculate the required speed for the right motor to comply with commanded linear and angular speeds
    geometry_msgs::Pose2D pwm_output_msg; 
    geometry_msgs::Pose2D encoder_msg; 
    geometry_msgs::Vector3Stamped speed_msg;                                //create a "speed_msg" ROS message

    ros::Subscriber<geometry_msgs::Twist> cmd_vel("cmd_vel", handle_cmd);   //create a subscriber to ROS topic for velocity commands (will execute "handle_cmd" function when receiving data)

    ros::Publisher speed_pub("speed", &speed_msg);                          //create a publisher to ROS topic "speed" using the "speed_msg" type
    ros::Publisher pwm_output_pub("pwm_output", &pwm_output_msg);
    ros::Publisher encoder_pub("encoder", &encoder_msg);

    void setup() {
    nh.initNode();                            //init ROS node
    nh.getHardware()->setBaud(57600);         //set baud for ROS serial communication
    nh.subscribe(cmd_vel);                    //suscribe to ROS topic for velocity commands
    nh.advertise(speed_pub);                  //prepare to publish speed in ROS topic

    //setting motor speeds to zero
    pinMode(PIN_L_IN1, OUTPUT);
    pinMode(PIN_L_IN2, OUTPUT);
    pinMode(PIN_L_PWM, OUTPUT);

    pinMode(PIN_R_IN1, OUTPUT);
    pinMode(PIN_R_IN2, OUTPUT);
    pinMode(PIN_R_PWM, OUTPUT);

    //setting PID parameters
    PID_leftMotor.SetOutputLimits(-max_speed, max_speed);
    PID_rightMotor.SetOutputLimits(-max_speed, max_speed);

    // Define the rotary encoder for left motor
    digitalWrite(PIN_L_ENCOD_A_MOTOR, HIGH);                // turn on pullup resistor
    digitalWrite(PIN_L_ENCOD_B_MOTOR, HIGH);
    attachInterrupt(0, encoderLeftMotor, RISING);

    // Define the rotary encoder for right motor
    digitalWrite(PIN_R_ENCOD_A_MOTOR, HIGH);                // turn on pullup resistor
    digitalWrite(PIN_R_ENCOD_B_MOTOR, HIGH);
    attachInterrupt(digitalPinToInterrupt(PIN_R_ENCOD_A_MOTOR), encoderRightMotor, RISING);

    void loop() {
    if ((millis() - lastMilli) >= LOOPTIME)
        lastMilli = millis();

        if (abs(pos_left) < 5) {                                                  //Avoid taking in account small disturbances
        speed_act_left = 0;
        else {
        speed_act_left = ((pos_left / 480) * 2 * PI) * (1000 / LOOPTIME) * radius; // calculate speed of left wheel             !!!! 990 encoder total pulse coun

        if (abs(pos_right) < 5) {                                                 //Avoid taking in account small disturbances
        speed_act_right = 0;
        else {
        speed_act_right = ((pos_right / 480) * 2 * PI) * (1000 / LOOPTIME) * radius; // calculate speed of right wheel          !!!! 990 encoder total pulse coun

        encoder_left = encoder_left + pos_left;
        encoder_right = encoder_right + pos_right;

        if(encoder_left>32000) encoder_left = 0; 
        if(encoder_right>32000) encoder_right = 0; 

        pos_left = 0;
        pos_right = 0;

        speed_cmd_left = constrain(speed_cmd_left, -max_speed, max_speed);
        PID_leftMotor.Compute();                                                 // compute PWM value for left motor
        PWM_leftMotor = constrain(((speed_req_left+sgn(speed_req_left)*0.0882)/0.00235) + (speed_cmd_left/0.00235), -255, 255);

        if (noCommLoops >= noCommLoopMax) {                   //Stopping if too much time without command
        analogWrite(PIN_L_PWM, 0);
        else if (speed_req_left == 0) {                       //Stopping
        analogWrite(PIN_L_PWM, 0);
        else if (PWM_leftMotor > 0) {                         //Going forward
        analogWrite(PIN_L_PWM, PWM_leftMotor);
        digitalWrite(PIN_L_IN1, HIGH);
        digitalWrite(PIN_L_IN2, LOW);
        else {                                               //Going backward
        analogWrite(PIN_L_PWM, abs(PWM_leftMotor));
        digitalWrite(PIN_L_IN1, LOW);
        digitalWrite(PIN_L_IN2, HIGH);

        speed_cmd_right = constrain(speed_cmd_right, -max_speed, max_speed);
        PID_rightMotor.Compute();                                                 // compute PWM value for right motor
        PWM_rightMotor = constrain(((speed_req_right+sgn(speed_req_right)*0.0882)/0.00235) + (speed_cmd_right/0.00235), -255, 255); 

        if (noCommLoops >= noCommLoopMax) {                   //Stopping if too much time without command
        analogWrite(PIN_R_PWM, 0);
        else if (speed_req_right == 0) {                      //Stopping
        analogWrite(PIN_R_PWM, 0);
        else if (PWM_rightMotor > 0) {                        //Going forward
        analogWrite(PIN_R_PWM, PWM_rightMotor);
        digitalWrite(PIN_R_IN1, HIGH);
        digitalWrite(PIN_R_IN2, LOW);
        else {                                                //Going backward
        analogWrite(PIN_R_PWM, abs(PWM_rightMotor));
        digitalWrite(PIN_R_IN1, LOW);
        digitalWrite(PIN_R_IN2, HIGH);
        if (noCommLoops == 65535) {
        noCommLoops = noCommLoopMax;

        publishSpeed();   //Publish odometry on ROS topic

    //Publish function for odometry, uses a vector type message to send the data (message type is not meant for that but that's easier than creating a specific message type)
    void publishSpeed() {
    speed_msg.header.stamp = nh.now();      //timestamp for odometry data
    speed_msg.vector.x = speed_act_left;    //left wheel speed (in m/s)
    speed_msg.vector.y = speed_act_right;   //right wheel speed (in m/s)
    speed_msg.vector.z = LOOPTIME / 1000;   //looptime, should be the same as specified in LOOPTIME (in s)

    encoder_msg.x= encoder_left;
    encoder_msg.y= encoder_right;
    encoder_pub.publish( &encoder_msg);

    pwm_output_msg.x = PWM_leftMotor;
    pwm_output_msg.y = PWM_rightMotor;
    nh.loginfo("Publishing odometry");

    //Left motor encoder counter
    void encoderLeftMotor() {
    if (digitalRead(PIN_L_ENCOD_A_MOTOR) == digitalRead(PIN_L_ENCOD_B_MOTOR)) pos_left++;
    else pos_left--;

    //Right motor encoder counter
    void encoderRightMotor() {
    if (digitalRead(PIN_R_ENCOD_A_MOTOR) == digitalRead(PIN_R_ENCOD_B_MOTOR)) pos_right--;
    else pos_right++;

    template <typename T> int sgn(T val) {
    return (T(0) < val) - (val < T(0));

“PID doesn’t work” does not describe the problem in useful way. The code is far too complex for someone else to debug for you.

Start simple.

Get the encoder and PID speed control algorithm working with just one motor, before combining and adding in all the extras. It should take no more than about 20 lines of code.

Thaks for your help and advice. I didnt write whole code. https://github.com/RBinsonB/Nox_robot/blob/master/Arduino/motor_controller_v2_AFMS/motor_controller_v2_AFMS.ino

This code works with the differrent motor brand and model
I did not understand only one line. If you understand the coefficient can you help me ?

PWM_rightMotor = constrain(((speed_req_right+sgn(speed_req_right)*0.0882)/0.00235) + (speed_cmd_right/0.00235), -255, 255);  

speed_cmd_right = PID output for right wheel
speed_req_right = PID setpoint value for right wheel

This code converts to PID output to PWM but I dont understand why to multiply 0.0882 and diveded 0.00235
I would appreciate it if you can understand why the values are like this.

Thank you for help again

I don’t know where those numbers come from either. Leave out that line until you understand the need for a constraint. It has nothing AT ALL to do with PID.

That is exactly the problem with using someone else’s code, made worse by the author’s failure to document their work.

1 Like

Okey I want to ask another question. Our PID control input is m/s and setpoint also m/s. I expect my output of PID m/s. How can I convert m/s to PWM -255 to 255

Once PID is working, convert wheel RPM to forward speed in m/s using the wheel radius.

linear speed (m/s) = (RPM/60)*(2PI)*R, where R = wheel radius in meters.

Note that since those gearmotors have stall current of 5 Amperes, the ancient, inefficient L298 driver will tend to overheat and shut down. PID won’t work in that case, so replace the L298 with a modern, efficient driver, like this one: https://www.pololu.com/product/2997

You are right I will try with another motor driver and I will write the result thank you again