关注微信公众号“混沌无形”,后台回复:13462F6。免费获取完整工程源码!
本文采用Arduino mega2560的6个外部中断来采集Microzone接收机的6个通道的PPM信号,主要是测量PPM信号中高电平的时间(单位是us),并将数据通过rosserial将数据发送给ROS,用于控制小车的运动
经本人开发的ROSMicrozoneBridge软件包,亲测有效,数据准确.
使用步骤
1.在公众号平台回复关键字,获取工程源码
2.为满足与ROS传输数据,需要自己创建消息文件(根据自己的需要创建)
make a file named selfmakemessage and a sub-file named msg
mkdir selfmakemessage/msg
touch Microzone.msg
编辑内容为:int ch1 int ch2 int ch3 int ch4 int ch5 int ch6
catkin_Make
3.下载安装rosserial
sudo apt-get install ros-indigo-rosserial-arduino
sudo apt-get install ros-indigo-rosserial
下载编译 rosserial https://github.com/ros-drivers/rosserial.git
主函数代码如下
#include <ros.h>
#include <selfmakemessage/Microzone.h>
#include <std_msgs/String.h>
#include <std_msgs/Empty.h>
#include "signalRead.h"
ros::NodeHandle nh;
selfmakemessage::Microzone Microzone_msg;
ros::Publisher MicrozonePublish("Microzone", &Microzone_msg);
unsigned int* arr;
//*arr *(arr+1) ...即为数组的元素
//Serial.print("\t");//输出元素之间有一定的间隔
void setup(){
// Serial.begin(115200); // Serial for debug
PWMReadInit();//PWMRead function initial
nh.initNode();
nh.advertise(MicrozonePublish);
}
void loop() {
//Serial.println(singlePWMRead(1));
arr = MixPWMRead();//read pwm signal
Microzone_msg.ch1 = (*arr);
Microzone_msg.ch2 = *(arr+1);
Microzone_msg.ch3 = *(arr+2);
Microzone_msg.ch4 = *(arr+3);
Microzone_msg.ch5 = *(arr+4);
Microzone_msg.ch6 = *(arr+5);
MicrozonePublish.publish(&Microzone_msg);
nh.spinOnce();
//Serial.println(*arr);
// nh.logdebug(debug);
nh.loginfo("in loop");
// nh.logwarn(warn);
// nh.logerror(error);
// nh.logfatal(fatal);
}
采集接收机信号的代码如下
#include "PWM.hpp"
// Global variables for ISR routines
unsigned int value[6];
byte last_state[6];
byte trigger_state[6];
unsigned long timers[6];
// ISR for pin 2
void ISR_2() {
unsigned long now = micros();
byte state_now = digitalRead(2);
if (state_now != last_state[0]) {
if (state_now == trigger_state[0]) {
timers[0] = now;
} else {
value[0] = now - timers[0];
}
last_state[0] = state_now;
}
}
// ISR for pin 3
void ISR_3() {
unsigned long now = micros();
byte state_now = digitalRead(3);
if (state_now != last_state[1]) {
if (state_now == trigger_state[1]) {
timers[1] = now;
} else {
value[1] = now - timers[1];
}
last_state[1] = state_now;
}
}
// ISR for pin 18
void ISR_18() {
unsigned long now = micros();
byte state_now = digitalRead(18);
if (state_now != last_state[2]) {
if (state_now == trigger_state[2]) {
timers[2] = now;
} else {
value[2] = now - timers[2];
}
last_state[2] = state_now;
}
}
// ISR for pin 19
void ISR_19() {
unsigned long now = micros();
byte state_now = digitalRead(19);
if (state_now != last_state[3]) {
if (state_now == trigger_state[3]) {
timers[3] = now;
} else {
value[3] = now - timers[3];
}
last_state[3] = state_now;
}
}
// ISR for pin 20
void ISR_20() {
unsigned long now = micros();
byte state_now = digitalRead(20);
if (state_now != last_state[4]) {
if (state_now == trigger_state[4]) {
timers[4] = now;
} else {
value[4] = now - timers[4];
}
last_state[4] = state_now;
}
}
// ISR for pin 21
void ISR_21() {
unsigned long now = micros();
byte state_now = digitalRead(21);
if (state_now != last_state[5]) {
if (state_now == trigger_state[5]) {
timers[5] = now;
} else {
value[5] = now - timers[5];
}
last_state[5] = state_now;
}
}
// Begin function to setup interrupt and attach ISR
void PWM::begin(byte pin, byte state) {
my_pin = pin;
switch (pin) {
case 2:
trigger_state[0] = state;
last_state[0] = digitalRead(pin);
attachInterrupt(digitalPinToInterrupt(pin), ISR_2, CHANGE);
break;
case 3:
trigger_state[1] = state;
last_state[1] = digitalRead(pin);
attachInterrupt(digitalPinToInterrupt(pin), ISR_3, CHANGE);
break;
case 18:
trigger_state[2] = state;
last_state[2] = digitalRead(pin);
attachInterrupt(digitalPinToInterrupt(pin), ISR_18, CHANGE);
break;
case 19:
trigger_state[3] = state;
last_state[3] = digitalRead(pin);
attachInterrupt(digitalPinToInterrupt(pin), ISR_19, CHANGE);
break;
case 20:
trigger_state[4] = state;
last_state[4] = digitalRead(pin);
attachInterrupt(digitalPinToInterrupt(pin), ISR_20, CHANGE);
break;
case 21:
trigger_state[5] = state;
last_state[5] = digitalRead(pin);
attachInterrupt(digitalPinToInterrupt(pin), ISR_21, CHANGE);
break;
default:
break;
}
}
// Function to return channel value
unsigned int PWM::getValue() {
switch (my_pin) {
case 2:
return value[0];
case 3:
return value[1];
case 18:
return value[2];
case 19:
return value[3];
case 20:
return value[4];
case 21:
return value[5];
default:
break;
}
}
喜欢的话,请关注微信公众号,可阅读更多好文!