关注微信公众号“混沌无形”，后台回复：13462EE。免费获取完整工程源码！
本文参考STM32F1与ROS的通信工程：https://blog.csdn.net/qq_36349536/article/details/82773064，针对STM32F4的相关寄存器进行修改，实现STM32F407与ROS kinetic实现串口通信 ，message格式遵循ROS消息标准。
并实现4WD机器人驱动控制及轨迹跟踪，其代码包含：GY901陀螺仪数据读取、通过CAN总线控制4个大疆电机M1、电机的PID控制以及4WD机器人的正逆运动学模型。
文件目录为：
C:.
├─BSP
├─CORE
├─DRIVER
│ ├─CAN1
│ ├─gy85
│ ├─Kinematics
│ ├─LED
│ ├─MOTORCONTROLL
│ ├─PID
│ └─WT901C
├─FWLIB
│ ├─inc
│ └─src
├─HARDWARE
│ ├─BEEP
│ ├─KEY
│ └─LED
├─OBJ
├─ROSLIB
│ ├─actionlib
│ ├─actionlib_msgs
│ ├─actionlib_tutorials
│ ├─bond
│ ├─control_msgs
│ ├─diagnostic_msgs
│ ├─driver_base
│ ├─dynamic_reconfigure
│ ├─examples
│ │ ├─ADC
│ │ ├─Blink
│ │ ├─BlinkM
│ │ ├─button_example
│ │ ├─Clapper
│ │ ├─HelloWorld
│ │ ├─IrRanger
│ │ ├─Logging
│ │ ├─Odom
│ │ ├─pubsub
│ │ ├─ServiceClient
│ │ ├─ServiceServer
│ │ ├─ServoControl
│ │ ├─Temperature
│ │ ├─TimeTF
│ │ └─Ultrasound
│ ├─gazebo_msgs
│ ├─geometry_msgs
│ ├─hector_mapping
│ ├─hector_nav_msgs
│ ├─laser_assembler
│ ├─nav_msgs
│ ├─nodelet
│ ├─pcl_msgs
│ ├─polled_camera
│ ├─riki_msgs
│ ├─ros
│ ├─roscpp
│ ├─roscpp_tutorials
│ ├─rosgraph_msgs
│ ├─rospy_tutorials
│ ├─rosserial_arduino
│ ├─rosserial_msgs
│ ├─ros_arduino_msgs
│ ├─sensor_msgs
│ ├─shape_msgs
│ ├─smach_msgs
│ ├─std_msgs
│ ├─std_srvs
│ ├─stereo_msgs
│ ├─tests
│ │ ├─array_test
│ │ ├─float64_test
│ │ └─time_test
│ ├─tf
│ ├─tf2_msgs
│ ├─theora_image_transport
│ ├─topic_tools
│ ├─trajectory_msgs
│ ├─turtlesim
│ ├─turtle_actionlib
│ └─visualization_msgs
├─SYSTEM
│ ├─delay
│ ├─sys
│ └─usart
└─USER
├─Listings
└─Objectsmain函数部分如下
#include <stdio.h>
#include "hardwareserial.h"
#include "millisecondtimer.h"
#include <ros.h>
#include "std_msgs/String.h"
#include "std_msgs/Float64.h"
#include "sstream"
#include <geometry_msgs/Vector3.h>
#include <geometry_msgs/Quaternion.h>
#include <ros/time.h>
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/Imu.h>
#include "led.h"
#include "Kinematics.h"
#include "wt901.h"
#include "can1.h"
#include "motorcontroll.h"
#include "PID.h"
Led led0(LED0);
Led led1(LED1);
Kinematics car(MAX_RPM, WHEEL_DIAMETER, BASE_WIDTH,X_COEFFICIENT_CAR, MOTORGEARRATIO);
PID motor1SpdPID(-16384, 16384, K_P, K_I, K_D);
PID motor2SpdPID(-16384, 16384, K_P, K_I, K_D);
PID motor3SpdPID(-16384, 16384, K_P, K_I, K_D);
PID motor4SpdPID(-16384, 16384, K_P, K_I, K_D);
//更新IMU信息
sensor_msgs::Imu updateIMUMsg()
{
sensor_msgs::Imu wt901Msg;
wt901Msg.angular_velocity.x = getAngularVelocity().angvel0;
wt901Msg.angular_velocity.y = getAngularVelocity().angvel1;
wt901Msg.angular_velocity.z = getAngularVelocity().angvel2;
wt901Msg.linear_acceleration.x = getLinearAccleration().linacc0;
wt901Msg.linear_acceleration.y = getLinearAccleration().linacc1;
wt901Msg.linear_acceleration.z = getLinearAccleration().linacc2;
wt901Msg.orientation.w = getOrientation().orien0;
wt901Msg.orientation.x = getOrientation().orien1;
wt901Msg.orientation.y = getOrientation().orien2;
wt901Msg.orientation.z = getOrientation().orien3;
return wt901Msg;
}
ros::NodeHandle nh;
//陀螺仪信息
sensor_msgs::Imu IMUMsg;
//发布IMU信息
ros::Publisher imu_msg_pub("imu", &IMUMsg);
//定义几何中心速度信息
float vx = 0;
float wz = 0;
uint32_t previous_command_time = 0;
//**********
geometry_msgs::Twist viewVel;
void vel_callback( const geometry_msgs::Twist& cmd_msg)
{
wz = cmd_msg.angular.z;
vx = cmd_msg.linear.x;
viewVel = cmd_msg;
}
ros::Subscriber<geometry_msgs::Twist> vel_cmd_sub("cmd_vel", &vel_callback);
//*************查看电机速度-临时使用
geometry_msgs::Quaternion mspd;
ros::Publisher mspd_msg_pub("mspd", &mspd);
ros::Publisher vel_msg_pub("viewVelCmd", &viewVel);
int main(void)
{
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2); //很有必要性
uint32_t previous_control_time = 0;
uint32_t previous_led_time = 0;
uint32_t previous_imu_time = 0;
uint32_t previous_motor_time = 0;
bool OnOff = true;
SystemInit(); //系统时钟初始化
initialise(); //延时函数初始化*/
CAN1_Configuration();
led0.init();
led1.init();
wt901Init(9600);
nh.initNode(); //初始化
while (!nh.connected()) //等待连接
{
nh.spinOnce();
}
nh.loginfo("robot_Star Connected!"); //打印连接成功
delay(10);
nh.advertise(imu_msg_pub);
nh.advertise(mspd_msg_pub);
nh.advertise(vel_msg_pub);
nh.subscribe(vel_cmd_sub);
delay(10);
while(1)
{
//速度控制
if( (millis() - previous_control_time) >= (1000/PID_RATE) )
{
Kinematics::motorSpd spd = car.VelToMotorSpd(vx, 0 ,wz);
int M1Spd = int(motor1SpdPID.compute(spd.m_left_front, readMotorSpd(1)));
int M2Spd = int(motor1SpdPID.compute(spd.m_right_front, readMotorSpd(2)));
int M3Spd = int(motor1SpdPID.compute(spd.m_left_back, readMotorSpd(3)));
int M4Spd = int(motor1SpdPID.compute(spd.m_right_back, readMotorSpd(4)));
Set_CM_Current(M1Spd, M2Spd, M3Spd, M4Spd);
//待改
// mspd.w = M1Spd;
// mspd.x = M2Spd;
// mspd.y = M3Spd;
// mspd.z = M4Spd;
mspd.w = spd.m_left_front;
mspd.x = spd.m_right_front;
mspd.y = spd.m_left_back;
mspd.z = spd.m_right_back;
previous_control_time = millis();
}
//停止电机
// if ((millis() - previous_command_time) >= 2000)
// {
// vx = 0;
// wz = 0;
// Set_CM_Current(0, 0, 0, 0);
// }
//发布陀螺仪信息
if ((millis() - previous_imu_time) >= (1000 / IMU_PUBLISH_RATE))
{
IMUMsg = updateIMUMsg();
imu_msg_pub.publish(&IMUMsg);
previous_imu_time = millis();
}
//发布电机信息---需要改
if ((millis() - previous_motor_time) >= (1000 / VEL_PUBLISH_RATE))
{
mspd_msg_pub.publish(&mspd);
vel_msg_pub.publish(&viewVel);
previous_motor_time = millis();
}
//led闪烁
if ((millis() - previous_led_time) >= (1000 / LED_RATE))
{
led0.on_off(OnOff);
OnOff = !OnOff;
previous_led_time = millis();
}
nh.spinOnce();
}
return 0;
}
喜欢的话，请关注微信公众号，可阅读更多好文！