-
Notifications
You must be signed in to change notification settings - Fork 1
/
Copy pathtracking_test.py
171 lines (164 loc) · 5.8 KB
/
tracking_test.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
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
import sys
import os
sys.path.append('/usr/local/lib/python2.7/site-packages')
import cv2
import cv2.aruco as aruco
import pygame
import numpy as np
from pygame.locals import*
import picamera
import picamera.array
from drivetrain import DriveTrain
import time
env_vars = [
("SDL_FBDEV", "/dev/fb1"),
("SDL_MOUSEDEV", "/dev/input/touchscreen"),
("SDL_MOUSEDRV", "TSLIB"),
]
#for speed = 0.15:
#0.02, 0.02 barely any detectable response
#0.5, 0.02 some response, no damping
#0.5, 0.2 nearly enough damping
#0.5, 0.3 either over or under damped!
#0.5, 0.25 too much damping
#for speed=0.25
#0.4,0.2 still ok but less responsive
#0.5, 0.3 ok (and max response increased to 0.5 - too much
TURN_P = 0.6
TURN_D = 0.3
for var_name, val in env_vars:
os.environ[var_name] = val
screen_width = 320
screen_centre = screen_width / 2
screen_height = 240
camera = picamera.PiCamera()
camera.resolution = (screen_width, screen_height)
camera.framerate = 30
camera.iso = 800
camera.shutter_speed = 12000
pygame.init()
screen = pygame.display.set_mode([240, 320])
video = picamera.array.PiRGBArray(camera)
drive = DriveTrain(timeout=120)
#create small cust dictionary
small_dict = aruco.Dictionary_create(6, 3)
print("setup complete, looking")
last_t_error = 0
speed = 0
MIN_SPEED = 0
STRAIGHT_SPEED = 0.5 #0.15 fine, 0.25 fine
STEERING_OFFSET = 0.0 #more positive make it turn left
CROP_WIDTH = 320
i = 0
TIMEOUT = 30.0
START_TIME = time.clock()
END_TIME = START_TIME + TIMEOUT
found = False
turn_number = 0
TURN_TARGET = 10
TURN_WIDTH = 30 #60 was good with speed 0.15. with speed 0.25, hit targets. 40 ok at speed 0.3
NINTY_TURN = 0.8 #0.4 (0.1, 0.1) works 10/10. 0.45(0.1,0.1) works 50%. 0.8 (0.05,0.05) works 100% 1(0.05, 0.05) works 90%, as does 1(0.04,0.04)
MAX_SPEED = 0
SETTLE_TIME = 0.05
TURN_TIME = 0.04
MARKER1 = 3
MARKER2 = 5
target_id = MARKER1
MAX_TURN_SPEED = 0.25
loop_start_time=0
def turn_right():
drive.move(NINTY_TURN, 0)
time.sleep(TURN_TIME)
drive.move(0,0)
time.sleep(SETTLE_TIME)
def turn_left():
drive.move(-NINTY_TURN, 0)
time.sleep(TURN_TIME)
drive.move(0,0)
time.sleep(SETTLE_TIME)
try:
for frameBuf in camera.capture_continuous(video, format ="rgb", use_video_port=True):
if time.clock() > END_TIME or turn_number > TURN_TARGET:
raise KeyboardInterrupt
frame = (frameBuf.array)
video.truncate(0)
frame = frame[30:190, (screen_centre - CROP_WIDTH/2):(screen_centre + CROP_WIDTH/2)]
# Our operations on the frame come here
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
parameters = aruco.DetectorParameters_create()
#print(parameters)
''' detectMarkers(...)
detectMarkers(image, dictionary[, corners[, ids[, parameters[, rejectedI
mgPoints]]]]) -> corners, ids, rejectedImgPoints
'''
#lists of ids and the corners beloning to each id
corners, ids, rejectedImgPoints = aruco.detectMarkers(gray, small_dict, parameters=parameters)
if ids != None:
if ids[0][0] == target_id:
found = True
#if found, comptue the centre and move the cursor there
#print(corners)
found_x = sum([arr[0] for arr in corners[0][0]]) / 4
found_y = sum([arr[1] for arr in corners[0][0]]) / 4
width = abs(corners[0][0][0][0]-corners[0][0][1][0]+corners[0][0][3][0]-corners[0][0][2][0])/2
#print ('marker width %s' % width)
if width > TURN_WIDTH:
#marker approached, start looking for new target marker
target_id = MARKER1 + MARKER2 - target_id
turn_number += 1
print ('Close to marker making turn %s' % turn_number)
pygame.mouse.set_pos(int(found_y), int(found_x))
t_error = (CROP_WIDTH/2 - found_x) / (CROP_WIDTH / 2)
turn = STEERING_OFFSET + TURN_P * t_error
if last_t_error is not 0:
#if there was a real error last time then do some damping
turn -= TURN_D *(last_t_error - t_error)
turn = min(max(turn,-MAX_TURN_SPEED),MAX_TURN_SPEED)
print turn
#if we're rate limting the turn, go slow
if abs(turn) == MAX_TURN_SPEED:
drive.move (turn, STRAIGHT_SPEED / 3)
else:
drive.move (turn, STRAIGHT_SPEED)
last_t_error = t_error
#print(camera.exposure_speed)
#print time.clock()-loop_start_time
loop_start_time = time.clock()
else:
if target_id == MARKER1:
turn_right()
else:
turn_left()
found = False
last_t_error = 0
else:
#if marker was found, then probably best to stop and look
if found:
drive.move(0,0)
else:
#otherwise, go looking for the marker
if target_id == MARKER1:
turn_right()
else:
turn_left()
found = False
last_t_error = 0
# Display the resulting frame
frame = pygame.surfarray.make_surface(cv2.flip(frame,1))
screen.fill([0,0,0])
screen.blit(frame, (0,0))
pygame.display.update()
if found:
img_name = str(i) + "Fimg.jpg"
else:
img_name = str(i) + "NFimg.jpg"
#filesave for debugging:
#cv2.imwrite(img_name, gray)
i += 1
for event in pygame.event.get():
if event.type == KEYDOWN:
raise KeyboardInterrupt
except KeyboardInterrupt,SystemExit:
drive.move(0,0)
pygame.quit()
cv2.destroyAllWindows()