如何利用ROS_Arduino_bridge实现两个马达PID控制？

官方提供的ROS_arduino_bridge是利用两个PID控制两个马达，但由于马达问题，机器人走直线，因此需要一个马达进行单独的PID控制，本文修改ROS_Arduino_bridge以实现两路PID控制

1.首先修改arduino代码

a.修改diff_controller.h文件
增加左右两个马达的PID控制变量：
/* PID 参数 */INT Kp = 20;整数 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;123456789101112131415分别定义左右两个马达的dorightPID()和doleftPID()函数
/* PID 程序来计算下一个电机命令 */void dorightID(SetPointInfo * p) {  长 Perror;  长输出；  整数输入；  //Perror = p->TargetTicksPerFrame - (p->Encoder - p->PrevEnc);  输入 = p->编码器 - p->PrevEnc;  Perror = p->TargetTicksPerFrame - 输入；  /*  * 避免导数踢并允许调整调整，  * 见 http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-derivative-kick/  * 见 http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/  */  //输出 = (Kp * Perror + Kd * (Perror - p->PrevErr) + Ki * p->Ierror) / Ko;  // p->PrevErr = Perror;  输出 = (right_Kp * Perror - right_Kd * (input - p->PrevInput) + p->ITerm) / right_Ko;  p->PrevEnc = p->编码器；  输出 += p-> 输出；  // 累积积分误差 * 或 * 限制输出。  // 当输出饱和时停止累积  如果（输出 >= MAX_PWM）    输出 = MAX_PWM;  否则如果（输出<= -MAX_PWM）    输出 = -MAX_PWM;  别的  /*  * 允许转向更改，参见 http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/  */    p->ITerm += Ki * Perror;  p-> 输出 = 输出；  p->PrevInput = input;}/* PID 程序来计算下一个电机命令 */void doleftPID(SetPointInfo * p) {  长 Perror;  长输出；  整数输入；  //Perror = p->TargetTicksPerFrame - (p->Encoder - p->PrevEnc);  输入 = p->编码器 - p->PrevEnc;  Perror =p->TargetTicksPerFrame + 输入；  /*  * 避免导数踢并允许调整调整，  * 见 http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-derivative-kick/  * 见 http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/  */  //输出 = (Kp * Perror + Kd * (Perror - p->PrevErr) + Ki * p->Ierror) / Ko;  // p->PrevErr = Perror;  输出 = (left_Kp * Perror - left_Kd * (input - p->PrevInput) + p->ITerm) / left_Ko;  p->PrevEnc = p->编码器；  输出 += p-> 输出；  // 累积积分误差 * 或 * 限制输出。  // 当输出饱和时停止累积  如果（输出 >= MAX_PWM）    输出 = MAX_PWM;  否则如果（输出<= -MAX_PWM）    输出 = -MAX_PWM;  别的  /*  * 允许转向更改，参见 http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/  */    p->ITerm += Ki * Perror;  p-> 输出 = 输出；  P-> PrevInput =输入;} 1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465666768697071727374修改updatePID()函数
无效更新PID（）{  /* 读取编码器 */  leftPID.Encoder =readEncoder(LEFT);  rightPID.Encoder = readEncoder(RIGHT);  /* 如果我们不移动，就没有什么可做的 */  如果（！移动）{    /*    * 重置 PID 一次，以防止启动尖峰，    * 见 http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-initialization/    * PrevInput 被认为是一个很好的检测代理    * 重置是否已经发生    */    if (leftPID.PrevInput != 0 || rightPID.PrevInput != 0) resetPID();    返回;  }  /* 计算每个电机的 PID 更新 */  dorightID(&rightPID);//执行右马达PID  doleftPID(&leftPID);//执行左马达PID  /* 相应地设置电机速度 */  setMotorSpeeds(leftPID.output, rightPID.output);}12345678910111213141516171819202122223242526b.修改ROSArduinoBridge.ino文件
修改argv1和argv2的数组长度
字符 argv1[32];字符 argv2[32];12修改pid_args的声明为如下代码
int pid_args[8];1修改runCommand()，修改case UPDATE_PID,把原来的Kp,K,Ki,Ko为调用函数的代码注释掉，修改如下的代码、
案例 UPDATE_PID：      while ((str = strtok_r(p, ":", &p)) != '') {        pid_args = atoi(str);        我++;      }// 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");      打破;12345678910111213141516171819202122现在arduino端已经支持对两个电机使用不同的PID参数调速了
2.修改上位机ROS包的代码

a.修改my_arduino_params.yaml
在PID参数部分增加左右两个马达的PID参数
# === PID 参数Kp: 10Kd: 12Ki: 0Ko: 50accel_limit: 1.0left_Kp: 10left_Kd: 12left_Ki: 0left_Ko: 50right_Kp: 8right_Kd: 12right_Ki: 0right_Ko: 51412161373b.修改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):        '''在Arduino上设置PID参数'''        打印“更新 PID 参数”        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) 1234567c.修改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 = 假        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)        # 设置PID参数并检查缺失值        self.setup_pid(pid_params)        # 每米有多少个编码器滴答声？        self.ticks_per_meter = self.encoder_resolution * self.gear_reduction / (self.wheel_diameter * pi)        # 改变车轮速度时我们能承受的最大加速度是多少？        self.max_accel = self.accel_limit * self.ticks_per_meter / self.rate # 跟踪我们得到错误编码器计数的频率（如果有的话）        self.bad_encoder_count = 0        现在 = rospy.Time.now()            self.then = now # 确定 dx/dy 的时间        self.t_delta = rospy.Duration(1.0 / self.rate)        self.t_next = now + self.t_delta # 内部数据                self.enc_left = None # 编码器读数        self.enc_right = 无        self.x = 0 # 在 xy 平面中的位置        自我.y = 0        self.th = 0 # 弧度旋转        self.v_left = 0        self.v_right = 0        self.v_des_left = 0 # cmd_vel 设定点        self.v_des_right = 0        self.last_cmd_vel = 现在 # 订阅        rospy.Subscriber("cmd_vel", Twist, self.cmdVelCallback)        # 清除所有旧的里程计信息        self.arduino.reset_encoders()        # 设置里程计广播器        self.odomPub = rospy.Publisher('odom', Odometry)        self.odomBroadcaster = TransformBroadcaster()        rospy.loginfo("为" + str(self.wheel_track) + "m 宽" + str(self.encoder_resolution) + " 每转滴答数" 的基础启动基础控制器")        123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566：rospy.loginfo（+ STR（self.rate）+ “赫兹使用” + STR（self.base_frame）+ “作为碱帧” “发布测距在数据”）修改def setup_pid(self, pid_params):
def setup_pid(self, pid_params):        # 检查是否缺少任何PID参数        对于 pid_params 中的参数，missing_params = False：            如果 pid_params[param] == "":                print("*** PID 参数" + param + " 丢失。***")                missing_params = True 如果 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)        #由威廉修改        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,可以分别对两个马达设置不同的PID参数进行调速。