ESP32 NodeMCU에서 encoder를 읽어 left_wheel_tick_count, right_wheel_tick_count topic을 publish합니다. 아래 글을 참고로 코드를 작성였습니다.
How to Publish Wheel Encoder Tick Data Using ROS and Arduino
간단하게 실행할 경우 launch 파일로 실행할 수 있습니다. encoder tick은 아래와 같이 확인이 가능합니다. 손으로 바퀴를 회전시키며 전진일 때 tick 증가, 후진일때 tick이 감소하는지 확인하세요.
실행 로그
... logging to /home/jetson/.ros/log/dbb4de82-d05d-11ed-b15e-48b02d2ecb9c/roslaunch-jp4612GCv346Py37-22365.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.
started roslaunch server <http://jp4612GCv346Py37:37641/>
SUMMARY
========
PARAMETERS
* /pwmConstants: [75, 42, 50, 80]
* /rosdistro: melodic
* /rosversion: 1.14.13
* /serial_node/baud: 115200
* /serial_node/port: /dev/esp32Nodemcu
NODES
/
serial_node (rosserial_python/serial_node.py)
auto-starting new master
process[master]: started with pid [22499]
ROS_MASTER_URI=http://localhost:11311
setting /run_id to dbb4de82-d05d-11ed-b15e-48b02d2ecb9c
process[rosout-1]: started with pid [22536]
started core service [/rosout]
process[serial_node-2]: started with pid [22539]
jetson@jp4612GCv346Py37:~/catkin_ws$ rostopic echo /left_ticks
data: 0
---
data: 0
---
data: 0
---
data: 0
---
data: 0
---
# 손으로 왼쪽 바퀴를 돌린다. data값이 증가한다.
:
data: 300
---
data: 300
---
:
// Encoder output to Arduino Interrupt pin. Tracks the tick count.
#define ENC_IN_LEFT_A 36 #EN_A1
#define ENC_IN_RIGHT_A 34 #EN_B1
// Other encoder output to Arduino to keep track of wheel direction
// Tracks the direction of rotation.
#define ENC_IN_LEFT_B 39 #EN_A2
#define ENC_IN_RIGHT_B 35 #EN_B2
4.1 Encoder counter ISR 함수
ENC_IN_RIGHT_A와 ENC_IN_RIGHT_B를 읽으면 모터의 방향을 확인할 수 있습니다. 방향에 따라 counter를 증가 또는 감소시킵니다. maximum보다 counter가 큰 경우 0로 reset합니다.
/////////////////////// Tick Data Publishing Functions ////////////////////////
// Increment the number of ticks
void IRAM_ATTR left_wheel_tick() {
// Read the value for the encoder for the left wheel
int val = digitalRead(ENC_IN_LEFT_B);
if (val == LOW) {
Direction_left = true; // Reverse
} else {
Direction_left = false; // Forward
}
if (Direction_left) {
if (left_wheel_tick_count.data == encoder_maximum) {
left_wheel_tick_count.data = encoder_minimum;
} else {
left_wheel_tick_count.data++;
}
} else {
if (left_wheel_tick_count.data == encoder_minimum) {
left_wheel_tick_count.data = encoder_maximum;
} else {
left_wheel_tick_count.data--;
}
}
}
// Increment the number of ticks
void IRAM_ATTR right_wheel_tick() {
// Read the value for the encoder for the right wheel
int val = digitalRead(ENC_IN_RIGHT_B);
if (val == LOW) {
Direction_right = false; // Reverse
} else {
Direction_right = true; // Forward
}
if (Direction_right) {
if (right_wheel_tick_count.data == encoder_maximum) {
right_wheel_tick_count.data = encoder_minimum;
} else {
right_wheel_tick_count.data++;
}
} else {
if (right_wheel_tick_count.data == encoder_minimum) {
right_wheel_tick_count.data = encoder_maximum;
} else {
right_wheel_tick_count.data--;
}
}
}
4.2 ISR함수 등록
// Every time the pin goes high, this is a tick
attachInterrupt(digitalPinToInterrupt(ENC_IN_LEFT_A), left_wheel_tick, RISING);
attachInterrupt(digitalPinToInterrupt(ENC_IN_RIGHT_A), right_wheel_tick, RISING);
3.3 ROS 기능 추가
ROS topic publish 기능을 추가합니다.
void setup() {
// Set pin states of the encoder
// ROS Setup
nh.getHardware()->setBaud(115200);
nh.initNode();
nh.advertise(rightPub);
nh.advertise(leftPub);
}
void loop() {
// Record the time
currentMillis = millis();
// If 100ms have passed, print the number of ticks
if (currentMillis - previousMillis > interval) {
previousMillis = currentMillis;
rightPub.publish(&right_wheel_tick_count);
leftPub.publish(&left_wheel_tick_count);
nh.spinOnce();
}
}