重要參考：

課程鏈接:https://www.bilibili.com/video/BV1Ci4y1L7ZZ

講義鏈接:Introduction · Autolabor-ROS機器人入門課程《ROS理論與實踐》零基礎教程

?

8.4.4 底盤實現_03Arduino端電機驅動

自定義電機驅動的實現與上一節的編碼器驅動流程類似：

1. ROSArduinoBridge.ino 中需要注釋之前的電機驅動，添加自定義電機驅動；
2. motor_driver.h 中設置左右電機引腳；
3. motor_driver.ino 中實現初始化與速度設置函數；
4. 測試

1.定義電機驅動

ROSArduinoBridge.ino需要添加電機宏定義，代碼如下：

#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/* 使用自定義的編碼器驅動 */#define ARDUINO_MY_COUNTER/* L298 Motor driver*///#define L298_MOTOR_DRIVER//使用自定義的L298P電機驅動#define L298P_MOTOR_DRIVER
#endif

2.修改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
#elif defined L298P_MOTOR_DRIVER#define DIRA 4#define PWMA 5#define DIRB 7#define PWMB 6
#endifvoid initMotorController();
void setMotorSpeed(int i, int spd);
void setMotorSpeeds(int leftSpeed, int rightSpeed);

3.修改motor_driver.ino 文件

主要添加內容如下：

#elif defined L298P_MOTOR_DRIVERvoid initMotorController(){pinMode(DIRA,OUTPUT);pinMode(PWMA,OUTPUT);pinMode(DIRB,OUTPUT);pinMode(PWMB,OUTPUT);}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) { digitalWrite(DIRA,HIGH);} else if (reverse == 1) { digitalWrite(DIRA,LOW);}analogWrite(PWMA,spd);} else /*if (i == RIGHT) //no need for condition*/ {if (reverse == 0) { digitalWrite(DIRB,LOW);        } else if (reverse == 1) { digitalWrite(DIRB,HIGH);}analogWrite(PWMB,spd);}}void setMotorSpeeds(int leftSpeed, int rightSpeed){setMotorSpeed(LEFT, leftSpeed);setMotorSpeed(RIGHT, rightSpeed);}

4.測試

編譯并上傳程序，打開串口監視器，然后輸入命令，命令格式為：?m num1 num2，num1和num2分別為單位時間內左右電機各自轉動的編碼器計數，而默認單位時間為 1/30 秒。

舉例，假設車輪旋轉一圈編碼器計數為 3960(減速比90，編碼器分辨率11且采用4倍頻計數)，當輸入命令為 m 200 100 時：

左電機轉速為：?200 * 30 * 60 / 3960 = 90.9 (r/m)

右電機轉速為：?100 * 30 * 60 / 3960 = 45.45 (r/m)