Skip to content

Commit 0461a46

Browse files
previprevi
committed
-
1 parent 540b92a commit 0461a46

File tree

10 files changed

+45
-37
lines changed

10 files changed

+45
-37
lines changed

‎camera.py

Lines changed: 9 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,7 @@
11
import time
22
import copy
33
import os
4+
import sys
45
import math
56
from PIL import Image as PILImage
67
from StringIO import StringIO
@@ -60,7 +61,8 @@ def run(self):
6061
try:
6162
self._camera.grab_start()
6263
while self._run:
63-
if time.time() - self._image_time > CAMERA_REFRESH_INTERVAL:
64+
sleep_time = CAMERA_REFRESH_INTERVAL - (time.time() - self._image_time)
65+
if sleep_time <= 0:
6466
ts = time.time()
6567
#print "run.1"
6668
self._image_lock.acquire()
@@ -71,7 +73,7 @@ def run(self):
7173
self.save_image(self._camera.get_image_jpeg())
7274
#print "run.3: " + str(time.time()-ts)
7375
else:
74-
time.sleep(CAMERA_REFRESH_INTERVAL - (time.time() - self._image_time))
76+
time.sleep(sleep_time)
7577

7678
if self.recording and time.time() - self.video_start_time > VIDEO_ELAPSE_MAX:
7779
self.video_stop()
@@ -274,38 +276,33 @@ def path_ahead(self):
274276
return coordY
275277

276278
def find_color(self, s_color):
277-
print s_color
278279
color = (int(s_color[1:3],16), int(s_color[3:5],16), int(s_color[5:7],16))
279280
code_data = None
280281
ts = time.time()
281282
self._image_lock.acquire()
282283
img = self.get_image(0)
283-
#print "signal.get_image: " + str(time.time() - ts)
284-
#warped = img.colorDistance(color).resize(160).warp(self._warp_corners_4).binarize(80)
285-
#print "oject.warp: " + str(time.time() - ts)
286-
#objects = warped.findBlobs(minsize=200, maxsize=4000)
284+
self._image_lock.release()
287285
bw = img.filter_color(color)
288-
objects = bw.find_blobs(minsize=50, maxsize=1000)
286+
#self.save_image(bw.to_jpeg())
287+
objects = bw.find_blobs(minsize=20, maxsize=1000)
289288
logging.debug("objects: " + str(objects))
290289
dist = -1
291290
angle = 180
292291

293292
if objects and len(objects):
294293
obj = objects[-1]
295294
bottom = obj.bottom
296-
logging.info("bottom: ", obj.center[0], obj.bottom)
295+
logging.info("bottom: " + str(obj.center[0]) + " " +str(obj.bottom))
297296
coords = bw.transform([(obj.center[0], obj.bottom)])
298-
logging.info("coordinates: " + coords)
297+
logging.info("coordinates: " + str(coords))
299298
x = coords[0][0]
300299
y = coords[0][1]
301-
#print "height: " + str(object.height())
302300
dist = math.sqrt(math.pow(12 + (68 * (120 - y) / 100),2) + (math.pow((x-80)*60/160,2)))
303301
angle = math.atan2(x - 80, 120 - y) * 180 / math.pi
304302
logging.info("object found, dist: " + str(dist) + " angle: " + str(angle))
305303
#img.drawText("object found, dist: " + str(dist) + " angle: " + str(angle), 0, 0, fontsize=32 )
306304
#self.save_image(self._camera.get_image_jpeg())
307305
#self.save_image(img.to_jpeg())
308-
self._image_lock.release()
309306
#print "object: " + str(time.time() - ts)
310307
return [dist, angle]
311308

‎coderbot.cfg

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1 +1 @@
1-
{"move_tr_speed": "75", "move_fw_elapse": "1.5", "show_page_program": "true", "load_at_start": "", "move_tr_elapse": "1", "sound_start": "$startup.mp3", "sound_stop": "$shutdown.mp3", "camera_exposure_mode": "auto", "prog_video_rec": "true", "show_control_move_commands": "true", "prog_level": "basic_move", "prog_scrollbars": "true", "ctrl_fw_speed": "100", "move_fw_speed": "100", "show_page_control": "true", "sound_shutter": "$shutter.mp3", "show_page_prefs": "true", "prog_maxblocks": "5", "ctrl_hud_image": "coderbot_hud_2.png", "button_func": "none", "move_motor_mode": "dc", "ctrl_fw_elapse": "-1", "ctrl_tr_elapse": "-1", "move_power_angle_2": "80", "move_power_angle_3": "80", "ctrl_tr_speed": "80", "move_power_angle_1": "40"}
1+
{"move_tr_speed": "75", "move_fw_elapse": "1.5", "show_page_program": "true", "load_at_start": "", "move_tr_elapse": "1", "sound_start": "$startup.mp3", "sound_stop": "$shutdown.mp3", "camera_exposure_mode": "auto", "prog_video_rec": "true", "show_control_move_commands": "true", "prog_level": "adv", "prog_scrollbars": "true", "ctrl_fw_speed": "100", "move_fw_speed": "100", "show_page_control": "true", "sound_shutter": "$shutter.mp3", "show_page_prefs": "true", "prog_maxblocks": "-1", "ctrl_hud_image": "", "button_func": "none", "move_motor_mode": "dc", "ctrl_fw_elapse": "-1", "ctrl_tr_elapse": "-1", "move_power_angle_2": "80", "move_power_angle_3": "80", "ctrl_tr_speed": "80", "move_power_angle_1": "70"}

‎data/program_egg_catcher.data

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1 @@
1+
{"dom_code": "<xml xmlns=\"http://www.w3.org/1999/xhtml\"><block type=\"controls_whileUntil\" id=\"5\" inline=\"false\" x=\"40\" y=\"19\"><field name=\"MODE\">WHILE</field><value name=\"BOOL\"><block type=\"logic_boolean\" id=\"6\"><field name=\"BOOL\">TRUE</field></block></value><statement name=\"DO\"><block type=\"variables_set\" id=\"7\" inline=\"true\"><field name=\"VAR\">egg_dist_angle</field><value name=\"VALUE\"><block type=\"coderbot_adv_findColor\" id=\"8\" inline=\"true\"><field name=\"RETVAL\">BOTH</field><value name=\"COLOR\"><block type=\"colour_picker\" id=\"9\"><field name=\"COLOUR\">#cc0000</field></block></value></block></value><next><block type=\"variables_set\" id=\"10\" inline=\"true\"><field name=\"VAR\">egg_dist</field><value name=\"VALUE\"><block type=\"math_round\" id=\"11\" inline=\"false\"><field name=\"OP\">ROUND</field><value name=\"NUM\"><block type=\"lists_getIndex\" id=\"12\" inline=\"true\"><mutation statement=\"false\" at=\"false\"></mutation><field name=\"MODE\">GET</field><field name=\"WHERE\">FIRST</field><value name=\"VALUE\"><block type=\"variables_get\" id=\"13\"><field name=\"VAR\">egg_dist_angle</field></block></value></block></value></block></value><next><block type=\"variables_set\" id=\"14\" inline=\"true\"><field name=\"VAR\">egg_angle</field><value name=\"VALUE\"><block type=\"math_round\" id=\"15\" inline=\"false\"><field name=\"OP\">ROUND</field><value name=\"NUM\"><block type=\"lists_getIndex\" id=\"16\" inline=\"true\"><mutation statement=\"false\" at=\"false\"></mutation><field name=\"MODE\">GET</field><field name=\"WHERE\">LAST</field><value name=\"VALUE\"><block type=\"variables_get\" id=\"17\"><field name=\"VAR\">egg_dist_angle</field></block></value></block></value></block></value><next><block type=\"text_print\" id=\"18\" inline=\"false\"><value name=\"TEXT\"><block type=\"text_join\" id=\"19\" inline=\"false\"><mutation items=\"4\"></mutation><value name=\"ADD0\"><block type=\"text\" id=\"20\"><field name=\"TEXT\">dist: </field></block></value><value name=\"ADD1\"><block type=\"variables_get\" id=\"21\"><field name=\"VAR\">egg_dist</field></block></value><value name=\"ADD2\"><block type=\"text\" id=\"22\"><field name=\"TEXT\"> angle: </field></block></value><value name=\"ADD3\"><block type=\"variables_get\" id=\"23\"><field name=\"VAR\">egg_angle</field></block></value></block></value><next><block type=\"controls_if\" id=\"24\" inline=\"false\"><mutation else=\"1\"></mutation><value name=\"IF0\"><block type=\"logic_compare\" id=\"25\" inline=\"true\"><field name=\"OP\">GTE</field><value name=\"A\"><block type=\"variables_get\" id=\"26\"><field name=\"VAR\">egg_dist</field></block></value><value name=\"B\"><block type=\"math_number\" id=\"27\"><field name=\"NUM\">0</field></block></value></block></value><statement name=\"DO0\"><block type=\"coderbot_motion_turn\" id=\"28\" inline=\"true\"><value name=\"ANGLE\"><block type=\"variables_get\" id=\"29\"><field name=\"VAR\">egg_angle</field></block></value><next><block type=\"coderbot_motion_move\" id=\"30\" inline=\"true\"><value name=\"DIST\"><block type=\"variables_get\" id=\"31\"><field name=\"VAR\">egg_dist</field></block></value></block></next></block></statement><next><block type=\"coderbot_sleep\" id=\"34\" inline=\"false\"><value name=\"ELAPSE\"><block type=\"math_number\" id=\"35\"><field name=\"NUM\">1.0</field></block></value></block></next></block></next></block></next></block></next></block></next></block></statement></block></xml>", "code": "import math\n\negg_dist_angle = None\negg_dist = None\negg_angle = None\n\n\nwhile True:\n get_prog_eng().check_end()\n egg_dist_angle = get_cam().find_color('#cc0000')\n egg_dist = round(egg_dist_angle[0])\n egg_angle = round(egg_dist_angle[-1])\n get_cam().set_text(''.join([str(temp_value) for temp_value in ['dist: ', egg_dist, ' angle: ', egg_angle]]))\n if egg_dist >= 0:\n get_motion().turn(angle=egg_angle)\n get_motion().move(dist=egg_dist)\n else:\n pass\n get_cam().sleep(1)\n", "name": "egg_catcher"}

‎data/program_egg_catcher_2.data

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1 @@
1+
{"dom_code": "<xml xmlns=\"http://www.w3.org/1999/xhtml\"><block type=\"controls_whileUntil\" id=\"5\" inline=\"false\" x=\"40\" y=\"19\"><field name=\"MODE\">WHILE</field><value name=\"BOOL\"><block type=\"logic_boolean\" id=\"6\"><field name=\"BOOL\">TRUE</field></block></value><statement name=\"DO\"><block type=\"variables_set\" id=\"7\" inline=\"true\"><field name=\"VAR\">egg_dist_angle</field><value name=\"VALUE\"><block type=\"coderbot_adv_findColor\" id=\"8\" inline=\"true\"><field name=\"RETVAL\">BOTH</field><value name=\"COLOR\"><block type=\"colour_picker\" id=\"9\"><field name=\"COLOUR\">#cc0000</field></block></value></block></value><next><block type=\"variables_set\" id=\"10\" inline=\"true\"><field name=\"VAR\">egg_dist</field><value name=\"VALUE\"><block type=\"math_round\" id=\"11\" inline=\"false\"><field name=\"OP\">ROUND</field><value name=\"NUM\"><block type=\"lists_getIndex\" id=\"12\" inline=\"true\"><mutation statement=\"false\" at=\"false\"></mutation><field name=\"MODE\">GET</field><field name=\"WHERE\">FIRST</field><value name=\"VALUE\"><block type=\"variables_get\" id=\"13\"><field name=\"VAR\">egg_dist_angle</field></block></value></block></value></block></value><next><block type=\"variables_set\" id=\"14\" inline=\"true\"><field name=\"VAR\">egg_angle</field><value name=\"VALUE\"><block type=\"math_round\" id=\"15\" inline=\"false\"><field name=\"OP\">ROUND</field><value name=\"NUM\"><block type=\"lists_getIndex\" id=\"16\" inline=\"true\"><mutation statement=\"false\" at=\"false\"></mutation><field name=\"MODE\">GET</field><field name=\"WHERE\">LAST</field><value name=\"VALUE\"><block type=\"variables_get\" id=\"17\"><field name=\"VAR\">egg_dist_angle</field></block></value></block></value></block></value><next><block type=\"text_print\" id=\"18\" inline=\"false\"><value name=\"TEXT\"><block type=\"text_join\" id=\"19\" inline=\"false\"><mutation items=\"4\"></mutation><value name=\"ADD0\"><block type=\"text\" id=\"20\"><field name=\"TEXT\">dist: </field></block></value><value name=\"ADD1\"><block type=\"variables_get\" id=\"21\"><field name=\"VAR\">egg_dist</field></block></value><value name=\"ADD2\"><block type=\"text\" id=\"22\"><field name=\"TEXT\"> angle: </field></block></value><value name=\"ADD3\"><block type=\"variables_get\" id=\"23\"><field name=\"VAR\">egg_angle</field></block></value></block></value><next><block type=\"controls_if\" id=\"24\" inline=\"false\"><mutation else=\"1\"></mutation><value name=\"IF0\"><block type=\"logic_compare\" id=\"25\" inline=\"true\"><field name=\"OP\">GTE</field><value name=\"A\"><block type=\"variables_get\" id=\"26\"><field name=\"VAR\">egg_dist</field></block></value><value name=\"B\"><block type=\"math_number\" id=\"27\"><field name=\"NUM\">0</field></block></value></block></value><statement name=\"DO0\"><block type=\"controls_if\" id=\"82\" inline=\"false\"><mutation elseif=\"3\" else=\"1\"></mutation><value name=\"IF0\"><block type=\"logic_compare\" id=\"117\" inline=\"true\"><field name=\"OP\">GTE</field><value name=\"A\"><block type=\"variables_get\" id=\"118\"><field name=\"VAR\">egg_angle</field></block></value><value name=\"B\"><block type=\"math_number\" id=\"119\"><field name=\"NUM\">15</field></block></value></block></value><statement name=\"DO0\"><block type=\"coderbot_adv_move\" id=\"120\" inline=\"true\"><field name=\"ACTION\">RIGHT</field><value name=\"SPEED\"><block type=\"math_number\" id=\"121\"><field name=\"NUM\">80</field></block></value><value name=\"ELAPSE\"><block type=\"math_number\" id=\"122\"><field name=\"NUM\">0.2</field></block></value></block></statement><value name=\"IF1\"><block type=\"logic_compare\" id=\"84\" inline=\"true\"><field name=\"OP\">GTE</field><value name=\"A\"><block type=\"variables_get\" id=\"85\"><field name=\"VAR\">egg_angle</field></block></value><value name=\"B\"><block type=\"math_number\" id=\"86\"><field name=\"NUM\">3</field></block></value></block></value><statement name=\"DO1\"><block type=\"coderbot_adv_move\" id=\"54\" inline=\"true\"><field name=\"ACTION\">RIGHT</field><value name=\"SPEED\"><block type=\"math_number\" id=\"55\"><field name=\"NUM\">80</field></block></value><value name=\"ELAPSE\"><block type=\"math_number\" id=\"56\"><field name=\"NUM\">0.05</field></block></value></block></statement><value name=\"IF2\"><block type=\"logic_compare\" id=\"123\" inline=\"true\"><field name=\"OP\">LTE</field><value name=\"A\"><block type=\"variables_get\" id=\"124\"><field name=\"VAR\">egg_angle</field></block></value><value name=\"B\"><block type=\"math_number\" id=\"125\"><field name=\"NUM\">-15</field></block></value></block></value><statement name=\"DO2\"><block type=\"coderbot_adv_move\" id=\"126\" inline=\"true\"><field name=\"ACTION\">LEFT</field><value name=\"SPEED\"><block type=\"math_number\" id=\"127\"><field name=\"NUM\">80</field></block></value><value name=\"ELAPSE\"><block type=\"math_number\" id=\"128\"><field name=\"NUM\">0.2</field></block></value></block></statement><value name=\"IF3\"><block type=\"logic_compare\" id=\"92\" inline=\"true\"><field name=\"OP\">LTE</field><value name=\"A\"><block type=\"variables_get\" id=\"93\"><field name=\"VAR\">egg_angle</field></block></value><value name=\"B\"><block type=\"math_number\" id=\"94\"><field name=\"NUM\">-3</field></block></value></block></value><statement name=\"DO3\"><block type=\"coderbot_adv_move\" id=\"95\" inline=\"true\"><field name=\"ACTION\">LEFT</field><value name=\"SPEED\"><block type=\"math_number\" id=\"96\"><field name=\"NUM\">80</field></block></value><value name=\"ELAPSE\"><block type=\"math_number\" id=\"97\"><field name=\"NUM\">0.05</field></block></value></block></statement><statement name=\"ELSE\"><block type=\"coderbot_motion_move\" id=\"187\" inline=\"true\"><value name=\"DIST\"><block type=\"variables_get\" id=\"189\"><field name=\"VAR\">egg_dist</field></block></value></block></statement></block></statement><statement name=\"ELSE\"><block type=\"coderbot_adv_move\" id=\"98\" inline=\"true\"><field name=\"ACTION\">RIGHT</field><value name=\"SPEED\"><block type=\"math_number\" id=\"99\"><field name=\"NUM\">80</field></block></value><value name=\"ELAPSE\"><block type=\"math_number\" id=\"100\"><field name=\"NUM\">0.4</field></block></value></block></statement><next><block type=\"coderbot_adv_move\" id=\"166\" inline=\"true\"><field name=\"ACTION\">RIGHT</field><value name=\"SPEED\"><block type=\"math_number\" id=\"167\"><field name=\"NUM\">0</field></block></value><value name=\"ELAPSE\"><block type=\"math_number\" id=\"168\"><field name=\"NUM\">0.5</field></block></value></block></next></block></next></block></next></block></next></block></next></block></statement></block></xml>", "code": "import math\n\negg_dist_angle = None\negg_dist = None\negg_angle = None\n\n\nwhile True:\n get_prog_eng().check_end()\n egg_dist_angle = get_cam().find_color('#cc0000')\n egg_dist = round(egg_dist_angle[0])\n egg_angle = round(egg_dist_angle[-1])\n get_cam().set_text(''.join([str(temp_value) for temp_value in ['dist: ', egg_dist, ' angle: ', egg_angle]]))\n if egg_dist >= 0:\n if egg_angle >= 15:\n get_bot().right(speed=80, elapse=0.2)\n elif egg_angle >= 3:\n get_bot().right(speed=80, elapse=0.05)\n elif egg_angle <= -15:\n get_bot().left(speed=80, elapse=0.2)\n elif egg_angle <= -3:\n get_bot().left(speed=80, elapse=0.05)\n else:\n get_motion().move(dist=egg_dist)\n else:\n get_bot().right(speed=80, elapse=0.4)\n get_bot().right(speed=0, elapse=0.5)\n", "name": "egg_catcher_2"}

‎data/program_test_motion_flow.data

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1 @@
1+
{"dom_code": "<xml xmlns=\"http://www.w3.org/1999/xhtml\"><block type=\"coderbot_motion_turn\" id=\"52\" inline=\"true\" x=\"229\" y=\"82\"><value name=\"ANGLE\"><block type=\"math_number\" id=\"53\"><field name=\"NUM\">12</field></block></value><next><block type=\"coderbot_motion_move\" id=\"32\" inline=\"true\"><value name=\"DIST\"><block type=\"math_number\" id=\"33\"><field name=\"NUM\">55</field></block></value></block></next></block></xml>", "code": "get_motion().turn(angle=12)\nget_motion().move(dist=55)\n", "name": "test_motion_flow"}

‎main.py

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -82,6 +82,7 @@ def handle_bot():
8282
motion.turn(angle=float(param2))
8383
elif cmd == "stop":
8484
bot.stop()
85+
motion.stop()
8586
elif cmd == "take_photo":
8687
cam.photo_take()
8788
bot.say(app.bot_config.get("sound_shutter"))
@@ -217,4 +218,4 @@ def run_server():
217218

218219
bot.set_callback(PIN_PUSHBUTTON, button_pushed, 100)
219220
bot.say(app.bot_config.get("sound_start"))
220-
app.run(host="0.0.0.0", port=8080, debug=True, use_reloader=False)
221+
app.run(host="0.0.0.0", port=8080, debug=True, use_reloader=False, threaded=True)

‎motion.py

Lines changed: 17 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -7,6 +7,7 @@
77

88
from coderbot import CoderBot
99
from camera import Camera
10+
from program import get_prog_eng
1011
from config import Config
1112

1213
lk_params = dict( winSize = (15, 15),
@@ -35,6 +36,7 @@ def __init__(self):
3536
self.ts = time()
3637
self.frame_gray = None
3738
self.prev_gray = None
39+
self.running = False
3840
self.delta_power = 0.0
3941
self.delta_dist = 0.0
4042
self.target_dist = 0.0
@@ -65,9 +67,12 @@ def turn(self, angle):
6567
self.target_angle = angle
6668
self.loop_turn()
6769

70+
def stop(self):
71+
self.running = False
72+
6873
def loop_move(self):
69-
done = False
70-
while not done:
74+
self.running = True
75+
while self.running:
7176
frame = self.cam.get_image()
7277
self.frame_gray = frame.grayscale()
7378

@@ -79,15 +84,15 @@ def loop_move(self):
7984

8085
if len(self.tracks) > 0:
8186
delta_angle, delta_dist = self.calc_motion()
82-
done = self.bot_move(self.target_dist, delta_dist, delta_angle)
87+
self.running = self.running and self.bot_move(self.target_dist, delta_dist, delta_angle)
8388

8489
self.frame_idx += 1
8590
self.prev_gray = self.frame_gray
86-
91+
self.bot.stop()
8792

8893
def loop_turn(self):
89-
done = False
90-
while not done:
94+
self.running = True
95+
while self.running:
9196
frame = self.cam.get_image()
9297
self.frame_gray = frame.grayscale()
9398

@@ -99,7 +104,7 @@ def loop_turn(self):
99104

100105
if len(self.tracks) > 0:
101106
delta_angle, delta_dist = self.calc_motion()
102-
done = self.bot_turn(self.target_angle, delta_angle)
107+
self.running = self.running and self.bot_turn(self.target_angle, delta_angle)
103108

104109
self.frame_idx += 1
105110
self.prev_gray = self.frame_gray
@@ -188,17 +193,17 @@ def calc_motion(self):
188193
return self.delta_angle, self.delta_dist
189194

190195
def bot_turn(self, target_angle, delta_angle):
191-
done = False
196+
run = True
192197
sign = (target_angle - delta_angle) / abs(target_angle - delta_angle)
193198
logging.info( "abs delta: " + str(abs(target_angle - delta_angle)) + " sign delta: " + str(sign) )
194199
for p_a in self.power_angles:
195-
if abs(target_angle - delta_angle) > p_a[0]:
200+
if abs(target_angle - delta_angle) > p_a[0] and self.running:
196201
#print "pow: ", p_a[1][0], " duration: ", p_a[1][1]
197202
self.bot.motor_control(sign * p_a[1][0], -1 * sign * p_a[1][0], p_a[1][1])
198-
done = p_a[1][0] == 0 #stopped
203+
run = p_a[1][0] > 0 #stopped
199204
break
200205

201-
return done
206+
return run
202207

203208
def bot_move(self, target_dist, delta_dist, delta_angle):
204209
base_power = 100 * (target_dist/abs(target_dist))
@@ -209,5 +214,5 @@ def bot_move(self, target_dist, delta_dist, delta_angle):
209214
self.bot.motor_control(min(max(base_power-self.delta_power,-100),100), min(max(base_power+self.delta_power,-100),100), -1)
210215
else:
211216
self.bot.stop()
212-
return abs(delta_dist) >= abs(target_dist)
217+
return abs(delta_dist) < abs(target_dist)
213218

0 commit comments

Comments
 (0)