HackMD
RaspberryPi 4B + ubuntu18.04 + PIGPIO + ROS 輸出 servo motor pwm control signal
- The image was uploaded to a note which you don't have access to
- The note which the image was originally uploaded to has been deleted
使用Raspberry 4B + ubuntu、2023/07/06最後更新
準備硬體設備：
1、 Raspberry 4B 開發板一塊
2、 可由5V驅動的Servo Motor 一顆 (DS3235SG 3Pin為GND、VDD、SIGNAL)
3、 鍵盤(接Raspberry 4B 開發板 USB port)
4、 可透過RPI4B連接網路的環境
4、 邏輯分析儀一臺(非必要，可用來確認輸出波行是否正確)
5、 WindowsPC一臺(非必要，可接邏輯分析儀監控輸出波型)
應用：攔截網的觸發
Learn More →
- The image was uploaded to a note which you don't have access to
- The note which the image was originally uploaded to has been deleted
準備軟體：
假設已擁有裝好ubuntu18.04與ROS(Melodic Morenia版)的RaspberryPi 4B開發板，並可透過RPI4B連上網路。無須準備其他軟體。
大致分為以下五步驟：
1、在ubuntu18.04中安裝PIGPIO。
2、在ROS中創建名為cap的專案(工作區)。
3、修改CMakeLists.txt檔案使ROS的C Compiler可連結PIGPIO函式庫。
4、使用catkin_make指令編譯.c檔，產生.exe檔案，下指令執行(sudo -./xxxx.exe)。
5、透過邏輯分析儀監控指定的GPIO輸出PULSE是否正確(非必要)。
P.S. PIGPIO函式庫中已有servo pwm pulse的相關範例與函式，可快速上手使用。
1、在ubuntu18.04中安裝PIGPIO。
在ubuntu server 18.04命令列中透過以下指令下載與安裝PIGPIO
測試時是在 /home/user 位置執行下載、解壓縮並安裝
https://abyz.me.uk/rpi/pigpio/index.html PIGPIO官方網站。說明PIGPIO在RPI4B上的腳位編號(Pin Define) (RPI4B參考Type3)
(0)前兩行是在重新安裝PIGPIO時，要先把舊版的GPIO資料夾與壓縮檔刪除
rm master.zip
sudo rm -rf pigpio-mast
(1)若是第一次安裝，則只需輸入以下五條指令
安裝Python相關工具
sudo apt install python-setuptools python3-setuptools
下載pigpio最新版本
wget https://github.com/joan2937/pigpio/archive/master.zip
解壓縮
unzip master.zip
進入解壓縮的資料夾
cd /home/user/pigpio-master
安裝pigpio
make
sudo make install
(2)安裝後進行測試
關閉所有正在執行中的pigpio node(注意指令中pigpiod結尾的"d"不要忘記打)
沒先執行這行會報錯 pigpio Can't lock /var/run/pigpio.pid
sudo killall pigpiod
check C I/F
sudo ./x_pigpio
start daemon
sudo pigpiod
check C I/F to daemon
./x_pigpiod_if2*
check Python I/F to daemon
./x_pigpio.py
check pigs I/F to daemon
./x_pigs
check pipe I/F to daemon
./x_pipe
================================================================
安裝PIGPIO之參考網頁
https://abyz.me.uk/rpi/pigpio/index.html PIGPIO官方網站。PIGPIO在不同硬體上的腳位編號(Pin Define)
https://abyz.me.uk/rpi/pigpio/ PIGPIO官方網站 (卡巴斯基會觸發警告)
https://abyz.me.uk/rpi/pigpio/ PIGPIO官方網站 下載與安裝
https://blog.csdn.net/weixin_41756645/article/details/125417549
================================================================
2、在ROS中創建名為cap的專案(工作區)。
在/home/user路徑下創建名為"cap"的工作資料夾
cd /home/user
mkdir cap
進入剛才創建的cap資料夾
cd /home/user/cap
在工作區中創建一個src資料夾用來放置package資料夾與程式檔案
mkdir src
進入剛才創建的src資料夾，創建一個名為catch_net的Package
cd /home/user/cap/src
catkin_create_pkg catch_net std_msgs rospy roscpp
在Package的資料夾中再創建一個src資料夾用來放置要執行的Node(程式檔案)
mkdir src
cd /home/user/cap/src/catch_net/src
創建名為servo_trig.c的Node並將要執行的動作寫進.c檔案
sudo vim servo_trig.c
================================================================
https://ithelp.ithome.com.tw/articles/10202723
https://ithelp.ithome.com.tw/articles/10203126
================================================================
3、修改CMakeLists.txt檔使ROS的C Compiler連結PIGPIO函式庫。
返回上一層，開啟CMakeLists.txt檔準備修改
cd /home/user/cap/src/catch_net
sudo vim CMakeLists.txt
//...上略
## Declare a C++ executable
## ...
## ...
## ...
add_executable(servo_trig.exe src/servo_trig.c)
target_link_libraries(servo_trig.exe ${catkin_LIBRARIES} -lpigpio -lrt)
## Rename C++ executable without prefix
//...下略
-lpigpio：pigpio library。連結pigpio函式庫。
-lrt：Realtime Extensions library。
4、catkin_make編譯.c檔，產生.exe，執行(sudo -./xxxx.exe)。
回到cap專案資料夾(工作區)並使用編譯器將.c檔編譯成.exe
cd /home/user/cap
catkin_make
進入檔資料夾執行編譯後產生的.exe檔
cd /home/user/cap/devel/lib/catch_net
sudo ./servo_trig.exe
P.S.
第二行
sudo ./servo_trig.exe
後面可以接數字，
代表要求輸出PWM的GPIO腳位編號，
若不輸入任何數字則默認用GPIO 4腳位(pin7)輸出。
一般來說要依據Servo控制pin所接的位置來決定要用哪個腳位輸出。
例如輸入
sudo ./servo_trig.exe 4 22
代表要求GPIO 4(pin7)與GPIO 22(pin15)兩個腳位同時輸出PWM訊號。
Raspberry 4b GPIO Pinout可以在官方網站查詢
https://pinout.xyz/
================================================================
================================================================
5、透過邏輯分析儀監控指定的GPIO輸出PULSE是否正確(非必要)。
================================================================
範例：使用邏輯分析儀檢查以下資訊
pigpio第4pin輸出的pwm訊號是否為50Hz，波寬1100us(1.1ms)/1900us(1.9ms)?
================================================================
P.S.1
以下測試用程式碼以50Hz控制頻率(PWM週期20ms)，每隔400ms切換1100us/1900us pwm pulse width驅動servo轉動。
P.S.2
以下內容是從官方Servo Pulse Generator範例文件Servo_demo.c修改而來。
#ifdef __cplusplus
extern "C"
{
#include <stdio.h>
#include <stdlib.h>
#include <signal.h>
#include <pigpio.h>
#include <time.h>
}
#endif
#include <ros/ros.h>
#include <termios.h>
#include <sys/select.h>
#include <stropts.h>
#include <unistd.h>
#include <sys/ioctl.h>
#define NUM_GPIO 32
#define MIN_WIDTH 500
#define MAX_WIDTH 2500
#define TRIG_GPIO_NUM 4
int run=1;
int step[NUM_GPIO];
int width[NUM_GPIO];
int used[NUM_GPIO];
//===========
#define TRIG_PERIOD 400000000 //nsec
#define MID_WIDTH 1500 //usec
#define OFFSET_WIDTH 400 //usec
#define TRIG_DURATION 350000000 //nsec
#define TRIG_WIDTH 1050 //usec
#define UNTRIG_WIDTH 1420 //usec
#define CMD_LEN 1
struct timespec trig_prev, t_now;
//===========
static int peek_character = -1;
int readch(void);
int _kbhit(void);
int randint(int from, int to)
{
return (random() % (to - from + 1)) + from;
}
void stop(int signum)
{
run = 0;
}
int readch()
{
char ch;
if(peek_character != -1)
{
ch = peek_character;
peek_character = -1;
return ch;
}
read(0,&ch,1);
return ch;
}
int _kbhit()
{
static const int STDIN = 0;
static bool initialized = false;
if (!initialized)
{
termios term;
tcgetattr(STDIN, &term);
term.c_lflag &= ~ICANON;
tcsetattr(STDIN,TCSANOW, &term);
setbuf(stdin, NULL);
initialized = true;
}
int bytesWaiting;
ioctl(STDIN, FIONREAD, &bytesWaiting);
return bytesWaiting;
}
int main(int argc, char *argv[])
{
int i, g;
if(gpioInitialise() < 0) return -1;
gpioSetSignalFunc(SIGINT, stop);
if(argc==1) used[TRIG_GPIO_NUM]=1;
else
{
for(i=1;i<argc;i++)
{
g = atoi(argv[i]);
if((g>=0) && (g<NUM_GPIO)) used[g] = 1;
}
}
for(g=0; g<NUM_GPIO; g++)
{
if(used[g])
{
width[g]=UNTRIG_WIDTH;
gpioServo(g, width[g]);
printf("[%d %d]\n", g, width[g]);
}
}
printf("Sending servos pulses to GPIO");
printf(", control C to stop.\n");
while(run)
{
int bytes_rcv = _kbhit();
if(bytes_rcv)
{
int c[CMD_LEN];
for(int i=0;i<CMD_LEN;i++)
{
c[i] = readch();
}
printf("\nlen:%d key:%d\n",bytes_rcv,c[0]);
if(c[0]=='#')
{
printf("\nlen:%d key:%d Trigger.\n",bytes_rcv,c[0]);
for(g=0; g<NUM_GPIO; g++)
{
if(used[g])
{
width[g]=TRIG_WIDTH;
gpioServo(g, width[g]);
printf("[%d %d]\n", g, width[g]);
}
}
time_sleep((float)TRIG_DURATION/(float)1000000000);
for(g=0; g<NUM_GPIO; g++)
{
if(used[g])
{
width[g]=UNTRIG_WIDTH;
gpioServo(g, width[g]);
printf("[%d %d]\n", g, width[g]);
}
}
time_sleep((float)TRIG_DURATION/(float)1000000000);
}
}
}
printf("\ntidying up\n");
for(g=0; g<NUM_GPIO; g++)
{
if(used[g]) gpioServo(g, 0);
}
gpioTerminate();
return 0;
}
P.S.3
以下內容是Shaw(紹瑋)最後實際寫進ROS的Pytron程式碼，用來控制Servo轉動固定角度，觸發攔截網發射。
import pigpio
import time
import rospy
from std_msgs.msg import Int32
servo_pin = 4 # GPIO pin number where the servo signal wire is connected
pi = pigpio.pi()
# Function to set the servo angle
fire = 0
def fire_cb(msg):
global fire
fire = msg.data
if fire == 1:
rospy.loginfo("Fire!")
pi.set_servo_pulsewidth(servo_pin, 1050)
time.sleep(2)
pi.set_servo_pulsewidth(servo_pin, 1420)
if __name__ == '__main__':
rospy.init_node('gps_init_py')
fire_sub = rospy.Subscriber('/fire', Int32, fire_cb)
# Create a PWM object with a frequency of 50Hz
pi.set_servo_pulsewidth(servo_pin, 1420)
rospy.spin()
