Assignment code for Georgia Tech CS 3630, 2014
Add working Python x Matlab code
+116
+75
Assignment 6/working code/logDataServo.py
+75
Assignment 6/working code/logDataServo.py
···
1
+
import time
2
+
import os.path
3
+
import sys
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
+
#raise SystemExit
69
+
#sys.exit("Done")
70
+
71
+
from Graphics import Window
72
+
import System
73
+
win = Window()
74
+
win.DeleteEvent += lambda obj, event: System.Environment.Exit(0)
75
+
sys.exit()
+41
Assignment 6/working code/visualservo.m
+41
Assignment 6/working code/visualservo.m
···
1
+
2
+
% get target image position
3
+
targetImageName = 'pic-end.jpg';
4
+
targetImage = imread(targetImageName);
5
+
6
+
% get list of existing images
7
+
images = dir('*.jpg')
8
+
imageNames = cell(size(images, 1), 1);
9
+
for i = 1 : size(images, 1)
10
+
imageNames{i} = images(i).name;
11
+
end
12
+
13
+
%while criterion
14
+
% construct motion plan
15
+
motionPlanSize = 1;
16
+
motionPlan = zeros(motionPlanSize, 3);
17
+
%CREATE MOTION PLAN HERE
18
+
motionPlan = [motionPlan; [1 1 1]];
19
+
motionPlan = [motionPlan; [0 0 1]];
20
+
21
+
% write motion plan to text
22
+
dlmwrite('motion_plan.txt', motionPlan, ' ');
23
+
24
+
% call python script to move robot and take picture
25
+
system('calico.bat --exec logDataServo.py');
26
+
27
+
% get new list of images
28
+
images = dir('*.jpg')
29
+
newImageNames = cell(size(images, 1), 1);
30
+
for i = 1 : size(images, 1)
31
+
newImageNames{i} = images(i).name;
32
+
end
33
+
34
+
% find the new image
35
+
new = ismember(newImageNames, imageNames);
36
+
newImageName = newImageNames(~new)
37
+
38
+
newImage = imread(newImageName{1});
39
+
40
+
imageNames = newImageNames;
41
+
%end