背景:拿到一个舵机,一个安装了linux和ROS的“小黑盒子”以及一个干干净净啥也不会的脑子,然后我从零开始学的,总算找到了个能操作舵机的程序。现在只是能跑的状态,提供一种思路,之后我还得大改代码。
注意:文件的读写需要权限,在运行节点之前,需要
su root
需要知道哪些:
舵机控制原理
ROS话题通信
对应开发板的引脚定义
(什么行业规范,我不知道,能跑就行)
因为Linux里面一切都是文件,所以驱动爪子只需要改对应文件的值就可以了。
#include <stdio.h>
#include <stdlib.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <unistd.h>
#include <string.h>
#include "ros/ros.h"
#include "std_msgs/Int32.h"
static char pwm_path[100];
static int pwm_config(const char *attr, const char *val)
{
char file_path[200];
int len;
int fd;
sprintf(file_path, "%s/%s", pwm_path, attr);
if (0 > (fd = open(file_path, O_WRONLY, S_IWUSR))) {
perror("open error");
return fd;
}
len = strlen(val);
if (len != write(fd, val, len)) {
ROS_INFO("erro in %s", attr);
perror("write error");
close(fd);
return -1;
}
close(fd);
return 0;
}
bool drive(const std_msgs::Int32::ConstPtr &duty){
char duty_s[16];
int legal_duty = 1000000;
duty->data < 1000000 ? legal_duty = 1000000 : duty->data > 1800000? legal_duty = 1800000 : legal_duty = duty->data;
sprintf(duty_s, "%d", legal_duty);
if (pwm_config("duty_cycle", duty_s)){
ROS_INFO("Change duty cycle failed.");
return false;
}
ROS_INFO("duty:<%s>",duty_s);
return true;
}
int main(int argc, char *argv[])
{
if (4 != argc) {
ROS_INFO("rosrun gripper gripper_driver <id> <period> <duty>\nlegal_duty:1000000 - 1800000");
exit(-1);
}
ROS_INFO("PWM config: id:<%s>, period:<%s>, duty:<%s>\n",
argv[1], argv[2],
argv[3]);
sprintf(pwm_path, "/sys/class/pwm/pwmchip%s/pwm1", argv[1]);
if (access(pwm_path, F_OK)) {
char temp[100];
int fd;
sprintf(temp, "/sys/class/pwm/pwmchip%s/export", argv[1]);
if (0 > (fd = open(temp, O_WRONLY, S_IWUSR))) {
perror("open export error");
exit(-1);
}
if (1 != write(fd, "1", 1)) {
perror("write export error");
close(fd);
exit(-1);
}
close(fd);
}
if (pwm_config("period", argv[2]))
exit(-1);
if (pwm_config("duty_cycle", argv[3]))
exit(-1);
pwm_config("enable", "1");
ROS_INFO("PWM OK");
setlocale(LC_ALL,"");
ros::init(argc,argv,"gripper_driver");
ros::NodeHandle nh;
ros::Subscriber sub = nh.subscribe<std_msgs::Int32>("gripper_duty",2,drive);
ros::spin();
exit(0);
}
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)