上位机的程序redwall_arm_server.cpp
功能是作为ROS的move_group客户端接收ROS规划的机械臂路点信息，进行三次样条插补获得各个关节或自由度的运动PVAT数据，然后通过TCP通信将处理好的数据发送给下位机的beaglebone轴控制器：
/* ROS action server */
#include <ros/ros.h>
#include <iostream>
#include <actionlib/server/simple_action_server.h>
#include <control_msgs/FollowJointTrajectoryAction.h>
#include <std_msgs/Float32MultiArray.h>
/* 三次样条插补 */
#include <string.h>
#include <stddef.h>
#include <stdio.h>
#include "cubicSpline.h"
/* 使用变长数组 */
#include <vector>
#include <algorithm>
/* 套接字通信 */
#include <sstream>
#include <stdlib.h>
#include <sys/types.h>
#include <stdio.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <string.h>
#include <signal.h>
#include <arpa/inet.h>
#include <unistd.h>
#include <errno.h>
#include <pthread.h>
#define PORT 7788
using namespace std;
vector<double> time_from_start_;
vector<double> p_lumbar_;
vector<double> p_big_arm_;
vector<double> p_small_arm_;
vector<double> p_wrist_;
vector<double> p_hand_;
vector<double> v_lumbar_;
vector<double> v_big_arm_;
vector<double> v_small_arm_;
vector<double> v_wrist_;
vector<double> v_hand_;
vector<double> a_lumbar_;
vector<double> a_big_arm_;
vector<double> a_small_arm_;
vector<double> a_wrist_;
vector<double> a_hand_;
/* 存储的结构体 p2*/
struct vel_data
{
int vector_len;
double time_from_begin;
double lumbar_pos;
double big_arm_pos;
double small_arm_pos;
double wrist_pos;
double hand_pos;
double lumbar_vel;
double big_arm_vel;
double small_arm_vel;
double wrist_vel;
double hand_vel;
double lumbar_acc;
double big_arm_acc;
double small_arm_acc;
double wrist_acc;
double hand_acc;
};
/* 数据收发结构体 */
struct vel_data p2;
char writebuf[sizeof(p2)];
/* 客户端套接字文件描述符和地址,端口 */
typedef struct MySocketInfo{
int socketCon;
char *ipaddr;
uint16_t port;
}_MySocketInfo;
/* 客户端连接所用数据的存储数组 */
struct MySocketInfo arrConSocket[10];
int conClientCount = 0; // 连接的客户端个数
/* 客户端连接所用套接字的存储数组 */
pthread_t arrThrReceiveClient[10];
int thrReceiveClientCount = 0; // 接受数据线程个数
/* action 服务端声明 */
typedef actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction> Server;
/* 初始化输入输出速度加速度 */
double acc = 0, vel = 0;
double x_out = 0, y_out = 0;
/* 判断路点数据是否改变 */
bool point_changed = false;
/* 三次样条无参构造 */
cubicSpline::cubicSpline()
{
}
/* 析构 */
cubicSpline::~cubicSpline()
{
releaseMem();
}
/* 初始化参数 */
void cubicSpline::initParam()
{
x_sample_ = y_sample_ = M_ = NULL;
sample_count_ = 0;
bound1_ = bound2_ = 0;
}
/* 释放参数 */
void cubicSpline::releaseMem()
{
delete x_sample_;
delete y_sample_;
delete M_;
initParam();
}
/* 加载关节位置数组等信息 */
bool cubicSpline::loadData(double *x_data, double *y_data, int count, double bound1, double bound2, BoundType type)
{
if ((NULL == x_data) || (NULL == y_data) || (count < 3) || (type > BoundType_Second_Derivative) || (type < BoundType_First_Derivative))
{
return false;
}
initParam();
x_sample_ = new double[count];
y_sample_ = new double[count];
M_ = new double[count];
sample_count_ = count;
memcpy(x_sample_, x_data, sample_count_*sizeof(double));
memcpy(y_sample_, y_data, sample_count_*sizeof(double));
bound1_ = bound1;
bound2_ = bound2;
return spline(type);
}
/* 计算样条插值 */
bool cubicSpline::spline(BoundType type)
{
if ((type < BoundType_First_Derivative) || (type > BoundType_Second_Derivative))
{
return false;
}
// 追赶法解方程求二阶偏导数
double f1=bound1_, f2=bound2_;
double *a=new double[sample_count_]; // a:稀疏矩阵最下边一串数
double *b=new double[sample_count_]; // b:稀疏矩阵最中间一串数
double *c=new double[sample_count_]; // c:稀疏矩阵最上边一串数
double *d=new double[sample_count_];
double *f=new double[sample_count_];
double *bt=new double[sample_count_];
double *gm=new double[sample_count_];
double *h=new double[sample_count_];
for(int i=0;i<sample_count_;i++)
b[i]=2; // 中间一串数为2
for(int i=0;i<sample_count_-1;i++)
h[i]=x_sample_[i+1]-x_sample_[i]; // 各段步长
for(int i=1;i<sample_count_-1;i++)
a[i]=h[i-1]/(h[i-1]+h[i]);
a[sample_count_-1]=1;
c[0]=1;
for(int i=1;i<sample_count_-1;i++)
c[i]=h[i]/(h[i-1]+h[i]);
for(int i=0;i<sample_count_-1;i++)
f[i]=(y_sample_[i+1]-y_sample_[i])/(x_sample_[i+1]-x_sample_[i]);
for(int i=1;i<sample_count_-1;i++)
d[i]=6*(f[i]-f[i-1])/(h[i-1]+h[i]);
// 追赶法求解方程
if(BoundType_First_Derivative == type)
{
d[0]=6*(f[0]-f1)/h[0];
d[sample_count_-1]=6*(f2-f[sample_count_-2])/h[sample_count_-2];
bt[0]=c[0]/b[0];
for(int i=1;i<sample_count_-1;i++)
bt[i]=c[i]/(b[i]-a[i]*bt[i-1]);
gm[0]=d[0]/b[0];
for(int i=1;i<=sample_count_-1;i++)
gm[i]=(d[i]-a[i]*gm[i-1])/(b[i]-a[i]*bt[i-1]);
M_[sample_count_-1]=gm[sample_count_-1];
for(int i=sample_count_-2;i>=0;i--)
M_[i]=gm[i]-bt[i]*M_[i+1];
}
else if(BoundType_Second_Derivative == type)
{
d[1]=d[1]-a[1]*f1;
d[sample_count_-2]=d[sample_count_-2]-c[sample_count_-2]*f2;
bt[1]=c[1]/b[1];
for(int i=2;i<sample_count_-2;i++)
bt[i]=c[i]/(b[i]-a[i]*bt[i-1]);
gm[1]=d[1]/b[1];
for(int i=2;i<=sample_count_-2;i++)
gm[i]=(d[i]-a[i]*gm[i-1])/(b[i]-a[i]*bt[i-1]);
M_[sample_count_-2]=gm[sample_count_-2];
for(int i=sample_count_-3;i>=1;i--)
M_[i]=gm[i]-bt[i]*M_[i+1];
M_[0]=f1;
M_[sample_count_-1]=f2;
}
else
return false;
delete a;
delete b;
delete c;
delete d;
delete gm;
delete bt;
delete f;
delete h;
return true;
}
/* 得到速度和加速度数组 */
bool cubicSpline::getYbyX(double &x_in, double &y_out)
{
int klo,khi,k;
klo=0;
khi=sample_count_-1;
double hh,bb,aa;
// 二分法查找x所在区间段
while(khi-klo>1)
{
k=(khi+klo)>>1;
if(x_sample_[k]>x_in)
khi=k;
else
klo=k;
}
hh=x_sample_[khi]-x_sample_[klo];
aa=(x_sample_[khi]-x_in)/hh;
bb=(x_in-x_sample_[klo])/hh;
y_out=aa*y_sample_[klo]+bb*y_sample_[khi]+((aa*aa*aa-aa)*M_[klo]+(bb*bb*bb-bb)*M_[khi])*hh*hh/6.0;
//test
acc = (M_[klo]*(x_sample_[khi]-x_in) + M_[khi]*(x_in - x_sample_[klo])) / hh;
vel = M_[khi]*(x_in - x_sample_[klo]) * (x_in - x_sample_[klo]) / (2 * hh)
- M_[klo]*(x_sample_[khi]-x_in) * (x_sample_[khi]-x_in) / (2 * hh)
+ (y_sample_[khi] - y_sample_[klo])/hh
- hh*(M_[khi] - M_[klo])/6;
//printf("[---位置、速度、加速度---]");
//printf("%0.9f, %0.9f, %0.9f\n",y_out, vel, acc);
//test end
return true;
}
/* SOCKET服务器端处理客户端连接函数 */
void *fun_thrAcceptHandler(void *socketListen)
{
while(1)
{
/* accept函数主要用于服务器端，建立好连接后，它返回的一个新的套接字
* 此后，服务器端即可使用这个新的套接字与该客户端进行通信，而原本的套接字则继续用于监听其他客户端的连接请求。 */
int sockaddr_in_size = sizeof(struct sockaddr_in);
struct sockaddr_in client_addr;
int _socketListen = *((int *)socketListen);
int socketCon = accept(_socketListen, (struct sockaddr *)(&client_addr), (socklen_t *)(&sockaddr_in_size));
if(socketCon < 0){
printf("连接失败\n");
}else{
printf("连接成功 ip: %s:%d\n",inet_ntoa(client_addr.sin_addr),client_addr.sin_port);
}
printf("连接套接字为：%d\n",socketCon);
/* 开启新的通讯线程，负责同连接上来的客户端进行通讯 */
_MySocketInfo socketInfo; // 用于保存客户端套接字的信息
socketInfo.socketCon = socketCon;
socketInfo.ipaddr = inet_ntoa(client_addr.sin_addr);
socketInfo.port = client_addr.sin_port;
arrConSocket[conClientCount] = socketInfo;
conClientCount++;
printf("连接了%d个用户\n",conClientCount);
//让进程休息1秒
sleep(1);
}
}
/* 判断线程是否被杀死 */
int checkThrIsKill(pthread_t thr)
{
/* 传递的pthread_kill的signal参数为0时，用这个保留的信号测试线程是否存在 */
int res = 1;
int res_kill = pthread_kill(thr,0);
if(res_kill == 0){
res = 0;
}
return res;
}
/* 收到action的goal后调用的回调函数 */
void execute(const control_msgs::FollowJointTrajectoryGoalConstPtr& goal, Server* as) {
/* move_group规划的路径包含的路点个数 */
/* 规划的路点数目 */
int point_num = goal->trajectory.points.size();
ROS_INFO("First Move_group give us %d points",point_num);
/* 各个关节位置 */
double p_lumbar[point_num];
double p_big_arm[point_num];
double p_small_arm[point_num];
double p_wrist[point_num];
double p_hand[point_num];
/* 各个关节速度 */
double v_lumbar[point_num];
double v_big_arm[point_num];
double v_small_arm[point_num];
double v_wrist[point_num];
double v_hand[point_num];
/* 各个关节加速度 */
double a_lumbar[point_num];
double a_big_arm[point_num];
double a_small_arm[point_num];
double a_wrist[point_num];
double a_hand[point_num];
/* 时间数组 */
double time_from_start[point_num];
for (int i = 0; i < point_num; i++) {
p_lumbar[i] = goal->trajectory.points[i].positions[0];
p_big_arm[i] = goal->trajectory.points[i].positions[1];
p_small_arm[i] = goal->trajectory.points[i].positions[2];
p_wrist[i] = goal->trajectory.points[i].positions[3];
p_hand[i] = goal->trajectory.points[i].positions[4];
v_lumbar[i] = goal->trajectory.points[i].velocities[0];
v_big_arm[i] = goal->trajectory.points[i].velocities[1];
v_small_arm[i] = goal->trajectory.points[i].velocities[2];
v_wrist[i] = goal->trajectory.points[i].velocities[3];
v_hand[i] = goal->trajectory.points[i].velocities[4];
a_lumbar[i] = goal->trajectory.points[i].accelerations[0];
a_big_arm[i] = goal->trajectory.points[i].accelerations[1];
a_small_arm[i] = goal->trajectory.points[i].accelerations[2];
a_wrist[i] = goal->trajectory.points[i].accelerations[3];
a_hand[i] = goal->trajectory.points[i].accelerations[4];
time_from_start[i] = goal->trajectory.points[i].time_from_start.toSec();
}
// 实例化样条
cubicSpline spline;
double max_time = time_from_start[point_num-1]; // 规划时间的最后一个
ROS_INFO("Second Move_group max_time is %f ",max_time);
double rate = 0.004; // 4ms
time_from_start_.clear(); // 清空
// lumbar
spline.loadData(time_from_start, p_lumbar, point_num, 0, 0, cubicSpline::BoundType_First_Derivative);
p_lumbar_.clear();
v_lumbar_.clear();
a_lumbar_.clear();
x_out = -rate;
while(x_out < max_time) {
x_out += rate;
spline.getYbyX(x_out, y_out);
time_from_start_.push_back(x_out); // 将新的时间存储，只需操作一次即可
p_lumbar_.push_back(y_out);
v_lumbar_.push_back(vel);
a_lumbar_.push_back(acc);
}
// big_arm
spline.loadData(time_from_start, p_big_arm, point_num, 0, 0, cubicSpline::BoundType_First_Derivative);
p_big_arm_.clear();
v_big_arm_.clear();
a_big_arm_.clear();
x_out = -rate;
while(x_out < max_time) {
x_out += rate;
spline.getYbyX(x_out, y_out);
p_big_arm_.push_back(y_out);
v_big_arm_.push_back(vel);
a_big_arm_.push_back(acc);
}
// small_arm
spline.loadData(time_from_start, p_small_arm, point_num, 0, 0, cubicSpline::BoundType_First_Derivative);
p_small_arm_.clear();
v_small_arm_.clear();
a_small_arm_.clear();
x_out = -rate;
while(x_out < max_time) {
x_out += rate;
spline.getYbyX(x_out, y_out);
p_small_arm_.push_back(y_out);
v_small_arm_.push_back(vel);
a_small_arm_.push_back(acc);
}
// wrist
spline.loadData(time_from_start, p_wrist, point_num, 0, 0, cubicSpline::BoundType_First_Derivative);
p_wrist_.clear();
v_wrist_.clear();
a_wrist_.clear();
x_out = -rate;
while(x_out < max_time) {
x_out += rate;
spline.getYbyX(x_out, y_out);
p_wrist_.push_back(y_out);
v_wrist_.push_back(vel);
a_wrist_.push_back(acc);
}
// hand
spline.loadData(time_from_start, p_hand, point_num, 0, 0, cubicSpline::BoundType_First_Derivative);
p_hand_.clear();
v_hand_.clear();
a_hand_.clear();
x_out = -rate;
while(x_out < max_time) {
x_out += rate;
spline.getYbyX(x_out, y_out);
p_hand_.push_back(y_out);
v_hand_.push_back(vel);
a_hand_.push_back(acc);
}
//control_msgs::FollowJointTrajectoryFeedback feedback;
//feedback = NULL;
//as->publishFeedback(feedback);
ROS_INFO("Now We get all joints P,V,A,T!");
point_changed = true;
as->setSucceeded();
}
/* 主函数主要用于动作订阅和套接字通信 */
int main(int argc, char** argv)
{
ros::init(argc, argv, "redwall_arm_control");
ros::NodeHandle nh;
// 定义一个服务器
Server server(nh, "redwall_arm/follow_joint_trajectory", boost::bind(&execute, _1, &server), false);
// 服务器开始运行
server.start();
printf("开始socket\n");
/* 创建TCP连接的Socket套接字 */
int socketListen = socket(AF_INET, SOCK_STREAM, 0);
if(socketListen < 0)
{
printf("创建TCP套接字失败\n");
exit(-1);
}
else{
printf("创建套接字成功\n");
}
/* 绑定服务器端口地址信息 */
struct sockaddr_in server_addr; // struct sockaddr_in是已经声明了的结构名
bzero(&server_addr,sizeof(struct sockaddr_in)); // 等价于memset(server_addr,0,sizeof(struct sockaddr_in));清零操作
server_addr.sin_family=AF_INET;
server_addr.sin_addr.s_addr=htonl(INADDR_ANY); // 这里地址使用全0，即所有可能的地址
server_addr.sin_port=htons(PORT); // htons一般是转换端口号为整数
if(bind(socketListen, (struct sockaddr *)&server_addr,sizeof(struct sockaddr)) != 0)
{
perror("绑定ip地址,端口号失败\n");
exit(-1);
}
else{
printf("绑定ip地址,端口号成功\n");
}
/* 开始监听相应的端口,最大不超过10个连接 */
if(listen(socketListen, 10) != 0){
printf("开启监听失败\n");
exit(-1);
}else{
printf("开启监听成功\n");
}
/* 创建一个子线程用于接受连接客户端连接 */
pthread_t thrAccept;
pthread_create(&thrAccept,NULL,fun_thrAcceptHandler,&socketListen);
/* 实时发送数据 */
while(ros :: ok())
{
ros::spinOnce();
if( point_changed )
{
/***** 判断客户端连接和数据发送是否成功 *****/
if(conClientCount <= 0)
{
printf("没有客户端连接\n");
exit(0);
}
else {
for (int i = 0; i < conClientCount; i++) {
for(int k = 0; k < time_from_start_.size() ; k++)
{
p2.vector_len = time_from_start_.size();
p2.time_from_begin = time_from_start_.at(k);
p2.lumbar_pos = p_lumbar_.at(k);
p2.big_arm_pos = p_big_arm_.at(k);
p2.small_arm_pos = p_small_arm_.at(k);
p2.wrist_pos = p_wrist_.at(k);
p2.hand_pos = p_hand_.at(k);
p2.lumbar_vel = v_lumbar_.at(k);
p2.big_arm_vel = v_big_arm_.at(k);
p2.small_arm_vel = v_small_arm_.at(k);
p2.wrist_vel = v_wrist_.at(k);
p2.hand_vel = v_hand_.at(k);
p2.lumbar_acc = a_lumbar_.at(k);
p2.big_arm_acc = a_big_arm_.at(k);
p2.small_arm_acc = a_small_arm_.at(k);
p2.wrist_acc = a_wrist_.at(k);
p2.hand_acc = a_hand_.at(k);
bzero(writebuf, sizeof(writebuf)); // 清零writebuf
memcpy(writebuf, &p2, sizeof(p2)); // 复制p2数据到writebuf
unsigned int sendMsg_len = write(arrConSocket[i].socketCon, writebuf,sizeof(p2)); //返回值：写入文档的字节数（成功）；-1（出错)
if (sendMsg_len > 0) {
//ROS_INFO("Send Msg_len %d successful!",sendMsg_len);
} else {
printf("向%s:%d发送失败\n", arrConSocket[i].ipaddr, arrConSocket[i].port);
}
}
}
}
/****** 只有当有数据更新的时候才发送 *******/
point_changed = false;
}
/***** 判断线程存活多少 ******/
for(int i=0;i<thrReceiveClientCount;i++)
{
if(checkThrIsKill(arrThrReceiveClient[i]) == 1)
{
printf("有个线程被杀了\n");
thrReceiveClientCount--;
}
}
//printf("当前有接受数据线程多少个：%d\n",thrReceiveClientCount);
}
/* 关闭原始套接字 */
close(socketListen);
return 0;
}
下位机程序redwall_arm_client.cpp
其作用是接收上述程序发送的插补信息，将关节的运动速度信息转换成PWM控制所对应占空比，将位置信息用于编码器和电机的反馈当中。因为本次设计的机械臂所用的是maxon的直流伺服电机，所以可以通过调节PWM占空比来控制电机的速度，而不同于步进电机的变频调速（步进电机调节占空比对速度没有影响，直流电机的扭力和频率有关）。
- 占空比变化,电机的功率就发生了变化.
- 如果驱动的是LED灯.亮度会变化.
- 如果驱动的是声音,音量会变化.
- 如果驱动的是直流电机,速度会变化.
- 如果驱动的是步进电机,扭力会变化.
#include <stdlib.h>
#include <sys/types.h>
#include <stdio.h>
#include <sys/socket.h>
#include <string.h>
#include <arpa/inet.h>
#include <unistd.h>
#include <pthread.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <errno.h>
#include <math.h>
#include <sys/time.h>
#include <sys/poll.h>
#include <sys/epoll.h>
#include <iostream>
/* 使用vector数组 */
#include <vector>
#include <algorithm>
/* PRU指令 */
#include <prussdrv.h>
#include <pruss_intc_mapping.h>
#define PORT 7788
#define zero 0
#define one 1
#define ADDR "192.168.7.1"
/* 使用的是PRU0 */
#define PRU_NUM 0
using namespace std;
/* 初始化数组和数组长度 */
int vector_len_ = -1;
vector<double> time_from_start;
vector<double> p_lumbar;
vector<double> p_big_arm;
vector<double> p_small_arm;
vector<double> p_wrist;
vector<double> p_hand;
vector<double> v_lumbar;
vector<double> v_big_arm;
vector<double> v_small_arm;
vector<double> v_wrist;
vector<double> v_hand;
vector<double> a_lumbar;
vector<double> a_big_arm;
vector<double> a_small_arm;
vector<double> a_wrist;
vector<double> a_hand;
/* 存储的结构体 p1*/
struct vel_data
{
int vector_len;
double time_from_begin;
double lumbar_pos;
double big_arm_pos;
double small_arm_pos;
double wrist_pos;
double hand_pos;
double lumbar_vel;
double big_arm_vel;
double small_arm_vel;
double wrist_vel;
double hand_vel;
double lumbar_acc;
double big_arm_acc;
double small_arm_acc;
double wrist_acc;
double hand_acc;
};
struct vel_data p1;
char recvbuf[sizeof(p1)];
typedef struct MySocketInfo
{
int socketCon;
unsigned long ipaddr;
unsigned short port;
}_MySocketInfo;
/*SOCKET客户端处理接受数据函数*/
void *fun_thrReceiveHandler(void *socketCon)
{
while(1)
{
/* 保存目标套接字信息 */
int _socketCon = *((int *)socketCon);
bzero(recvbuf, sizeof(p1));
unsigned int buffer_length = read(_socketCon,recvbuf,sizeof(p1));
if(buffer_length == 0){
printf("服务器端异常关闭\n");
exit(-1);
}else if(buffer_length < 0){
printf("接受客户端数据失败\n");
break;
}
//printf("Receive buffer length %d\n",buffer_length);
/* 将接收到的速度控制信息以p1结构体格式解码*/
memcpy(&p1,recvbuf,sizeof(recvbuf));
time_from_start.push_back(p1.time_from_begin);
vector_len_ = p1.vector_len;
p_lumbar.push_back(p1.lumbar_pos);
p_big_arm.push_back(p1.big_arm_pos);
p_small_arm.push_back(p1.small_arm_pos);
p_wrist.push_back(p1.wrist_pos);
p_hand.push_back(p1.hand_pos);
v_lumbar.push_back(p1.lumbar_vel);
v_big_arm.push_back(p1.big_arm_vel);
v_small_arm.push_back(p1.small_arm_vel);
v_wrist.push_back(p1.wrist_vel);
v_hand.push_back(p1.hand_vel);
a_lumbar.push_back(p1.lumbar_acc);
a_big_arm.push_back(p1.big_arm_acc);
a_small_arm.push_back(p1.small_arm_acc);
a_wrist.push_back(p1.wrist_acc);
a_hand.push_back(p1.hand_acc);
}
}
/* 调用PRU程序，发送特定波形的PWM，精确控制延时和电机速度 */
void *lumbar_motor(void *)
{
while(1)
{
usleep(1000);
// cout<< vector_len_<<endl;
if(v_lumbar.size() == vector_len_)
{
// 使用 prussdrv_pruintc_intc 初始化
// PRUSS_INTC_INITDATA 使用的是 pruss_intc_mapping.h头文件
tpruss_intc_initdata pruss_intc_initdata = PRUSS_INTC_INITDATA;
// 分配并初始化内存空间
prussdrv_init ();
prussdrv_open (PRU_EVTOUT_0);
// 映射 PRU 的中断
prussdrv_pruintc_init(&pruss_intc_initdata);
// 存储周期数组,谐波减速器的减速比是50,速度数组的单位是弧度每秒
// 占空比0-速度为0 占空比100-速度为46.26π rad/s 还要考虑到周期的，需要测量
// 先使用随机数测试
srand((unsigned int)time(NULL));
unsigned int lumbar_duty[vector_len_];
for (int i=0; i<vector_len_; i++){
lumbar_duty[i] = rand()%50+1;
}
// 映射内存
static void *pru0DataMemory;
static unsigned int *pru0DataMemory_int;
prussdrv_map_prumem(PRUSS0_PRU0_DATARAM, &pru0DataMemory);
pru0DataMemory_int = (unsigned int *) pru0DataMemory;
// 数据写入PRU内核空间
unsigned int sampletimestep = 10; //delay factor 暂取10us
*(pru0DataMemory_int) = sampletimestep;
unsigned int numbersamples = vector_len_; //number of samples
*(pru0DataMemory_int+1) = numbersamples;
for (int i=0; i< vector_len_; i++)
{
*(pru0DataMemory_int+2+i) = lumbar_duty[i];
}
// PRU开始时间
struct timeval start;
gettimeofday(&start,NULL);
// 加载并执行 PRU 程序
prussdrv_exec_program (PRU_NUM, "./redwall_arm_client.bin");
// 等待来自pru的事件完成，返回pru 事件号
int n = prussdrv_pru_wait_event (PRU_EVTOUT_0);
// pru结束时间
struct timeval end;
gettimeofday(&end,NULL);
double diff;
diff = end.tv_sec -start.tv_sec + (end.tv_usec - start.tv_usec)*0.000001;
cout<< "EBB PRU程序已完成,历时约 "<< diff << "秒！" << endl;
// 清空数组
p_lumbar.clear();
v_lumbar.clear();
a_lumbar.clear();
time_from_start.clear();
// 初始化数组长度
vector_len_ = -1;
// 禁用pru并关闭内存映射
prussdrv_pru_disable(PRU_NUM);
prussdrv_exit ();
}
}
}
/* 主函数完成接受和发送数据 */
int main(int argc, char **argv)
{
if(getuid()!=0)
{
printf("必须使用root权限执行.\n");
exit(EXIT_FAILURE);
}
/***** 开始套接字通信 *****/
printf("开始socket\n");
int socketCon = socket(AF_INET, SOCK_STREAM, 0);
if(socketCon < 0)
{
printf("创建TCP连接套接字失败\n");
exit(-1);
}
/***** 绑定服务器端口地址信息 *****/
struct sockaddr_in server_addr;
bzero(&server_addr,sizeof(struct sockaddr_in));
server_addr.sin_family=AF_INET;
server_addr.sin_port=htons(PORT);
if(inet_pton(AF_INET, ADDR, &server_addr.sin_addr) !=1)
{
printf("ipv4地址转换失败");
}
/***** 连接服务器 *****/
int res_con = connect(socketCon,(struct sockaddr *)(&server_addr),sizeof(struct sockaddr));
if(res_con != 0)
{
printf("连接服务器失败\n");
exit(-1);
}
printf("连接成功,连接结果为：%d\n",res_con);
/***** 开启新的实时接受数据线程 *****/
pthread_t thrReceive;
pthread_create(&thrReceive,NULL,fun_thrReceiveHandler,&socketCon);
/***** 开启处理电机数据线程,其他几个线程也是重复，就不写了*****/
pthread_t id1;
pthread_create(&id1,NULL,lumbar_motor,NULL);
// pthread_t id2;
// pthread_create(&id2,NULL,bigarm_motor,NULL);
// pthread_t id3;
// pthread_create(&id3,NULL,smallarm_motor,NULL);
// pthread_t id4;
// pthread_create(&id4,NULL,wrist_motor,NULL);
// pthread_t id5;
// pthread_create(&id5,NULL,hand_motor,NULL);
pthread_detach(id1);
// pthread_detach(id2);
// pthread_detach(id3);
// pthread_detach(id4);
// pthread_detach(id5);
pthread_join(thrReceive,NULL);
/***** 关闭套接字 *****/
close(socketCon);
return 0;
}
上述代码并不是完整的程序，先使用随机数1~50作为占空比来测试PRU是否正常运行，并观察程序的延时和执行的时间是否足够精确。 在以上PWM功率控制的描述中，没有任何地方特别提到频率作为要控制的参数。重要的是占空比，即导通时间与导通时间加截止时间之比。您可以设置占空比以控制传递到电机绕组的功率。PWM占空比可以从零（始终关闭）到百分之一百（始终打开）变化，而PWM频率几乎可以是任何东西。但是，如果频率太低（ON / OFF周期的时间太长），则在该周期的ON部分会发生过多的发热或电源过载。尽管冷却时间在较低的频率下会延长，但在循环的“关闭”部分可能不足以充分冷却。OTOH，如果频率太高，则由于电机绕组中的电感会导致在循环的“开”部分电流不足。 其实就是这个公式n=60f/p，最大转速对应PWM最大占空比，所以其它转速与PWM占空比的关系为OCRx=(TOP*60*f)/(nmax*p);其中TOP就是计数器的计数上限值，OCRx就是占空比，f是电源频率，nmax就是最大转速,p是转子磁极对数。 无刷直流电机的最大转速跟电机的本身特性有关，电机加工好后其反电势系数就会定下来了，该系数跟电机铁心内径、电机每相匝数、电机铁心长度以及气隙磁密有关。那么在指定电压下的转速就会固定下来，另外负载加大的情况，电机转速会相应降下来，原因是电机线圈的电流会增大，线圈所分的电压会增加，从而降低了反电势大小，转速自然会降下来。另外电机在转动一段时间后，温度会升上来，气隙磁密会降低，电机的最高转速会提上来，同样在同转矩下需要的电流也会增加。 直流伺服电机转速在有载和空载是不一样的，一般电机转速可以用光电编码器或者自制的光点编码盘或者是霍尔器件去测量，用单片机的计数器记脉冲的个数，从而得出电机转速，PWM调节电机速度确实没错，但是好像没有现成的公式吧，所以需要自己拟一个。
上述客户端程序所调用的PRU汇编程序
redwall_arm_client.p
作用大致就是根据上述的占空比数组，传递到PRU内存空间当中之后，周期的执行。例如，ROS规划的时间是4s，之前确定的延时是4ms，则根据插补速度数组得到占空比数组为int lumbar_duty[1000]，delay factor 暂取10，采样率为100，由于由于PRU以200 MHz运行，因此执行一条指令所需时间为5纳秒。
// 实现周期相同(10us)占空比不同持续时间相同(4ms)的连续pwm波形，从而实现控制伺服电机的速度连续控制
// Output is r30.5 (P9_27)
.origin 0
.entrypoint START
#define PRU0_R31_VEC_VALID 32 // 允许程序完成通知
#define PRU_EVTOUT_0 3 // 发送回的事件号
// PWM的周期为delay_factor * 采样数 * 2 * 5ns,期望的延时时间为4ms , 所以计算器的次数为4ms / (delay_factor * 采样数 * 2 * 5ns)
START:
// r0临时地址存储，r2延迟因子，r5 样本数 r6是计数器 r7即lambar_duty[i],r8延时4ms 占高占低是r1，r3， r4 是临时存储r1，r3的
MOV r0, 0x00000000
LBBO r2, r0, 0, 4
MOV r0, 0x00000004
LBBO r5, r0, 0, 4
MOV r6, 0
MOV r7, 0x00000008
CONTINUE:
// r7成为下一个点的地址，r1 = 占空比高
LBBO r1, r7, 0, 4 // 感觉数组第一个点没取到，所以这里我调换了一下顺序
MOV r8, 400
ADD r7, r7, 4
ADD r6, r6, 1
MOV r3, 100 //load 100 into r3
SUB r3, r3, r1 // 占高占低是r1，r3
PWMCONTROL:
// r4 占空比高
MOV r4, r1
SET r30.t5 // set the output P9_27 high
SIGNAL_HIGH:
// r0 延迟因子
MOV r0, r2
DELAY_HIGH:
// 也就是 r0 * r4 * 2 * 5ns
SUB r0, r0, 1
QBNE DELAY_HIGH, r0, 0 // 延迟r0
SUB r4, r4, 1
QBNE SIGNAL_HIGH, r4, 0 // 延迟高电平
// r4占空比低
MOV r4, r3
CLR r30.t5 // set the output P9_27 low
SIGNAL_LOW:
// r0 延迟因子
MOV r0, r2
DELAY_LOW:
// 也就是 r0 * r4 * 2 * 5ns
SUB r0, r0, 1
QBNE DELAY_LOW, r0, 0
SUB r4, r4, 1
QBNE SIGNAL_LOW, r4, 0
DELAYON:
SUB r8, r8, 1 // REG8 -= pwm周期
QBNE PWMCONTROL, r8, 0 // 执行PWMCONTROL, 除非 REG0=0
QBNE CONTINUE, r6, r5 // r6 = r5 ，说明所有数据执行完成了，结束
END:
MOV R31.b0, PRU0_R31_VEC_VALID | PRU_EVTOUT_0
HALT
由于客户端调用PRU程序的时候，涉及到进程之间的切换，这个切换时间需要考虑，通过如下的方式计算该切换时间： 将上述PRU程序的循环部分全部删除，如下，再次执行，通过gettimeofday函数就可以计算出切换的时间
.origin 0
.entrypoint START
#define PRU0_R31_VEC_VALID 32 // 允许程序完成通知
#define PRU_EVTOUT_0 3 // 发送回的事件号
START:
END:
MOV R31.b0, PRU0_R31_VEC_VALID | PRU_EVTOUT_0
HALT
当使用如下程序执行的时候,获取到PRU进程切换的时间大约在0.5ms~0.9ms左右,也就是通过gettimeofday获取的运行时间减去PRU进程切换时间和ROS规划的时间相比较就是程序总的误差时间,这个时间大约在0.5s左右，误差非常大。问题就出在 MOV r8, 400 SUB r8, r8, 1 // REG8 -= pwm周期 因为通过delay_factor和采样数计算得到的pwm周期为10us，PWM的周期为delay_factor * 采样数 * 2 * 5ns,期望的延时时间为4ms , 所以计算器的次数为 4ms / (delay_factor * 采样数 * 2 * 5ns) = 400 。没有考虑到循环开销，通过示波器查看发现pwm周期为11.6us。于是需要修正计数器count,也就会修改延时4ms对应的计数器的值,原本的公式 (err + PWM周期) * count = 4ms; 当设err为0的时候,计算出count = 400,误差在0.5秒左右,也就是说不考虑err的值会带来0.5秒的误差,所以需要修正err,但是PRU的循环很难确定循环开销,这个值大约在1~2us之间,所以我们通过多次改变count的值,获取最合适的count,最终确定count=347,通过gettimeofday计算,减去PRU进程切换之后,得到的运行时间和ROS路径规划的时间相差在6ms以内。取count=346，误差比较在1ms以内,但是规划时间较长的时候,通过gettimeofday计算会小于ROS规划时间，这个误差在2ms左右。 综合之下考虑,取count = 346,这样ROS规划时间在3秒以内的路径,误差小于1ms.大于3秒的路径,误差在2ms以内.做如下修改即可。MOV r8, 346
开心洋葱 , 版权所有丨如未注明 , 均为原创丨未经授权请勿修改 , 转载请注明通过ROS控制真实机械臂(9)—上、下位机和PRU程序！
