重要參考：
課程鏈接:https://www.bilibili.com/video/BV1Ci4y1L7ZZ
講義鏈接:Introduction · Autolabor-ROS機器人入門課程《ROS理論與實踐》零基礎教程
8.4.2 底盤實現_01Arduino端入口
ros_arduino_bridge/ros_arduino_firmware/src/libraries/ROSArduinoBridge下的RosArduinoBridge.ino是Arduino端程序的主入口，
源文件(添加中文注釋)內容如下：
/********************************************************************** ROSArduinoBridge可以通過一組簡單的串口命令來控制差分機器人并接收回傳的傳感器與里程計數據，默認使用的是 Arduino Mega + Pololu電機驅動模塊，如果使用其他的編碼器或電機驅動需要重寫readEncoder()與setMotorSpeed()函數A set of simple serial commands to control a differential driverobot and receive back sensor and odometry data. Default configuration assumes use of an Arduino Mega + Pololu motorcontroller shield + Robogaia Mega Encoder shield. Edit thereadEncoder() and setMotorSpeed() wrapper functions if using different motor controller or encoder method.Created for the Pi Robot Project: http://www.pirobot.organd the Home Brew Robotics Club (HBRC): http://hbrobotics.orgAuthors: Patrick Goebel, James NugenInspired and modeled after the ArbotiX driver by Michael FergusonSoftware License Agreement (BSD License)Copyright (c) 2012, Patrick Goebel.All rights reserved.Redistribution and use in source and binary forms, with or withoutmodification, are permitted provided that the following conditionsare met:* Redistributions of source code must retain the above copyrightnotice, this list of conditions and the following disclaimer.* Redistributions in binary form must reproduce the abovecopyright notice, this list of conditions and the followingdisclaimer in the documentation and/or other materials providedwith the distribution.THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOTLIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESSFOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THECOPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVERCAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICTLIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING INANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE* POSSIBILITY OF SUCH DAMAGE.*********************************************************************/
//是否啟用基座控制器
//#define USE_BASE // Enable the base controller code
#undef USE_BASE // Disable the base controller code/* Define the motor controller and encoder library you are using */
//啟用基座控制器需要設置的電機驅動以及編碼器驅動
#ifdef USE_BASE/* The Pololu VNH5019 dual motor driver shield */#define POLOLU_VNH5019/* The Pololu MC33926 dual motor driver shield *///#define POLOLU_MC33926/* The RoboGaia encoder shield */#define ROBOGAIA/* Encoders directly attached to Arduino board *///#define ARDUINO_ENC_COUNTER/* L298 Motor driver*///#define L298_MOTOR_DRIVER
#endif//是否啟用舵機
#define USE_SERVOS // Enable use of PWM servos as defined in servos.h
//#undef USE_SERVOS // Disable use of PWM servos/* Serial port baud rate */
//波特率
#define BAUDRATE 57600/* Maximum PWM signal */
//最大PWM值
#define MAX_PWM 255//根據Arduino型號來包含對應的頭文件
#if defined(ARDUINO) && ARDUINO >= 100
#include "Arduino.h"
#else
#include "WProgram.h"
#endif/* Include definition of serial commands */
//串口命令
#include "commands.h"/* Sensor functions */
//傳感器文件
#include "sensors.h"/* Include servo support if required */
//如果啟用舵機，需要包含的頭文件
#ifdef USE_SERVOS#include <Servo.h>#include "servos.h"
#endif//如果啟用基座控制器需要包含的頭文件
#ifdef USE_BASE/* Motor driver function definitions */#include "motor_driver.h" //電機驅動/* Encoder driver function definitions */#include "encoder_driver.h" //編碼器驅動/* PID parameters and functions */#include "diff_controller.h" //PID調速/* Run the PID loop at 30 times per second */#define PID_RATE 30 // Hz 調速頻率/* Convert the rate into an interval */const int PID_INTERVAL = 1000 / PID_RATE; //調速周期/* Track the next time we make a PID calculation */unsigned long nextPID = PID_INTERVAL;/* Stop the robot if it hasn't received a movement commandin this number of milliseconds */#define AUTO_STOP_INTERVAL 2000 //自動結束時間(可按需修改)long lastMotorCommand = AUTO_STOP_INTERVAL;
#endif/* Variable initialization */// A pair of varibles to help parse serial commands (thanks Fergs)
int arg = 0;
int index = 0;// Variable to hold an input character
char chr;// Variable to hold the current single-character command
char cmd;// Character arrays to hold the first and second arguments
char argv1[16];
char argv2[16];// The arguments converted to integers
long arg1;
long arg2;/* Clear the current command parameters */
//重置命令
void resetCommand() {cmd = NULL;memset(argv1, 0, sizeof(argv1));memset(argv2, 0, sizeof(argv2));arg1 = 0;arg2 = 0;arg = 0;index = 0;
}/* Run a command. Commands are defined in commands.h */
//執行串口命令
int runCommand() {int i = 0;char *p = argv1;char *str;int pid_args[4];arg1 = atoi(argv1);arg2 = atoi(argv2);switch(cmd) {case GET_BAUDRATE:Serial.println(BAUDRATE);break;case ANALOG_READ:Serial.println(analogRead(arg1));break;case DIGITAL_READ:Serial.println(digitalRead(arg1));break;case ANALOG_WRITE:analogWrite(arg1, arg2);Serial.println("OK"); break;case DIGITAL_WRITE:if (arg2 == 0) digitalWrite(arg1, LOW);else if (arg2 == 1) digitalWrite(arg1, HIGH);Serial.println("OK"); break;case PIN_MODE:if (arg2 == 0) pinMode(arg1, INPUT);else if (arg2 == 1) pinMode(arg1, OUTPUT);Serial.println("OK");break;case PING:Serial.println(Ping(arg1));break;
#ifdef USE_SERVOScase SERVO_WRITE:servos[arg1].setTargetPosition(arg2);Serial.println("OK");break;case SERVO_READ:Serial.println(servos[arg1].getServo().read());break;
#endif#ifdef USE_BASEcase READ_ENCODERS:Serial.print(readEncoder(LEFT));Serial.print(" ");Serial.println(readEncoder(RIGHT));break;case RESET_ENCODERS:resetEncoders();resetPID();Serial.println("OK");break;case MOTOR_SPEEDS: //傳入電機控制命令/* Reset the auto stop timer */lastMotorCommand = millis();if (arg1 == 0 && arg2 == 0) {setMotorSpeeds(0, 0);resetPID();moving = 0;}else moving = 1;leftPID.TargetTicksPerFrame = arg1;rightPID.TargetTicksPerFrame = arg2;Serial.println("OK"); break;case UPDATE_PID:while ((str = strtok_r(p, ":", &p)) != '\0') {pid_args[i] = atoi(str);i++;}Kp = pid_args[0];Kd = pid_args[1];Ki = pid_args[2];Ko = pid_args[3];Serial.println("OK");break;
#endifdefault:Serial.println("Invalid Command");break;}
}/* Setup function--runs once at startup. */
void setup() {Serial.begin(BAUDRATE);// Initialize the motor controller if used */
#ifdef USE_BASE#ifdef ARDUINO_ENC_COUNTER//set as inputsDDRD &= ~(1<<LEFT_ENC_PIN_A);DDRD &= ~(1<<LEFT_ENC_PIN_B);DDRC &= ~(1<<RIGHT_ENC_PIN_A);DDRC &= ~(1<<RIGHT_ENC_PIN_B);//enable pull up resistorsPORTD |= (1<<LEFT_ENC_PIN_A);PORTD |= (1<<LEFT_ENC_PIN_B);PORTC |= (1<<RIGHT_ENC_PIN_A);PORTC |= (1<<RIGHT_ENC_PIN_B);// tell pin change mask to listen to left encoder pinsPCMSK2 |= (1 << LEFT_ENC_PIN_A)|(1 << LEFT_ENC_PIN_B);// tell pin change mask to listen to right encoder pinsPCMSK1 |= (1 << RIGHT_ENC_PIN_A)|(1 << RIGHT_ENC_PIN_B);// enable PCINT1 and PCINT2 interrupt in the general interrupt maskPCICR |= (1 << PCIE1) | (1 << PCIE2);#endifinitMotorController(); //初始化電機控制resetPID(); //重置 PID
#endif/* Attach servos if used */#ifdef USE_SERVOSint i;for (i = 0; i < N_SERVOS; i++) {servos[i].initServo(servoPins[i],stepDelay[i],servoInitPosition[i]);}#endif
}/* Enter the main loop. Read and parse input from the serial portand run any valid commands. Run a PID calculation at the targetinterval and check for auto-stop conditions.
*/
void loop() {//讀取串口命令while (Serial.available() > 0) {// Read the next characterchr = Serial.read();// Terminate a command with a CRif (chr == 13) {if (arg == 1) argv1[index] = NULL;else if (arg == 2) argv2[index] = NULL;runCommand();resetCommand();}// Use spaces to delimit parts of the commandelse if (chr == ' ') {// Step through the argumentsif (arg == 0) arg = 1;else if (arg == 1) {argv1[index] = NULL;arg = 2;index = 0;}continue;}else {if (arg == 0) {// The first arg is the single-letter commandcmd = chr;}else if (arg == 1) {// Subsequent arguments can be more than one characterargv1[index] = chr;index++;}else if (arg == 2) {argv2[index] = chr;index++;}}}// If we are using base control, run a PID calculation at the appropriate intervals
#ifdef USE_BASEif (millis() > nextPID) {updatePID();//PID調速nextPID += PID_INTERVAL;}// Check to see if we have exceeded the auto-stop intervalif ((millis() - lastMotorCommand) > AUTO_STOP_INTERVAL) {;setMotorSpeeds(0, 0);moving = 0;}
#endif// Sweep servos
#ifdef USE_SERVOSint i;for (i = 0; i < N_SERVOS; i++) {servos[i].doSweep();}
#endif
}這其中，需要關注的是基座控制器以及串口命令的相關部分，而由于沒有使用舵機，所以舵機控制器部分暫不介紹。
1.串口命令
在主程序中，包含了 commands.h，該文件中包含了當前程序預定義的串口命令，可以編譯程序并上傳至 Arduino 電路板，然后打開串口監視器測試(當前程序并未修改，所以并非所有串口可用)：
- w 可以用于控制引腳電平
- x 可以用于模擬輸出
以LED燈控制為例，通過串口監視器錄入命令：
- w 13 0 == LED燈關閉
- w 13 1 == LED燈打開
- x 13 50 == LED燈PWM值為50
2.啟用基座控制器
源碼默認沒有啟用基座控制器、啟用了舵機，我們需要啟用基座控制器，禁用舵機。修改后代碼如下：
#define USE_BASE // Enable the base controller code
//#undef USE_BASE // Disable the base controller code/* Define the motor controller and encoder library you are using */
#ifdef USE_BASE/* The Pololu VNH5019 dual motor driver shield *///#define POLOLU_VNH5019/* The Pololu MC33926 dual motor driver shield *///#define POLOLU_MC33926/* The RoboGaia encoder shield *///#define ROBOGAIA/* Encoders directly attached to Arduino board *///#define ARDUINO_ENC_COUNTER/* L298 Motor driver*///#define L298_MOTOR_DRIVER
#endif//#define USE_SERVOS // Enable use of PWM servos as defined in servos.h
#undef USE_SERVOS // Disable use of PWM servos注意：我們沒有使用官方的電機驅動模塊以及編碼器，后期需要自定義電機驅動與編碼器實現。
)
