Assignment code for Georgia Tech CS 3630, 2014
Update code
+951
+69
Assignment 6/working code 4-16 pre/logDataServo.py
+69
Assignment 6/working code 4-16 pre/logDataServo.py
···
1
+
import time
2
+
import os.path
3
+
import System
4
+
from Myro import *
5
+
6
+
# Run commands to move the robot
7
+
def runCommands(log, commands):
8
+
for c in commands:
9
+
start = time.time()
10
+
motors(c[0],c[1])
11
+
while (time.time() - start < c[2]):
12
+
logNow(log, c[0],c[1],0);
13
+
time.sleep(0.1) # Read sensors at 1Hz
14
+
15
+
def takePhoto():
16
+
pic_fname = "pic-%d.jpg" % time.time()
17
+
print('\tTaking picture:', pic_fname)
18
+
picture = takePicture();
19
+
print('\tPicture taken!')
20
+
logNow(log, 0,0,pic_fname);
21
+
print('\tSaving picture...')
22
+
savePicture(picture, pic_fname)
23
+
print('\tPicture saved!')
24
+
25
+
# Run commands followed by taking picture
26
+
def runCommand(log, cmd):
27
+
28
+
# Move the robot
29
+
commands = [];
30
+
commands.append(cmd);
31
+
commands.append([0,0,0.1]);
32
+
runCommands(log,commands);
33
+
34
+
# Log motor commands
35
+
def logNow(log, l, r, fname):
36
+
log.write("%s %s %s\n" %(l, r, fname))
37
+
38
+
# write to output log
39
+
fname = "motion_log.txt"
40
+
if os.path.exists(fname):
41
+
log = open(fname, 'a')
42
+
else:
43
+
log = open(fname, 'w')
44
+
45
+
# read from command log
46
+
cmdFile = open('motion_plan.txt', 'r')
47
+
commands = cmdFile.readlines()
48
+
cmdFile.close()
49
+
50
+
# setup robot
51
+
print('Connecting to Scribbler...')
52
+
init("COM8")
53
+
setIRPower(140)
54
+
setForwardness(1)
55
+
56
+
print('Starting motion plan...')
57
+
for command in commands:
58
+
cmd = command.split()
59
+
roboCmd = [float(cmd[0]), float(cmd[1]), float(cmd[2])]
60
+
print('Running command:', roboCmd)
61
+
runCommand(log, roboCmd)
62
+
print('Motion plan complete.')
63
+
takePhoto()
64
+
65
+
log.close();
66
+
67
+
print('Script finished!')
68
+
69
+
System.Environment.Exit(0)
+770
Assignment 6/working code 4-16 pre/motion_log.txt
+770
Assignment 6/working code 4-16 pre/motion_log.txt
···
1
+
1.0 1.0 0
2
+
1.0 1.0 0
3
+
1.0 1.0 0
4
+
1.0 1.0 0
5
+
1.0 1.0 0
6
+
1.0 1.0 0
7
+
1.0 1.0 0
8
+
1.0 1.0 0
9
+
1.0 1.0 0
10
+
1.0 1.0 0
11
+
0 0 0
12
+
0.0 0.0 0
13
+
0.0 0.0 0
14
+
0.0 0.0 0
15
+
0.0 0.0 0
16
+
0.0 0.0 0
17
+
0 0 0
18
+
1.0 0.0 0
19
+
1.0 0.0 0
20
+
1.0 0.0 0
21
+
1.0 0.0 0
22
+
1.0 0.0 0
23
+
1.0 0.0 0
24
+
1.0 0.0 0
25
+
1.0 0.0 0
26
+
0 0 0
27
+
0.0 0.0 0
28
+
0.0 0.0 0
29
+
0.0 0.0 0
30
+
0.0 0.0 0
31
+
0.0 0.0 0
32
+
0 0 0
33
+
0.0 1.0 0
34
+
0.0 1.0 0
35
+
0.0 1.0 0
36
+
0.0 1.0 0
37
+
0.0 1.0 0
38
+
0.0 1.0 0
39
+
0.0 1.0 0
40
+
0.0 1.0 0
41
+
0 0 0
42
+
0.0 0.0 0
43
+
0.0 0.0 0
44
+
0.0 0.0 0
45
+
0.0 0.0 0
46
+
0.0 0.0 0
47
+
0 0 0
48
+
0 0 0
49
+
1.0 1.0 0
50
+
1.0 1.0 0
51
+
1.0 1.0 0
52
+
1.0 1.0 0
53
+
1.0 1.0 0
54
+
1.0 1.0 0
55
+
1.0 1.0 0
56
+
1.0 1.0 0
57
+
1.0 1.0 0
58
+
1.0 1.0 0
59
+
0 0 0
60
+
0.0 0.0 0
61
+
0.0 0.0 0
62
+
0.0 0.0 0
63
+
0.0 0.0 0
64
+
0.0 0.0 0
65
+
0.0 0.0 0
66
+
0.0 0.0 0
67
+
0.0 0.0 0
68
+
0.0 0.0 0
69
+
0.0 0.0 0
70
+
0 0 0
71
+
0 0 0
72
+
1.0 1.0 0
73
+
1.0 1.0 0
74
+
1.0 1.0 0
75
+
1.0 1.0 0
76
+
1.0 1.0 0
77
+
1.0 1.0 0
78
+
1.0 1.0 0
79
+
1.0 1.0 0
80
+
1.0 1.0 0
81
+
1.0 1.0 0
82
+
0 0 0
83
+
0.0 0.0 0
84
+
0.0 0.0 0
85
+
0.0 0.0 0
86
+
0.0 0.0 0
87
+
0.0 0.0 0
88
+
0.0 0.0 0
89
+
0.0 0.0 0
90
+
0.0 0.0 0
91
+
0.0 0.0 0
92
+
0.0 0.0 0
93
+
0 0 0
94
+
0 0 0
95
+
1.0 1.0 0
96
+
1.0 1.0 0
97
+
1.0 1.0 0
98
+
1.0 1.0 0
99
+
1.0 1.0 0
100
+
1.0 1.0 0
101
+
1.0 1.0 0
102
+
1.0 1.0 0
103
+
1.0 1.0 0
104
+
1.0 1.0 0
105
+
0 0 0
106
+
0.0 0.0 0
107
+
0.0 0.0 0
108
+
0.0 0.0 0
109
+
0.0 0.0 0
110
+
0.0 0.0 0
111
+
0.0 0.0 0
112
+
0.0 0.0 0
113
+
0.0 0.0 0
114
+
0.0 0.0 0
115
+
0.0 0.0 0
116
+
0 0 0
117
+
0 0 pic-1428992065.jpg
118
+
0 0 0
119
+
1.0 1.0 0
120
+
1.0 1.0 0
121
+
1.0 1.0 0
122
+
1.0 1.0 0
123
+
1.0 1.0 0
124
+
1.0 1.0 0
125
+
1.0 1.0 0
126
+
1.0 1.0 0
127
+
1.0 1.0 0
128
+
1.0 1.0 0
129
+
0 0 0
130
+
0.0 0.0 0
131
+
0.0 0.0 0
132
+
0.0 0.0 0
133
+
0.0 0.0 0
134
+
0.0 0.0 0
135
+
0.0 0.0 0
136
+
0.0 0.0 0
137
+
0.0 0.0 0
138
+
0.0 0.0 0
139
+
0.0 0.0 0
140
+
0 0 0
141
+
0 0 pic-1428992151.jpg
142
+
0 0 0
143
+
1.0 1.0 0
144
+
1.0 1.0 0
145
+
1.0 1.0 0
146
+
1.0 1.0 0
147
+
1.0 1.0 0
148
+
1.0 1.0 0
149
+
1.0 1.0 0
150
+
1.0 1.0 0
151
+
1.0 1.0 0
152
+
1.0 1.0 0
153
+
0 0 0
154
+
0.0 0.0 0
155
+
0.0 0.0 0
156
+
0.0 0.0 0
157
+
0.0 0.0 0
158
+
0.0 0.0 0
159
+
0.0 0.0 0
160
+
0.0 0.0 0
161
+
0.0 0.0 0
162
+
0.0 0.0 0
163
+
0.0 0.0 0
164
+
0 0 0
165
+
0 0 pic-1428992230.jpg
166
+
0 0 0
167
+
1.0 1.0 0
168
+
1.0 1.0 0
169
+
1.0 1.0 0
170
+
1.0 1.0 0
171
+
1.0 1.0 0
172
+
1.0 1.0 0
173
+
1.0 1.0 0
174
+
1.0 1.0 0
175
+
1.0 1.0 0
176
+
1.0 1.0 0
177
+
0 0 0
178
+
0.0 0.0 0
179
+
0.0 0.0 0
180
+
0.0 0.0 0
181
+
0.0 0.0 0
182
+
0.0 0.0 0
183
+
0.0 0.0 0
184
+
0.0 0.0 0
185
+
0.0 0.0 0
186
+
0.0 0.0 0
187
+
0.0 0.0 0
188
+
0 0 0
189
+
0 0 pic-1428992452.jpg
190
+
0 0 0
191
+
1.0 1.0 0
192
+
1.0 1.0 0
193
+
1.0 1.0 0
194
+
1.0 1.0 0
195
+
1.0 1.0 0
196
+
1.0 1.0 0
197
+
1.0 1.0 0
198
+
1.0 1.0 0
199
+
1.0 1.0 0
200
+
1.0 1.0 0
201
+
0 0 0
202
+
0.0 0.0 0
203
+
0.0 0.0 0
204
+
0.0 0.0 0
205
+
0.0 0.0 0
206
+
0.0 0.0 0
207
+
0.0 0.0 0
208
+
0.0 0.0 0
209
+
0.0 0.0 0
210
+
0.0 0.0 0
211
+
0.0 0.0 0
212
+
0 0 0
213
+
0 0 0
214
+
1.0 1.0 0
215
+
1.0 1.0 0
216
+
1.0 1.0 0
217
+
1.0 1.0 0
218
+
1.0 1.0 0
219
+
1.0 1.0 0
220
+
1.0 1.0 0
221
+
1.0 1.0 0
222
+
1.0 1.0 0
223
+
1.0 1.0 0
224
+
0 0 0
225
+
0.0 0.0 0
226
+
0.0 0.0 0
227
+
0.0 0.0 0
228
+
0.0 0.0 0
229
+
0.0 0.0 0
230
+
0.0 0.0 0
231
+
0.0 0.0 0
232
+
0.0 0.0 0
233
+
0.0 0.0 0
234
+
0.0 0.0 0
235
+
0 0 0
236
+
0 0 0
237
+
1.0 1.0 0
238
+
1.0 1.0 0
239
+
1.0 1.0 0
240
+
1.0 1.0 0
241
+
1.0 1.0 0
242
+
1.0 1.0 0
243
+
1.0 1.0 0
244
+
1.0 1.0 0
245
+
1.0 1.0 0
246
+
1.0 1.0 0
247
+
0 0 0
248
+
0.0 0.0 0
249
+
0.0 0.0 0
250
+
0.0 0.0 0
251
+
0.0 0.0 0
252
+
0.0 0.0 0
253
+
0.0 0.0 0
254
+
0.0 0.0 0
255
+
0.0 0.0 0
256
+
0.0 0.0 0
257
+
0.0 0.0 0
258
+
0 0 0
259
+
0 0 0
260
+
1.0 1.0 0
261
+
1.0 1.0 0
262
+
1.0 1.0 0
263
+
1.0 1.0 0
264
+
1.0 1.0 0
265
+
1.0 1.0 0
266
+
1.0 1.0 0
267
+
1.0 1.0 0
268
+
1.0 1.0 0
269
+
1.0 1.0 0
270
+
0 0 0
271
+
0.0 0.0 0
272
+
0.0 0.0 0
273
+
0.0 0.0 0
274
+
0.0 0.0 0
275
+
0.0 0.0 0
276
+
0.0 0.0 0
277
+
0.0 0.0 0
278
+
0.0 0.0 0
279
+
0.0 0.0 0
280
+
0.0 0.0 0
281
+
0 0 0
282
+
0 0 0
283
+
1.0 1.0 0
284
+
1.0 1.0 0
285
+
1.0 1.0 0
286
+
1.0 1.0 0
287
+
1.0 1.0 0
288
+
1.0 1.0 0
289
+
1.0 1.0 0
290
+
1.0 1.0 0
291
+
1.0 1.0 0
292
+
1.0 1.0 0
293
+
0 0 0
294
+
0.0 0.0 0
295
+
0.0 0.0 0
296
+
0.0 0.0 0
297
+
0.0 0.0 0
298
+
0.0 0.0 0
299
+
0.0 0.0 0
300
+
0.0 0.0 0
301
+
0.0 0.0 0
302
+
0.0 0.0 0
303
+
0.0 0.0 0
304
+
0 0 0
305
+
0 0 0
306
+
1.0 1.0 0
307
+
1.0 1.0 0
308
+
1.0 1.0 0
309
+
1.0 1.0 0
310
+
1.0 1.0 0
311
+
1.0 1.0 0
312
+
1.0 1.0 0
313
+
1.0 1.0 0
314
+
1.0 1.0 0
315
+
1.0 1.0 0
316
+
0 0 0
317
+
0.0 0.0 0
318
+
0.0 0.0 0
319
+
0.0 0.0 0
320
+
0.0 0.0 0
321
+
0.0 0.0 0
322
+
0.0 0.0 0
323
+
0.0 0.0 0
324
+
0.0 0.0 0
325
+
0.0 0.0 0
326
+
0.0 0.0 0
327
+
0 0 0
328
+
0 0 0
329
+
1.0 1.0 0
330
+
1.0 1.0 0
331
+
1.0 1.0 0
332
+
1.0 1.0 0
333
+
1.0 1.0 0
334
+
1.0 1.0 0
335
+
1.0 1.0 0
336
+
1.0 1.0 0
337
+
1.0 1.0 0
338
+
0 0 0
339
+
0.0 0.0 0
340
+
0.0 0.0 0
341
+
0.0 0.0 0
342
+
0.0 0.0 0
343
+
0.0 0.0 0
344
+
0.0 0.0 0
345
+
0.0 0.0 0
346
+
0.0 0.0 0
347
+
0.0 0.0 0
348
+
0 0 0
349
+
0 0 0
350
+
1.0 1.0 0
351
+
1.0 1.0 0
352
+
1.0 1.0 0
353
+
1.0 1.0 0
354
+
1.0 1.0 0
355
+
1.0 1.0 0
356
+
1.0 1.0 0
357
+
1.0 1.0 0
358
+
1.0 1.0 0
359
+
0 0 0
360
+
0.0 0.0 0
361
+
0.0 0.0 0
362
+
0.0 0.0 0
363
+
0.0 0.0 0
364
+
0.0 0.0 0
365
+
0.0 0.0 0
366
+
0.0 0.0 0
367
+
0.0 0.0 0
368
+
0.0 0.0 0
369
+
0 0 0
370
+
0 0 pic-1428993303.jpg
371
+
0 0 0
372
+
1.0 1.0 0
373
+
1.0 1.0 0
374
+
1.0 1.0 0
375
+
1.0 1.0 0
376
+
1.0 1.0 0
377
+
1.0 1.0 0
378
+
1.0 1.0 0
379
+
1.0 1.0 0
380
+
1.0 1.0 0
381
+
0 0 0
382
+
0.0 0.0 0
383
+
0.0 0.0 0
384
+
0.0 0.0 0
385
+
0.0 0.0 0
386
+
0.0 0.0 0
387
+
0.0 0.0 0
388
+
0.0 0.0 0
389
+
0.0 0.0 0
390
+
0.0 0.0 0
391
+
0 0 0
392
+
0 0 pic-1428993341.jpg
393
+
0 0 0
394
+
1.0 1.0 0
395
+
1.0 1.0 0
396
+
1.0 1.0 0
397
+
1.0 1.0 0
398
+
1.0 1.0 0
399
+
1.0 1.0 0
400
+
1.0 1.0 0
401
+
1.0 1.0 0
402
+
1.0 1.0 0
403
+
0 0 0
404
+
0.0 0.0 0
405
+
0.0 0.0 0
406
+
0.0 0.0 0
407
+
0.0 0.0 0
408
+
0.0 0.0 0
409
+
0.0 0.0 0
410
+
0.0 0.0 0
411
+
0.0 0.0 0
412
+
0.0 0.0 0
413
+
0 0 0
414
+
0 0 pic-1428993434.jpg
415
+
0 0 0
416
+
1.0 1.0 0
417
+
1.0 1.0 0
418
+
1.0 1.0 0
419
+
1.0 1.0 0
420
+
1.0 1.0 0
421
+
1.0 1.0 0
422
+
1.0 1.0 0
423
+
1.0 1.0 0
424
+
1.0 1.0 0
425
+
0 0 0
426
+
0.0 0.0 0
427
+
0.0 0.0 0
428
+
0.0 0.0 0
429
+
0.0 0.0 0
430
+
0.0 0.0 0
431
+
0.0 0.0 0
432
+
0.0 0.0 0
433
+
0.0 0.0 0
434
+
0.0 0.0 0
435
+
0 0 0
436
+
0 0 pic-1428993545.jpg
437
+
0 0 0
438
+
1.0 1.0 0
439
+
1.0 1.0 0
440
+
1.0 1.0 0
441
+
1.0 1.0 0
442
+
1.0 1.0 0
443
+
1.0 1.0 0
444
+
1.0 1.0 0
445
+
1.0 1.0 0
446
+
1.0 1.0 0
447
+
0 0 0
448
+
0.0 0.0 0
449
+
0.0 0.0 0
450
+
0.0 0.0 0
451
+
0.0 0.0 0
452
+
0.0 0.0 0
453
+
0.0 0.0 0
454
+
0.0 0.0 0
455
+
0.0 0.0 0
456
+
0.0 0.0 0
457
+
0 0 0
458
+
0 0 pic-1428993605.jpg
459
+
0 0 0
460
+
1.0 1.0 0
461
+
1.0 1.0 0
462
+
1.0 1.0 0
463
+
1.0 1.0 0
464
+
1.0 1.0 0
465
+
1.0 1.0 0
466
+
1.0 1.0 0
467
+
1.0 1.0 0
468
+
1.0 1.0 0
469
+
1.0 1.0 0
470
+
0 0 0
471
+
0.0 0.0 0
472
+
0.0 0.0 0
473
+
0.0 0.0 0
474
+
0.0 0.0 0
475
+
0.0 0.0 0
476
+
0.0 0.0 0
477
+
0.0 0.0 0
478
+
0.0 0.0 0
479
+
0.0 0.0 0
480
+
0.0 0.0 0
481
+
0 0 0
482
+
0 0 pic-1429146711.jpg
483
+
0 0 0
484
+
1.0 1.0 0
485
+
1.0 1.0 0
486
+
1.0 1.0 0
487
+
1.0 1.0 0
488
+
1.0 1.0 0
489
+
1.0 1.0 0
490
+
1.0 1.0 0
491
+
1.0 1.0 0
492
+
1.0 1.0 0
493
+
1.0 1.0 0
494
+
0 0 0
495
+
0.0 0.0 0
496
+
0.0 0.0 0
497
+
0.0 0.0 0
498
+
0.0 0.0 0
499
+
0.0 0.0 0
500
+
0.0 0.0 0
501
+
0.0 0.0 0
502
+
0.0 0.0 0
503
+
0.0 0.0 0
504
+
0.0 0.0 0
505
+
0 0 0
506
+
0 0 pic-1429146736.jpg
507
+
0 0 0
508
+
1.0 1.0 0
509
+
1.0 1.0 0
510
+
1.0 1.0 0
511
+
1.0 1.0 0
512
+
1.0 1.0 0
513
+
1.0 1.0 0
514
+
1.0 1.0 0
515
+
1.0 1.0 0
516
+
1.0 1.0 0
517
+
1.0 1.0 0
518
+
0 0 0
519
+
0.0 0.0 0
520
+
0.0 0.0 0
521
+
0.0 0.0 0
522
+
0.0 0.0 0
523
+
0.0 0.0 0
524
+
0.0 0.0 0
525
+
0.0 0.0 0
526
+
0.0 0.0 0
527
+
0.0 0.0 0
528
+
0.0 0.0 0
529
+
0 0 0
530
+
0 0 pic-1429146896.jpg
531
+
0 0 0
532
+
1.0 1.0 0
533
+
1.0 1.0 0
534
+
1.0 1.0 0
535
+
1.0 1.0 0
536
+
1.0 1.0 0
537
+
1.0 1.0 0
538
+
1.0 1.0 0
539
+
1.0 1.0 0
540
+
1.0 1.0 0
541
+
1.0 1.0 0
542
+
0 0 0
543
+
0.0 0.0 0
544
+
0.0 0.0 0
545
+
0.0 0.0 0
546
+
0.0 0.0 0
547
+
0.0 0.0 0
548
+
0.0 0.0 0
549
+
0.0 0.0 0
550
+
0.0 0.0 0
551
+
0.0 0.0 0
552
+
0.0 0.0 0
553
+
0 0 0
554
+
0 0 pic-1429146945.jpg
555
+
0 0 0
556
+
1.0 1.0 0
557
+
1.0 1.0 0
558
+
1.0 1.0 0
559
+
1.0 1.0 0
560
+
1.0 1.0 0
561
+
1.0 1.0 0
562
+
1.0 1.0 0
563
+
1.0 1.0 0
564
+
1.0 1.0 0
565
+
1.0 1.0 0
566
+
0 0 0
567
+
0.0 0.0 0
568
+
0.0 0.0 0
569
+
0.0 0.0 0
570
+
0.0 0.0 0
571
+
0.0 0.0 0
572
+
0.0 0.0 0
573
+
0.0 0.0 0
574
+
0.0 0.0 0
575
+
0.0 0.0 0
576
+
0.0 0.0 0
577
+
0 0 0
578
+
0 0 pic-1429146984.jpg
579
+
0 0 0
580
+
1.0 1.0 0
581
+
1.0 1.0 0
582
+
1.0 1.0 0
583
+
1.0 1.0 0
584
+
1.0 1.0 0
585
+
1.0 1.0 0
586
+
1.0 1.0 0
587
+
1.0 1.0 0
588
+
1.0 1.0 0
589
+
1.0 1.0 0
590
+
0 0 0
591
+
0.0 0.0 0
592
+
0.0 0.0 0
593
+
0.0 0.0 0
594
+
0.0 0.0 0
595
+
0.0 0.0 0
596
+
0.0 0.0 0
597
+
0.0 0.0 0
598
+
0.0 0.0 0
599
+
0.0 0.0 0
600
+
0.0 0.0 0
601
+
0 0 0
602
+
0 0 pic-1429147001.jpg
603
+
0 0 0
604
+
1.0 1.0 0
605
+
1.0 1.0 0
606
+
1.0 1.0 0
607
+
1.0 1.0 0
608
+
1.0 1.0 0
609
+
1.0 1.0 0
610
+
1.0 1.0 0
611
+
1.0 1.0 0
612
+
1.0 1.0 0
613
+
1.0 1.0 0
614
+
0 0 0
615
+
0.0 0.0 0
616
+
0.0 0.0 0
617
+
0.0 0.0 0
618
+
0.0 0.0 0
619
+
0.0 0.0 0
620
+
0.0 0.0 0
621
+
0.0 0.0 0
622
+
0.0 0.0 0
623
+
0.0 0.0 0
624
+
0.0 0.0 0
625
+
0 0 0
626
+
0 0 pic-1429147033.jpg
627
+
0 0 0
628
+
1.0 1.0 0
629
+
1.0 1.0 0
630
+
1.0 1.0 0
631
+
1.0 1.0 0
632
+
1.0 1.0 0
633
+
1.0 1.0 0
634
+
1.0 1.0 0
635
+
1.0 1.0 0
636
+
1.0 1.0 0
637
+
1.0 1.0 0
638
+
0 0 0
639
+
0.0 0.0 0
640
+
0.0 0.0 0
641
+
0.0 0.0 0
642
+
0.0 0.0 0
643
+
0.0 0.0 0
644
+
0.0 0.0 0
645
+
0.0 0.0 0
646
+
0.0 0.0 0
647
+
0.0 0.0 0
648
+
0.0 0.0 0
649
+
0 0 0
650
+
0 0 pic-1429147051.jpg
651
+
0 0 0
652
+
1.0 1.0 0
653
+
1.0 1.0 0
654
+
1.0 1.0 0
655
+
1.0 1.0 0
656
+
1.0 1.0 0
657
+
1.0 1.0 0
658
+
1.0 1.0 0
659
+
1.0 1.0 0
660
+
1.0 1.0 0
661
+
1.0 1.0 0
662
+
0 0 0
663
+
0.0 0.0 0
664
+
0.0 0.0 0
665
+
0.0 0.0 0
666
+
0.0 0.0 0
667
+
0.0 0.0 0
668
+
0.0 0.0 0
669
+
0.0 0.0 0
670
+
0.0 0.0 0
671
+
0.0 0.0 0
672
+
0.0 0.0 0
673
+
0 0 0
674
+
0 0 pic-1429156024.jpg
675
+
0 0 0
676
+
1.0 1.0 0
677
+
1.0 1.0 0
678
+
1.0 1.0 0
679
+
1.0 1.0 0
680
+
1.0 1.0 0
681
+
1.0 1.0 0
682
+
1.0 1.0 0
683
+
1.0 1.0 0
684
+
1.0 1.0 0
685
+
1.0 1.0 0
686
+
0 0 0
687
+
0.0 0.0 0
688
+
0.0 0.0 0
689
+
0.0 0.0 0
690
+
0.0 0.0 0
691
+
0.0 0.0 0
692
+
0.0 0.0 0
693
+
0.0 0.0 0
694
+
0.0 0.0 0
695
+
0.0 0.0 0
696
+
0.0 0.0 0
697
+
0 0 0
698
+
0 0 pic-1429156108.jpg
699
+
0 0 0
700
+
1.0 1.0 0
701
+
1.0 1.0 0
702
+
1.0 1.0 0
703
+
1.0 1.0 0
704
+
1.0 1.0 0
705
+
1.0 1.0 0
706
+
1.0 1.0 0
707
+
1.0 1.0 0
708
+
1.0 1.0 0
709
+
1.0 1.0 0
710
+
0 0 0
711
+
0.0 0.0 0
712
+
0.0 0.0 0
713
+
0.0 0.0 0
714
+
0.0 0.0 0
715
+
0.0 0.0 0
716
+
0.0 0.0 0
717
+
0.0 0.0 0
718
+
0.0 0.0 0
719
+
0.0 0.0 0
720
+
0.0 0.0 0
721
+
0 0 0
722
+
0 0 pic-1429156152.jpg
723
+
0 0 0
724
+
1.0 1.0 0
725
+
1.0 1.0 0
726
+
1.0 1.0 0
727
+
1.0 1.0 0
728
+
1.0 1.0 0
729
+
1.0 1.0 0
730
+
1.0 1.0 0
731
+
1.0 1.0 0
732
+
1.0 1.0 0
733
+
1.0 1.0 0
734
+
0 0 0
735
+
0.0 0.0 0
736
+
0.0 0.0 0
737
+
0.0 0.0 0
738
+
0.0 0.0 0
739
+
0.0 0.0 0
740
+
0.0 0.0 0
741
+
0.0 0.0 0
742
+
0.0 0.0 0
743
+
0.0 0.0 0
744
+
0.0 0.0 0
745
+
0 0 0
746
+
0 0 pic-1429156170.jpg
747
+
0 0 0
748
+
1.0 1.0 0
749
+
1.0 1.0 0
750
+
1.0 1.0 0
751
+
1.0 1.0 0
752
+
1.0 1.0 0
753
+
1.0 1.0 0
754
+
1.0 1.0 0
755
+
1.0 1.0 0
756
+
1.0 1.0 0
757
+
1.0 1.0 0
758
+
0 0 0
759
+
0.0 0.0 0
760
+
0.0 0.0 0
761
+
0.0 0.0 0
762
+
0.0 0.0 0
763
+
0.0 0.0 0
764
+
0.0 0.0 0
765
+
0.0 0.0 0
766
+
0.0 0.0 0
767
+
0.0 0.0 0
768
+
0.0 0.0 0
769
+
0 0 0
770
+
0 0 pic-1429156188.jpg
Assignment 6/working code 4-16 pre/pic-end.jpg
Assignment 6/working code 4-16 pre/pic-end.jpg
This is a binary file and will not be displayed.
Assignment 6/working code 4-16 pre/pic-start.jpg
Assignment 6/working code 4-16 pre/pic-start.jpg
This is a binary file and will not be displayed.
+109
Assignment 6/working code 4-16 pre/visualservo.m
+109
Assignment 6/working code 4-16 pre/visualservo.m
···
1
+
2
+
% get target image position
3
+
targetImageName = 'img-end.jpg';
4
+
targetImage = imread(targetImageName);
5
+
6
+
num = 4;
7
+
8
+
Tc = transl(0,0,0);
9
+
10
+
targetC = icorner(targetImage, 'nfeat', num, 'color');
11
+
imageSize = size(targetImage);
12
+
corners = [0, 0;
13
+
imageSize(1), 0;
14
+
0, imageSize(2);
15
+
imageSize(1:2)];
16
+
targetP = zeros(2 * num, 1);
17
+
for i = 1 : num
18
+
distance = inf;
19
+
closest = -1;
20
+
for j = 1 : num
21
+
curDistance = sum((corners(i, :) - [targetC(j).u, targetC(j).v]) .^ 2);
22
+
if curDistance < distance
23
+
distance = curDistance;
24
+
closest = j;
25
+
end
26
+
end
27
+
targetP(2 * i - 1 : 2 * i, :) = [targetC(closest).u, targetC(closest).v];
28
+
end
29
+
30
+
lambda = 0.5;
31
+
32
+
% get list of existing images
33
+
images = dir('*.jpg')
34
+
imageNames = cell(size(images, 1), 1);
35
+
36
+
for i = 1 : size(images, 1)
37
+
imageNames{i} = images(i).name;
38
+
end
39
+
40
+
newImage = imread('img-start.jpg');
41
+
42
+
J = zeros(6,6);
43
+
cam = CentralCamera('default');
44
+
45
+
testCriterion = 0;
46
+
while testCriterion < 3
47
+
% process current image
48
+
cornersC = icorner(newImage, 'nfeat', num, 'color');
49
+
currentP = zeros(2 * num, 1);
50
+
for i = 1 : num
51
+
distance = inf;
52
+
closest = -1;
53
+
for j = 1 : num
54
+
curDistance = sum((corners(i, :) - [cornersC(j).u, cornersC(j).v]) .^ 2);
55
+
if curDistance < distance
56
+
distance = curDistance;
57
+
closest = j;
58
+
end
59
+
end
60
+
61
+
currentP(2 * i - 1 : 2 * i, :) = [cornersC(closest).u, cornersC(closest).v];
62
+
J(2 * i - 1: 2 * i, :) = cam.visjac_p([cornersC(closest).u; cornersC(closest).v], 2);
63
+
end
64
+
J
65
+
diff = targetP - currentP
66
+
invJ = pinv(J); %infJ = inv(J' * J) * J'; %same result
67
+
v = lambda * (invJ * diff)
68
+
Tc = trnorm(Tc * delta2tr(v))
69
+
% diff' * invJ * diff
70
+
% v' * v
71
+
%304.8mm in 1ft
72
+
73
+
% debug - display image and corners
74
+
idisp(targetImage);
75
+
targetC.plot();
76
+
figure
77
+
idisp(newImage);
78
+
cornersC.plot();
79
+
80
+
% construct motion plan
81
+
motionPlanSize = 1;
82
+
motionPlan = zeros(motionPlanSize, 3);
83
+
%CREATE MOTION PLAN HERE
84
+
motionPlan = [motionPlan; [1 1 1]];
85
+
motionPlan = [motionPlan; [0 0 1]];
86
+
87
+
% write motion plan to text
88
+
dlmwrite('motion_plan.txt', motionPlan, ' ');
89
+
90
+
% call python script to move robot and take picture
91
+
system('calico.bat --exec logDataServo.py');
92
+
disp 'Python done, returning to Matlab.'
93
+
94
+
% get new list of images
95
+
images = dir('*.jpg');
96
+
newImageNames = cell(size(images, 1), 1);
97
+
for i = 1 : size(images, 1)
98
+
newImageNames{i} = images(i).name;
99
+
end
100
+
101
+
% find the new image
102
+
new = ismember(newImageNames, imageNames);
103
+
newImageName = newImageNames(~new);
104
+
105
+
newImage = imread(newImageName{1});
106
+
imageNames = newImageNames;
107
+
108
+
testCriterion = testCriterion + 1;
109
+
end