-
Notifications
You must be signed in to change notification settings - Fork 0
/
assignment.py
136 lines (117 loc) · 3.62 KB
/
assignment.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
from __future__ import print_function
import time
from sr.robot import *
a_th = 2.0
""" float: Threshold for the control of the linear distance"""
d_th = 0.4
""" float: Threshold for the control of the orientation"""
g_th = 2.0
""" float: Threshold for the control of direction"""
s_th = 0.9
""" float: Threshold for linear distance between robot and gold"""
R = Robot()
""" instance of the class Robot"""
def drive(speed, seconds):
"""
Function for setting a linear velocity
Args: speed (int): the speed of the wheels
seconds (int): the time interval
"""
R.motors[0].m0.power = speed
R.motors[0].m1.power = speed
time.sleep(seconds)
R.motors[0].m0.power = 0
R.motors[0].m1.power = 0
def turn(speed, seconds):
"""
Function for setting an angular velocity
Args: speed (int): the speed of the wheels
seconds (int): the time interval
"""
R.motors[0].m0.power = speed
R.motors[0].m1.power = -speed
time.sleep(seconds)
R.motors[0].m0.power = 0
R.motors[0].m1.power = 0
def find_silver_token(angle,length):
"""
Function to find the closest silver token
Returns:
dist (float): distance of the closest silver token (-1 if no silver token is detected)
rot_y (float): angle between the robot and the silver token (-1 if no silver token is detected)
"""
#angle signifies the angle between the silver token and the robot
#length signifies the length of the field visible ahead
dist=100
for token in R.see():
if token.dist < dist and token.info.marker_type is MARKER_TOKEN_SILVER and angle+length>=token.rot_y>=angle-length:
dist=token.dist
rot_y=token.rot_y
if dist==100:
return -1,-1
else:
return dist,rot_y
def find_golden_token(angle,length):
"""
Function to find the closest golden token
Returns:
dist (float): distance of the closest golden token (-1 if no golden token is detected)
rot_y (float): angle between the robot and the golden token (-1 if no golden token is detected)
"""
#angle signifies the angle between the silver token and the robot
#length signifies the length of the field visible ahead
dist=100
for token in R.see():
if token.dist < dist and token.info.marker_type is MARKER_TOKEN_GOLD and angle+length>=token.rot_y>=angle-length:
dist=token.dist
rot_y=token.rot_y
if dist==100:
return -1, -1
else:
return dist, rot_y
def left_winger():
dist_l,rot_yl = find_golden_token(-90,30)
dist_r,rot_yr = find_golden_token(90,30)
##it will check whether the distance to the left is more or right
print("left distance=",dist_l)
print("right distance=",dist_r)
if dist_l>dist_r:
return True
else:
return False
def avoid_gold_wall():
dist,rot_y=find_golden_token(0,70)
if dist==-1:
return
elif dist<s_th:
left=left_winger()
if left:
while(rot_y<g_th):
turn(-20,0.1)
dist,rot_y=find_golden_token(0,5)
else :
while(rot_y<g_th):
turn(+20,0.1)
dist,rot_y=find_golden_token(0,5)
def pick_silver():
dist,rot_y=find_silver_token(0,50)
dist_g,rot_yg=find_golden_token(rot_y,50)
if dist==-1:
return
elif dist<d_th:
R.grab()
turn(-20,2.5)
R.release()
turn(20,2.5)
return
elif -a_th<=rot_y<=a_th:
drive(50,0.1)
elif -a_th>rot_y:
turn(-1,0.1)
elif a_th<rot_y:
turn(1,0.1)
return
while 1:
drive(35,0.1)
pick_silver()
avoid_gold_wall()