Assignment code for Georgia Tech CS 3630, 2014
Add Python control code
+66
+66
Assignment 6/logDataServo.py
+66
Assignment 6/logDataServo.py
···
1
+
import time
2
+
import os.path
3
+
from myro import *
4
+
5
+
# Run commands to move the robot
6
+
def runCommands(log, commands):
7
+
for c in commands:
8
+
start = time.time()
9
+
motors(c[0],c[1])
10
+
while (time.time() - start < c[2]):
11
+
logNow(log, c[0],c[1],0);
12
+
time.sleep(0.1) # Read sensors at 1Hz
13
+
14
+
def takePhoto():
15
+
pic_fname = "pic-%d.jpg" % time.time()
16
+
print '\tTaking picture:', pic_fname
17
+
picture = takePicture();
18
+
print '\tPicture taken!'
19
+
logNow(log, 0,0,pic_fname);
20
+
print '\tSaving picture...'
21
+
savePicture(picture, pic_fname)
22
+
print '\tPicture saved!'
23
+
24
+
# Run commands followed by taking picture
25
+
def runCommand(log, cmd):
26
+
27
+
# Move the robot
28
+
commands = [];
29
+
commands.append(cmd);
30
+
commands.append([0,0,0.1]);
31
+
runCommands(log,commands);
32
+
33
+
# Log motor commands
34
+
def logNow(log, l, r, fname):
35
+
log.write("%s %s %s\n" %(l, r, fname))
36
+
37
+
# write to output log
38
+
fname = "motion_log.txt"
39
+
if os.path.exists(fname):
40
+
log = open(fname, 'a')
41
+
else:
42
+
log = open(fname, 'w')
43
+
44
+
# read from command log
45
+
cmdFile = open('motion_plan.txt', 'r')
46
+
commands = cmdFile.readlines()
47
+
cmdFile.close()
48
+
49
+
# setup robot
50
+
print 'Connecting to Scribbler...'
51
+
init("COM8")
52
+
setIRPower(140)
53
+
setForwardness(1)
54
+
55
+
print 'Starting motion plan...'
56
+
for command in commands:
57
+
cmd = command.split()
58
+
roboCmd = [float(cmd[0]), float(cmd[1]), float(cmd[2])]
59
+
print 'Running command:', roboCmd
60
+
runCommand(log, roboCmd)
61
+
print 'Motion plan complete.'
62
+
#takePhoto()
63
+
64
+
log.close();
65
+
66
+
print 'Script finished!'