mbc_code/mbc_method1.py at main · aztechell/mbc_code · GitHub

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
from pybricks.hubs import PrimeHub
from pybricks.parameters import Direction, Port
from pybricks.pupdevices import ColorSensor, Motor
from pybricks.tools import wait

prime_hub = PrimeHub()
line = ColorSensor(Port.C)
motorL = Motor(Port.B, Direction.COUNTERCLOCKWISE)
motorR = Motor(Port.F, Direction.CLOCKWISE)

def getBlackLineError():
    '''Возвращает ошибку положения относительно черной линии (0 = центр, <0 влево, >0 вправо), закодированную в reflection().'''
    return line.reflection() // 10 - 8

def getBlackLineWidth():
    '''Возвращает "ширину" черной линии (0..9), закодированную в младшей цифре reflection().'''
    return line.reflection() % 10

def getWhiteLineError():
    '''Возвращает ошибку положения относительно белой линии (0 = центр, <0 влево, >0 вправо), закодированную в целой части ambient().'''
    return int(line.ambient()) - 8

def getWhiteLineWidth():
    '''Возвращает "ширину" белой линии (0..9), закодированную в первой цифре после запятой ambient().'''
    a = line.ambient()
    return int((a - int(a)) * 10) % 10

def followBlackLine(base_dc=30, kp=3, kd=50, stop_width=8, dt_ms=10):
    '''Едет по черной линии PD-регулятором, пока ширина линии не станет >= stop_width.'''
    last_error = getBlackLineError()
    while getBlackLineWidth() < stop_width:
        e = getBlackLineError()
        u = e * kp + (e - last_error) * kd
        motorL.dc(base_dc - u)
        motorR.dc(base_dc + u)
        last_error = e
        wait(dt_ms)
    motorL.dc(0)
    motorR.dc(0)

def followWhiteLine(base_dc=30, kp=3, kd=50, stop_width=8, dt_ms=10):
    '''Едет по белой линии PD-регулятором, пока ширина линии не станет >= stop_width.'''
    last_error = getWhiteLineError()
    while getWhiteLineWidth() < stop_width:
        e = getWhiteLineError()
        u = e * kp + (e - last_error) * kd
        motorL.dc(base_dc - u)
        motorR.dc(base_dc + u)
        last_error = e
        wait(dt_ms)
    motorL.dc(0)
    motorR.dc(0)

followBlackLine(50, 3, 1, 8, 10)
followWhiteLine(50, 10, 1, 8, 10)