ISTE/line_follower.py at master · la-firestone/ISTE · 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
from zumi.zumi import Zumi
from zumi.util.screen import Screen
import time

zumi = Zumi()
speed = 30
threashold = 70
prev_state = -1
HEAD = zumi.read_z_angle()

try:
    while True:
        ir_readings = zumi.get_all_IR_data()
        left = ir_readings[1]
        right = ir_readings[3]

        print(left, right)

        if left > threashold and right > threashold:
            print('1')
            zumi.control_motors(speed, speed, 0)
            prev_state = 0
        elif left < threashold and right > threashold:
            print('2')
            zumi.control_motors(-int(speed/3), int(speed/3), 0)
            prev_state = 1
        elif left > threashold and right < threashold:
            print('3')
            zumi.control_motors(int(speed/3), -int(speed/3), 0)
            prev_state = 2
        else:
            if prev_state ==1 :
                print('11')
                zumi.control_motors(int(speed/2), -int(speed/2), 0)
            else:
                print('22')
                zumi.control_motors(-int(speed/2), int(speed/2), 0)
            prev_state = 3