-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathmain.py
118 lines (95 loc) · 3.41 KB
/
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
#!/usr/bin/env pybricks-micropython
"""
This program requires LEGO® EV3 MicroPython v2.0 or higher.
"""
from pybricks.ev3devices import Motor, ColorSensor, InfraredSensor, UltrasonicSensor
from pybricks.nxtdevices import ColorSensor as ColorSensorNXT
from pybricks.parameters import Port
from pybricks.hubs import EV3Brick
from pybricks.parameters import Color
from pybricks.tools import wait
from pybricks.robotics import DriveBase
# Initialize ports.
motorL = Port.A
motorR = Port.D
colorSenEv3 = Port.S4
colorSenNXT = Port.S1
infraredSen = Port.S3
ultrasonicSen = Port.S2
wheel_diameter_ = 55.5
towerDistance = 30
towerSpeedDistance = 70
tableEnd = 150
turn_angle = 18
seventydegrees = 63
DRIVE_SPEED = 100
ev3 = EV3Brick()
left_motor = Motor(motorL)
right_motor = Motor(motorR)
line_sensor = ColorSensor(colorSenEv3)
tower_sensor = ColorSensorNXT(colorSenNXT)
infrared = InfraredSensor(infraredSen)
ultrasonic = UltrasonicSensor(ultrasonicSen)
robot = DriveBase(left_motor, right_motor, wheel_diameter=wheel_diameter_, axle_track=104)
# Calculate the light threshold. CALIBRATION NEEDED!
BLACK = 7
WHITE = 47
threshold = (BLACK + WHITE) / 2
PROPORTIONAL_GAIN = 4
# Start following the line endlessly.
while True:
# if state = 'Table end':
# if state = 'Obstacle':
# if state = 'tower':
# if state = 'other object':
# if state = 'last tower':
# if state = 'line following':
if infrared.distance() >= 20:
robot.drive(0, 0)
ev3.screen.print(ultrasonic.distance())
ev3.speaker.beep()
wait(50000)
if ultrasonic.distance() <= towerDistance:
ev3.screen.print(tower_sensor.color(), tower_sensor.color() == 'Color.BLUE' or tower_sensor.color() == 'Color.RED')
side = 1
if tower_sensor.color() == Color.BLUE:
side = 1
elif tower_sensor.color() == Color.RED:
side = -1
robot.straight(-70)
#make a slight turn right or left for 1.2 seconds
robot.turn(-100*side)
#looking for line
while line_sensor.reflection() > BLACK+2:
robot.drive(DRIVE_SPEED, turn_angle*side)
#going straight for 0.2 seonds
robot.straight(70)
while line_sensor.reflection() > BLACK+2:
robot.turn(-10*side)
deviation = line_sensor.reflection() - threshold
# Calculate the turn rate.
turn_rate = PROPORTIONAL_GAIN * deviation
# Set the drive base speed and turn rate.
robot.drive(DRIVE_SPEED, threshold)
elif ultrasonic.distance() <= towerSpeedDistance:
DRIVE_SPEED = 50
deviation = line_sensor.reflection() - threshold
# Calculate the turn rate.
turn_rate = PROPORTIONAL_GAIN * deviation
# Set the drive base speed and turn rate.
robot.drive(DRIVE_SPEED, turn_rate)
# You can wait for a short time or do other things in this loop.
wait(10)
else:
DRIVE_SPEED = 100
#Check if the robot drove through the gate
# if tower_sensor.color == Color.RED or tower_sensor.color == Color.BLUE:
# ev3.speaker.beep()
# Calculate the deviation from the threshold.
deviation = line_sensor.reflection() - threshold
# Calculate the turn rate.
turn_rate = PROPORTIONAL_GAIN * deviation
# Set the drive base speed and turn rate.
robot.drive(DRIVE_SPEED, turn_rate)
# You can wait for a short time or do other things in this loop.
wait(10)