Arduino Tank Car Kit Lesson 8: Use encoder to synchronize motor speed

Byamber

Arduino Tank Car Kit Lesson 8: Use encoder to synchronize motor speed

{:en}

Authorized Amazon Seller:

Robot Tank Car Chassis

Buy from US Buy from CA Buy from UK Buy from DE Buy from IT Buy from FR Buy from ES

Robot Tank Car Electronic Parts Kit

Buy from US Buy from CA Buy from UK Buy from DE Buy from IT Buy from FR Buy from ES

Objective:

In our previous lesson, we know that the motor speed is powered and controlled by PWM(Pulse Width Modulation) current from L298N. However, due to manufacturing inaccuracy, left and right motors might run at different speed even if they powered with same PWM signal. This will cause the robot car offtrack straight line.

To solve the problem, we need collect the speed data from left and right motors and use the data to adjust PWM current and synchronize the speed of both side motors.

In this lesson, we will use build-in Hall Encoder in the motor to collect speed data and send the data to Arduino, then use the data to synchronize motor speed and make car running straight.

The wire connection of Hall encoder(speed sensor).

Black wire(VM): Power for Motor

Red wire (GND): GND for Motor

White wire(V)：Power for hall sensor (5V)

Yellow wire(G）：GND for hall sensor

Orange wire (S1)：Signal for the 1st hall sensor

Green(S2)：Signal for the 2st hall sensor

motor

Connect the wires to the ESP8266 wifi board as per the following diagram:

encoder motor(left)-esp8266 encoder motor(right)-esp8266

Motor(Left)
Encoder Esp8266 uart wifi shield
V 5V
G GND
S1 D2
S2 D4
Motor(Right)
Encoder Esp8266 uart wifi shield
V 5V
G GND
S1 D3
S2 D7

Explanation:

Unlike other sensor, Hall encoder has two signal pin S1 and S2 which should work together to get speed data. Arduino also needs two digital pins to input data from S1 and S2.  Our Arduino sample code will use an algorithm called PID to synchronize the speed of left and right motor. The detail of PID  algorithm is beyond the scope of this lesson. If you are interested in this algorithm, you can google it for detail.

Software Installation:

Step 1: Install latest Arduino IDE (If you have Arduino IDE version after 1.1.16, please skip this step)
Download Arduino IDE from https://www.arduino.cc/en/Main/Software?setlang=en , then install the software.

Step 2:Download the libraries from

http://www.kookye.com/download/Arduino_Tank_Car_Kit/datascope.zip
http://www.kookye.com/download/Arduino_Tank_Car_Kit/MsTimer2.zip
http://www.kookye.com/download/Arduino_Tank_Car_Kit/PinChangeInt.zip

open arduino IDE  -> click sketch -> Include Libraries->Add .zip Libraries -> choose zip file “datascope.zip”, “MsTimer2.zip” and “PinChangeInt.zip” in turns -> Upload three .zip files

Step 3:Download Lesson 8 sample code from http://www.kookye.com/download/Arduino_Tank_Car_Kit/tank_robot_lesson8.zip, unzip the download zip file tank_robot_lesson8.zip, you will see a folder

Step 4: Connect UNO R3 board to PC with USB cable, Open Arduino IDE -> click file -> click Open -> choose code “tank_robot_lesson2_encoder.ino” in tank_robot_lesson2_encoder folder, load the code into arduino.

open

Step 5: Choose corresponding board and port for your project,upload the sketch to the board.

port

Understand the Code:

Step 1: Define the pinout of left and right motor

const byte encoder0pinA = 2;//A pin -> the interrupt pin 0
const byte encoder0pinB = 4;//B pin -> the digital pin 4
const byte rencodPinA = 3;  //rencodPinA -> the interrupt pin 1
const byte rencodPinB = 7;  //rencodPinB -> the digital pin 11

Step 2:Set the sampling period to be 10ms. When up to 10ms, enter to the interrupt instructure control

 MsTimer2::set(10, control);  //use timer2 to set the 10ms timer interrupt
 MsTimer2::start();          //enable interrupt

Step 3: Initialization of encoder pinout and use the arduino external interruption via pinout

[编码器接到arduino uno外部中断引脚上。编码器引脚初始化，左右电机编码的A相输出分别使用arduino外部中断0和外部中断1，使用跳变沿（上升沿和下降沿）]

  l_direction = true;//default -> Forward  
  r_direction = true;//default -> Forward  
  pinMode(encoder0pinB,INPUT);  
  pinMode(rencodPinB,INPUT);  
  attachInterrupt(0, lwheelSpeed, CHANGE);
  attachInterrupt(1, rwheelSpeed, CHANGE);

Step 4: Count pluse per revolution .[电机转动时候对脉冲计数。编码器中断服务函数,当有跳变沿到来时候，会执行中断服务函数，对脉冲加1或减1]

void lwheelSpeed()
{
  int Lstate = digitalRead(encoder0pinA);
  if((encoder0PinALast == LOW) && Lstate==HIGH)
  {
    int val = digitalRead(encoder0pinB);
    if(val == LOW && l_direction)
    {
      l_direction = false; //Reverse
    }
    else if(val == HIGH && !l_direction)
    {
      l_direction = true;  //Forward
    }
  }
  encoder0PinALast = Lstate;
 
  if(!l_direction)  duration++;
  else  duration--;
}

void rwheelSpeed()
{
  int Lstate = digitalRead(rencodPinA);
  if((encoder0PinALast1 == LOW) && Lstate==HIGH)
  {
    int val = digitalRead(rencodPinB);
    if(val == LOW && r_direction)
    {
      r_direction = false; //Reverse
    }
    else if(val == HIGH && !r_direction)
    {
      r_direction = true;  //Forward
    }
  }
  encoder0PinALast1 = Lstate;
 
  if(!r_direction)  duration1++;
  else  duration1--;
}

Step 5:Getting error correction power via PI algorithm and use the power to adjust the motor speed.

[通过PI算法得到一个误差补给量power，将这个power作用于电机上，调节电机速度。
(上面的函数就是增量式PI算法的C语言实现，左右电机脉冲相减作为误差，误差的累积作为积分项，误差为0表示右边电机速度和左边电机速度一致；大于0说明右边电机比左边电机慢；小于0表示右边电机速度比左边电机快。通过PI算法会得到一个误差补给power，将这个power作用与右边电机上，就能达到调节速度的目的。)]

int PID_controller(int master,int slave)
{ 
  static float power,error,integralerror,lasterror;
  if(master < 0) master = -master;
  if(slave < 0) slave = -slave;
  error = master - slave;
  integralerror += error;
  power += Kp *(error-lasterror) + Ki * error;
  lasterror = error;
  return power;
}

Step 6: Run the instruction control ( ) per 10ms.

[每10ms执行一次control()函数。(10ms到达时候会执行定时器中断服务函数，在定时器中断服务函数中，将PI得到的误差补给作用到右边电机上。)]

void control()
{
   sei();//enable global interrupts
   if(++i >=4)//20ms
   {
   master_pulse = duration ,duration = 0;
   slave_pulse = duration1, duration1 = 0;
   pwm = PID_controller(master_pulse,slave_pulse);
   i = 0;
   }
   int newpower1 = motorspeed+pwm;
   constrain(newpower1,0,255);
   analogWrite(ENB,newpower1),analogWrite(ENA,motorspeed);
   cli();//disenable global interrupts
}
const float Kp =20;
const float Ki =1;

Hardware Installation:

Step 1: Install ESP8266 Expansion Board on UNO R3 board.
lesson2-HW-1
Step 2: Move the wire connected to digit ports(D5,D6,D8,D9,D10,D12) in UNO R3 board to its counterpart digit pin in ESP8266 wifi Board.

L298N-esp8266 wifi
Step 3: Turn the switch of esp8266 to “1” and “2” position, as the following photo shows.

lesson2-HW-2
(If you have finished the above steps on lesson one, please skip these step.)

Step 4: Testing. Mark a red label on both side tracks and turn on the battery switch so as to  see if it synchronizes. You can adjust Kp and Ki parameter to make the speed synchronized.(Note:The car can not go straight if the smooth ground created the least friction to slip)

{:}{:zh}概述

这是坦克智能小车的最后一课，在前面已经提到过，由于电机之间存在一定误差，即使给两个电机相同的电压，其转速一般是不一样的，这样坦克小车就无法行走直线。要想小车走直线就需要用编码器调节两个电机速度，使其速度一样。

编码器原理

编码器分为光电式编码器和霍尔编码器，我们使用的是霍尔式编码器。霍尔编码器是一种通过磁电转换将输出轴上机械几何位移转换成脉冲或数字量的传感器。霍尔编码器由霍尔码盘和霍尔元件组成。霍尔码盘是在一定直径的直板上等分地布置有不同的磁极，霍尔码盘与电机同轴，电机旋转时，霍尔元件检测输出若干脉冲信号，为判定转向，一般输出两组存在一定相位差的脉冲信号。

编码器接线说明

电机一共有6pin，其中VM和GM是控制电机的，接到L298N上，这在之前已经讲过，剩下的4pin分别是

V：霍尔编码器电源，一般接5V

G：霍尔编码器地，接GND

S1：编码器的A相输出

S2：编码器B相输出

motor

拧下亚克力面板上的四颗螺帽，将电机上编码器的四条线穿过金属面板和亚克力面板，按照下表接到esp8266 uart wifi shield上（翻译的时候把表变成连线图）

左边电机
编码器 esp8266 uart wifi shield
V 5V
G GND
S1 D2
S2 D4
右边电机
编码器 esp8266 uart wifi shield
V 5V
G GND
S1 D3
S2 D7

采集脉冲

因为编码器输出的是方波信号，可以用arduino直接读取，在上面的接线图中我们分别将编码器的A相输出接到arduino的外部中断引脚，用中断的方式采集脉冲；B相接到普通GPIO口，B相的电平用来判断正反转。通过arduino定时器把采样周期设定为10ms，采集完两个电机的脉冲后，需要用PID算法来调节电机速度。所谓PID就是对偏差进行比例、积分、微分，PID由3个单元组成，分别是比例(P)单元、积分(I)单元和微分(D)单元。在工程实践中，比例单元是必须的，所以衍生出许多组合的PID控制器，如PD、PI、PID，更多关于PID算法详细介绍请看这里。在本课中我才用增量式PI算法。在这个PI算法中把左边电机的速度作为目标速度，通过PI算法调节右边电机速度使其与左边电机速度一致。

软件

下载http://osoyoo.com/driver/tank_robot_lesson7.zip并解压文件，用arduino IDE打开tank_robot_lesson7.ino文件，下面对部分代码做简要说明。

第一步：上面四行代码分别定义了左右两个电机A、B相接口

const byte encoder0pinA = 2;//A pin -> the interrupt pin 0
const byte encoder0pinB = 4;//B pin -> the digital pin 4
const byte rencodPinA = 3;  //rencodPinA -> the interrupt pin 1
const byte rencodPinB = 7;  //rencodPinB -> the digital pin 11

第二步：设定采样周期为10ms，10ms到达后，进入定时器中断服务函数control中

 MsTimer2::set(10, control);  //use timer2 to set the 10ms timer interrupt
 MsTimer2::start();          //enable interrupt

第三步：编码器接到arduino uno外部中断引脚上。（编码器引脚初始化，左右电机编码的A相输出分别使用arduino外部中断0和外部中断1，使用跳变沿（上升沿和下降沿））。

  l_direction = true;//default -> Forward  
  r_direction = true;//default -> Forward  
  pinMode(encoder0pinB,INPUT);  
  pinMode(rencodPinB,INPUT);  
  attachInterrupt(0, lwheelSpeed, CHANGE);
  attachInterrupt(1, rwheelSpeed, CHANGE);

第四步：电机转动时候对脉冲计数。(编码器中断服务函数。当有跳变沿到来时候，会执行中断服务函数，对脉冲加1或减1)

void lwheelSpeed()
{
  int Lstate = digitalRead(encoder0pinA);
  if((encoder0PinALast == LOW) && Lstate==HIGH)
  {
    int val = digitalRead(encoder0pinB);
    if(val == LOW && l_direction)
    {
      l_direction = false; //Reverse
    }
    else if(val == HIGH && !l_direction)
    {
      l_direction = true;  //Forward
    }
  }
  encoder0PinALast = Lstate;
 
  if(!l_direction)  duration++;
  else  duration--;
}

void rwheelSpeed()
{
  int Lstate = digitalRead(rencodPinA);
  if((encoder0PinALast1 == LOW) && Lstate==HIGH)
  {
    int val = digitalRead(rencodPinB);
    if(val == LOW && r_direction)
    {
      r_direction = false; //Reverse
    }
    else if(val == HIGH && !r_direction)
    {
      r_direction = true;  //Forward
    }
  }
  encoder0PinALast1 = Lstate;
 
  if(!r_direction)  duration1++;
  else  duration1--;
}

第五步：通过PI算法得到一个误差补给量power，将这个power作用于电机上，调节电机速度。（上面的函数就是增量式PI算法的C语言实现，左右电机脉冲相减作为误差，误差的累积作为积分项，误差为0表示右边电机速度和左边电机速度一致；大于0说明右边电机比左边电机慢；小于0表示右边电机速度比左边电机快。通过PI算法会得到一个误差补给power，将这个power作用与右边电机上，就能达到调节速度的目的。）

int PID_controller(int master,int slave)
{ 
  static float power,error,integralerror,lasterror;
  if(master < 0) master = -master;
  if(slave < 0) slave = -slave;
  error = master - slave;
  integralerror += error;
  power += Kp *(error-lasterror) + Ki * error;
  lasterror = error;
  return power;
}

第六步：每10ms执行一次control()函数。10ms到达时候会执行定时器中断服务函数，在定时器中断服务函数中，将PI得到的误差补给作用到右边电机上。

void control()
{
   sei();//enable global interrupts
   if(++i >=4)//20ms
   {
   master_pulse = duration ,duration = 0;
   slave_pulse = duration1, duration1 = 0;
   pwm = PID_controller(master_pulse,slave_pulse);
   i = 0;
   }
   int newpower1 = motorspeed+pwm;
   constrain(newpower1,0,255);
   analogWrite(ENB,newpower1),analogWrite(ENA,motorspeed);
   cli();//disenable global interrupts
}

 

const float Kp =20;
const float Ki =1;

测试

step 1 烧录程序：用usb线把坦克小车与pc连接起来，选择正确的板子型号和端口号，将esp8266 uart wifi shield上SW1拨到off，把程序烧录到arduino中。

step 2 在两边履带相同位置绑一条红线之类的绳子。

step 3 用手拿着安装电池盒的金属车板，打开电池盒电源，观察2个电机上绳子转的快慢是否一致。如果不一致可以通过调节Kp、Ki两个参数。

step 4 将小车放在相对粗糙的地板上，打开电源开关，小车就能走直线了。（注意：如果地板光滑，会因为摩擦过小导致小车无法走直线）{:}

About the Author

amber administrator

You must be logged in to post a comment

Copyright © 2016 KOOKYE. All rights reserved.