今天從淘寶上訂購的兩個機械臂終於到貨了，從這篇開始逐步講解ROS機械臂的控制，這兩個機械臂又花掉了800塊，配的MG99R模擬舵機，如果配數字舵機每個機械臂要555元，價格還是有點小貴。機械臂安裝好後如下圖

本來有一塊通過串列埠控制24路舵機控制的模組，但由於樹莓派的串列埠已經被藍芽所使用，現在藍芽還要接藍芽音箱，所以只好在找其他的舵機控制版，最後找到這款2路電機，和16路舵機控制模組，可以直接插在arduino上，通過ardunio的i2c介面控制舵機，同時通過arduino的D6、D7、D11、D12口控制電機，正好滿足需求，而且其還留了兩排arduino引腳插孔，可以自己焊接上兩排插針，做擴充套件板用，這塊板子淘寶價格是58元。

下圖是裝完機械臂後的Diego 1#的圖片，看到是挺酷的

1.修改電機驅動程式碼

因為更換了電機驅動板，所以先要更改一下電機驅動的程式碼，其實很簡單，只需要修改DualL298PMotorShield_h中介面中其中一個電機的控制對應arduino引腳，由原來的4和5改為11和12就可以了，程式碼如下：

#ifndef DualL298PMotorShield_h
#define DualL298PMotorShield_h

#include <Arduino.h>

class DualL298PMotorShield
{
  public:  
    // CONSTRUCTORS
 

    DualL298PMotorShield(); // Default pin selection.
    DualL298PMotorShield(unsigned char M1DIR, unsigned char M1PWM,
                           unsigned char M2DIR, unsigned char M2PWM); // User-defined pin selection. 

    // PUBLIC METHODS
    void init(); // Initialize TIMER 1, set the PWM to 20kHZ. 
 

    void setM1Speed(int speed); // Set speed for M1.
    void setM2Speed(int speed); // Set speed for M2.
    void setSpeeds(int m1Speed, int m2Speed); // Set speed for both M1 and M2.

  private:
    //static const unsigned char _M1DIR = 5;
    static const unsigned char _M1DIR = 12;
    static const unsigned char _M2DIR = 7;
    //static const unsigned char _M1PWM = 4;
    static const unsigned char _M1PWM = 11;
    static const unsigned char _M2PWM = 6;
};

#endif

2.修改碼盤資料讀取的程式碼

由於這款舵機控制模組是通過arduino 的I2C介面控制舵機，即arduino上的A4,A5。我們之前有一個電機碼盤的資料是通過A4,A5讀取的，所以修改到其他其他的模擬量介面，我在這裡用了A2,A3。只需要修改encoder_driver.h對應介面引腳即可，同時要保證實際的連線也要做相應的調整，程式碼如下：

#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_A PC2  //pin A2
  //#define RIGHT_ENC_PIN_B PC5   //pin A5
  #define RIGHT_ENC_PIN_B PC3   //pin A3
#endif

long readEncoder(int i);
void resetEncoder(int i);
void resetEncoders();

3.舵機控制
修改servos.h

//定義12 個舵機
#define N_SERVOS 12

//Servo servos [N_SERVOS];
//byte servoPins [N_SERVOS] = {3, 5};//由於本身沒有使用arduino的PWM引腳控制舵機，所以這行註釋
short servoslastpos [N_SERVOS];//0~180//記錄舵機當前的位置

修改ROSArduinoBridge-diego.uno的void setup()的servo初始化程式碼。

void setup() {
  Serial.begin(BAUDRATE);

  // Initialize the motor controller if used */
#ifdef USE_BASE
#ifdef ARDUINO_ENC_COUNTER
  //set as inputs
  DDRD &= ~(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 resistors
  PORTD |= (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 pins
  PCMSK2 |= (1 << LEFT_ENC_PIN_A) | (1 << LEFT_ENC_PIN_B);
  // tell pin change mask to listen to right encoder pins
  PCMSK1 |= (1 << RIGHT_ENC_PIN_A) | (1 << RIGHT_ENC_PIN_B);

  // enable PCINT1 and PCINT2 interrupt in the general interrupt mask
  PCICR |= (1 << PCIE1) | (1 << PCIE2);
#endif
  initMotorController();
  resetPID();
#endif

  /* Attach servos if used */
#ifdef USE_SERVOS
  //int i;
  //for (i = 0; i < N_SERVOS; i++) {
    //servos[i].attach(servoPins[i]);
  //}
  int i;
  for (i = 0; i < N_SERVOS; i++) {
    servosPos[i]=90;
  }
  servodriver.begin();
  servodriver.setPWMFreq(50);
  #endif
}

修改頭部的巨集定義，啟用servo

#define USE_SERVOS  // Enable use of PWM servos as defined in servos.h
//#undef USE_SERVOS     // Disable use of PWM servos

修改頭部的巨集定義，增加servo的引用和ServoDriver的控制變數servodriver

#ifdef USE_SERVOS
//#include <Servo.h>
#include "servos.h"
#include <Wire.h>
#include <ServoDriver.h>
#define SERVOMIN  102 // this is the 'minimum' pulse length count (out of 4096)
#define SERVOMAX  512 // this is the 'maximum' pulse length count (out of 4096)
ServoDriver servodriver = ServoDriver();
#endif

修改runCommand舵機控制部分程式碼如下

#ifdef USE_SERVOS
    case SERVO_WRITE:
      //servos[arg1].write(arg2);
      servodriver.setPWM(arg1,0,short(SERVOMIN+arg2*(SERVOMAX-SERVOMIN)/180));//將舵機我這0~180轉換為舵機控制脈衝長度
      servosPos[arg1]=short(arg2);
      Serial.println("OK");
      break;
    case SERVO_READ:
      //Serial.println(servos[arg1].read());
      Serial.println(servosPos[arg1]);
      break;
#endif