仿pioneer3at的机器人，缺点当负载过大时，会产生跳齿等问题，效果一般电机使用的是富兴公司的伺服电机
控制参考：步进电机与ros通信，做的can与stm32通信，进行轮速的设定和位置的反馈，对反馈的编码值进行处理，得到移动平台的位置姿态，通过STM32的串口反馈给ros.加了遥控器遥控
程序可参考https://www.ncnynl.com/archives/201703/1416.html
将STM32程序下载到STM32上，ROS程序写好。
将STM32线连接到笔记本
roscore
sudo chmod a+rw /dev/ttyUSB0
rosrun base_controller base_controller
rosrun teleop_twist_keyboard teleop_twist_keyboard.py
就可以控制下位机小车移动了
#include "stm32f10x.h"
#include "stm32f10x_it.h"
#include "rcc_conf.h"
#include "led.h"
#include "delay.h"
#include "key.h"
#include "sys.h"
#include "lcd.h"
#include "usart.h"
#include "can.h"
#include "CONTACT.h"
#include "nvic_conf.h"
#include "odometry.h"
#include "tim2_5_6.h"
#include "stdbool.h"
#include <stdio.h>
#include "string.h"
#include "math.h"
char odometry_data[21]={0}; //发送给串口的里程计数据数组
float odometry_right=0,odometry_left=0;//串口得到的左右轮速度
int i,j,m,t;
/*********************************************** 输入 *****************************************************************/
extern float position_x,position_y,oriention,velocity_linear,velocity_angular; //计算得到的里程计数值
extern u8 USART_RX_BUF[USART_REC_LEN]; //串口接收缓冲,最大USART_REC_LEN个字节.
extern u16 USART_RX_STA; //串口接收状态标记
extern float Milemeter_L_Motor,Milemeter_R_Motor; //dt时间内的左右轮速度,用于里程计计算
u8 main_sta=0; //用作处理主函数各种if，去掉多余的flag（1打印里程计）（2调用计算里程计数据函数）（3串口接收成功）（4串口接收失败）
union recieveData //接收到的数据
{
float d; //左右轮速度
unsigned char data[4];
}leftdata,rightdata; //接收的左右轮数据
union odometry //里程计数据共用体
{
float odoemtry_float;
unsigned char odometry_char[4];
}x_data,y_data,theta_data,vel_linear,vel_angular; //要发布的里程计数据，分别为：X，Y方向移动的距离，当前角度，线速度，角速度
int main(void)
{
unsigned char key_485;
u8 i=0,t=0;
delay_init(); //延时函数初始化
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);//设置中断优先级分组为组2：2位抢占优先级，2位响应优先级
uart_init(115200); //串口初始化为115200
LED_Init(); //初始化与LED连接的硬件接口
LCD_Init(); //初始化LCD
KEY_Init(); //按键初始化
CAN_Mode_Init(CAN_SJW_1tq,CAN_BS2_8tq,CAN_BS1_9tq,4,CAN_Mode_LoopBack);//CAN初始化环回模式,波特率500Kbps
RCC_Configuration(); //系统时钟配置
TIM2_PWM_Init(); //PWM输出初始化
TIM5_Configuration(); //速度计算定时器初始化
TIM1_Configuration(); //里程计发布定时器初始化
NVIC_Configuration(); //中断优先级配置
MOTOR_init();//电机初始化
while(1)
{
key_485=KEY_Scan(0);
if (key_485==1)
{
MOTOR_stop();
}
if (key_485==2)
{
MOTOR_speed(300,300);
}
if(main_sta&0x01)//执行发送里程计数据步骤
{
//里程计数据获取
x_data.odoemtry_float=position_x;//单位mm
y_data.odoemtry_float=position_y;//单位mm
theta_data.odoemtry_float=oriention;//单位rad
vel_linear.odoemtry_float=velocity_linear;//单位mm/s
vel_angular.odoemtry_float=velocity_angular;//单位rad/s
LCD_ShowxNum(60,80,210,24,24,position_x);
LCD_ShowxNum(90,80,210,24,24,position_y);
//将所有里程计数据存到要发送的数组
for(j=0;j<4;j++)
{
odometry_data[j]=x_data.odometry_char[j];
odometry_data[j+4]=y_data.odometry_char[j];
odometry_data[j+8]=theta_data.odometry_char[j];
odometry_data[j+12]=vel_linear.odometry_char[j];
odometry_data[j+16]=vel_angular.odometry_char[j];
}
odometry_data[20]='\n';//添加结束符
//发送数据要串口
for(i=0;i<21;i++)
{
USART_ClearFlag(USART1,USART_FLAG_TC); //在发送第一个数据前加此句，解决第一个数据不能正常发送的问题
USART_SendData(USART1,odometry_data[i]);//发送一个字节到串口
while(USART_GetFlagStatus(USART1, USART_FLAG_TC) == RESET); //等待发送结束
LCD_ShowString(30,80,210,24,24,"Send");
}
main_sta&=0xFE;//执行计算里程计数据步骤
}
if(main_sta&0x02)//执行计算里程计数据步骤
{
odometry(Milemeter_R_Motor,Milemeter_L_Motor);//计算里程计
main_sta&=0xFD;//执行发送里程计数据步骤
}
if(main_sta&0x08) //当发送指令没有正确接收时
{
USART_ClearFlag(USART1,USART_FLAG_TC); //在发送第一个数据前加此句，解决第一个数据不能正常发送的问题
for(m=0;m<3;m++)
{
USART_SendData(USART1,0x00);
while(USART_GetFlagStatus(USART1, USART_FLAG_TC) == RESET);
}
USART_SendData(USART1,'\n');
while(USART_GetFlagStatus(USART1, USART_FLAG_TC) == RESET);
main_sta&=0xF7;
}
if(USART_RX_STA&0x8000) // 串口1接收函数
{
LCD_ShowString(30,100,210,24,24,"Rec");
//接收左右轮速度
for(t=0;t<4;t++)
{
rightdata.data[t]=USART_RX_BUF[t];
leftdata.data[t]=USART_RX_BUF[t+4];
}
//储存左右轮速度
odometry_right=rightdata.d;//单位mm/s
odometry_left=leftdata.d;//单位mm/s
LCD_ShowxNum(60,120,210,24,24,odometry_right);
LCD_ShowxNum(60,140,210,24,24,odometry_left);
USART_RX_STA=0;//清楚接收标志位
MOTOR_speed(rightdata.d,leftdata.d); //将接收到的左右轮速度赋给小车
}
}
}
/******************************************************************
基于串口通信的ROS小车基础控制器，功能如下：
1.实现ros控制数据通过固定的格式和串口通信，从而达到控制小车的移动
2.订阅了/cmd_vel主题，只要向该主题发布消息，就能实现对控制小车的移动
3.发布里程计主题/odm
串口通信说明：
1.写入串口
（1）内容：左右轮速度，单位为mm/s
（2）格式：１０字节,[右轮速度４字节][左轮速度４字节][结束符"\r\n"２字节]
2.读取串口
（1）内容：小车x,y坐标，方向角，线速度，角速度，单位依次为：mm,mm,rad,mm/s,rad/s
（2）格式：２１字节，[Ｘ坐标４字节][Ｙ坐标４字节][方向角４字节][线速度４字节][角速度４字节][结束符"\n"１字节]
*******************************************************************/
#include "ros/ros.h" //ros需要的头文件
#include <geometry_msgs/Twist.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
//以下为串口通讯需要的头文件
#include <string>
#include <iostream>
#include <cstdio>
#include <unistd.h>
#include <math.h>
#include "serial/serial.h"
/****************************************************************************/
using std::string;
using std::exception;
using std::cout;
using std::cerr;
using std::endl;
using std::vector;
/*****************************************************************************/
float ratio = 1000.0f ; //转速转换比例，执行速度调整比例
float D = 0.412f ; //两轮间距，单位是m
float linear_temp=0,angular_temp=0;//暂存的线速度和角速度
/****************************************************/
unsigned char data_terminal0=0x0d; //“/r"字符
unsigned char data_terminal1=0x0a; //“/n"字符
unsigned char speed_data[10]={0}; //要发给串口的数据
string rec_buffer; //串口数据接收变量
//发送给下位机的左右轮速度，里程计的坐标和方向
union floatData //union的作用为实现char数组和float之间的转换
{
float d;
unsigned char data[4];
}right_speed_data,left_speed_data,position_x,position_y,oriention,vel_linear,vel_angular;
/************************************************************/
void callback(const geometry_msgs::Twist & cmd_input)//订阅/cmd_vel主题回调函数
{
string port("/dev/ttyUSB0"); //小车串口号
unsigned long baud = 115200; //小车串口波特率
serial::Serial my_serial(port, baud, serial::Timeout::simpleTimeout(1000)); //配置串口
angular_temp = cmd_input.angular.z ;//获取/cmd_vel的角速度,rad/s
linear_temp = cmd_input.linear.x ;//获取/cmd_vel的线速度.m/s
//将转换好的小车速度分量为左右轮速度
left_speed_data.d = linear_temp - 0.5f*angular_temp*D ;
right_speed_data.d = linear_temp + 0.5f*angular_temp*D ;
//left_speed_data.d=angular_temp;
//right_speed_data.d=linear_temp;
//存入数据到要发布的左右轮速度消息
left_speed_data.d*=ratio; //放大１０００倍，mm/s
right_speed_data.d*=ratio;//放大１０００倍，mm/s
for(int i=0;i<4;i++) //将左右轮速度存入数组中发送给串口
{
speed_data[i]=right_speed_data.data[i];
speed_data[i+4]=left_speed_data.data[i];
}
//在写入串口的左右轮速度数据后加入”/r/n“
speed_data[8]=data_terminal0;
speed_data[9]=data_terminal1;
//写入数据到串口
my_serial.write(speed_data,10);
}
int main(int argc, char **argv)
{
string port("/dev/ttyUSB0");//小车串口号
unsigned long baud = 115200;//小车串口波特率
serial::Serial my_serial(port, baud, serial::Timeout::simpleTimeout(1000));//配置串口
ros::init(argc, argv, "base_controller");//初始化串口节点
ros::NodeHandle n; //定义节点进程句柄
ros::Subscriber sub = n.subscribe("cmd_vel", 20, callback); //订阅/cmd_vel主题
ros::Publisher odom_pub= n.advertise<nav_msgs::Odometry>("odom", 20); //定义要发布/odom主题
static tf::TransformBroadcaster odom_broadcaster;//定义tf对象
geometry_msgs::TransformStamped odom_trans;//创建一个tf发布需要使用的TransformStamped类型消息
nav_msgs::Odometry odom;//定义里程计对象
geometry_msgs::Quaternion odom_quat; //四元数变量
//定义covariance矩阵，作用为解决文职和速度的不同测量的不确定性
float covariance[36] = {0.01, 0, 0, 0, 0, 0, // covariance on gps_x
0, 0.01, 0, 0, 0, 0, // covariance on gps_y
0, 0, 99999, 0, 0, 0, // covariance on gps_z
0, 0, 0, 99999, 0, 0, // large covariance on rot x
0, 0, 0, 0, 99999, 0, // large covariance on rot y
0, 0, 0, 0, 0, 0.01}; // large covariance on rot z
//载入covariance矩阵
for(int i = 0; i < 36; i++)
{
odom.pose.covariance[i] = covariance[i];;
}
ros::Rate loop_rate(10);//设置周期休眠时间
while(ros::ok())
{
rec_buffer =my_serial.readline(25,"\n"); //获取串口发送来的数据
const char *receive_data=rec_buffer.data(); //保存串口发送来的数据
if(rec_buffer.length()==21) //串口接收的数据长度正确就处理并发布里程计数据消息
{
for(int i=0;i<4;i++)//提取X，Y坐标，方向，线速度，角速度
{
position_x.data[i]=receive_data[i];
position_y.data[i]=receive_data[i+4];
oriention.data[i]=receive_data[i+8];
vel_linear.data[i]=receive_data[i+12];
vel_angular.data[i]=receive_data[i+16];
}
//将X，Y坐标，线速度缩小1000倍
position_x.d/=1000; //m
position_y.d/=1000; //m
vel_linear.d/=1000; //m/s
//里程计的偏航角需要转换成四元数才能发布
odom_quat = tf::createQuaternionMsgFromYaw(oriention.d);//将偏航角转换成四元数
//载入坐标（tf）变换时间戳
odom_trans.header.stamp = ros::Time::now();
//发布坐标变换的父子坐标系
odom_trans.header.frame_id = "odom";
odom_trans.child_frame_id = "base_footprint";
//tf位置数据：x,y,z,方向
odom_trans.transform.translation.x = position_x.d;
odom_trans.transform.translation.y = position_y.d;
odom_trans.transform.translation.z = 0.0;
odom_trans.transform.rotation = odom_quat;
//发布tf坐标变化
odom_broadcaster.sendTransform(odom_trans);
//载入里程计时间戳
odom.header.stamp = ros::Time::now();
//里程计的父子坐标系
odom.header.frame_id = "odom";
odom.child_frame_id = "base_footprint";
//里程计位置数据：x,y,z,方向
odom.pose.pose.position.x = position_x.d;
odom.pose.pose.position.y = position_y.d;
odom.pose.pose.position.z = 0.0;
odom.pose.pose.orientation = odom_quat;
//载入线速度和角速度
odom.twist.twist.linear.x = vel_linear.d;
//odom.twist.twist.linear.y = odom_vy;
odom.twist.twist.angular.z = vel_angular.d;
//发布里程计
odom_pub.publish(odom);
ros::spinOnce();//周期执行
loop_rate.sleep();//周期休眠
}
//程序周期性调用
//ros::spinOnce(); //callback函数必须处理所有问题时，才可以用到
}
return 0;
}键盘控制
#!/usr/bin/env python
from __future__ import print_function
import roslib; roslib.load_manifest('teleop_twist_keyboard')
import rospy
from geometry_msgs.msg import Twist
import sys, select, termios, tty
msg = """
Reading from the keyboard and Publishing to Twist!
---------------------------
Moving around:
u i o
j k l
m , .
For Holonomic mode (strafing), hold down the shift key:
---------------------------
U I O
J K L
M < >
t : up (+z)
b : down (-z)
anything else : stop
q/z : increase/decrease max speeds by 10%
w/x : increase/decrease only linear speed by 10%
e/c : increase/decrease only angular speed by 10%
CTRL-C to quit
"""
moveBindings = {
'i':(1,0,0,0),
'o':(1,0,0,-1),
'j':(1,0,0,1),
'l':(1,0,0,-1),
'u':(1,0,0,1),
',':(-1,0,0,0),
'.':(-1,0,0,1),
'm':(-1,0,0,-1),
'O':(1,-1,0,0),
'I':(1,0,0,0),
'J':(0,1,0,0),
'L':(0,-1,0,0),
'U':(1,1,0,0),
'<':(-1,0,0,0),
'>':(-1,-1,0,0),
'M':(-1,1,0,0),
't':(0,0,1,0),
'b':(0,0,-1,0),
}
speedBindings={
'q':(1.1,1.1),
'z':(.9,.9),
'w':(1.1,1),
'x':(.9,1),
'e':(1,1.1),
'c':(1,.9),
}
def getKey():
tty.setraw(sys.stdin.fileno())
select.select([sys.stdin], [], [], 0)
key = sys.stdin.read(1)
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
return key
def vels(speed,turn):
return "currently:\tspeed %s\tturn %s " % (speed,turn)
if __name__=="__main__":
settings = termios.tcgetattr(sys.stdin)
pub = rospy.Publisher('cmd_vel', Twist, queue_size = 1)
rospy.init_node('teleop_twist_keyboard')
speed = rospy.get_param("~speed", 0.1)
turn = rospy.get_param("~turn", 0.4)
x = 0
y = 0
z = 0
th = 0
status = 0
try:
print(msg)
print(vels(speed,turn))
while(1):
key = getKey()
if key in moveBindings.keys():
x = moveBindings[key][0]
y = moveBindings[key][1]
z = moveBindings[key][2]
th = moveBindings[key][3]
elif key in speedBindings.keys():
speed = speed * speedBindings[key][0]
turn = turn * speedBindings[key][1]
print(vels(speed,turn))
if (status == 14):
print(msg)
status = (status + 1) % 15
else:
x = 0
y = 0
z = 0
th = 0
if (key == '\x03'):
break
twist = Twist()
twist.linear.x = x*speed; twist.linear.y = y*speed; twist.linear.z = z*speed;
twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = th*turn
pub.publish(twist)
except Exception as e:
print(e)
finally:
twist = Twist()
twist.linear.x = 0; twist.linear.y = 0; twist.linear.z = 0
twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0
pub.publish(twist)
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)其他学习4个can转向电机4个rs485轮毂电机及4个can编码器
在linux控制电机程序为：
调试最后总出现编码器CRC校验错误，怀疑为linux下多串口通信的问题
改为用单片机直接控制
为了调试方便加了遥控
本文章为转载内容，我们尊重原作者对文章享有的著作权。如有内容错误或侵权问题，欢迎原作者联系我们进行内容更正或删除文章。
