ROS與Arduino學習（八）電機控制(基於rosserial_arduino)
Tutorial Level:小案例節點通信
Next Tutorial：ros_arduino_brige固件
Tips 1 Arduino上實現ROS Node，訂閱Twist msg
a.首先需要包含ros的頭文件
#include <PID_v1.h> #include <ArduinoHardware.h> #include <ros.h> #include <geometry_msgs/Twist.h> #include <ros/time.h> #include <tf/transform_broadcaster.h> #include <nav_msgs/Odometry.h>
b. 在聲明部分什么node句柄
ros::NodeHandle nh;
c.定義收到Twist msg后的處理函數
void motor_cb(const geometry_msgs::Twist& vel)
{
linear = vel.linear.x * 100; //ROS中的單位是m/s;這里換算成cm的單位，在Diego機器人中使用CM作為單位
angular = vel.angular.z;
}
ros::Subscriber<geometry_msgs::Twist> sub("/turtle1/cmd_vel", motor_cb);/////這里先暫時訂閱Turtle1 package的Twist消息，后面根據自己的需要可以修改
d .在loog中執行
nh.spinOnce();
到這里Arduino已經可以作為一個Node的節點接收上位機的Twist msg了
Tips 2 底盤驅動及PID控制
a. 引腳定義
底盤馬達驅動采用了L298P模塊
#define E_left 5 //L298P直流電機驅動板的左輪電機使能端口連接到數字接口5 #define M_left 4 //L298P直流電機驅動板的左輪電機轉向端口連接到數字接口4 #define E_right 6 //連接小車右輪電機的使能端口到數字接口6 #define M_right 7 //連接小車右輪電機的轉向端口到數字接口7
電機馬達碼盤中斷，
#define Pin_left 2 //外部中斷0,左輪 #define Pin_right 3 //外部中斷1,右輪
b.底盤前進控制
void advance()//前進
{
digitalWrite(M_left, HIGH);
analogWrite(E_left, val_left);
digitalWrite(M_right, HIGH);
analogWrite(E_right, val_right);
}
void back()//后退
{
digitalWrite(M_left, LOW);
analogWrite(E_left, val_left);
digitalWrite(M_right, LOW);
analogWrite(E_right, val_right);
}
void Stop()//停止
{
digitalWrite(E_right, LOW);
digitalWrite(E_left, LOW);
}
c.PID控制
采用Ardunio的PID控制包Arduino-PID-Library https://github.com/br3ttb/Arduino-PID-Library/
由於需要分別對兩個馬達控制所以需要分別設定兩個馬達的PID控制參數
//////PID double left_Setpoint, left_Input, left_Output, left_setpoint; double left_kp = 1, left_ki = 0.005, left_kd = 0.0001; PID left_PID(&left_Input, &left_Output, &left_Setpoint, left_kp, left_ki, left_kd, DIRECT); double right_Setpoint, right_Input, right_Output, right_setpoint; double right_kp = 0.8, right_ki = 0.005, right_kd = 0.0021; PID right_PID(&right_Input, &right_Output, &right_Setpoint, right_kp, right_ki, right_kd, DIRECT);
即使相同型號的電機，其PID的調節參數都可能不一樣，需要單獨調節，需要反復測試調節，相關調節方法可以到百度
#include <PID_v1.h>
//#include <ArduinoHardware.h>
#include <ros.h>
#include <geometry_msgs/Twist.h>
#include <ros/time.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#define Pin_left 2 //外部中斷0,左輪
#define Pin_right 3 //外部中斷1,右輪
#define max_linear 20 //最大線速度cm/秒
#define max_turn_line 18 //最大轉彎線速度
//#define max_angular 1.45
#define max_linear_pwd 255
#define hole_number 2 //碼盤孔數
#define diameter 18.535 //輪cm直徑
#define diamete_ratio 1.12167 //左輪相對於右輪輪徑比系數,往左偏,調小,往右偏調大
#define center_speed 220 //小車電機的PWM功率初始值
#define gear_ratio 75 //轉速比
#define car_width 27 //小車寬度
#define car_length 27 //小車長度
#define E_left 5 //L298P直流電機驅動板的左輪電機使能端口連接到數字接口5
#define M_left 4 //L298P直流電機驅動板的左輪電機轉向端口連接到數字接口4
#define E_right 6 //連接小車右輪電機的使能端口到數字接口6
#define M_right 7 //連接小車右輪電機的轉向端口到數字接口7
int val_right_count_target = 0; //小車右輪碼盤每秒計數PID調節目標值,根據這個值PID val_rigth;
int val_right = 0; //小車右輪電機的PWM功率值
int val_left_count_target = 0; //小車左輪碼盤每秒計數PID調節目標值,根據這個值PID val_left;
int val_left = 0; //左輪電機PWM功率值。以左輪為基速度,PID調節右輪的速度
int count_left = 0; //左輪編碼器碼盤脈沖計數值;用於PID調整
int count_right = 0; //右輪編碼器碼盤脈沖計數值;用於PID調整
/////////
char run_direction = 'f'; //f:前進;b:后退;s:stop
int linear = 0;//15; //cm/second線速度
int angular = 0;//1; //角速度,ros的angular.z
///轉彎半徑一定要大於小車寬度的一半,也就是linear / angular一定是大於13.5,也就是最小轉彎半徑是13.5
/////////
unsigned long left_old_time = 0, right_old_time = 0; // 時間標記
unsigned long time1 = 0, time2 = 0; // 時間標記
////ros
ros::NodeHandle nh;
//geometry_msgs::TransformStamped t;
//tf::TransformBroadcaster broadcaster;
//char base_link[] = "/base_link";
//char odom[] = "/odom";
//nav_msgs::Odometry odom1;
void motor_cb(const geometry_msgs::Twist& vel)
{
linear = vel.linear.x * 100; //ROS中的單位是m/s;這里換算成cm的單位
angular = vel.angular.z;
}
ros::Subscriber<geometry_msgs::Twist> sub("/turtle1/cmd_vel", motor_cb);
//////PID
double left_Setpoint, left_Input, left_Output, left_setpoint;
double left_kp = 1, left_ki = 0.005, left_kd = 0.0001; //kp = 0.040,ki = 0.0005,kd =0.0011;
PID left_PID(&left_Input, &left_Output, &left_Setpoint, left_kp, left_ki, left_kd, DIRECT);
double right_Setpoint, right_Input, right_Output, right_setpoint;
double right_kp = 0.8, right_ki = 0.005, right_kd = 0.0021; //kp = 0.040,ki = 0.0005,kd =0.0011;
PID right_PID(&right_Input, &right_Output, &right_Setpoint, right_kp, right_ki, right_kd, DIRECT);
void setup() {
// put your setup code here, to run once:
Serial.begin(9600); // 啟動串口通信，波特率為9600b/s
// reserve 200 bytes for the inputString
pinMode(M_left, OUTPUT); //L298P直流電機驅動板的控制端口設置為輸出模式
pinMode(E_left, OUTPUT);
pinMode(M_right, OUTPUT);
pinMode(E_right, OUTPUT);
//定義外部中斷0和1的中斷子程序Code(),中斷觸發為下跳沿觸發
//當編碼器碼盤的OUT脈沖信號發生下跳沿中斷時，
//將自動調用執行中斷子程序Code()。
left_old_time = millis();
right_old_time = millis();
attachInterrupt(0, Code1, FALLING);//小車左車輪電機的編碼器脈沖中斷函數
attachInterrupt(1, Code2, FALLING);//小車右車輪電機的編碼器脈沖中斷函數
nh.initNode();
nh.subscribe(sub);
//broadcaster.init(nh);
left_PID.SetOutputLimits(-254, 254);
left_PID.SetSampleTime(500);
left_PID.SetMode(AUTOMATIC);
left_PID.SetTunings(left_kp, left_ki, left_kd);
right_PID.SetOutputLimits(-254, 254);
right_PID.SetSampleTime(500);
right_PID.SetMode(AUTOMATIC);
right_PID.SetTunings(right_kp, right_ki, right_kd);
}
//子程序程序段
void advance()//前進
{
digitalWrite(M_left, HIGH);
analogWrite(E_left, val_left);
digitalWrite(M_right, HIGH);
analogWrite(E_right, val_right);
}
void back()//后退
{
digitalWrite(M_left, LOW);
analogWrite(E_left, val_left);
digitalWrite(M_right, LOW);
analogWrite(E_right, val_right);
}
void Stop()//停止
{
digitalWrite(E_right, LOW);
digitalWrite(E_left, LOW);
}
void loop() {
nh.spinOnce();
// put your main code here, to run repeatedly:
if (angular == 0) { //直行
if (linear > 0) { //前進
Serial.println("Go Forward!\n");
if (linear > max_linear)
linear = max_linear;
float linear_left = linear; //左內圈線速度
float linear_right = linear; //右外圈線速度
val_right_count_target = linear_right * gear_ratio / (diameter / hole_number); //左內圈線速度對應的孔數
val_left_count_target = linear_left * gear_ratio / (diameter * diamete_ratio / hole_number); //右外圈線速度對應的孔數
val_right = linear_right * (max_linear_pwd / max_linear); //根據輪徑參數計算出來的線速度對應的PWD值,左輪
val_left = linear_left * (max_linear_pwd / max_linear); //根據輪徑參數計算出來的線速度對應的PWD值,右
left_Setpoint = val_left_count_target;
right_Setpoint = val_right_count_target;
advance();
run_direction = 'f';
} else if (linear < 0) { //后退
Serial.println("Go Backward!\n");
linear = abs(linear);
if (linear > max_linear)
linear = max_linear;
float linear_left = linear; //左內圈線速度
float linear_right = linear; //右外圈線速度
val_right_count_target = linear_right * gear_ratio / (diameter * diamete_ratio / hole_number); //左內圈線速度對應的孔數
val_left_count_target = linear_left * gear_ratio / (diameter / hole_number); //右外圈線速度對應的孔數
val_right = linear_right * (max_linear_pwd / max_linear); //根據輪徑參數計算出來的線速度對應的PWD值,左輪
val_left = linear_left * (max_linear_pwd / max_linear); //根據輪徑參數計算出來的線速度對應的PWD值,右輪
left_Setpoint = val_left_count_target;
right_Setpoint = val_right_count_target;
back();
run_direction = 'b';
}
} else if (angular > 0) { //左轉
Serial.println("Turn Left!\n");
if (linear > max_turn_line) //////限制最大轉彎線速度
{
angular = angular * max_turn_line / linear;
linear = max_turn_line;
} else if (linear == 0) {
linear = max_turn_line;
}
float radius = linear / angular; //計算半徑
if (radius < car_width / 2) ///////如果計算的轉彎半徑小於最小半徑,則設置為最小轉彎半徑
radius = car_width / 2;
float radius_left = radius - car_width / 2; //左內圈半徑
float radius_right = radius + car_width / 2; //右外圈半徑
float linear_left = radius_left * angular; //左內圈線速度
float linear_right = radius_right * angular; //右外圈線速度
if (linear == max_turn_line) {
linear_left = 255 * (linear_left / linear_right);
linear_right = 255;
}
val_right_count_target = linear_right * gear_ratio / (diameter / hole_number); //左內圈線速度對應的孔數
val_left_count_target = linear_left * gear_ratio / (diameter * diamete_ratio / hole_number); //右外圈線速度對應的孔數
val_right = linear_right * (max_linear_pwd / max_linear); //根據輪徑參數計算出來的線速度對應的PWD值,左輪
val_left = linear_left * (max_linear_pwd / max_linear); //根據輪徑參數計算出來的線速度對應的PWD值,右輪
left_Setpoint = val_left_count_target;
right_Setpoint = val_right_count_target;
run_direction = 'f';
advance();
} else if (angular < 0) { //右轉
Serial.println("Turn Right!");
if (linear > max_turn_line) //////限制最大轉彎線速度
{
angular = angular * max_turn_line / linear;
linear = max_turn_line;
} else if (linear == 0) {
linear = max_turn_line;
}
float radius = linear / angular;
if (radius < car_width / 2) ///////如果計算的轉彎半徑小於最小半徑,則設置為最小轉彎半徑
radius = car_width / 2;
float radius_left = radius + car_width / 2;
float radius_right = radius - car_width / 2;
float linear_left = radius_left * angular;
float linear_right = radius_right * angular;
if (linear == max_turn_line) {
linear_right = 255 * (linear_right / linear_left);
linear_left = 255;
}
val_right_count_target = linear_right * gear_ratio / (diameter / hole_number); //左內圈線速度對應的孔數
val_left_count_target = linear_left * gear_ratio / (diameter * diamete_ratio / hole_number); //右外圈線速度對應的孔數
val_right = linear_right * (max_linear_pwd / max_linear); //根據輪徑參數計算出來的線速度對應的PWD值,左輪
val_left = linear_left * (max_linear_pwd / max_linear); //根據輪徑參數計算出來的線速度對應的PWD值,右輪
left_Setpoint = val_left_count_target;
right_Setpoint = val_right_count_target;
advance();
run_direction = 'f';
}
delay(1000);
val_left_count_target = 0;
left_Setpoint = 0;
val_right_count_target = 0;
right_Setpoint = 0;
linear = 0;
angular = 0;
Stop();
run_direction = 's';
}
void PID_left() {
Serial.println("********************************begin PID left");
left_Input = count_left * 10;
left_PID.Compute();
val_left = val_left + left_Output;
if (val_left > 255)
val_left = 255;
if (val_left < 0)
val_left = 0;
if (run_direction == 'f') //根據剛剛調節后的小車電機PWM功率值，及時修正小車前進或者后退狀態
advance();
if (run_direction == 'b')
back();
Serial.println("********************************end PID Left");
}
void PID_right() {
Serial.println("********************************begin PID Right");
right_Input = count_right * 10;
right_PID.Compute();
val_right = val_right + right_Output;
if (val_right > 255)
val_right = 255;
if (val_right < 0)
val_right = 0;
if (run_direction == 'f') //根據剛剛調節后的小車電機PWM功率值，及時修正小車前進或者后退狀態
advance();
if (run_direction == 'b')
back();
Serial.println("********************************end PID Right");
}
實踐證明ROS_lib是非常占用arduino資源的，如果要訂閱Twist，同時發布TF，Odometry消息則至少需要3k的SRAM, Arduino UNO只能作為接收Twist消息，來控制底盤，如果用rosserial_arduino做到完整的Base Controller就只能上Arduino Mega2560了，這無疑會增加不少成本，所以筆者認為又更好的選擇，那就是使用ros_arduino_bridge作為Base Controller，把邏輯的運算放在上位機上運行，Arduino單純的作為硬件的控制器，在下一篇，將為大家講解如何用ros_arduino_bridge作為base controller。