def __init__(self):
self.motor_log = Logger("Motors")
threading.Thread.__init__(self)
self.plotter = RopePlotter(L_ROPE_0, R_ROPE_0, ROPE_ATTACHMENT_WIDTH, Kp=KP, Ki=TI, Kd=TD, max_spd=MAX_SPEED)
self.throttle = Throttler(MOTOR_CMD_RATE)
def __init__(self):
threading.Thread.__init__(self)
self.throttle = Throttler(MOTOR_CMD_RATE)
class MotorThread(threading.Thread):
"""
This thread interacts with the plotter via the global 'c' which containts plotter commands.
"""
def __init__(self):
self.motor_log = Logger("Motors")
threading.Thread.__init__(self)
self.plotter = RopePlotter(L_ROPE_0, R_ROPE_0, ROPE_ATTACHMENT_WIDTH, Kp=KP, Ki=TI, Kd=TD, max_spd=MAX_SPEED)
self.throttle = Throttler(MOTOR_CMD_RATE)
def run(self):
global c
print "Starting motor thread"
while running:
if type(c) == dict:
# We got settings
if 'kp' in c:
#We got pid settings
self.plotter.Kp = c['kp']
self.plotter.Ti = c['ti']
self.plotter.Td = c['td']
self.plotter.max_speed = c['max_speed']
wsSend("PID parameters set")
if 'll' in c:
#we got rope length settings
self.plotter.l_rope_0 = c['ll']
self.plotter.r_rope_0 = c['lr']
self.plotter.att_dist = c['aw']
wsSend("Plotter settings set")
c=''
if c == 'left-fwd':
#print "Running left motor fwd"
self.plotter.left_fwd()
elif c == 'left-stop':
#print "Stopping left motor"
self.plotter.left_stop()
c = ''
elif c == 'right-fwd':
#print "Running right motor forward"
self.plotter.right_fwd()
elif c == 'right-back':
#print "Running right motor back"
self.plotter.right_back()
elif c == 'right-stop':
#print "Stopping right motor"
self.plotter.right_stop()
c = ''
elif c == 'left-back':
#print "Running left motor back"
self.plotter.left_back()
elif c == 'pu':
#print "Pulling pen up"
self.plotter.pen_up()
c = ''
elif c == 'pd':
#print "Putting pen down"
self.plotter.pen_down()
c = ''
elif c == 'stop':
self.plotter.left_stop()
self.plotter.right_stop()
c = ''
#print "Stopped"
elif c == 'testdrive':
self.plotter.test_drive()
c = ''
elif c == 'plot':
# c stays 'plot' until another command is sent trough the socket
plot_action = self.plotter.plot_from_file('uploads/coords.csv')
c = 'plotting'
elif c == 'plotting':
try:
pct_done = next(plot_action)
wsSend("[ {0:.2f}V ] Plot {1:.2f}% done".format(self.plotter.battery.measured_voltage/1000000.0, pct_done))
except StopIteration:
c = ''
wsSend("Done plotting")
elif c == 'zero':
wsSend("zero motor positions")
self.plotter.set_control_zeroes()
c = ''
elif c == 'plotcircles':
wsSend("Plotting circles")
# c stays 'plot' until another command is sent trough the socket
plot_action = self.plotter.plot_circles()
c = 'plotting'
#BrickPiUpdateValues() # BrickPi updates the values for the motors
self.throttle.throttle() #Don't go too fast.
#Stopped running. Shutting down all motors.
self.plotter.stop_all_motors()
class MotorThread(threading.Thread):
"""
This thread interacts with the plotter via the global 'c' which containts plotter commands.
"""
def __init__(self):
threading.Thread.__init__(self)
self.throttle = Throttler(MOTOR_CMD_RATE)
def run(self):
global c, plotter
buttons = ev.Button()
right_or_up_pressed_earlier = False
left_or_down_pressed_earlier = False
while running:
if type(c) == dict:
# We got settings
if 'kp' in c:
#We got pid settings
plotter.Kp = c['kp']
plotter.Ti = c['ti']
plotter.Td = c['td']
plotter.cm_to_deg = int(c['cm_to_deg'])
wsSend("PID parameters set")
if 'll' in c:
#we got rope length settings
plotter.l_rope_0 = c['ll']
plotter.r_rope_0 = c['lr']
plotter.att_dist = c['aw']
plotter.r_step = float(c['rs'])
wsSend("Plotter settings set")
c = ''
# Socket commands
if c == 'left-fwd':
plotter.left_fwd()
elif c == 'left-back':
plotter.left_back()
elif c == 'right-fwd':
plotter.right_fwd()
elif c == 'right-back':
plotter.right_back()
elif buttons.right:
plotter.left_fwd()
right_or_up_pressed_earlier = True
elif buttons.up:
plotter.left_back()
right_or_up_pressed_earlier = True
elif buttons.down:
plotter.right_fwd()
left_or_down_pressed_earlier = True
elif buttons.left:
plotter.right_back()
left_or_down_pressed_earlier = True
elif not (buttons.left
and buttons.down) and left_or_down_pressed_earlier:
plotter.right_stop()
left_or_down_pressed_earlier = False
elif not (buttons.right
and buttons.up) and right_or_up_pressed_earlier:
plotter.left_stop()
right_or_up_pressed_earlier = False
elif c == 'right-stop':
plotter.right_stop()
c = ''
elif c == 'left-stop':
plotter.left_stop()
c = ''
elif c == 'pu':
plotter.pen_up()
c = ''
elif c == 'pd':
plotter.pen_down()
c = ''
elif c == 'stop':
plotter.left_stop()
plotter.right_stop()
c = ''
elif c == 'reload':
plotter.reload_chalk()
elif c == 'testdrive':
plotter.test_drive()
c = ''
elif c == 'plot':
start_time = time.time()
# c stays 'plot' until another command is sent trough the socket
plot_action = plotter.plot_from_file('uploads/coords.csv')
c = 'plotting'
elif c == 'plotting':
try:
pct_done = next(plot_action)
wsSend(str(pct_done))
#wsSend("[ {0:.2f}V ] Plot {1:.2f}% done".format(plotter.battery.measured_voltage/1000000.0, pct_done))
except StopIteration:
c = ''
if start_time:
elapsed = time.time() - start_time
elapsed_str = time.strftime("%Hh %Mm %Ss",
time.gmtime(elapsed))
wsSend("Done plotting after " + elapsed_str)
elif c == 'zero':
wsSend("zero motor positions")
plotter.set_control_zeroes()
c = ''
elif c == 'plotcircles':
start_time = time.time()
wsSend("Plotting circles")
# c stays 'plot' until another command is sent trough the socket
plot_action = plotter.optimized_etch()
c = 'plotting'
elif c == 'plotwaves':
wsSend("Plotting waves")
# c stays 'plot' until another command is sent trough the socket
plot_action = plotter.plot_circle_waves()
c = 'plotting'
# Button commands
if buttons.backspace:
tornado.ioloop.IOLoop.instance().stop()
# Close all sockets
for ws in websockets:
ws.close()
self.throttle.throttle() # Don't go too fast.
# Stopped running. Shutting down all motors.
plotter.stop_all_motors()
plotter_log.info("Socket thread stopped")
