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
nh.advertise(encoder_pub);
nh.advertise(pwm_output_pub);
//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.SetSampleTime(95);
PID_rightMotor.SetSampleTime(95);
PID_leftMotor.SetOutputLimits(-max_speed, max_speed);
PID_rightMotor.SetOutputLimits(-max_speed, max_speed);
PID_leftMotor.SetMode(AUTOMATIC);
PID_rightMotor.SetMode(AUTOMATIC);
// Define the rotary encoder for left motor
pinMode(PIN_L_ENCOD_A_MOTOR, INPUT);
pinMode(PIN_L_ENCOD_B_MOTOR, INPUT);
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
pinMode(PIN_R_ENCOD_A_MOTOR, INPUT);
pinMode(PIN_R_ENCOD_B_MOTOR, INPUT);
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() {
nh.spinOnce();
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);
}
noCommLoops++;
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)
speed_pub.publish(&speed_msg);
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;
pwm_output_pub.publish(&pwm_output_msg);
nh.spinOnce();
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.
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