OpenMv-arduino-uno实现循迹

coding>Python
14k 词

参考：巡线小车 · OpenMV中文入门教程

转载自ljweb.xyz

1.OpenMv(main.py)

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
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
'''
功能描述
    完整功能包括：
    * 直线巡线
    * 直角转弯判定　(左转or右转)
    * T字形路口判定
    * 十字形路口判定
    其中Ｔ字形跟十字形可以用作四轴悬停的参考点。
原理介绍
    算法的主要核心在于，讲整个画面分割出来5个ROI区域
    * 上方横向采样
    * 中间横向采样
    * 下方横向采样
    * 左侧垂直采样
    * 右侧垂直采样
    通过判断5个图片的组合关系给出路口类型的判断
'''
import sensor
import image
import time
import math
import pyb
from pyb import Pin, Timer, UART,LED
from GeometryFeature import GeometryFeature

LED(4).on()
is_debug = True
#--------------感光芯片配置  START -------------------

DISTORTION_FACTOR = 1.5 # 设定畸变系数
IMG_WIDTH  = 64
IMG_HEIGHT = 64
def init_sensor():
    '''
    初始化感光芯片
    '''
    sensor.reset()
    sensor.set_pixformat(sensor.GRAYSCALE)
    sensor.set_framesize(sensor.B64X64)                  # 分辨率为B64X64
    sensor.skip_frames(time=2000)
    sensor.set_auto_gain(False)                         # 颜色追踪关闭自动增益
    sensor.set_auto_whitebal(False)                     # 颜色追踪关闭白平衡

init_sensor()
#--------------感光芯片配置  END -------------------

def ternleft():
    uart.write('G')
    time.sleep_ms(650)
    uart.write('L')
    time.sleep_ms(600)

def ternright():
    uart.write('G')
    time.sleep_ms(650)
    uart.write('R')
    time.sleep_ms(590)

#--------------串口UART部分  START -------------------
uart = pyb.UART(3,115200,timeout_char = 1000) #串口初始化

def get_symbol(num):
    '''
    根据数值正负，返回数值对应的符号
    正数：‘+’， 负数‘-’ 主要为了方便C语言解析待符号的数值。
    '''
    if num >=0:
        return '+'
    else:
        return '-'

def data_format_wrapper(yaw_angle, sum_x, sum_y, cx_mean, cx, cy, is_left_angle, is_t, last_y):
    '''
    根据通信协议封装数据
    TODO 重新编写通信协议  与配套C解析代码

    yaw_angle,sum_x, sum_y 没有用到
    cx_mean: roi 1 2 3 对应的bolb中心的x坐标加权平均， 如果没有就补32  都没有就赋值为原来的值
    cx : roi 1 3 blob 中心坐标的加权平均， 如果一方丢失， 就赋值为另外一个  都没有就赋值为原来的值
    cy : roi 4 5 blob 中心坐标的加权平均，如果一方丢失， 就赋值为另外一个， 都没有就赋值为原来的值
    is_left_angle :代表是否有直角，！！！信息丢失
             其实，可以用一个字符来表示，是左转还是右转 T(T字形)  L(Left) R(Right)
    last_x, last_y 是两个直线的交叉点的坐标  改写为 intersect_x，intersect_y

    args = [
        get_symbol(yaw_angle), # 偏航角符号
        abs(int(yaw_angle)), # 偏航角
        get_symbol(sum_x), # 光流数据sux_x的符号
        abs(int(sum_x)), # 光流数据sum_x
        get_symbol(sum_y), # 光流数据sum_y的符号
        abs(int(sum_y)), # 光流数据 sum_y
        int(cx_mean), # x的中心，三个取样区域色块中心x坐标的平均值
        int(cx),
        int(cy),
        int(is_left_angle),
        int(last_x),
        int(last_y)
    ]
    # 将数值列表按照通信协议，转换为待发送的字符
    info = 's%c%.2d%c%.2d%c%.2d%.2d%.2d%.2d%.2d%.2d%.2d#'%tuple(args)
    global is_debug
    if is_debug:
        print('s%c%.2d%c%.2d%c%.2d  cx_mean=%.2d cx=%.2d cy=%.2d Turn Left: %.2d  %.2d%.2d#'%tuple(args))
            '''

    if cx_mean >= 22 and cx_mean <=40 :
        info='G'
    if cx_mean<22  :
        info='L'
    if cx_mean>40  :
        info='R'
    if is_t        :
        info='S'
        ternleft()

    return info
#--------------串口UART部分 END -------------------

#--------------定时器部分 START -------------------

is_need_send_data = False # 是否需要发送数据的信号标志
def uart_time_trigger(timer):
    '''
    串口发送数据的定时器，定时器的回调函数
    '''
    global is_need_send_data
    is_need_send_data = True

# 初始化定时器 频率为20HZ 每秒执行20次
tim = Timer(4, freq=20)
# 设定定时器的回调函数
tim.callback(uart_time_trigger)
#--------------定时器部分 END -------------------

#--------------直线与直角检测部分 START -------------------

INTERSERCT_ANGLE_THRESHOLD = (45,90)

# 直线灰度图颜色阈值
LINE_COLOR_THRESHOLD = [(20, 80)]
# 如果直线是白色的，阈值修改为：
# LINE_COLOR_THRESHOLD = [(128, 255)]

# 取样窗口
ROIS = {
    'down': (0, 55, 64, 8), # 横向取样-下方 1
    'middle': (0, 28, 64, 8), # 横向取样-中间 2
    'up': (0, 0, 64, 8), # 横向取样-上方 3
    'left': (0, 0, 8, 64), # 纵向取样-左侧 4
    'right': (56, 0, 8, 64) # 纵向取样-右侧 5
}

def find_blobs_in_rois(img):
    '''
    在ROIS中寻找色块，获取ROI中色块的中心区域与是否有色块的信息
    '''
    global ROIS
    global is_debug

    roi_blobs_result = {}  # 在各个ROI中寻找色块的结果记录
    for roi_direct in ROIS.keys():
        roi_blobs_result[roi_direct] = {
            'cx': -1,
            'cy': -1,
            'blob_flag': False
        }
    for roi_direct, roi in ROIS.items():
        blobs=img.find_blobs(LINE_COLOR_THRESHOLD, roi=roi, merge=True, pixels_area=10)
        if len(blobs) == 0:
            continue

        largest_blob = max(blobs, key=lambda b: b.pixels())
        x,y,width,height = largest_blob[:4]

        if not(width >=5 and width <= 15 and height >= 5 and height <= 15):
            # 根据色块的宽度进行过滤
            continue

        roi_blobs_result[roi_direct]['cx'] = largest_blob.cx()
        roi_blobs_result[roi_direct]['cy'] = largest_blob.cy()
        roi_blobs_result[roi_direct]['blob_flag'] = True

        if is_debug:
            img.draw_rectangle((x,y,width, height), color=(255))

    return roi_blobs_result

def visualize_result(canvas, cx_mean, cx, cy, is_turn_left, is_turn_right, is_t, is_cross):
    '''
    可视化结果
    '''
    if not(is_turn_left or is_turn_right or is_t or is_cross):
        mid_x = int(canvas.width()/2)
        mid_y = int(canvas.height()/2)
        # 绘制x的均值点
        canvas.draw_circle(int(cx_mean), mid_y, 5, color=(255))
        # 绘制屏幕中心点
        canvas.draw_circle(mid_x, mid_y, 8, color=(0))
        canvas.draw_line((mid_x, mid_y, int(cx_mean), mid_y), color=(255))

    turn_type = 'N' # 啥转角也不是

    if is_t or is_cross:
        # 十字形或者T形
        canvas.draw_cross(int(cx), int(cy), size=10, color=(255))
        canvas.draw_circle(int(cx), int(cy), 5, color=(255))

    if is_t:
        turn_type = 'T' # T字形状
    elif is_cross:
        turn_type = 'C' # 十字形
    elif is_turn_left:
        turn_type = 'L' # 左转
    elif is_turn_right:
        turn_type = 'R' # 右转

    canvas.draw_string(0, 0, turn_type, color=(0))

#--------------直线与直角检测部分 END -------------------

#---------------------MAIN-----------------------
last_cx = 0
last_cy = 0

while True:
    if not is_need_send_data:
        # 不需要发送数据
        continue
    is_need_send_data = False

    # 拍摄图片
    img = sensor.snapshot()
    # 去除图像畸变
    img.lens_corr(DISTORTION_FACTOR)
    # 创建画布
    # canvas = img.copy()
    # 为了IDE显示方便，直接在代码结尾 用IMG绘制

    # 注意：林林的代码里 计算直线之间的交点的代码没有用到
    lines = img.find_lines(threshold=1000, theta_margin = 50, rho_margin = 50)
    # 寻找相交的点 要求满足角度阈值
    intersect_pt = GeometryFeature.find_interserct_lines(lines, angle_threshold=(45,90), window_size=(IMG_WIDTH, IMG_HEIGHT))
    if intersect_pt is None:
        # 直线与直线之间的夹角不满足阈值范围
        intersect_x = 0
        intersect_y = 0
    else:
        intersect_x, intersect_y = intersect_pt

    reslut = find_blobs_in_rois(img)

    # 判断是否需要左转与右转
    is_turn_left = False
    is_turn_right = False

    if (not reslut['up']['blob_flag'] ) and reslut['down']['blob_flag']:
        if reslut['left']['blob_flag']:
            is_turn_left = True
        if reslut['right']['blob_flag']:
            is_turn_right = True

    # 判断是否为T形的轨道
    is_t = False
    # 判断是否十字形轨道
    is_cross = False

    cnt = 0
    for roi_direct in ['up', 'down', 'left', 'right']:
        if reslut[roi_direct]['blob_flag']:
            cnt += 1
    is_t = cnt == 3
    is_cross = cnt == 4

    # cx_mean 用于确定视角中的轨道中心
    # 用于表示左右偏移量
    cx_mean = 0
    for roi_direct in ['up', 'down', 'middle']:
        if reslut[roi_direct]['blob_flag']:
            cx_mean += reslut[roi_direct]['cx']
        else:
            cx_mean += IMG_WIDTH / 2
    cx_mean /= 3

    # cx, cy 只有在T形区域检测出来的时候才有用，
    # 用于确定轨道中圆形的大致区域， 用于定点， 是计算圆心的一种近似方法

    cx = 0
    cy = 0

    if is_cross or is_t:
        # 只在出现十字形或者T字形才计算圆心坐标
        cnt = 0
        for roi_direct in ['up', 'down']:
            if reslut[roi_direct]['blob_flag']:
                cnt += 1
                cx += reslut[roi_direct]['cx']
        if cnt == 0:
            cx = last_cx
        else:
            cx /= cnt

        cnt = 0
        for roi_direct in ['left', 'right']:
            if reslut[roi_direct]['blob_flag']:
                cnt += 1
                cy += reslut[roi_direct]['cy']
        if cnt == 0:
            cy = last_cy
        else:
            cy /= cnt

    # 为了兼容之前的程序，按照之前的数据通信协议发送
    # 林林的代码里没有用到的变量均设为 0， 且巡线演示历程中，只用到了左转
    # 发送信息格式，可以自行改造, 例如 添加 is_turn_left
    info = data_format_wrapper(0, 0, 0, cx_mean, cx, cy, is_turn_left, is_t, 0)
    uart.write(info)
    print(info)

    last_cx = cx
    last_cy = cy

    if is_debug:
        visualize_result(img, cx_mean, cx, cy, is_turn_left, is_turn_right, is_t, is_cross)

2.car.py

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
from pyb import Pin, Timer
inverse_left=False  #change it to True to inverse left wheel
inverse_right=False #change it to True to inverse right wheel

ain1 =  Pin('P0', Pin.OUT_PP)
ain2 =  Pin('P1', Pin.OUT_PP)
bin1 =  Pin('P2', Pin.OUT_PP)
bin2 =  Pin('P3', Pin.OUT_PP)
ain1.low()
ain2.low()
bin1.low()
bin2.low()

pwma = Pin('P7')
pwmb = Pin('P8')
tim = Timer(4, freq=1000)
ch1 = tim.channel(1, Timer.PWM, pin=pwma)
ch2 = tim.channel(2, Timer.PWM, pin=pwmb)
ch1.pulse_width_percent(0)
ch2.pulse_width_percent(0)

def run(left_speed, right_speed):
    if inverse_left==True:
        left_speed=(-left_speed)
    if inverse_right==True:
        right_speed=(-right_speed)

    if left_speed < 0:
        ain1.low()
        ain2.high()
    else:
        ain1.high()
        ain2.low()
    ch1.pulse_width_percent(int(abs(left_speed)))

    if right_speed < 0:
        bin1.low()
        bin2.high()
    else:
        bin1.high()
        bin2.low()
    ch2.pulse_width_percent(int(abs(right_speed)))

2.pid.py

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
from pyb import millis
from math import pi, isnan

class PID:
    _kp = _ki = _kd = _integrator = _imax = 0
    _last_error = _last_derivative = _last_t = 0
    _RC = 1/(2 * pi * 20)
    def __init__(self, p=0, i=0, d=0, imax=0):
        self._kp = float(p)
        self._ki = float(i)
        self._kd = float(d)
        self._imax = abs(imax)
        self._last_derivative = float('nan')

    def get_pid(self, error, scaler):
        tnow = millis()
        dt = tnow - self._last_t
        output = 0
        if self._last_t == 0 or dt > 1000:
            dt = 0
            self.reset_I()
        self._last_t = tnow
        delta_time = float(dt) / float(1000)
        output += error * self._kp
        if abs(self._kd) > 0 and dt > 0:
            if isnan(self._last_derivative):
                derivative = 0
                self._last_derivative = 0
            else:
                derivative = (error - self._last_error) / delta_time
            derivative = self._last_derivative + 
                                     ((delta_time / (self._RC + delta_time)) * 
                                        (derivative - self._last_derivative))
            self._last_error = error
            self._last_derivative = derivative
            output += self._kd * derivative
        output *= scaler
        if abs(self._ki) > 0 and dt > 0:
            self._integrator += (error * self._ki) * scaler * delta_time
            if self._integrator < -self._imax: self._integrator = -self._imax
            elif self._integrator > self._imax: self._integrator = self._imax
            output += self._integrator
        return output
    def reset_I(self):
        self._integrator = 0
        self._last_derivative = float('nan')

arduino:

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
#include <SPI.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#define OLED_RESET 4
Adafruit_SSD1306 display(OLED_RESET);

#if (SSD1306_LCDHEIGHT != 64)
#error("Height incorrect, please fix Adafruit_SSD1306.h!");
#endif

#define LF1 10
#define LF2 11
#define RI1 6
#define RI2 5
#define HW1 13
#define HW2 12

#define RED A0
#define GREEN A1
#define TZOUTPUT 8

int leftpwm = 255;
int rightpwm = 255;
int quyao = 0;

char com;
unsigned char continue_time = 0;

void go()
{
  analogWrite(LF1, leftpwm);
  analogWrite(LF2, 0);
  analogWrite(RI1, rightpwm);
  analogWrite(RI2, 0);
}
void right()
{
  analogWrite(LF1, 180);
  analogWrite(LF2, 0);
  analogWrite(RI1, 0);
  analogWrite(RI2, 180);
}

void left()
{
  analogWrite(LF1, 0);
  analogWrite(LF2, 180);
  analogWrite(RI1, 180);
  analogWrite(RI2, 0);
}

void back()
{
  analogWrite(LF1, 0);
  analogWrite(LF2, leftpwm);
  analogWrite(RI1, 0);
  analogWrite(RI2, rightpwm);
}

void carstop()
{
  analogWrite(LF1, 0);
  analogWrite(LF2, 0);
  analogWrite(RI1, 0);
  analogWrite(RI2, 0);
}

void doudong()
{
  analogWrite(LF1, 150);
  analogWrite(LF2, 0);
  analogWrite(RI1, 0);
  analogWrite(RI2, 150);
}

void red()
{
  digitalWrite(RED,LOW);
}

void green()
{
  digitalWrite(GREEN,LOW);
}

void notlight()
{
  digitalWrite(RED,HIGH);
  digitalWrite(GREEN,HIGH);  
}

void palldown()
{
  analogWrite(LF1, 80);
  analogWrite(LF2, 0);
  analogWrite(RI1, 85);
  analogWrite(RI2, 0);
}
void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  display.clearDisplay();
  display.display();
  //delay(2000);
  pinMode(LF1, OUTPUT);
  pinMode(LF2, OUTPUT);
  pinMode(RI1, OUTPUT);
  pinMode(RI2, OUTPUT);
  pinMode(HW1, INPUT);
  pinMode(HW2, INPUT);

  pinMode(RED, OUTPUT);
  pinMode(GREEN, OUTPUT);
  pinMode(TZOUTPUT, OUTPUT);
  digitalWrite(TZOUTPUT,LOW);
  digitalWrite(RED,HIGH);
  digitalWrite(GREEN,HIGH);

}
void oledisplay()
{
  // text display tests
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(WHITE);
  display.setCursor(0, 0);
  display.print("leftpwm:");
  display.println(leftpwm);
  display.print("rightpwm:");
  display.println(rightpwm);
  display.setTextSize(2);
  display.print("com:");
  display.println(com);
  display.display();
}
void cktx()
{

  //指令接收部分
  //蓝牙遥控部分
  // 读取串口发送的信息:
  if (Serial.available() > 0 )
  {
    char inChar;
    inChar = Serial.read();
    if (com != inChar)
    {
      //  bt_rec_flag = 1;
      com = inChar;
    }

  }

          //  else    // 调试用
            //com = 'S';

      switch (com)
      {
        case 'G':    go(); Serial.println("直行"); break;
        case 'L':  left(); Serial.println("左转"); break;
        case 'R':  right(); Serial.println("右转"); break;
        case 'B':  back(); Serial.println("后退"); break;
        case 'S':  carstop(); Serial.println("停车"); break;
        case '1':  red(); Serial.println("RED");break;
        case '2':  green();Serial.println("GREEN");break;
        case '3':  notlight();Serial.println("熄灭");break;
        case 'H':  palldown();Serial.println("速度减半");break;
        default : break;
      }

}

void tinzhi()
{
  if(digitalRead(HW1)==HIGHdigitalRead(HW2)==HIGH)
  {
    digitalWrite(TZOUTPUT,HIGH);
    }
}
void loop() {
  cktx();
  tinzhi();
 // Serial.println(quyao);
 // doudong();
  //oledisplay();
  //delay(10);
}