This repository has been archived by the owner on Aug 2, 2018. It is now read-only.
/
drive_to_ball_7.py
105 lines (94 loc) · 3.76 KB
/
drive_to_ball_7.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
import SimpleCV,time,math
import robot_com
#Initialisation
robot=robot_com.robot()
ballcolor = (180,45,15)
display = SimpleCV.Display()
cam = SimpleCV.Camera(1,prop_set={"width":320, "height":240})
normaldisplay = True
#VARIABLES
last_turn_right=True
cropped=25 #pixels
no_blobs_frame_counter=0
while display.isNotDone():
#Display function
if display.mouseRight:
normaldisplay = not(normaldisplay)
print "Display Mode:", "Normal" if normaldisplay else "Segmented"
#Find blobs
img = cam.getImage().crop(0,cropped,320,200).dilate()
dist = img.colorDistance(ballcolor)
segmented = img-dist
segmented = segmented.binarize(30).invert().dilate()
blobs = segmented.findBlobs()
if blobs:
no_blobs_frame_counter=0
#Select the closest blob
idx=-1
y=0
for i in range(len(blobs)):
print blobs[i].circleDistance()
if blobs[i].maxY()>y:
idx=i
ball=blobs[idx]
img.drawCircle((ball.x, ball.y), ball.radius(),SimpleCV.Color.GREEN,ball.radius())
#Get it values (x,y,r,distance,angle)
x=ball.coordinates()[0]
y=ball.maxY()+cropped
r=ball.radius()
ball_distance=8.7*math.tan(math.radians(50+37.6*((240-y)/240.0)))
ball_at_angle=0
if x>=160:
ball_at_angle=26.5*((x-160)/160.0)
else:
ball_at_angle=26.5*((160-x)/160.0)
print "\nCoordinates:",x,y,"Distance to the ball:",ball_distance, "At angle ",ball_at_angle
#Determine speeds needed
turn_speed=20
move_speed=50
if ball_distance<150:
move_speed=int(ball_distance)
elif ball_distance<30:
move_speed=int(ball_distance*2.5)
if x>140 and x<180:
if ball_distance<12:
robot.go_forward(20)
time.sleep(1.5)
for i in range(5):
robot.turn_right(30)
time.sleep(1)
robot.stop()
robot.go_forward(20)
break
print "Ball in reached"
else:
robot.go_forward(move_speed)
print "Go forward"
elif x>180:
print "Turning RIGHT"
robot.go_forward(move_speed)
robot.go_pid(1,int(-move_speed*1.2))
last_turn_right=True
elif x<140:
print "Turning LEFT"
robot.go_forward(move_speed)
robot.go_pid(2,int(move_speed*1.2))
last_turn_right=False
else:
no_blobs_frame_counter+=1
if no_blobs_frame_counter>=5:
print "Turn around"
if last_turn_right:
robot.turn_right(50)
else:
robot.turn_left(50)
time.sleep(0.2)
robot.stop()
time.sleep(0.3)
else:
robot.stop()
if normaldisplay:
img.show()
else:
segmented.show()
robot.all_off()