Assignment code for Georgia Tech CS 3630, 2014
1import time
2import os.path
3from myro import *
4
5# Run commands to move the robot
6def 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
14def 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
25def 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
34def logNow(log, l, r, fname):
35 log.write("%s %s %s\n" %(l, r, fname))
36
37# write to output log
38fname = "motion_log.txt"
39if os.path.exists(fname):
40 log = open(fname, 'a')
41else:
42 log = open(fname, 'w')
43
44# read from command log
45cmdFile = open('motion_plan.txt', 'r')
46commands = cmdFile.readlines()
47cmdFile.close()
48
49# setup robot
50print 'Connecting to Scribbler...'
51init("COM8")
52setIRPower(140)
53setForwardness(1)
54
55print 'Starting motion plan...'
56for 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)
61print 'Motion plan complete.'
62#takePhoto()
63
64log.close();
65
66print 'Script finished!'