首页 › Makeblock › 使用MegaPi扩展模块TB6612驱动直流编码电机

190808149 发表于 2019-5-15 14:55

使用MegaPi扩展模块TB6612驱动直流编码电机

本帖最后由 190808149 于 2019-6-4 16:37 编辑

http://player.youku.com/player.php/sid/XNDE4ODc1NzA1Ng==/v.swf

MegaPi运动控制板由Arduino Mega2560和扩展槽构成，可以安装步进电机模块（MakeBlock订货号12035 模块名Megapi Motor Driver V1）和直流电机模块（MakeBlock订货号12040 模块名Megapi Encoder/DC Motor Driver v1）
我2016年得到MakeBlock MegaPi运动控制板试用套装，使用其步进电机模块尝试制作了Delta机械臂https://www.arduino.cn/thread-22156-1-1.html。
受开源3D打印机原理影响，以前，我主要学习Arduino步进电机的运动控制方法。虽然步进电机开环控制精确，扭矩大，实时性好。但是电机重量大，不易于产品的小型化，系统成本也高。近期，我在学习机器人相关知识，发现仿工业机器人式样的DIY机械臂的主流驱动方案是舵机、伺服电机或者编码电机+减速器控制。由于机械臂各关节层层级联，越靠近末端关节，越需要执行器电机扭矩大，重量轻。参看了国外thingiverse.com和hackaday.io网站，网友们的机械臂DIY项目，末端关节使用步进电机的机械臂设计就很勉强了。其中最巧妙的设计来自hackaday.io的a vintage industrial robot，为减轻机械臂扭矩需求，将步进电机尽量靠后放置，使用同步带将动力传送到前端下一个关节。
我想，在基于Arduino的仿工业机器人的多轴机械臂DIY设计中，如果要摆脱沉重的步进电机，那我们可以向玩具机器人学习，舵机是简易的伺服电机，使用玩具航模舵机作为执行器，控制简单，机器人通电后有较好的静力矩。我使用成本不到100元的7个舵机制作了可以在ROS机器人操作系统下支持MoveIt运动控制软件的7轴机械臂，可以保持末端关节在可控空间内的位置和角度灵活性。https://www.arduino.cn/thread-85107-1-1.html
但是，购买的成品航模舵机，变速箱已经装配完成，无法更换，扭矩无法调节和增加。机器人的后端关节多需要大扭矩电机执行器，工业机器人使用伺服电机和谐波减速器、RV减速器来构成轻重量，大扭矩的执行器。伺服电机和减速器都很贵。如果创客DIY项目，估计伺服电机也是奢望。
国外hackaday.io网站有oDrive项目。oDrive支持Arduino，oDrive驱动板可连接无刷电机和各类编码器，做出很专业伺服电机系统。但因为国内仿制较少，oDrive驱动板在淘宝上价格也很高，也不适用DIY。

最后，我觉得可能的轻重量、大扭力、低成本DIY方案，只有淘宝上购买工厂二手或玩具配件级的编码减速直流电机。而且解决了无法通过3D打印制作齿轮精密配合的减速器的问题，还避免了在机器人关节上安装减速器的成本。单独购买减速器成本高过电机本身，重量也够呛。

我回到2016年得到的MegaPI运动控制板，使用直流电机模块（MakeBlock订货号12040 模块名Megapi Encoder/DC Motor Driver v1），模块核心是TB6612芯片。开源3d打印机控制板的设计供应商Pololu设计有TB6612FNG板。淘宝上也很便宜。http://www.hobbytronics.co.uk/pololu-tb6612fng


MegaPI TB6612模块接线有NET1 NET2 5V GND BO1 BO2六根线。
MegaPi的18口接模块NE2，31口NE1。但从MegaPi使用说明书http://learn.makeblock.com/ultimate2-arduino-programming/的例程序看，NE1却是31口，18是终端口。恰好和模块上印刷的字样相冲突。不过还是可以模块推断NET1 NET2可能是连接编码电机A相，B相。

#include “MeMegaPi.h”
const byte interruptPin =18;   
const byte NE1=31;
...
pinMode(interruptPin, INPUT_PULLUP);    pinMode(NE1, INPUT);
    attachInterrupt(digitalPinToInterrupt(interruptPin), blink,RISING);         
...
void blink()
{
    if (digitalRead(NE1)>0)   
    count++;
    else
    count–;
}

MegaPi的1B+和1B-正好与模块BO1 BO2对应，
推测（1）参照TB6612FNG原理https://blog.csdn.net/qq_38721302/article/details/83447870，TB6612并不提供编码功能，MegaPi电机12040模块的NET1 NET2应该只是转接MegaPi的18和31口。
推测（2）根据MegaPI说明书，模块驱动直流电机，不读取编码信息，使用的就是MegaPi的1A+ 1A- 1B+ 1B-，例程序写MeMegaPiDCMotor motor1(PORT1B);使用B口，正好印证MegaPi的1B+和1B-与模块BO1 BO2对应因此，使用模块BO1应接编码电机正极，BO2应接编码电机负极。

另外模块5V和GND口，应该是为编码器供电的，应接编码器5V和GND相应端口。

#include "MeMegaPi.h"
const byte interruptPin =18;
const byte NE1=31;
long count=0;
unsigned long time;
unsigned long last_time;

MeMegaPiDCMotor motor1(PORT1B);
int motorSpeed = 100;
void setup()
{
    pinMode(interruptPin, INPUT_PULLUP);
    pinMode(NE1, INPUT);
    attachInterrupt(digitalPinToInterrupt(interruptPin), blink,RISING);
    Serial.begin(9600);
}
void loop()
{
    motor1.run(motorSpeed);// value: between -255 and 255
    time =millis();
    if(time-last_time>2000)
    {
      Serial.println(count);
      last_time=time;
    }
}
void blink()
{
    if(digitalRead(NE1)>0)
      count++;
    else
      count--;
}

在sketch中录入MakeBlock说明书编码电机示例程序，电机正常运转，并计数。但打开和关闭串口时，电机都会停止旋转，串口重新计数。好像是串口操作触发Arduino程序重启。

http://player.youku.com/player.php/sid/XNDE4ODc1NzA1Ng==/v.swf

测试成功，上文推断的MegaPi直流电机模块和编码电机间的连线是正确的。

NET1->电机编码器A相
NET2->电机编码器B相
5V->电机编码器5V接口
GND->电机编码器GND接口
BO1->电机正极
BO2->电机负极



2.在电机上增加减速器，并编写PID速度控制代码

#include <FlexiTimer2.h>
#include "MeMegaPi.h"

#define ERROR_INTEGRAL_MAX 100
const byte interruptPin =18;
const byte NE1=31;
volatile long count, last_count, count_per_10ms;
volatile int dest, error, error_prev, error_integral, error_derivative;
double control_output;
//Specify the links and initial tuning parameters

unsigned long time;
unsigned long last_time;


MeMegaPiDCMotor motor1(PORT1B);
int motorSpeed = 15;
int ratio = 155;
void setup()
{
    pinMode(interruptPin, INPUT_PULLUP);
    pinMode(NE1, INPUT);
    attachInterrupt(digitalPinToInterrupt(interruptPin), isr_for_encoder, RISING);
    Serial.begin(9600);

//initialize the variables we're linked to

dest = 1;
//turn the PID on
FlexiTimer2::set(10, 1.0/1000, ISR_per_10ms);
FlexiTimer2::start();

}
void loop()
{
    control_output=((int32_t)5*error)+((int32_t)2*error_integral)-((int32_t)1*error_derivative);
   
    if(control_output>255) control_output=255;
    else if(control_output<-255) control_output=-255;
   
    motor1.run(control_output);// value: between -255 and 255
    time =millis();
    if(time-last_time>2000)
    {
      Serial.println(control_output);
      Serial.println(error);
      Serial.println(error_integral);
      Serial.println(error_derivative);
      
      Serial.println(count_per_10ms);
      last_time=time;
    }
}
void isr_for_encoder()
{
    if(digitalRead(NE1)>0)
      count++;
    else
      count--;
}
void ISR_per_10ms() {
    count_per_10ms = count - last_count;
    last_count = count;
    error = dest - count_per_10ms;
    error_integral+=error;
    if(error_integral>ERROR_INTEGRAL_MAX) error_integral=ERROR_INTEGRAL_MAX;
    else if(error_integral<-ERROR_INTEGRAL_MAX) error_integral=-ERROR_INTEGRAL_MAX;
    error_derivative=error_prev-error;
    error_prev=error;
   
}


运行效果：
https://v.youku.com/v_show/id_XNDIxNTMzMTEwNA==.html?spm=a2h0j.11185381.listitem_page1.5!2~A


3.加装减速器，并编写位置PID控制代码。



#include <FlexiTimer2.h>
#include "MeMegaPi.h"

#define ERROR_INTEGRAL_MAX 100000
const byte interruptPin =18;
const byte NE1=31;
volatile long count, last_count;
volatile int dest, error, error_prev, error_integral, error_derivative;
double control_output;
//Specify the links and initial tuning parameters

unsigned long time;
unsigned long last_time;


MeMegaPiDCMotor motor1(PORT1B);
int ratio = 155;
void setup()
{
    pinMode(interruptPin, INPUT_PULLUP);
    pinMode(NE1, INPUT);
    attachInterrupt(digitalPinToInterrupt(interruptPin), isr_for_encoder, RISING);
    Serial.begin(9600);

//initialize the variables we're linked to

dest = 1500;
//turn the PID on
FlexiTimer2::set(10, 1.0/1000, ISR_per_10ms);
FlexiTimer2::start();

}
void loop()
{
    control_output=(1*error)+(0.01*error_integral)-(1*error_derivative);
   
    if(control_output>255) control_output=255;
    else if(control_output<-255) control_output=-255;
   
    motor1.run(control_output);// value: between -255 and 255
    time =millis();
    if(time-last_time>2000)
    {
      Serial.println(control_output);
      Serial.println(error);
      Serial.println(error_integral);
      Serial.println(error_derivative);
      Serial.println(count);
      last_time=time;
    }
}
void isr_for_encoder()
{
    if(digitalRead(NE1)>0)
      count++;
    else
      count--;
}
void ISR_per_10ms() {
    error = dest - count;
    error_integral+=error;
    if(error_integral>ERROR_INTEGRAL_MAX) error_integral=ERROR_INTEGRAL_MAX;
    else if(error_integral<-ERROR_INTEGRAL_MAX) error_integral=-ERROR_INTEGRAL_MAX;
    error_derivative=error_prev-error;
    error_prev=error;
}


运行效果：
https://v.youku.com/v_show/id_XNDIxNTMzMzA4OA==.html?spm=a2h0j.11185381.listitem_page1.5~A

查看完整版本: 使用MegaPi扩展模块TB6612驱动直流编码电机