PID 控制算法实践 — 基于 Arduino UNO 的直流电机 PWM 调速系统软件设计

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12

#include<Arduino.h>

int main() {
    init();
    setup();
    for(;;) {
        loop();
        if(serialEventRun)
            serialEventRun();
    }
    return 0;
}

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28

#include <SSD1306.h>
#include "DATASCOPE.h" /*PC 端上位机的库文件*/
#include <PinChangeInt.h> /*外部中断*/
#include <MsTimer2.h> /*定时中断*/
/**********OLED 显示屏引脚**********/
#define OLED_DC 5
#define OLED_CLK 8
#define OLED_MOSI 7
#define OLED_RESET 6
SSD1306 oled(OLED_MOSI, OLED_CLK, OLED_DC, OLED_RESET, 0);
/**********TB6612 驱动引脚**********/
#define PWM 9 /*PWM 引脚*/
#define IN1 10
#define IN2 11
/**********编码器引脚**********/
#define ENCODER_A 4
#define ENCODER_B 2
/**********按键引脚**********/
#define KEY_Down 16 /*减速键*/
#define KEY_Up 17 /*加速键*/
/**********其他设定**********/
DATASCOPE data;
/*实例化一个上位机对象，对象名称为 data*/
float Velocity=0,Position=0,Motor=0;
/*定义：速度，位置，pid 控制输出*/
float Target_Velocity=0; /*初始目标速度（设为停机状态）*/
unsigned char Send_Count; /*上位机相关变量*/
float Velocity_KP,Velocity_KI; /*PID 系数*/

 1
 2
 3
 4
 5
 6
 7
 8
 9
10

int Incremental_PI (int Encoder,int Target) {
    static int Bias, Pwm, Last_bias;
    Bias = Encoder - Target; /*计算偏差*/
    Pwm += Velocity_KP * (Bias - Last_bias) + Velocity_KI * Bias;
    /*增量式 PI 控制器*/
    if (Pwm > 7200) 
        Pwm=7200; /*限幅*/
    Last_bias = Bias; /*保存上一次偏差*/
    return Pwm; /*增量输出*/
}

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14

uint32_t oled_pow(uint8_t m,uint8_t n) {
    uint32_t result = 1;
    while (n--)
        result *= m;
    return result;
}
void OLED_ShowNumber(uint8_t x,uint8_t y,uint32_t num,uint8_t len) {
    u8 t,temp;
    u8 enshow=0;
    for (t=0; t<len; t++) {
        temp = (num / oled_pow(10, len-t-1)) % 10;
        oled.drawchar(x + 6 * t, y, temp + '0');
    }
}

  1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216

#ifndef Arduino_h
#define Arduino_h

#include <stdlib.h>
#include <stdbool.h>
#include <string.h>
#include <math.h>
#include <avr/pgmspace.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include "binary.h"

#ifdef __cplusplus
extern "C"
{
#endif

void yield(void);

#define HIGH 0x1
#define LOW  0x0
#define INPUT 0x0
#define OUTPUT 0x1
#define INPUT_PULLUP 0x2
#define PI 3.1415926535897932384626433832795
#define HALF_PI 1.5707963267948966192313216916398
#define TWO_PI 6.283185307179586476925286766559
#define DEG_TO_RAD 0.017453292519943295769236907684886
#define RAD_TO_DEG 57.295779513082320876798154814105
#define EULER 2.718281828459045235360287471352
#define SERIAL  0x0
#define DISPLAY 0x1
#define LSBFIRST 0
#define MSBFIRST 1
#define CHANGE 1
#define FALLING 2
#define RISING 3
#if defined(__AVR_ATtiny24__) || defined(__AVR_ATtiny44__) || defined(__AVR_ATtiny84__) || defined(__AVR_ATtiny25__) || defined(__AVR_ATtiny45__) || defined(__AVR_ATtiny85__)
#define DEFAULT 0
#define EXTERNAL 1
#define INTERNAL 2
#else
#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) || defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__) || defined(__AVR_ATmega644__) || defined(__AVR_ATmega644A__) || defined(__AVR_ATmega644P__) || defined(__AVR_ATmega644PA__)
#define INTERNAL1V1 2
#define INTERNAL2V56 3
#else
#define INTERNAL 3
#endif
#define DEFAULT 1
#define EXTERNAL 0
#endif
// undefine stdlib's abs if encountered
#ifdef abs
#undef abs
#endif
#define min(a, b) ((a)<(b)?(a):(b))
#define max(a, b) ((a)>(b)?(a):(b))
#define abs(x) ((x)>0?(x):-(x))
#define constrain(amt, low, high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
#define round(x)     ((x)>=0?(long)((x)+0.5):(long)((x)-0.5))
#define radians(deg) ((deg)*DEG_TO_RAD)
#define degrees(rad) ((rad)*RAD_TO_DEG)
#define sq(x) ((x)*(x))
#define interrupts() sei()
#define noInterrupts() cli()
#define clockCyclesPerMicrosecond() ( F_CPU / 1000000L )
#define clockCyclesToMicroseconds(a) ( (a) / clockCyclesPerMicrosecond() )
#define microsecondsToClockCycles(a) ( (a) * clockCyclesPerMicrosecond() )
#define lowByte(w) ((uint8_t) ((w) & 0xff))
#define highByte(w) ((uint8_t) ((w) >> 8))
#define bitRead(value, bit) (((value) >> (bit)) & 0x01)
#define bitSet(value, bit) ((value) |= (1UL << (bit)))
#define bitClear(value, bit) ((value) &= ~(1UL << (bit)))
#define bitWrite(value, bit, bitvalue) (bitvalue ? bitSet(value, bit) : bitClear(value, bit))
// avr-libc defines _NOP() since 1.6.2
#ifndef _NOP
#define _NOP() do { __asm__ volatile ("nop"); } while (0)
#endif
typedef unsigned int word;
#define bit(b) (1UL << (b))
typedef bool boolean;
typedef uint8_t byte;

void init(void);

void initVariant(void);

int atexit(void (*func)()) __attribute__((weak));

void pinMode(uint8_t, uint8_t);

void digitalWrite(uint8_t, uint8_t);

int digitalRead(uint8_t);

int analogRead(uint8_t);

void analogReference(uint8_t mode);

void analogWrite(uint8_t, int);

unsigned long millis(void);

unsigned long micros(void);

void delay(unsigned long);

void delayMicroseconds(unsigned int us);

unsigned long pulseIn(uint8_t pin, uint8_t state, unsigned long timeout);

unsigned long pulseInLong(uint8_t pin, uint8_t state, unsigned long timeout);

void shiftOut(uint8_t dataPin, uint8_t clockPin, uint8_t bitOrder, uint8_t val);

uint8_t shiftIn(uint8_t dataPin, uint8_t clockPin, uint8_t bitOrder);

void attachInterrupt(uint8_t, void (*)(void), int mode);

void detachInterrupt(uint8_t);

void setup(void);

void loop(void);
// Get the bit location within the hardware port of the given virtual pin.
// This comes from the pins_*.c file for the active board configuration.
#define analogInPinToBit(P) (P)
// On the ATmega1280, the addresses of some of the port registers are
// greater than 255, so we can't store them in uint8_t's.
extern const uint16_t PROGMEM
port_to_mode_PGM[];
extern const uint16_t PROGMEM
port_to_input_PGM[];
extern const uint16_t PROGMEM
port_to_output_PGM[];
extern const uint8_t PROGMEM
digital_pin_to_port_PGM[];
// extern const uint8_t PROGMEM digital_pin_to_bit_PGM[];
extern const uint8_t PROGMEM
digital_pin_to_bit_mask_PGM[];
extern const uint8_t PROGMEM
digital_pin_to_timer_PGM[];
// Get the bit location within the hardware port of the given virtual pin.
// This comes from the pins_*.c file for the active board configuration.
// These perform slightly better as macros compared to inline functions
//
#define digitalPinToPort(P) ( pgm_read_byte( digital_pin_to_port_PGM + (P) ) )
#define digitalPinToBitMask(P) ( pgm_read_byte( digital_pin_to_bit_mask_PGM + (P) ) )
#define digitalPinToTimer(P) ( pgm_read_byte( digital_pin_to_timer_PGM + (P) ) )
#define analogInPinToBit(P) (P)
#define portOutputRegister(P) ( (volatile uint8_t *)( pgm_read_word( port_to_output_PGM + (P))) )
#define portInputRegister(P) ( (volatile uint8_t *)( pgm_read_word( port_to_input_PGM + (P))) )
#define portModeRegister(P) ( (volatile uint8_t *)( pgm_read_word( port_to_mode_PGM + (P))) )
#define NOT_A_PIN 0
#define NOT_A_PORT 0
#define NOT_AN_INTERRUPT -1
#ifdef ARDUINO_MAIN
#define PA 1
#define PB 2
#define PC 3
#define PD 4
#define PE 5
#define PF 6
#define PG 7
#define PH 8
#define PJ 10
#define PK 11
#define PL 12
#endif
#define NOT_ON_TIMER 0
#define TIMER0A 1
#define TIMER0B 2
#define TIMER1A 3
#define TIMER1B 4
#define TIMER1C 5
#define TIMER2  6
#define TIMER2A 7
#define TIMER2B 8
#define TIMER3A 9
#define TIMER3B 10
#define TIMER3C 11
#define TIMER4A 12
#define TIMER4B 13
#define TIMER4C 14
#define TIMER4D 15
#define TIMER5A 16
#define TIMER5B 17
#define TIMER5C 18
#ifdef __cplusplus
} // extern "C"
#endif
#ifdef __cplusplus
#include "WCharacter.h"
#include "WString.h"
#include "HardwareSerial.h"
#include "USBAPI.h"
#if defined(HAVE_HWSERIAL0) && defined(HAVE_CDCSERIAL)
#error "Targets with both UART0 and CDC serial not supported"
#endif
uint16_t makeWord(uint16_t w);
uint16_t makeWord(byte h, byte l);
#define word(...) makeWord(__VA_ARGS__)
unsigned long pulseIn(uint8_t pin, uint8_t state, unsigned long timeout = 1000000L);
unsigned long pulseInLong(uint8_t pin, uint8_t state, unsigned long timeout = 1000000L);
void tone(uint8_t _pin, unsigned int frequency, unsigned long duration = 0);
void noTone(uint8_t _pin);
// WMath prototypes
long random(long);
long random(long, long);
void randomSeed(unsigned long);
long map(long, long, long, long, long);
#endif

#include "pins_arduino.h"

#endif

  1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112

/****************************************************************
> MiniBalance 上位机。h 文件
***************************************************************/
#ifndef _DATASCOPE_H__
#define _DATASCOPE_H__
//导入 Arduino 核心头文件
#include"Arduino.h"

extern unsigned char DataScope_OutPut_Buffer[42];
class DATASCOPE {
    public:
        DATASCOPE();
        void Float2Byte(float *target, unsigned char *buf, unsigned char beg);
        void DataScope_Get_Channel_Data(float Data, unsigned char Channel);
        unsigned char DataScope_Data_Generate(unsigned char Channel_Number);
};
#endif
/****************************************************************
> MiniBalance 上位机。Cpp 文件
****************************************************************/
#include"DATASCOPE.h"

unsigned char DataScope_OutPut_Buffer[42] = {0};  //串口发送缓冲区	

DATASCOPE::DATASCOPE(){}

void DATASCOPE::Float2Byte(float *target, unsigned char *buf, unsigned char beg) {
    unsigned char *point;
    point = (unsigned char *) target;      //得到 float 的地址
    buf[beg] = point[0];
    buf[beg + 1] = point[1];
    buf[beg + 2] = point[2];
    buf[beg + 3] = point[3];
}

void DATASCOPE::DataScope_Get_Channel_Data(float Data, unsigned char Channel) {
    if ((Channel > 10) || (Channel == 0)) return;  //通道个数大于 10 或等于 0，												//直接跳出，不执行函数
    else {
        switch (Channel) {
            case 1:
                Float2Byte(&Data, DataScope_OutPut_Buffer, 1);
                break;
            case 2:
                Float2Byte(&Data, DataScope_OutPut_Buffer, 5);
                break;
            case 3:
                Float2Byte(&Data, DataScope_OutPut_Buffer, 9);
                break;
            case 4:
                Float2Byte(&Data, DataScope_OutPut_Buffer, 13);
                break;
            case 5:
                Float2Byte(&Data, DataScope_OutPut_Buffer, 17);
                break;
            case 6:
                Float2Byte(&Data, DataScope_OutPut_Buffer, 21);
                break;
            case 7:
                Float2Byte(&Data, DataScope_OutPut_Buffer, 25);
                break;
            case 8:
                Float2Byte(&Data, DataScope_OutPut_Buffer, 29);
                break;
            case 9:
                Float2Byte(&Data, DataScope_OutPut_Buffer, 33);
                break;
            case 10:
                Float2Byte(&Data, DataScope_OutPut_Buffer, 37);
                break;
        }
    }
}

unsigned char DATASCOPE::DataScope_Data_Generate(unsigned char Channel_Number) {
    if ((Channel_Number > 10) || (Channel_Number == 0)) { return 0; }  //通道个数大于 10 或等于 0，直接跳出，不执行函数
    else {
        DataScope_OutPut_Buffer[0] = '$';  //帧头
        switch (Channel_Number) {
            case 1:
                DataScope_OutPut_Buffer[5] = 5;
                return 6;
            case 2:
                DataScope_OutPut_Buffer[9] = 9;
                return 10;
            case 3:
                DataScope_OutPut_Buffer[13] = 13;
                return 14;
            case 4:
                DataScope_OutPut_Buffer[17] = 17;
                return 18;
            case 5:
                DataScope_OutPut_Buffer[21] = 21;
                return 22;
            case 6:
                DataScope_OutPut_Buffer[25] = 25;
                return 26;
            case 7:
                DataScope_OutPut_Buffer[29] = 29;
                return 30;
            case 8:
                DataScope_OutPut_Buffer[33] = 33;
                return 34;
            case 9:
                DataScope_OutPut_Buffer[37] = 37;
                return 38;
            case 10:
                DataScope_OutPut_Buffer[41] = 41;
                return 42;
        }
    }
    return 0;
}

  1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259

#include <SSD1306.h>
#include "DATASCOPE.h"       /*PC 端上位机的库文件*/
#include <PinChangeInt.h>    /*外部中断*/
#include <MsTimer2.h>        /*定时中断*/
/**********OLED 显示屏引脚**********/
#define OLED_DC 5
#define OLED_CLK 8
#define OLED_MOSI 7
#define OLED_RESET 6

SSD1306 oled(OLED_MOSI, OLED_CLK, OLED_DC, OLED_RESET, 0);

/**********TB6612 驱动引脚**********/
#define PWM 9                /*PWM 引脚*/
#define IN1 10
#define IN2 11
/**********编码器引脚**********/
#define ENCODER_A 4
#define ENCODER_B 2
/**********按键引脚**********/
#define KEY_Down 16        /*减速键*/
#define KEY_Up 17           /*加速键*/
/**********其他设定**********/
DATASCOPE data;              /*实例化一个上位机对象，对象名称为 data*/
float Velocity = 0, Position = 0, Motor = 0;    /*定义：速度，位置，pid 控制输出*/
float Target_Velocity = 0;                /*初始目标速度（设为停机状态）*/
unsigned char Send_Count;                /*上位机相关变量*/
float Velocity_KP = 20, Velocity_KI = 30;    /*PID 系数*/

/*************************************************************
函数功能：向上位机发送数据
入口参数：无
返回  值：无
****************************************************************/
void DataScope(void) {
    int i;
    data.DataScope_Get_Channel_Data(Velocity, 1);  /*显示第一个数据*/
    data.DataScope_Get_Channel_Data(Target_Velocity, 2);
    /*显示第二个数据*/
    Send_Count = data.DataScope_Data_Generate(2);
    for (i = 0; i < Send_Count; i++) {
        Serial.write(DataScope_OutPut_Buffer[i]);
    }
    delay(50);  /*控制发送时序*/
}

/****************************************************************
函数功能：给定值调节
入口参数：无
返回  值：无
****************************************************************/
void Adjust(void) {
    float Velocity_Amplitude = 0.019;  /*速度调节增量的赋值 (6.9 度）*/
    int temp;   /*定义临时变量*/
    temp = My_click();   /*采集按键信息*/
    /*改变运行速度*/
    if (temp == 2)Target_Velocity += Velocity_Amplitude;
    if (temp == 1)Target_Velocity -= Velocity_Amplitude;
    if (Target_Velocity > 60)Target_Velocity = 60; /*速度最大值限幅*/
    if (Target_Velocity < 0)Target_Velocity = 0;   /*速度最小值限幅*/
}

/****************************************************************
函数功能：按键扫描
入口参数：无
返回  值：unsigned char 型
****************************************************************/
unsigned char My_click(void) {
    static unsigned char flag_key = 1; /*按键标志，默认到断开*/
    if (digitalRead(KEY_Down) == 0 ||
        digitalRead(KEY_Up) == 0) /*如果发生单击事件*/
    {
        flag_key = 0;                            /*按键按下*/
        if (digitalRead(KEY_Down) == 0)           /*减速键*/
        {
            return 1;
        }
        if (digitalRead(KEY_Up) == 0)        /*提速键*/
        {
            return 2;
        }
    } else if (digitalRead(KEY_Down) == 1 && digitalRead(KEY_Up) == 1)
        flag_key = 1; /*所有案件都松开了*/
    return 0;/*无按键按下*/
}

/****************************************************************
函数功能：计算 m 的 n 次幂
入口参数：uint8_t 型
返回  值：uint32_t 型
****************************************************************/
uint32_t oled_pow(uint8_t m, uint8_t n) {
    uint32_t result = 1;
    while (n--)
        result *= m;
    return result;
}

/****************************************************************
函数功能：OLED 显示
入口参数：uint8_t 型、uint32_t 型
返回  值：无
****************************************************************/
void OLED_ShowNumber(uint8_t x, uint8_t y, uint32_t num, uint8_t len) {
    u8 t, temp;
    u8 enshow = 0;
    for (t = 0; t < len; t++) {
        temp = (num / oled_pow(10, len - t - 1)) % 10;
        oled.drawchar(x + 6 * t, y, temp + '0');
    }
}

/****************************************************************
函数功能：将控制量发送给 PWM 寄存器
入口参数：int 型
返回  值：无
****************************************************************/
void Set_Pwm(int motor) {
    if (motor < 0) {
        digitalWrite(IN1, HIGH);
        digitalWrite(IN2, LOW);   /*TB6612 的电平控制*/
    } else {
        digitalWrite(IN1, LOW);
        digitalWrite(IN2, HIGH);  /*TB6612 的电平控制*/
    }
    analogWrite(PWM, abs(motor)); /*赋值给 PWM 寄存器*/
}

/****************************************************************
函数功能：将控制量发送给 PWM 寄存器
入口参数：int 型
返回  值：int 型
****************************************************************/
int Incremental_PI(int Encoder, int Target) {
    static int Bias, Pwm, Last_bias;
    Bias = Encoder - Target;                          /*计算偏差*/
    Pwm += Velocity_KP * (Bias - Last_bias) + Velocity_KI * Bias;
    /*增量式 PI 控制器*/
    if (Pwm > 7200) Pwm = 7200;                    /*限幅*/
    Last_bias = Bias;                           /*保存上一次偏差*/
    return Pwm;                                  /*增量输出*/
}

/****************************************************************
函数功能： 电机控制函数 核心代码
入口参数： 无
返回  值： 无
****************************************************************/
void control() {
    static unsigned char Count_Velocity;  /*位置检测用的变量*/
    sei();                                  /*全局中断开启*/
    if (++Count_Velocity > 4) {
        Velocity = Position / 52 * 50;    /*单位时间内读取位置信息*/
        Position = 0;           /*并清零*/
        Count_Velocity = 1;     /*计数器置 1*/
        Motor = Incremental_PI(Velocity, Target_Velocity);  /*速度 PI 控制器*/
    }
    Set_Pwm(Motor / 28);    /*输出电机控制量*/
    Adjust();              /*PID 参数调节*/
}

/****************************************************************
函数功能：初始化函数
入口参数：无
返回  值：无
***************************************************************/
void setup() {
    oled.ssd1306_init(SSD1306_SWITCHCAPVCC);
    oled.clear();                /*clears the screen and buffer*/
    pinMode(IN1, OUTPUT);
    /*设置 TB6612 方向控制引脚为输出模式*/
    pinMode(IN2, OUTPUT);
    /*设置 TB6612 方向控制引脚为输出模式*/
    pinMode(PWM, OUTPUT);
    /*设置 TB6612 速度控制引脚为输出模式*/
    digitalWrite(IN1, 0);        /*TB6612 方向控制引脚下拉*/
    digitalWrite(IN2, 0);        /*TB6612 方向控制引脚下拉*/
    digitalWrite(PWM, 0);        /*TB6612 速度控制引脚下拉*/
    pinMode(ENCODER_A, INPUT);   /*设置编码器引脚为输入模式*/
    pinMode(ENCODER_B, INPUT);   /*设置编码器引脚为输入模式*/
    Serial.begin(128000);            /*开启串口*/
    delay(200);                      /*延时等待初始化完成*/
    MsTimer2::set(5, control);      /*使用 Timer2 设置 5ms 定时中断*/
    MsTimer2::start();               /*中断使能*/
    ttachInterrupt(0, READ_ENCODER_A, CHANGE);
    /*开启外部中断*/
    ttachPinChangeInterrupt(4, READ_ENCODER_B, CHANGE);
}

/***************************************************************
函数功能：主循环程序体
入口参数：无
返回  值：无
***************************************************************/
void loop() {
    oled.drawstring(00, 0, "VELOCITY MODE");  /*速度模式*/
    if (Velocity != 0) 
        oled.drawstring(00, 01, "MOTOR O N"); /*速度不为零则电机开启*/
    else 
        oled.drawstring(00, 01, "MOTOR OFF"); /*速度为零则电机关闭*/

    /***********第 3 行显示速度 PID 控制 P 参数**************/
    oled.drawstring(0, 02, "V-KP:");
    OLED_ShowNumber(30, 02, Velocity_KP, 3);
    /*************第 4 行显示速度 PID 控制 I 参数************/
    oled.drawstring(0, 03, "V-KI:");
    OLED_ShowNumber(30, 03, Velocity_KI, 3);
    /****************第 6 行显示速度测量参数***************/
    oled.drawstring(00, 5, "VELOCITY:");
    OLED_ShowNumber(60, 5, Velocity, 5);
    /**************第 7 行显示速度目标值*******************/
    oled.drawstring(00, 6, "TARGET:");
    OLED_ShowNumber(60, 6, Target_Velocity, 5);
    /************************刷新*************************/
    oled.display();
    DataScope();
}

/***************************************************************
函数功能：外部中断读取编码器数据
入口参数：无
返回  值：无
***************************************************************/
void READ_ENCODER_A() {
    if (digitalRead(ENCODER_A) == HIGH) /*如果是上升沿触发的中断*/
    {
        if (digitalRead(ENCODER_B) == LOW)
            Position++;
        else
            Position--;
    } else /*如果是下降沿触发的中断*/
    {
        if (digitalRead(ENCODER_B) == LOW)
            Position--;
        else
            Position++;
    }
}

/****************************************************************
函数功能：外部中断读取编码器数据
入口参数：无
返回  值：无
****************************************************************/
void READ_ENCODER_B() {
    if (digitalRead(ENCODER_B) == LOW) /*如果是下降沿触发的中断*/
    {
        if (digitalRead(ENCODER_A) == LOW)
            Position++;
        else
            Position--;
    } else /*如果是上升沿触发的中断*/
    {
        if (digitalRead(ENCODER_A) == LOW)
            Position--;
        else
            Position++;
    }
}