基于树莓派ubuntu20.04的ros-noetic小车
目录
一、小车的架构
1.1 总体的概述
1.2 驱动系统
1.3 控制系统
二、驱动系统开发
2.1 PC端Ubuntu20.04安装
2.2 树莓派Ubuntu20.04安装
2.3 PC端虚拟机设置静态IP
2.4 树莓派设置静态IP
2.5 树莓派启动ssh进行远程开发
2.5 arduino ide 开发环境搭建
2.5.1 PC端Ubuntu20.04安装arduino ide
2.5.2 PC端arduino的配置
2.5.3 将arduino连接到Ubuntu
2.6 arduino mega2560的连接
2.7 小车底盘运动控制
ROSArduinoBridge.ino
commands.h
diff_controller.h
encoder_driver.h
encoder_driver.ino
motor_driver.h
motor_driver.ino
代码解释
PID调试
底层驱动调试
三、控制系统开发
3.1 将PC的虚拟机设置为桥接模式
3.2 ROS-noetic的安装
3.3 ROS 分布式通信
3.3.1 修改 .bashrc文件
3.3.2 测试ros分布式通信
3.3.3 ros_arduino_bridge上位机参数的修改。
3.3.5 创建树莓派的工作空间
3.4 实地测试小车
四、结语
一、小车的架构
1.1 总体的概述
本文从零开始为读者介绍做ros小车的过程，主要是用Arduino的mega2560作为下位机，以树莓派作为上位机，用ros-arduino-bridge作为通讯的桥梁，向mega2560发送各种指令，目前本文所作的小车只是用了PC端的上的键盘节点来控制小车运动，后续应该考虑加入更多的功能，例如自主导航和自主避障。
1.2 驱动系统
驱动控制系统是由Arduino的国产mega2560、电机驱动TB6612组成的。Arduino将树莓派上位机发送的控制指令转换成对应的响应。
1.3 控制系统
树莓派和PC上的Ubuntu20.04，以及搭载在Ubuntu上的ROS-noetic系统。
二、驱动系统开发
使用开源的ros-arduino-bridge作为小车的驱动程序，ros-arduino-bridge主要是用作双轮的差速驱动的小车或者是机器人上的，如果需要作为四轮的小车的话，需要自己去修改很多东西。
2.1 PC端Ubuntu20.04安装
需要在PC端安装Ubuntu20.04的话，首先需要先下载vmware 虚拟机管理器
Vmware下载Vmware下载Vmware下载
随后需要去下载Ubuntu20.04的镜像，如果去官方下载的话，需要花费很长的时间，所以我还是建议去国内的镜像网站下载，作者推荐的网站有：
清华源tuna: Index of /ubuntu-releases/20.04/ | 清华大学开源软件镜像站 | Tsinghua Open Source Mirror
阿里云镜像：
阿里巴巴开源镜像站-OPSX镜像站-阿里云开发者社区 (aliyun.com)
下载完镜像之后，就需要将Ubuntu镜像导入到VMware上了。
选择新建虚拟机
选择自己刚刚下载的镜像的地址，后面就是设置虚拟机的名字和用户名的那些，磁盘大小那些可以按照他推荐的那些就可以了，后续不够的话可以自己再另行修改。
刚安装的Ubuntu首次进去可能会比较慢，所以请耐心等待，在开机进去之后，可能会让你进行更新，更新的过程也会很慢，等他更新完之后就可以正常使用了。打开Ubuntu终端的快捷命令是 CTRL ATL T，记着这个快捷命令会比较好，因为后面会打开多个终端，用快捷命令可以提高我们的工作效率。
下载完成之后，我们需要给Ubuntu安装上SSH，在后续进行远程开发的时候会用到。
首先先更新系统的软件包：
sudo apt update安装OpenSSH服务：
sudo apt install openssh-server启动SSH服务：
sudo systemctl start ssh设置 SSH 开机自启，确保 SSH 服务在每次启动时自动启动：
sudo systemctl enable ssh为了保险起见，最好还是检查 SSH 是否正在运行：
sudo systemctl status ssh如果成功安装了，那么status会显示active或者是running的状态，这样就算成功安装了。
有读者可能会问，为什么不先进行换源呢，其实换源也可以，换源之后下载东西就更快了，但是现在这个阶段没有必要，因为在后续安装ros的时候还会再换一次源，所以我们就不用现在大费心思去搞换源了，直接跟着步骤走就行。
2.2 树莓派Ubuntu20.04安装
使用树莓派下载Ubuntu会比较方便一点，因为树莓派有他自己的烧录软件，可以很方便的帮我们烧录系统，本文使用的树莓派系统是树莓派4b。为什么不用Ubuntu18.04呢，18.04比20.04更见的稳定，也没有那么多的bug，究其原因就是因为树莓派4b，作者的树莓派4b不支持Ubuntu18.04，在烧录18.04进去之后，会启动不到系统的内核，后面发现系统提示 please update to the newest，很可惜作者的树莓派4b不支持Ubuntu18.04，所以后来只能决定安装Ubuntu20.04了。
使用树莓派4b烧录系统时，首先需要下载官方的烧录软甲 Raspberry Pi Imager
https://www.raspberrypi.com/software/
打开之后我们选择raspberry pi4
选择要写入的操作系统的时候，选择other general-purpose OS，然后选择Ubuntu,在烧录器下载的20.04只能是Ubuntu server，也就是没有桌面的那种，所以后面可能需要你自己安装桌面，其实不安装桌面也行，看你个人意愿。由于作者的是4b，所以就选择了Ubuntu server的64-bit
在选完SD卡烧录之后，烧录器会让你编辑一些系统的设置，你需要将你自己的WiFi名字、用户名以及密码等输入进去，需要注意的是，你的WiFi名最好是中间是没有空格的，不然你的WiFi是不能正常被连接到的，也就不能使用ssh了，如果真的是不能修改的话，那你只能后面在烧录完之后通过串口登录或者让直接用树莓派接上一个显示器来修改netplan文件了，接下来就教读者如何修改，如果开机后可以正常连接上WiFi的话，就不用管下面这一些了。
先是直接用显示屏连接的树莓派：先进入到这个文件
sudo cd /etc/netplan/这个文件下应该是有一个yaml文件的，此时你需要进入这个文件进行修改， 每个人的文件名字可能会不一样，请读者根据自己的名字修改
sudo vim 01-network-manager-all.yaml文件里面的格式应该是下面的这个，vim的使用方法是，按 i 进去写模式，写完之后按Esc退出，按冒号：进入命令模式，最后输入 wq ，回车，退出vim，w的意思是写，q的意思是退出，如果你后续修改的时候，发现写错了很多，但又不想一个个修改，就可以直接用 q! 来强制退出vim，强制退出是不会帮你保存的。
network:
version: 2
wifis:
wlan0:
dhcp4: true
access-points:
"YOUR_SSID": # 将 YOUR_SSID 替换为你的 Wi-Fi 名称
password: "YOUR_PASSWORD" # 将 YOUR_PASSWORD 替换为你的 Wi-Fi 密码找到你刚刚在烧录器填的WiFi名字，把那个名字加上双引号“”，然后就可以退出vim了。
执行以下命令进行应用,然后等一下，应该就能连接成功了，如果连接不成功的话，尝试重启一下树莓派。
sudo netplan apply然后是串口连接的树莓派，串口连接是为了应对那些手上没有多余显示屏的读者的，串口连接就是连线的时候会麻烦一点，只要进入了命令行之后，修改的方法和上述都是一样的。将usb转ttl的TX接到树莓派的RX上，RX接到树莓派的TX上。
要注意，如果你不是使用电脑的电源供电的话，你需要将usb转ttl的GND也接到树莓派上，如果你是使用电脑来供电的话，就不用接GND来实现公地了，usb转ttl的5V可以不接，防止树莓派自己启动。
当接好线之后，使用一个很方便的工具MobaXterm来连接，该工具的下载地址是：
https://mobaxterm.mobatek.net/
下好之后请自行连接，树莓派下载的Ubuntu20.04默认的波特率是115200。当你配置好后，此时可能会出现黑框但是没有数据，此时需要将树莓派重新充电，将供电口拔了再重新接上。
2.3 PC端虚拟机设置静态IP
编辑Netoplan文件
sudo vim /etc/netplan/01-network-manager-all.yaml修改文件的内容，将DHCP4自动分配IP关闭，并设置自己的静态IP，设置的静态IP一定要和树莓派上设置的静态IP的网关一致，不然后续PC的虚拟机将不能和树莓派通信。
# Let NetworkManager manage all devices on this system
network:
version: 2
renderer: NetworkManager
ethernets:
ens33:
dhcp4: no
addresses:
- 192.168.43.126/24 # 设置静态 IP 地址
gateway4: 192.168.43.1 # 默认网关
nameservers:
addresses:
- 8.8.8.8 # DNS 服务器
- 8.8.4.4
我这里将我的虚拟机的静态IP设置为192.168.43.126，树莓派的静态IP设置为192.168.43.128。那个接口的名字，按照你实际的名字来设置，我这里的名字是ens33，你可以通过ifconfig命令来查看你的接口名字。
pi@ubuntu:~$ ifconfig
ens33: flags=4163<UP,BROADCAST,RUNNING,MULTICAST> mtu 1500
inet 192.168.43.126 netmask 255.255.255.0 broadcast 192.168.43.255
inet6 fe80::20c:29ff:fe55:a54e prefixlen 64 scopeid 0x20<link>
ether 00:0c:29:55:a5:4e txqueuelen 1000 (Ethernet)
RX packets 69 bytes 6722 (6.7 KB)
RX errors 0 dropped 0 overruns 0 frame 0
TX packets 103 bytes 9535 (9.5 KB)
TX errors 0 dropped 0 overruns 0 carrier 0 collisions 0
lo: flags=73<UP,LOOPBACK,RUNNING> mtu 65536
inet 127.0.0.1 netmask 255.0.0.0
inet6 ::1 prefixlen 128 scopeid 0x10<host>
loop txqueuelen 1000 (Local Loopback)
RX packets 277 bytes 23706 (23.7 KB)
RX errors 0 dropped 0 overruns 0 frame 0
TX packets 277 bytes 23706 (23.7 KB)
TX errors 0 dropped 0 overruns 0 carrier 0 collisions 0
应用新的网络配置
sudo netplan apply2.4 树莓派设置静态IP
趁现在有线连接着树莓派，我们可以为树莓派设置一个静态IP。
sudo vim /etc/netplan/01-network-manager-all.yaml也是打开刚刚的那个文件，加入这段东西
addresses:
- 192.168.43.128/24
gateway4: 192.168.43.1 # 根据你的网络设置调整
nameservers:
addresses:
- 8.8.8.8
- 8.8.4.4 整个文件里面就是这样的
network:
version: 2
renderer: networkd
wifis:
wlan0:
access-points:
"YOUR_SSID":
password: "YOUR_PAWWWORD"
dhcp4: false
addresses:
- 192.168.43.128/24 #你的静态IP
gateway4: 192.168.43.1 # 根据你的网络设置调整
nameservers:
addresses:
- 8.8.8.8
- 8.8.4.4
optional: true
设置完静态IP之后，就可以去进行ssh的远程开发了。
你可以使用 ip addr 命令来查看静态IP是否设置成功。
2.5 树莓派启动ssh进行远程开发
使用MobaXterm开发工具就可以使用ssh连接树莓派了，MobaXterm是一个强大的开发工具，他提供了多种的连接方式，也提供了 X 服务器，X服务器使得我们在后续即使不安装Ubuntu桌面也可以正常使用到ros的功能。
由于我们在烧录系统的时候已经在编辑系统设置将SSH打开了，所以我们不用自己去开启SSH，新建一个会话，选择SSH，远程主机输入你刚刚给树莓派设置的静态IP，用户名一般指定为pi，端口号为22。
成功连接之后就会显示以下的界面，要注意最后一行那个do-release-upgrade不要管他，要是运行了他的话那你的系统将会自动被升级到Ubuntu24.04了。连接到了SSH之后，就可以只给树莓派供电了，现在就可以开始远程开发了。
2.6 arduino ide 开发环境搭建
arduino ide 需要装在PC端的虚拟机上，以便于进行程序的修改、烧录和一些调试等，在树莓派上读者也可自行决定是否要安装arduino ide，因为在树莓派上也安装上的话就可以方便后续的一些调试。
2.6.1 PC端Ubuntu20.04安装arduino ide
在Ubuntu中是可以直接用apt来下载到arduino的，但是通过apt下载到的arduino的版本一般是比较旧的，通常很多的好用的新功能他都没有，例如说作者用sudo apt install arduino下载到的arduino是没有那个串口绘图仪的，这对后面调PID的时候很不方便，所以还是建议读者自行手动下载，我们可以通过下面这个网站下载到Arduino软件：
https://www.arduino.cc/en/software
我们可以下载这个文件arduino-ide_2.3.2_Linux_64bit.AppImage，也就是Linux在x86架构下的64位，这是一个可执行文件，直接运行就可以了，作者是把该软件放在了桌面上，以免找不到，读者也可以在根目录下创建一个application文件夹来收录这些软件。
由于我们是在网站上下载的，所以我们要想办法将该文件传输到我们的虚拟机上，将文件传输上去我们可以使用一个很方便的开发工具：FileZilla，FileZilla是一个很方便的文件传输的工具，他可以通过SSH（这个在上面安装Ubuntu的时候已经开启了，没开的记得往回看看）和虚拟机连接上，从而给虚拟机发送文件。
FileZilla的下载地址如下：
https://www.filezilla.cn/download
输入你虚拟机的ip地址和用户名等，端口号是22，直接点快速连接就可以了，然后把 arduino-ide_2.3.2_Linux_64bit.AppImage拖过去就能上传成功了。
得到该执行文件之后，你需要在终端执行以下命令赋予他可执行的权限。
chmod +x arduino-ide_2.3.2_Linux_64bit.AppImage接下来，运行该文件：
./arduino-ide_2.3.2_Linux_64bit.AppImage这将直接启动 Arduino IDE，无需进行传统的安装过程，是不是很方便。
2.6.2 PC端arduino的配置
在我们进入到arduino之后，有些读者可以看不懂英文，此时我们就可以点开左上角的File，选择下面的倒数第二个那个首选项（Preferences） ，将编辑器语言改成中文的就行。在我们写代码的时候，有些读者可能没有arduino的mega2560，手上只有ESP8266或者ESP32等其他的板子，但是在Arduino上又找不到该类型的板子，所以这时候我们需要自行来添加板子。
首先也是点开首选项，选择下面的附加开发板管理器网址
将下面的这个网址添加到里面去，这是ESP8266的URL，如果读者需要其他的板子，请另行去寻找，本文只教学如何添加板子。
http://arduino.esp8266.com/stable/package_esp8266com_index.json添加完之后，选择工具—>开发板管理器，最后搜索一下esp8266，进行下载即可，下载的过程中如果没有魔法的话，可能会下载得比较慢，所以请耐心等待。
2.6.3 树莓派安装arduino ide
树莓派arduino的安装包也是在刚刚那个网站下载，在那个网站滑下去就可以看见有以下这些：
树莓派4b的架构是arm64，所以选择Linux 64 bits那个下载就行，下载完之后应该会得到一个压缩包 arduino-1.8.19-linuxaarch64.tar.xz，此时也是要用FileZilla开发工具将该压缩包上传到树莓派上。
首先，进入到下载文件所在的目录并解压 arduino-1.8.19-linuxaarch64.tar.xz 文件：
cd Desktoptar -xvf arduino-1.8.19-linuxaarch64.tar.xz将解压出来的 Arduino IDE 文件夹移动到/opt目录，这是一个常见的用于存放可选软件包的位置：
sudo mv arduino-1.8.19 /opt/运行安装脚本，确保一切必要的依赖项都已安装并配置好：
cd /opt/arduino-1.8.19
sudo ./install.sh这将会自动为 Arduino IDE 创建桌面图标和启动器。
启动arduino ide，我们可以直接在终端运行arduino来打开ide
arduino此时下载就完成了。
2.6.4 将arduino连接到Ubuntu
PC端和树莓派的操作方法都是一致的，此处只演示在PC端虚拟机的做法。
arduino Mega 2560 开发板连接至 USB 端口，选择连接到虚拟机。
在Ubuntu中查看USB设备是否连接，名字是ttyUSB0或者是ttyACM0
sudo ll /dev/ttyUSB0查看之后，会显示出一个dialout的用户，我们需要将用户添加到dialout用户组，获得对该设备的操作权限，如果不操作这一步，会识别不到arduino的端口。用groups命令可查看当前的用户组，里面并没有dialout。
sudo usermod -a -G dialout pi #你自己的用户名执行该命令之后，dialout就成功加入到用户组当中了，pi那个参数是你自己系统的用户名字。要注意的是，在添加了新的用户之后，需要重启才能将用户组更新。
2.6 arduino mega2560的连接
与mega2560的连接：工具—>开发板—>选择板子Arduino Mega or Mega 2560，端口：选择你刚刚查看到的设备端口，连接成功之后，可以试着烧录一些案例进去测试一下是否有问题。
如果识别不到端口，在ubuntu下，可能预置安装了一个叫brltty的程序与Arduino有冲突，卸载即可
sudo apt-get remove brlttyLED灯闪烁
/*
Blink without Delay
Turns on and off a light emitting diode (LED) connected to a digital pin,
without using the delay() function. This means that other code can run at the
same time without being interrupted by the LED code.
The circuit:
- Use the onboard LED.
- Note: Most Arduinos have an on-board LED you can control. On the UNO, MEGA
and ZERO it is attached to digital pin 13, on MKR1000 on pin 6. LED_BUILTIN
is set to the correct LED pin independent of which board is used.
If you want to know what pin the on-board LED is connected to on your
Arduino model, check the Technical Specs of your board at:
https://www.arduino.cc/en/Main/Products
created 2005
by David A. Mellis
modified 8 Feb 2010
by Paul Stoffregen
modified 11 Nov 2013
by Scott Fitzgerald
modified 9 Jan 2017
by Arturo Guadalupi
This example code is in the public domain.
https://www.arduino.cc/en/Tutorial/BuiltInExamples/BlinkWithoutDelay
*/
// constants won't change. Used here to set a pin number:
const int ledPin = LED_BUILTIN; // the number of the LED pin
// Variables will change:
int ledState = LOW; // ledState used to set the LED
// Generally, you should use "unsigned long" for variables that hold time
// The value will quickly become too large for an int to store
unsigned long previousMillis = 0; // will store last time LED was updated
// constants won't change:
const long interval = 1000; // interval at which to blink (milliseconds)
void setup() {
// set the digital pin as output:
pinMode(ledPin, OUTPUT);
}
void loop() {
// here is where you'd put code that needs to be running all the time.
// check to see if it's time to blink the LED; that is, if the difference
// between the current time and last time you blinked the LED is bigger than
// the interval at which you want to blink the LED.
unsigned long currentMillis = millis();
if (currentMillis - previousMillis >= interval) {
// save the last time you blinked the LED
previousMillis = currentMillis;
// if the LED is off turn it on and vice-versa:
if (ledState == LOW) {
ledState = HIGH;
} else {
ledState = LOW;
}
// set the LED with the ledState of the variable:
digitalWrite(ledPin, ledState);
}
}
2.7 小车底盘运动控制
在GitHub上有现成封装好的ros_arduino_bridge的驱动包，可以很好的提高我们的开发效率，但是要注意：有些ros_arduino_bridge只支持Ubuntu18.04的melodic，不支持Ubuntu20.04的最新版noetic，要是你下载错的话，一开始是没有出现情况的，但是到你连接arduino的时候，会出现错误，所以一定一定要下载python3版本的ros_arduino_bridge，不然到后面将不能正常和arduino通信，会出现 Cannot connect to Arduino!
https://github.com/LowPower-Center/ROS_Arduino_bridge
我们需要修改固件包的源码，文件的路径是/home/pi/ROS_Arduino_bridge-main/ros_arduino_bridge_Python3/ros_arduino_firmwaree/src/libraries/ROSArduinoBridge，并将其烧录到arduino mega2560。
打开arduino ide，将这个文件夹打开，就可以开始修改了，以下我就直接将修改后的源码贴在下面了。
ROSArduinoBridge.ino
/***************************************************************
Motor driver definitions
Add a "#elif defined" block to this file to include support
for a particular motor driver. Then add the appropriate
#define near the top of the main ROSArduinoBridge.ino file.
*************************************************************/
#ifdef USE_BASE
#ifdef POLOLU_VNH5019
/* Include the Pololu library */
#include "DualVNH5019MotorShield.h"
/* Create the motor driver object */
DualVNH5019MotorShield drive;
/* Wrap the motor driver initialization */
void initMotorController() {
drive.init();
}
/* Wrap the drive motor set speed function */
void setMotorSpeed(int i, int spd) {
if (i == LEFT) drive.setM1Speed(spd);
else drive.setM2Speed(spd);
}
// A convenience function for setting both motor speeds
void setMotorSpeeds(int leftSpeed, int rightSpeed) {
setMotorSpeed(LEFT, leftSpeed);
setMotorSpeed(RIGHT, rightSpeed);
}
#elif defined POLOLU_MC33926
/* Include the Pololu library */
#include "DualMC33926MotorShield.h"
/* Create the motor driver object */
DualMC33926MotorShield drive;
/* Wrap the motor driver initialization */
void initMotorController() {
drive.init();
}
/* Wrap the drive motor set speed function */
void setMotorSpeed(int i, int spd) {
if (i == LEFT) drive.setM1Speed(spd);
else drive.setM2Speed(spd);
}
// A convenience function for setting both motor speeds
void setMotorSpeeds(int leftSpeed, int rightSpeed) {
setMotorSpeed(LEFT, leftSpeed);
setMotorSpeed(RIGHT, rightSpeed);
}
#elif defined L298_MOTOR_DRIVER
void initMotorController() {
digitalWrite(RIGHT_MOTOR_ENABLE, HIGH);
digitalWrite(LEFT_MOTOR_ENABLE, HIGH);
}
void setMotorSpeed(int i, int spd) {
unsigned char reverse = 0;
if (spd < 0)
{
spd = -spd;
reverse = 1;
}
if (spd > 255)
spd = 255;
if (i == LEFT) {
if (reverse == 0) { analogWrite(RIGHT_MOTOR_FORWARD, spd); analogWrite(RIGHT_MOTOR_BACKWARD, 0); }
else if (reverse == 1) { analogWrite(RIGHT_MOTOR_BACKWARD, spd); analogWrite(RIGHT_MOTOR_FORWARD, 0); }
}
else /*if (i == RIGHT) //no need for condition*/ {
if (reverse == 0) { analogWrite(LEFT_MOTOR_FORWARD, spd); analogWrite(LEFT_MOTOR_BACKWARD, 0); }
else if (reverse == 1) { analogWrite(LEFT_MOTOR_BACKWARD, spd); analogWrite(LEFT_MOTOR_FORWARD, 0); }
}
}
void setMotorSpeeds(int leftSpeed, int rightSpeed) {
setMotorSpeed(LEFT, leftSpeed);
setMotorSpeed(RIGHT, rightSpeed);
}
#else
#error A motor driver must be selected!
#endif
#endif
commands.h
/* Define single-letter commands that will be sent by the PC over the
serial link.
*/
#ifndef COMMANDS_H
#define COMMANDS_H
#define ANALOG_READ 'a'
#define GET_BAUDRATE 'b'
#define PIN_MODE 'c'
#define DIGITAL_READ 'd'
#define READ_ENCODERS 'e'
#define MOTOR_SPEEDS 'm'
#define PING 'p'
#define RESET_ENCODERS 'r'
#define SERVO_WRITE 's'
#define SERVO_READ 't'
#define UPDATE_PID 'u'
#define DIGITAL_WRITE 'w'
#define ANALOG_WRITE 'x'
#define LEFT 0
#define RIGHT 1
#endif
diff_controller.h
/* Functions and type-defs for PID control.
Taken mostly from Mike Ferguson's ArbotiX code which lives at:
http://vanadium-ros-pkg.googlecode.com/svn/trunk/arbotix/
*/
/* PID setpoint info For a Motor */
typedef struct {
double TargetTicksPerFrame; // target speed in ticks per frame
long Encoder; // encoder count
long PrevEnc; // last encoder count
/*
* Using previous input (PrevInput) instead of PrevError to avoid derivative kick,
* see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-derivative-kick/
*/
int PrevInput; // last input
//int PrevErr; // last error
/*
* Using integrated term (ITerm) instead of integrated error (Ierror),
* to allow tuning changes,
* see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/
*/
//int Ierror;
int ITerm; //integrated term
long output; // last motor setting
}
SetPointInfo;
SetPointInfo leftPID, rightPID;
/* PID Parameters */
int Kp = 20;
int Kd = 12;
int Ki = 0;
int Ko = 50;
unsigned char moving = 0; // is the base in motion?
/*
* Initialize PID variables to zero to prevent startup spikes
* when turning PID on to start moving
* In particular, assign both Encoder and PrevEnc the current encoder value
* See http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-initialization/
* Note that the assumption here is that PID is only turned on
* when going from stop to moving, that's why we can init everything on zero.
*/
void resetPID(){
leftPID.TargetTicksPerFrame = 0.0;
leftPID.Encoder = readEncoder(LEFT);
leftPID.PrevEnc = leftPID.Encoder;
leftPID.output = 0;
leftPID.PrevInput = 0;
leftPID.ITerm = 0;
rightPID.TargetTicksPerFrame = 0.0;
rightPID.Encoder = readEncoder(RIGHT);
rightPID.PrevEnc = rightPID.Encoder;
rightPID.output = 0;
rightPID.PrevInput = 0;
rightPID.ITerm = 0;
}
/* PID routine to compute the next motor commands */
void doPID(SetPointInfo * p) {
long Perror;
long output;
int input;
//Perror = p->TargetTicksPerFrame - (p->Encoder - p->PrevEnc);
input = p->Encoder - p->PrevEnc;
Perror = p->TargetTicksPerFrame - input;
/*
* Avoid derivative kick and allow tuning changes,
* see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-derivative-kick/
* see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/
*/
//output = (Kp * Perror + Kd * (Perror - p->PrevErr) + Ki * p->Ierror) / Ko;
// p->PrevErr = Perror;
output = (Kp * Perror - Kd * (input - p->PrevInput) + p->ITerm) / Ko;
p->PrevEnc = p->Encoder;
output += p->output;
// Accumulate Integral error *or* Limit output.
// Stop accumulating when output saturates
if (output >= MAX_PWM)
output = MAX_PWM;
else if (output <= -MAX_PWM)
output = -MAX_PWM;
else
/*
* allow turning changes, see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/
*/
p->ITerm += Ki * Perror;
p->output = output;
p->PrevInput = input;
}
/* Read the encoder values and call the PID routine */
void updatePID() {
/* Read the encoders */
leftPID.Encoder = readEncoder(LEFT);
rightPID.Encoder = readEncoder(RIGHT);
/* If we're not moving there is nothing more to do */
if (!moving){
/*
* Reset PIDs once, to prevent startup spikes,
* see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-initialization/
* PrevInput is considered a good proxy to detect
* whether reset has already happened
*/
if (leftPID.PrevInput != 0 || rightPID.PrevInput != 0) resetPID();
return;
}
/* Compute PID update for each motor */
doPID(&rightPID);
doPID(&leftPID);
/* Set the motor speeds accordingly */
setMotorSpeeds(leftPID.output, rightPID.output);
}
encoder_driver.h
/* *************************************************************
Encoder driver function definitions - by James Nugen
************************************************************ */
#ifdef ARDUINO_ENC_COUNTER
//below can be changed, but should be PORTD pins;
//otherwise additional changes in the code are required
#define LEFT_ENC_PIN_A PD2 //pin 2
#define LEFT_ENC_PIN_B PD3 //pin 3
//below can be changed, but should be PORTC pins
#define RIGHT_ENC_PIN_A PC4 //pin A4
#define RIGHT_ENC_PIN_B PC5 //pin A5
#endif
long readEncoder(int i);
void resetEncoder(int i);
void resetEncoders();
encoder_driver.ino
由于我这是测试版本的小车，所以并没有装编码器，但为了后续的拓展，我也将编码器的代码写了。
/* *************************************************************
Encoder definitions
Add an "#ifdef" block to this file to include support for
a particular encoder board or library. Then add the appropriate
#define near the top of the main ROSArduinoBridge.ino file.
************************************************************ */
#ifdef USE_BASE
#ifdef ROBOGAIA
/* The Robogaia Mega Encoder shield */
#include "MegaEncoderCounter.h"
/* Create the encoder shield object */
MegaEncoderCounter encoders = MegaEncoderCounter(4); // Initializes the Mega Encoder Counter in the 4X Count mode
/* Wrap the encoder reading function */
long readEncoder(int i) {
if (i == LEFT) return encoders.YAxisGetCount();
else return encoders.XAxisGetCount();
}
/* Wrap the encoder reset function */
void resetEncoder(int i) {
if (i == LEFT) return encoders.YAxisReset();
else return encoders.XAxisReset();
}
#elif defined(ARDUINO_ENC_COUNTER)
volatile long left_enc_pos = 0L;
volatile long right_enc_pos = 0L;
static const int8_t ENC_STATES [] = {0,1,-1,0,-1,0,0,1,1,0,0,-1,0,-1,1,0}; //encoder lookup table
/* Interrupt routine for LEFT encoder, taking care of actual counting */
ISR (PCINT2_vect){
static uint8_t enc_last=0;
enc_last <<=2; //shift previous state two places
enc_last |= (PIND & (3 << 2)) >> 2; //read the current state into lowest 2 bits
left_enc_pos += ENC_STATES[(enc_last & 0x0f)];
}
/* Interrupt routine for RIGHT encoder, taking care of actual counting */
ISR (PCINT1_vect){
static uint8_t enc_last=0;
enc_last <<=2; //shift previous state two places
enc_last |= (PINC & (3 << 4)) >> 4; //read the current state into lowest 2 bits
right_enc_pos += ENC_STATES[(enc_last & 0x0f)];
}
/* Wrap the encoder reading function */
long readEncoder(int i) {
if (i == LEFT) return left_enc_pos;
else return right_enc_pos;
}
/* Wrap the encoder reset function */
void resetEncoder(int i) {
if (i == LEFT){
left_enc_pos=0L;
return;
} else {
right_enc_pos=0L;
return;
}
}
#else
#error A encoder driver must be selected!
#endif
/* Wrap the encoder reset function */
void resetEncoders() {
resetEncoder(LEFT);
resetEncoder(RIGHT);
}
#endif
motor_driver.h
/***************************************************************
Motor driver function definitions - by James Nugen
*************************************************************/
#ifdef L298_MOTOR_DRIVER
#define RIGHT_MOTOR_BACKWARD 5
#define LEFT_MOTOR_BACKWARD 6
#define RIGHT_MOTOR_FORWARD 9
#define LEFT_MOTOR_FORWARD 10
#define RIGHT_MOTOR_ENABLE 12
#define LEFT_MOTOR_ENABLE 13
#endif
void initMotorController();
void setMotorSpeed(int i, int spd);
void setMotorSpeeds(int leftSpeed, int rightSpeed);
motor_driver.ino
/***************************************************************
Motor driver definitions
Add a "#elif defined" block to this file to include support
for a particular motor driver. Then add the appropriate
#define near the top of the main ROSArduinoBridge.ino file.
*************************************************************/
#ifdef USE_BASE
#ifdef POLOLU_VNH5019
/* Include the Pololu library */
#include "DualVNH5019MotorShield.h"
/* Create the motor driver object */
DualVNH5019MotorShield drive;
/* Wrap the motor driver initialization */
void initMotorController() {
drive.init();
}
/* Wrap the drive motor set speed function */
void setMotorSpeed(int i, int spd) {
if (i == LEFT) drive.setM1Speed(spd);
else drive.setM2Speed(spd);
}
// A convenience function for setting both motor speeds
void setMotorSpeeds(int leftSpeed, int rightSpeed) {
setMotorSpeed(LEFT, leftSpeed);
setMotorSpeed(RIGHT, rightSpeed);
}
#elif defined POLOLU_MC33926
/* Include the Pololu library */
#include "DualMC33926MotorShield.h"
/* Create the motor driver object */
DualMC33926MotorShield drive;
/* Wrap the motor driver initialization */
void initMotorController() {
drive.init();
}
/* Wrap the drive motor set speed function */
void setMotorSpeed(int i, int spd) {
if (i == LEFT) drive.setM1Speed(spd);
else drive.setM2Speed(spd);
}
// A convenience function for setting both motor speeds
void setMotorSpeeds(int leftSpeed, int rightSpeed) {
setMotorSpeed(LEFT, leftSpeed);
setMotorSpeed(RIGHT, rightSpeed);
}
#elif defined L298_MOTOR_DRIVER
void initMotorController() {
digitalWrite(RIGHT_MOTOR_ENABLE, HIGH);
digitalWrite(LEFT_MOTOR_ENABLE, HIGH);
}
void setMotorSpeed(int i, int spd) {
unsigned char reverse = 0;
if (spd < 0)
{
spd = -spd;
reverse = 1;
}
if (spd > 255)
spd = 255;
if (i == LEFT) {
if (reverse == 0) { analogWrite(RIGHT_MOTOR_FORWARD, spd); analogWrite(RIGHT_MOTOR_BACKWARD, 0); }
else if (reverse == 1) { analogWrite(RIGHT_MOTOR_BACKWARD, spd); analogWrite(RIGHT_MOTOR_FORWARD, 0); }
}
else /*if (i == RIGHT) //no need for condition*/ {
if (reverse == 0) { analogWrite(LEFT_MOTOR_FORWARD, spd); analogWrite(LEFT_MOTOR_BACKWARD, 0); }
else if (reverse == 1) { analogWrite(LEFT_MOTOR_BACKWARD, spd); analogWrite(LEFT_MOTOR_FORWARD, 0); }
}
}
void setMotorSpeeds(int leftSpeed, int rightSpeed) {
setMotorSpeed(LEFT, leftSpeed);
setMotorSpeed(RIGHT, rightSpeed);
}
#else
#error A motor driver must be selected!
#endif
#endif
由于没有用到舵机，所以舵机那部分的代码修不修改都一样的，此处略。
代码解释
ROSArduinoBridge.ino这个文件是主文件，是用来接收上位机发送的命令并进行处理的，在那个文件也按照自己的需求来修改波特率，在该主文件执行各类控制函数。
diff_controller.h，该文件是用来控制PID的文件，该文件可以调PID，但由于我并没有接编码器，所以这部分的PID并调不到。
encoder_driver这部分文件是编码器的驱动文件，需要根据自己的编码器的类型和需求来更改，我这次是采用了AB相编码器的写法来写驱动函数的，有需要的话读者可自行修改，但最好函数名字和之前源代码的名字一样，这样修改起来就没有那么困难了。
motor_driver这部分是电机的驱动函数，源代码是使用了L298N来作为电机的驱动，作者使用的是Tb6612作为电机的驱动，所以需要修改一下。
PID调试
如果有装编码器的读者，可以试着调一下PID，不过作者建议还是等将mega 2560装到小车上时才开始调PID，不然等下重量改变之后就要重新调PID了，在这里作者就先教你们如何调PID。
先去下载PID库
git clone https://github.com/br3ttb/Arduino-PID-Library将该PID库导入到arduino当中，就可以开始调了。打开串口绘图仪（较新版的arduino才会有的绘图仪），可以根据绘图仪的波形来慢慢调PID。
代码：
/* 复制测速代码、编写控制电机运动代码、PID库
* 修改测速代码：测速单位时间 2000 => 50毫秒
* 添加电机转向、PWM 控制引脚
* 包含 PID 头文件、创建 PID 对象
* 在 setup()中启用 PID 对象，调用 PID.compute()，输出控制电机 PWM 值
*/
#include<PID_v1.h>
int DIR_LEFT = 10; // L298P 电机驱动板的电机控制脉冲输出口
int PWM_LEFT = 12;
int encoder_A = 21; // 中断逻辑口：2
int encoder_B = 20; // 中断逻辑口：3
volatile int count = 0;
void count_a(){
if(digitalRead(encoder_A)==HIGH){
if(digitalRead(encoder_B)==HIGH){
count++;
}else{ count--; }
}else{
if(digitalRead(encoder_B)==LOW){
count++;
}else{count--;} }}
void count_b(){
if(digitalRead(encoder_B)==HIGH){
if(digitalRead(encoder_A)== LOW){
count++;
}else{ count--;}
}else{
if(digitalRead(encoder_A)== HIGH){
count++;
}else{
count--; } } }
long start_time = millis();
int interval_time = 50;
int per_round = 30 * 13 * 4;
double vel; // 全局变量：车轮速度
void get_current_vel() {
long right_now = millis();
long pass_time = right_now - start_time;
if(pass_time >= interval_time) {
noInterrupts();
vel = (double)count / per_round /interval_time * 1000 * 60;
// Serial.println("vel = ");
Serial.println(vel);
count = 0;
start_time = right_now;
interrupts(); } }
double pwm;
double target = 30; // 目标速度
double kp = 1.5;
double ki = 3.0;
double kd = 0.1;
PID pid(&vel, &pwm, &target, kp, ki, kd,DIRECT); // DIRECT ，REVERSE：对 KID 参数取反
void update_vel() {
get_current_vel(); // 更新 vel 数值
pid.Compute(); // 计算 pwm
int pwm_new = 128 + (int)pwm / 2 ;
digitalWrite(DIR_LEFT,HIGH);
analogWrite(PWM_LEFT, pwm_new);// 最小149
}
void setup() {
Serial.begin(57600);
pinMode(encoder_A, INPUT);
pinMode(encoder_B, INPUT);
attachInterrupt(2, count_a, CHANGE);
attachInterrupt(3, count_b, CHANGE);
pinMode(DIR_LEFT,OUTPUT);
pinMode(PWM_LEFT,OUTPUT);
pid.SetMode(AUTOMATIC);
}
void loop() {
delay(10);
update_vel();
}底层驱动调试
在烧录完mega2560的底层驱动之后，把线都接好，我们就可以来测试一下驱动代码是否能用了，打开PC虚拟机的串口监视器，当我们输入 m num1 num2的时候，小车的轮子会根据你设定的速度自动转两秒钟。
当我们输入 b 就可以得到mega2560当前的波特率，还有更多的命令，想要获取更多的信息就去参考commands.h文件，只要你输入相应的命令，就能得到当前板子反馈给你的信息。
三、控制系统开发
3.1 将PC的虚拟机设置为桥接模式
在VMware选择虚拟机->设置，将NAT模式（默认的），改为桥接模式。
然后试着ping一下树莓派的静态IP，看看能不能ping通，如果是出现以下的这种情况，说明你是不能上网的，也就是桥接模式出了问题。
pi@ubuntu:~/Desktop$ ping 192.168.43.128
PING 192.168.43.128 (192.168.43.128) 56(84) bytes of data.
From 192.168.43.126 icmp_seq=1 Destination Host Unreachable
From 192.168.43.126 icmp_seq=2 Destination Host Unreachable这时我们需要在VMware上选择编辑->虚拟网络编辑器，查看Vmnet0是否已经有了外部连接，如果Vmnet0的桥接是自动的话，那我们需要手动选择，选择桥接至WiFi那里，如下图。
如果在这里没找到类似于这样的桥接名字的话，那就去检查你自己的WiFi是否是处于网桥状态的，按win键进入搜索，搜索 查看网络连接，进去就可以看到你的WiFi是否在网桥上了，如果在就将其从网桥上删除。如果将WiFi从桥上删除之后还是不行的话，就去看一下你是否在电脑上设置了WSL等其他的虚拟机，也是按win键进入搜索 Hyper-V管理器，进去之后点虚拟交换机管理器，将你电脑上的虚拟机交换机连接到专用网络上，别连接到外部网络上就行了，不然他会和VMware上的Ubuntu抢了外部网络，导致Ubuntu不能正常上网。
设置完之后再ping一下树莓派，如果出现这样的话，就是ping成功了。
3.2 ROS-noetic的安装
终于到最重要的一步了，如果是自己安装的ROS-noetic的话，可能会有非常多的报错，所以在这里我们就使用鱼香ROS老师的方法来安装noetic，在这里特别鸣谢鱼香ROS。树莓派和PC虚拟机的安装方法都是一样的，所以这里就不过多展示了。
首先输入一段神秘代码。
wget http://fishros.com/install -O fishros && . fishros等待他加载出来，然后选择1，[1]:一键安装(推荐):ROS(支持ROS/ROS2,树莓派Jetson)
然后再选择 1，更换源并继续安装。
选择2，更换源并清理第三方源。
最后一步就选择ros1的noetic就行了，随后就等待他下载完成了，时间可能会有点长，但是是真的很方便，只需要几步就可以弄完了。
3.3 ROS 分布式通信
ROS（Robot Operating System）通过分布式通信机制，允许多个节点（独立的执行单元）在不同设备上进行通信，形成一个功能协作的分布式系统。ROS 的分布式通信架构基于发布/订阅模型、服务调用和动作服务器等机制，能够支持机器人系统中的并行计算、模块化开发和多机协作。
ROS中只能有一台主机，但是可以有多台从机，所以这里让树莓派作为主机，PC的虚拟机作为从机。
3.3.1 修改 .bashrc文件
修改PC端虚拟机的 .bashrc文件: 加入树莓派的IP地址和计算机名，主从机一定要分开，设置好主从机的IP地址。
sudo vim .bashrcROS_MASTER_URI填的是树莓派的IP地址，ROS_HOSTNAME填的是PC端虚拟机的IP地址。
执行该命令使环境变量配置生效。
source .bashrc修改树莓派的.bashrc。
ROS_MASTER_URI和ROS_HOSTNAME都是填树莓派的地址。
3.3.2 测试ros分布式通信
用MobaXterm在树莓派主机端启动节点，打开小乌龟，等待从机发布消息。
roscore再打开另外一个终端，启动小乌龟。
rosrun turtlesim turtlesim_node在PC端虚拟机打开键盘节点，控制小乌龟。
rosrun turtlesim turtle_teleop_key要注意，想要用键盘控制小乌龟，鼠标需要点一下虚拟机的终端，并把鼠标放在该终端的范围内，这样才能用键盘控制小乌龟，运行的结果如下。
3.3.3 ros_arduino_bridge上位机参数的修改。
首先先在树莓派主机上下载串口工具，因为作者的树莓派使用的是python3.8，所以要下载python3-serial。
sudo apt install python3-serial 在从机虚拟机上，ros运行的脚本的文件都是来自于ros_arduino_python，脚本程序的入口是launch文件下的arduino.launch，所以我们要去到arduino.launch找到脚本文件的路径。文件的路径是/home/pi/ROS_Arduino_bridge-main/ros_arduino_bridge_Python3/ros_arduino_python/launch，在虚拟机上由于有图形化界面，所以可以直接点开来修改就行。 
在config下的yaml文件就是我们所要配置的文件，将my_arduino_params.yaml这个文件格式的命名复制下来，然后打开config，我们在GitHub上下载的文件已经帮我们复制好了，所以也很感谢那个提供python3版本的ros_arduino_bridge的作者。
修改上位机的参数配置。
3.3.5 创建树莓派的工作空间
当把文件改好之后，我们需要在树莓派中为ros创建一个工作空间，在工作空间中进行编译。
mkdir -p catkin_ws/srccatkin_ws是 ROS（Robot Operating System）中的工作空间（workspace），用于管理 ROS 包的开发、编译和运行。ROS 中的catkin是一个基于Cmake的构建系统，帮助开发者组织和构建多个包。catkin_ws是一个典型的开发环境，它包括了 ROS 包的源码、编译输出和运行环境。
以下是catkin_ws工作空间通常由的目录结构。
catkin_ws/
│
├── src/ # 源代码目录，所有 ROS 包的源代码存放在这里
│ └── <your packages>/
├── devel/ # 开发环境目录，编译后的目标文件和开发环境变量
├── build/ # 编译过程中的中间文件存放的目录
└── install/ # 可选的安装目录，存放编译后可以分发的包
src：是存放源代码的目录。所有需要编译的 ROS 包都应该放在这个目录下。可以从外部导入已经存在的包，也可以在这里创建自己的包。所以等下我们就要将ros_arduino_bridge的源码放进这个src里面。
build/：这是存放构建过程中的中间文件的目录。catkin 编译过程会在这里生成各种中间文件，如对象文件。
devel/：这是开发空间目录，存放编译后的目标文件和开发过程中使用的环境变量设置。我们可以直接在开发环境中使用这些编译后的包，而不需要安装它们。
install/：这是一个可选的目录，通常用于生成可分发和发布的包。如果我们使用catkin_make install，生成的包将会被复制到这个目录中。但本文中并不需要使用catkin_make install。
在PC虚拟机端将ros_arduino_bridge文件通过scp命令上传到树莓派的catkin_ws/src的目录下。scp(Secure Copy)命令是 Linux 和 Unix 系统中用于通过 SSH 协议在不同主机之间安全地复制文件或目录的命令。它可以在本地主机与远程主机之间、以及两台远程主机之间传输文件，传输过程中使用加密，确保数据安全。
scp的命令格式是：scp [选项] [源文件路径] [目标路径]。
scp -r ros_arduino_bridge/ pi@192.168.43.128:/home/pi/catkin_ws/src
在树莓派主机上确认收到之后， 就要开始编译文件了。
catkin_make #编译等待编译完成之后就可以开始实测了。
3.4 实地测试小车
将mega2560的数据线接到树莓派上，之前是接到了PC端上给mega进行烧录的。
我们先要在PC虚拟机上下载一个虚拟键盘，方便我们等下控制小车。
sudo apt-get install ros-noetic-teleop-twist-keyboard等待其安装完成，随后我们回到树莓派上，我们首先要先加载工作空间的环境变量。
source devel/setup.bash然后roslaunch 启动arduino的控制节点。
roslaunch ros_arduino_python arduino.launch如果读者是按照我给的步骤走的话，现在会出现已经成功连接到了arduino。
如果是出现了以下的错误，那就一定一定要回头看看是不是已经下载了作者刚刚给的python3版本的ros_arduino_bridge，如果不是的话，就请返回去下载，因为这个根本就不是端口的原因，而是因为python2和python3版本不兼容的原因，旧版的ros_arduino_bridge是使用的python2来写的，现在已经不兼容了。
如果已经下载了发现还不能的话，那么就去检查一下：
1. ll /dev/ttyUSB0（或ttyACM0），查看是否存在。
2.查看端口是否被占用， 使用lsof确认/dev/ttyUSB0没有其他进程使用该端口
3.检查权限，确保当前用户具有访问串口的权限，用ls -l /dev/ttyUSB0确认设备的权限设置，如果没有，可以尝试将用户加入到dialout组，sudo usermod -a -G dialout pi
4.检查波特率是否已经设置好了，要确保节点的波特率和arduino上设置的波特率要一致。
5.检查一下驱动程序是否已经有了，mega2560需要的驱动程序应该是ch340或者是ch341，用lsmod | grep ch341查看。
6.还有一个比较隐蔽的错误，就是你在打开了串口监视器的同时，又运行了那个脚本命令，所以这时只要你将串口监视器关掉，再重新运行命令就行了。
最后在PC虚拟机上，打开键盘，就可以开始控制你的ros小车了。
rosrun teleop_twist_keyboard teleop_twist_keyboard.py如果报错rosrun 执行不了，那就去检查一下你的环境变量是否已经设置了，如果没有就去设置一下。
echo "source /opt/ros/noetic/setup.bash" >> ~/.bashrc
source ~/.bashrc执行成功之后，会出现一个几个英文字母，他们各自对应的就是前进后退左转右转什么的。
到此，你就可以把一台用ros简单控制的小车做出来了，将小车的基本操作做出来之后，后续你就可以嵌入自主导航和自主避障了。
四、结语
该小车虽然说目前只实现了简单的控制，但是后续可以加入ROS navigation导航功能，包括slam建图，amcl定位和move_base路径规划等功能，也可以嵌入opencv做图像识别，有了小车的基建，后续就方便很多了，小车的上限取决于读者的努力，作者也会慢慢加一些功能到小车上，如果有问题需要询问的话，欢迎在评论区或者私信提问。
参考文献：
http://t.csdnimg.cn/zipWu
2.树莓派4b+ubuntu18.04（ros版本melodic）+arduino mega自制两轮差速小车，实现建图导航功能_树莓派ros运动-CSDN博客