ROS机器人Diego制作14-机械臂控制之arduino驱动

ROS机器人Diego制作14-机械臂的控制之arduino驱动

说明：

• 介绍如何驱动机械臂

硬件：

• 舵机:MG99R模拟舵机，配数字舵机价格要高，图：
  

• 可通过串口控制24路舵机控制的模块，笔者的树莓派的串口已经被蓝牙所使用，现在蓝牙还要接蓝牙音箱。改下面方案

• 驱动板：这款2路电机和16路舵机控制模块，可以直接插在arduino上，通过ardunio的i2c接口控制舵机，同时通过arduino的D6、D7、D11、D12口控制电机，正好满足需求，而且其还留了两排arduino引脚插孔，可以自己焊接上两排插针，做扩展板用，图：
  

• 完整装配图：

修改电机驱动代码

• 因为更换了电机驱动板，所以先要更改一下电机驱动的代码，
• 只需要修改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

修改码盘数据读取的代码

• 由于这款舵机控制模块是通过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();

舵机控制

• 修改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