五一疫情防控不能外出，呆在家里实在无聊，决定跟儿子一起做个排爆机器人，需要同时控制4个舵机、2个底盘电机、1个无线手柄的控制和接收，但L298P只有1个伺候，再外挂个16路舵机板的话有点太乱了。偶然间看到一块功能比较全面的驱动板，可以同时驱动电机、舵机、蓝牙、超声波等，我们就使用这块板子尝试做个排爆机器人。

先给个效果图：

一、概述

这款驱动版是奇果派出品的Arduino MotorShiled V5.5，能同时驱动4个支流电机或2个步进电机、8路全速PWM伺候控制、PS2无线手柄、蓝牙、超声波，最大的优点是，它占用了很少的引脚，使用I2C通信的PCA9685 PWM驱动芯片控制电机速度和方向，只用2根IO引脚(SDA&SCL)就可以驱动多个电机，多个驱动板还可以级联使用，看起来相当不错。

接口说明及端口占用情况：

参数说明：

• 使用东芝高性能驱动芯片，提供每路2.2A（3.6A峰值）输出，可运行4.5-13.5V直流电机；
• 可以驱动4个直流电机或2个步进电机；
• 额外提供8路PWM输出，可以控制8路舵机；
• 支持PS2无线手柄；
• 支持蓝牙模块和超声波传感器；
• 可自定义I2C地址，支持多个驱动器叠加级联；
• 提供完整的代码库和丰富示例程序，支持图形编程，简单易用。

二、供电方案

1.单电源供电

• 单电源可同时为驱动器和开发板供电
• 板载内嵌5V稳压器，可为舵机端口提供5V3A电源。
• 电源输出功率（放电电流）要够大，否则会影响稳定性

2.舵机独立供电

当需要接入多个舵机或大功率舵机的时候，请选择此方案供电

• 先拔掉左上角跳线帽，断开板载电源
• 接上跟舵机匹配电压匹配的电源，注意匹配参数，大多数舵机耐压是5-6V，电压太高会使舵机烧毁

 

三、功能验证

1、舵机控制（Servo）

驱动板的舵机控制通过PWM专用芯片PCA9685实现，对应的编号分别为0-7接口。驱动器内嵌稳压电路，可以输出5V，3A的电流给舵机供电。尽量避免舵机堵转或超负荷，以免影响系统稳定性或损坏舵机。

舵机接线：

注意：对于大功率的舵机（电源要求超过5V3A），请拔掉红色跳线帽，再使用跟舵机匹配的电源通过“舵机辅助电源”接口独立供电。实测来看，基本>2个MG995金属舵机就需要独立供电了。

代码示例：

#include <Wire.h>
#include "QGPMaker_MotorShield.h"

// Create the motor shield object with the default I2C address
QGPMaker_MotorShield AFMS = QGPMaker_MotorShield();

const int SERVOS = 4;       //定义变量SERVOS,表示舵机数，预先赋值4
int INITANGLE[SERVOS];      //定义变量数组INITANGLE[SERVOS]，用于存放当前舵机的初始角度值

// 设置舵机频率
void setServoPulse( uint8_t n, double pulse )  //n:舵机号； pulse：脉冲时间
{
    double pulselength;
    pulselength = 1000000;      // 百万分之一秒
    pulselength /= 50;          //50 Hz
    pulselength /= 4096;        //(12位分辨率)
    pulse *= 1000;              //乘以1000
    pulse /= pulselength;       //除以pulselength变量
    AFMS.setPWM( n, pulse );    //设置PWM值
}

// PWM值转化为角度
void writeServo( uint8_t n, uint8_t angle )  //n:舵机号； angle：舵机需要转到的角度
{
    double pulse;
    angle = (int)angle*0.922222;  //根据舵机批次的不同，此批舵机实际转角与理论输入值存在0.922222的系数关系。如果舵机实际转角与理论输入值一样，则可删除此语句
    pulse = 0.5 + angle / 90.0;  //脉冲时间和角度值的关系公式
    setServoPulse( n, pulse );  //让指定的舵机通过相对应的脉冲时间转到指定的角度
}



//初始化舵机参数
void initServos(){  
    INITANGLE[0] = 75;  //右脚舵机原点角度
    INITANGLE[1] = 75;  //左脚舵机原点角度
    INITANGLE[2] = 90;  //右腿舵机原点角度
    INITANGLE[3] = 90;  //左腿舵机原点角度
}


void setup() {
    Serial.begin(9600);           // set up Serial library at 9600 bps
    Serial.println("Servo PWM test!");

    // 初始化参数
    initServos();

    // 启动驱动板
    AFMS.begin( 50 );
    
    // 初始化舵机角度
    for ( int i = 0; i < SERVOS; i ++ ){  //For循环初始化赋值设定角度（开始将8个舵机回原点）
        // 旋转到指定角度
        writeServo(i, INITANGLE[i]);
    }
}


void loop() {
    // 抬左腿
    for ( int i = 0; i < 130 - INITANGLE[0] ; i++ ){  //For循环初始化赋值设定角度（开始将8个舵机回原点）
        writeServo( 0, INITANGLE[0]+i );//右脚
        writeServo( 1, INITANGLE[1]+i );//左脚
        delay(100);
    }
    //writeServo( 0, 120 );//右脚
    //writeServo( 1, 120 );//左脚
    delay(2000);
    // 左腿前移
    writeServo( 2, 70 );//右腿
    writeServo( 3, 70 );//左腿
    delay(2000);
    // 落左腿
    writeServo( 0, 75 );//右脚
    writeServo( 1, 75 );//左脚
    delay(2000);
    
    // 抬右腿
    //writeServo( 0, 30 );
    //writeServo( 1, 30 );
    for ( int i = 0; i < INITANGLE[0] - 30 ; i++ ){  //For循环初始化赋值设定角度（开始将8个舵机回原点）
        writeServo( 0, INITANGLE[0]-i );//右脚
        writeServo( 1, INITANGLE[1]-i );//左脚
        delay(100);
    }
    delay(2000);
    // 右腿前移
    writeServo( 2, 110 );
    writeServo( 3, 110 );
    delay(2000);
    // 落左腿
    writeServo( 0, 75 );//右脚
    writeServo( 1, 75 );//左脚
    delay(3000);
}

2、直流编码电机控制（DC Motor）

电机接线：

电机编码器接线：

代码示例：

#include <Wire.h>
#include "QGPMaker_MotorShield.h"
#include "QGPMaker_Encoder.h"

QGPMaker_MotorShield AFMS = QGPMaker_MotorShield();
QGPMaker_DCMotor *DCMotor_1 = AFMS.getMotor(1);  // 1-4马达
QGPMaker_DCMotor *DCMotor_2 = AFMS.getMotor(2);
QGPMaker_Encoder Encoder1(1);
QGPMaker_Encoder Encoder2(2);

void setup(){
  AFMS.begin(50);  // create with the default frequency 50Hz
  Serial.begin(9600);
  DCMotor_1->setSpeed(50); //设置电机速度(0 stopped ~ 255 full speed)
  DCMotor_1->run(FORWARD); //启动电机(FORWARD 正转, BACKWARD 反转 ,BRAKE 刹车急停 or RELEASE 关闭电机）
  DCMotor_2->setSpeed(50);
  DCMotor_2->run(FORWARD);
}

void loop(){

  Serial.print(Encoder1.read()); //获取编码器数值
  Serial.print('-');
  Serial.print(Encoder2.read());
  Serial.print('-');
  Serial.println(Encoder1.getRPM());//获取编码器电机对应的每分钟转速（RPM）
  delay(50);
}

*注：如果要使用PID，接线和PID参数会有所不同，需要每个轮子单独调试转动方向和pid参数。

PID调试方法详见https://blog.yanjingang.com/?p=5604。

3、无线手柄控制（PS2）

驱动器内置PS2接收器插座，将接收器直接插入使用。

ps2控制底盘电机移动代码示例：

#include "PS2X_lib.h"  //调用库文件PS2X_lib，这个是无线手柄库
#include "QGPMaker_Encoder.h"
#include "QGPMaker_MotorShield.h"

#define PS2_DAT        12  //14    
#define PS2_CMD        11  //15
#define PS2_SEL        10  //16
#define PS2_CLK        13  //17

#define pressures   true
#define rumble      true

PS2X ps2x; // create PS2 Controller Class

//right now, the library does NOT support hot pluggable controllers, meaning
//you must always either restart your Arduino after you connect the controller,
//or call config_gamepad(pins) again after connecting the controller.

int error = 0;//连接错误
byte type = 0;//手柄形式
byte vibrate = 0;//震动

// Create the motor shield object with the default I2C address
QGPMaker_MotorShield AFMS = QGPMaker_MotorShield();

QGPMaker_DCMotor  *DCMotor_1 = AFMS.getMotor(1);
QGPMaker_DCMotor  *DCMotor_2 = AFMS.getMotor(2);

// 舵机角度 
int currentAngle = 90;
// 马达速度
int motorSpeed = 150;
// 设置舵机频率
void setServoPulse( uint8_t n, double pulse ) //n:舵机号； pulse：脉冲时间
{
  double pulselength;
  pulselength = 1000000; // 百万分之一秒
  pulselength /= 50; //50 Hz
  pulselength /= 4096; //(12位分辨率)
  pulse *= 1000; //乘以1000
  pulse /= pulselength; //除以pulselength变量
  AFMS.setPWM( n, pulse ); //设置PWM值
}

// PWM值转化为角度
void writeServo( uint8_t n, uint8_t angle ) //n:舵机号； angle：舵机需要转到的角度
{
  double pulse;
  angle = (int)angle*0.922222; //根据舵机批次的不同，此批舵机实际转角与理论输入值存在0.922222的系数关系。如果舵机实际转角与理论输入值一样，则可删除此语句
  pulse = 0.5 + angle / 90.0; //脉冲时间和角度值的关系公式
  setServoPulse( n, pulse ); //让指定的舵机通过相对应的脉冲时间转到指定的角度
}

void setup() {
  Serial.begin(9600);
  AFMS.begin(50);
  Serial.println("===========beging==========");
  int wait_ps2 = 500; //搜索手柄时间
  do {
    error = ps2x.config_gamepad(13, 11, 10, 12, true, true);  // setup pins and settings: GamePad(clock, command, attention, data, Pressures, Rumble)
    if (error == 0) {
      Serial.println("\nConfigured successful! ");
      break;
    } else {
      delay(100);
      wait_ps2 -= 100;
    }
  }while(wait_ps2 > 0); //while循环直到手柄连接成功

  // 舵机角度初始化
  writeServo( 0, currentAngle );
}

void loop() {
  ps2x.read_gamepad(false, vibrate); //read controller and set large motor to spin at 'vibrate' speed
  if (ps2x.Button(PSB_START))        //will be TRUE as long as button is pressed
    Serial.println("Start is being held");
  if (ps2x.Button(PSB_SELECT))
    Serial.println("Select is being held");

  if (ps2x.Button(PSB_PAD_UP)) {     //will be TRUE as long as button is pressed
    Serial.print("Up held this hard: ");
    Serial.println(ps2x.Analog(PSAB_PAD_UP), DEC);
    DCMotor_1->setSpeed(motorSpeed);
    DCMotor_2->setSpeed(motorSpeed);
    DCMotor_1->run(FORWARD);
    DCMotor_2->run(FORWARD);
  } else  if (ps2x.Button(PSB_PAD_RIGHT)) {
    Serial.print("Right held this hard: ");
    Serial.println(ps2x.Analog(PSAB_PAD_RIGHT), DEC);
    DCMotor_1->setSpeed(motorSpeed);
    DCMotor_2->setSpeed(motorSpeed);
    DCMotor_1->run(BACKWARD);
    DCMotor_2->run(FORWARD);
  } else  if (ps2x.Button(PSB_PAD_LEFT)) {
    Serial.print("LEFT held this hard: ");
    Serial.println(ps2x.Analog(PSAB_PAD_LEFT), DEC);
    DCMotor_1->setSpeed(motorSpeed);
    DCMotor_2->setSpeed(motorSpeed);
    DCMotor_1->run(FORWARD);
    DCMotor_2->run(BACKWARD);
  } else  if (ps2x.Button(PSB_PAD_DOWN)) {
    Serial.print("DOWN held this hard: ");
    Serial.println(ps2x.Analog(PSAB_PAD_DOWN), DEC);
    DCMotor_1->setSpeed(motorSpeed);
    DCMotor_2->setSpeed(motorSpeed);
    DCMotor_1->run(BACKWARD);
    DCMotor_2->run(BACKWARD);
  } else {
    DCMotor_1->setSpeed(0);
    DCMotor_1->run(RELEASE);
    DCMotor_2->setSpeed(0);
    DCMotor_2->run(RELEASE);
  }

  if (ps2x.Button(PSB_CROSS)) {
    Serial.println("X-X-X-X");
    ps2x.read_gamepad(true, 200);
    delay(300);
    ps2x.read_gamepad(false, 0);
  }

  vibrate = ps2x.Analog(PSAB_CROSS);  //this will set the large motor vibrate speed based on how hard you press the blue (X) button
  if (ps2x.NewButtonState()) {        //will be TRUE if any button changes state (on to off, or off to on)
    if (ps2x.Button(PSB_L3))
      Serial.println("L3 pressed");
    if (ps2x.Button(PSB_R3))
      Serial.println("R3 pressed");
    if (ps2x.Button(PSB_L2))
      Serial.println("L2 pressed");
    if (ps2x.Button(PSB_R2))
      Serial.println("R2 pressed");
    if (ps2x.Button(PSB_TRIANGLE))
      Serial.println("Triangle pressed");
  }

  if (ps2x.ButtonPressed(PSB_CIRCLE)){               //will be TRUE if button was JUST pressed
    Serial.println("Circle just pressed");
    // 舵机角度
    for ( int i = 0; i < 10 ; i++ ){ 
        currentAngle += i;
        writeServo( 0, currentAngle );
        delay(10);
    }
  }
  if (ps2x.NewButtonState(PSB_CROSS))              //will be TRUE if button was JUST pressed OR released
    Serial.println("X just changed");
  if (ps2x.ButtonReleased(PSB_SQUARE)){              //will be TRUE if button was JUST released
    Serial.println("Square just released");
    // 舵机角度
    for ( int i = 0; i < 10 ; i++ ){ 
        currentAngle -= i;
        writeServo( 0, currentAngle );
        delay(10);
    }
  }

  if (ps2x.Button(PSB_L1) || ps2x.Button(PSB_R1)) { //print stick values if either is TRUE
    Serial.print("Stick Values:");
    Serial.print(ps2x.Analog(PSS_LY), DEC); //Left stick, Y axis. Other options: LX, RY, RX
    Serial.print(",");
    Serial.print(ps2x.Analog(PSS_LX), DEC);
    Serial.print(",");
    Serial.print(ps2x.Analog(PSS_RY), DEC);
    Serial.print(",");
    Serial.println(ps2x.Analog(PSS_RX), DEC);
  }
  delay(50);
}

注意：

• PS2手柄为非热插拔，在实际项目用，需要编写重连检测，如无法找到设备，请重启Arduino控制器。
• 手柄一段时间不操作会自动休眠，按start或analog唤醒。

 

4、继电器控制

将电子开关、继电器或者相关模块控制端，接到Servo舵机引脚（输入电源默认不需要接）。

代码示例：

#include <Wire.h>
#include "QGPMaker_MotorShield.h"
QGPMaker_MotorShield AFMS = QGPMaker_MotorShield();
void setup(){
  AFMS.begin(50);
}
void loop(){
  AFMS.setPin(2, HIGH); //2#舵机口输出高电平（电子开关打开）
  //AFMS.setPWM(2, 100); //2#舵机口输出PWM值4000（可作为PWM控制，电机调速，LED亮度调节等）
  delay(300);
  AFMS.setPWM(2, LOW);
  delay(3000);
  }

 

四、排爆机器人

这次做的排爆机器人V1.0主要实现了机械臂控制、底盘控制、无线手柄控制，后续再增加摄像头和自动识别和控制。

实现方法其实也很简单，参考上方的驱动版使用方法即可，不在这里赘述，代码详见：https://github.com/yanjingang/robot_arm_control

运行效果：

 

yan 22.5.4

 

参考：

Arduino MotorShiled电机驱动器说明

欢迎关注下方“非著名资深码农“公众号进行交流～