ROS机器人Diego 1#制作（十四）机械臂的控制---arduino驱动

更多创客作品，请关注笔者网站园丁鸟，搜集全球极具创意，且有价值的创客作品
ROS机器人知识请关注，diegorobot
业余时间完成的一款在线统计过程分析工具SPC，及SPC知识分享网站qdo

---

今天从淘宝上订购的两个机械臂终于到货了，从这篇开始逐步讲解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