4WD机器人运动控制MDK工程（实现ROS与STM32F407通信）

关注微信公众号“混沌无形”，后台回复：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
    └─Objects

main函数部分如下

#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;

}

喜欢的话，请关注微信公众号，可阅读更多好文！