本文博客为转载，并对原博客部分错误进行更改 远程仓库地址：github 官方提供的ROS_arduino_bridge是利用一套PID控制两个马达，但是由于马达问题，机器人不能走直线，因此就需要对两个马达进行单独的PID控制，本文修改ROS_Arduino_bridge以实现两路PID控制
1.首先修改arduino代码
a.修改diff_controller.h文件
增加左右两个马达的PID控制变量：
/* PID Parameters */
int Kp = 20;
int Kd = 12;
int Ki = 0;
int Ko = 50;
int left_Kp=Kp;
int left_Kd=Kd;
int left_Ki=Ki;
int left_Ko=Ko;
int right_Kp=Kp;
int right_Kd=Kd;
int right_Ki=Ki;
int right_Ko=Ko;
分别定义左右两个马达的dorightPID()和doleftPID()函数
/* PID routine to compute the next motor commands */
void dorightID(SetPointInfo * p) {
long Perror;
long output;
int input;
//Perror = p->TargetTicksPerFrame - (p->Encoder - p->PrevEnc);
input = p->Encoder - p->PrevEnc;
Perror = p->TargetTicksPerFrame - input;
/*
* Avoid derivative kick and allow tuning changes,
* see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-derivative-kick/
* see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/
*/
//output = (Kp * Perror + Kd * (Perror - p->PrevErr) + Ki * p->Ierror) / Ko;
// p->PrevErr = Perror;
output = (right_Kp * Perror - right_Kd * (input - p->PrevInput) + p->ITerm) / right_Ko;
p->PrevEnc = p->Encoder;
output += p->output;
// Accumulate Integral error *or* Limit output.
// Stop accumulating when output saturates
if (output >= MAX_PWM)
output = MAX_PWM;
else if (output <= -MAX_PWM)
output = -MAX_PWM;
else
/*
* allow turning changes, see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/
*/
p->ITerm += Ki * Perror;
p->output = output;
p->PrevInput = input;
}
/* PID routine to compute the next motor commands */
void doleftPID(SetPointInfo * p) {
long Perror;
long output;
int input;
//Perror = p->TargetTicksPerFrame - (p->Encoder - p->PrevEnc);
input = p->Encoder - p->PrevEnc;
Perror =p->TargetTicksPerFrame + input;
/*
* Avoid derivative kick and allow tuning changes,
* see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-derivative-kick/
* see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/
*/
//output = (Kp * Perror + Kd * (Perror - p->PrevErr) + Ki * p->Ierror) / Ko;
// p->PrevErr = Perror;
output = (left_Kp * Perror - left_Kd * (input - p->PrevInput) + p->ITerm) / left_Ko;
p->PrevEnc = p->Encoder;
output += p->output;
// Accumulate Integral error *or* Limit output.
// Stop accumulating when output saturates
if (output >= MAX_PWM)
output = MAX_PWM;
else if (output <= -MAX_PWM)
output = -MAX_PWM;
else
/*
* allow turning changes, see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/
*/
p->ITerm += Ki * Perror;
p->output = output;
p->PrevInput = input;
}
修改updatePID()函数
void updatePID() {
/* Read the encoders */
leftPID.Encoder =readEncoder(LEFT);
rightPID.Encoder = readEncoder(RIGHT);
/* If we're not moving there is nothing more to do */
if (!moving){
/*
* Reset PIDs once, to prevent startup spikes,
* see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-initialization/
* PrevInput is considered a good proxy to detect
* whether reset has already happened
*/
if (leftPID.PrevInput != 0 || rightPID.PrevInput != 0) resetPID();
return;
}
/* Compute PID update for each motor */
dorightID(&rightPID);//执行右马达PID
doleftPID(&leftPID);//执行左马达PID
/* Set the motor speeds accordingly */
setMotorSpeeds(leftPID.output, rightPID.output);
}b.修改ROSArduinoBridge.ino文件
修改argv1 和argv2的数组长度
char argv1[32];
char argv2[32];
修改pid_args的声明为如下代码
int pid_args[8];
修改runCommand()函数，修改case UPDATE_PID,把原来的Kp,Kd,Ki,Ko赋值的代码注释掉，修改为如下的代码、
case UPDATE_PID:
while ((str = strtok_r(p, ":", &p)) != '\0') {
pid_args[i] = atoi(str);
i++;
}
// Kp = pid_args[0];
// Kd = pid_args[1];
// Ki = pid_args[2];
// Ko = pid_args[3];
left_Kp = pid_args[0];
left_Kd = pid_args[1];
left_Ki = pid_args[2];
left_Ko = pid_args[3];
right_Kp = pid_args[4];
right_Kd = pid_args[5];
right_Ki = pid_args[6];
right_Ko = pid_args[7];
Serial.println("OK");
break;
现在arduino端已经支持对两个电机使用不同的PID参数调速了
2.修改上位机ROS包的代码
a.修改my_arduino_params.yaml
在PID参数部分增加左右两个马达的PID参数
# === PID parameters
Kp: 10
Kd: 12
Ki: 0
Ko: 50
accel_limit: 1.0
left_Kp: 10
left_Kd: 12
left_Ki: 0
left_Ko: 50
right_Kp: 8
right_Kd: 12
right_Ki: 0
right_Ko: 50
b.修改arduino_driver.py文件
修改update_pid()函数
def update_pid(self, left_Kp, left_Kd, left_Ki, left_Ko, right_Kp, right_Kd, right_Ki, right_Ko):
''' Set the PID parameters on the Arduino
'''
print "Updating PID parameters"
cmd = 'u ' + str(left_Kp) + ':' + str(left_Kd) + ':' + str(left_Ki) + ':' + str(left_Ko) + ':' + str(right_Kp) + ':' + str(right_Kd) + ':' + str(right_Ki) + ':' + str(right_Ko)
self.execute_ack(cmd)
c.修改base_controller.py文件
修改def init(self, arduino, base_frame):函数，增加左右两个马达PID参数的初始化代码
def __init__(self, arduino, base_frame):
self.arduino = arduino
self.base_frame = base_frame
self.rate = float(rospy.get_param("~base_controller_rate", 10))
self.timeout = rospy.get_param("~base_controller_timeout", 1.0)
self.stopped = False
pid_params = dict()
pid_params['wheel_diameter'] = rospy.get_param("~wheel_diameter", "")
pid_params['wheel_track'] = rospy.get_param("~wheel_track", "")
pid_params['encoder_resolution'] = rospy.get_param("~encoder_resolution", "")
pid_params['gear_reduction'] = rospy.get_param("~gear_reduction", 1.0)
pid_params['left_Kp'] = rospy.get_param("~left_Kp", 20)
pid_params['left_Kd'] = rospy.get_param("~left_Kd", 12)
pid_params['left_Ki'] = rospy.get_param("~left_Ki", 0)
pid_params['left_Ko'] = rospy.get_param("~left_Ko", 50)
pid_params['right_Kp'] = rospy.get_param("~right_Kp", 20)
pid_params['right_Kd'] = rospy.get_param("~right_Kd", 12)
pid_params['right_Ki'] = rospy.get_param("~right_Ki", 0)
pid_params['right_Ko'] = rospy.get_param("~right_Ko", 50)
self.accel_limit = rospy.get_param('~accel_limit', 0.1)
self.motors_reversed = rospy.get_param("~motors_reversed", False)
# Set up PID parameters and check for missing values
self.setup_pid(pid_params)
# How many encoder ticks are there per meter?
self.ticks_per_meter = self.encoder_resolution * self.gear_reduction / (self.wheel_diameter * pi)
# What is the maximum acceleration we will tolerate when changing wheel speeds?
self.max_accel = self.accel_limit * self.ticks_per_meter / self.rate
# Track how often we get a bad encoder count (if any)
self.bad_encoder_count = 0
now = rospy.Time.now()
self.then = now # time for determining dx/dy
self.t_delta = rospy.Duration(1.0 / self.rate)
self.t_next = now + self.t_delta
# internal data
self.enc_left = None # encoder readings
self.enc_right = None
self.x = 0 # position in xy plane
self.y = 0
self.th = 0 # rotation in radians
self.v_left = 0
self.v_right = 0
self.v_des_left = 0 # cmd_vel setpoint
self.v_des_right = 0
self.last_cmd_vel = now
# subscriptions
rospy.Subscriber("cmd_vel", Twist, self.cmdVelCallback)
# Clear any old odometry info
self.arduino.reset_encoders()
# Set up the odometry broadcaster
self.odomPub = rospy.Publisher('odom', Odometry)
self.odomBroadcaster = TransformBroadcaster()
rospy.loginfo("Started base controller for a base of " + str(self.wheel_track) + "m wide with " + str(self.encoder_resolution) + " ticks per rev")
rospy.loginfo("Publishing odometry data at: " + str(self.rate) + " Hz using " + str(self.base_frame) + " as base frame")
修改def setup_pid(self, pid_params):
def setup_pid(self, pid_params):
# Check to see if any PID parameters are missing
missing_params = False
for param in pid_params:
if pid_params[param] == "":
print("*** PID Parameter " + param + " is missing. ***")
missing_params = True
if missing_params:
os._exit(1)
self.wheel_diameter = pid_params['wheel_diameter']
self.wheel_track = pid_params['wheel_track']
self.encoder_resolution = pid_params['encoder_resolution']
self.gear_reduction = pid_params['gear_reduction']
#self.Kp = pid_params['Kp']
#self.Kd = pid_params['Kd']
#self.Ki = pid_params['Ki']
#self.Ko = pid_params['Ko']
#self.arduino.update_pid(self.Kp, self.Kd, self.Ki, self.Ko)
#modify by william
self.left_Kp = pid_params['left_Kp']
self.left_Kd = pid_params['left_Kd']
self.left_Ki = pid_params['left_Ki']
self.left_Ko = pid_params['left_Ko']
self.right_Kp = pid_params['right_Kp']
self.right_Kd = pid_params['right_Kd']
self.right_Ki = pid_params['right_Ki']
self.right_Ko = pid_params['right_Ko']
self.arduino.update_pid(self.left_Kp, self.left_Kd, self.left_Ki, self.left_Ko, self.right_Kp, self.right_Kd, self.right_Ki, self.right_Ko)
修改完上述代码后ros_arduino_bridge可以分别对两个马达设置不同的PID参数进行调速。
