class TestMotor(unittest.TestCase):
get_input('Attach a motor on port A then continue')
def __init__(self, *args, **kwargs):
super(TestMotor, self).__init__(*args, **kwargs)
self.d = Motor(port=Motor.PORT.A)
def setUp(self):
self.d.reset()
def test_run(self):
self.d.run_mode = 'forever'
self.d.regulation_mode = True
self.d.pulses_per_second_sp = 200
self.d.start()
time.sleep(5)
self.d.stop()
def test_run_forever(self):
self.d.run_forever(50, regulation_mode=False)
time.sleep(5)
self.d.stop()
self.d.run_forever(200, regulation_mode=True)
time.sleep(5)
self.d.stop()
def test_run_time_limited(self):
self.d.run_time_limited(time_sp=10000,
speed_sp=80,
regulation_mode=False,
stop_mode=Motor.STOP_MODE.COAST,
ramp_up_sp=1000,
ramp_down_sp=1000)
time.sleep(12)
def test_run_position_limited(self):
self.d.position = 0
self.d.run_position_limited(position_sp=360,
speed_sp=800,
stop_mode=Motor.STOP_MODE.BRAKE,
ramp_up_sp=1000,
ramp_down_sp=1000)
time.sleep(5)
def test_run_position_limited_relative(self):
self.d.position_mode = Motor.POSITION_MODE.RELATIVE
self.d.run_position_limited(position_sp=160,
speed_sp=800,
stop_mode=Motor.STOP_MODE.BRAKE,
ramp_up_sp=1000,
ramp_down_sp=1000)
time.sleep(5)
class TestMotor(unittest.TestCase):
get_input('Attach a motor on port A then continue')
def __init__(self,*args, **kwargs):
super(TestMotor, self).__init__(*args, **kwargs)
self.d=Motor(port=Motor.PORT.A)
def setUp(self):
self.d.reset()
def test_run(self):
self.d.run_mode = 'forever'
self.d.regulation_mode = True
self.d.pulses_per_second_sp = 200
self.d.start()
time.sleep(5)
self.d.stop()
def test_run_forever(self):
self.d.run_forever(50, regulation_mode=False)
time.sleep(5)
self.d.stop()
self.d.run_forever(200, regulation_mode=True)
time.sleep(5)
self.d.stop()
def test_run_time_limited(self):
self.d.run_time_limited(time_sp=10000, speed_sp=80, regulation_mode=False,
stop_mode=Motor.STOP_MODE.COAST, ramp_up_sp=1000, ramp_down_sp=1000)
time.sleep(12)
def test_run_position_limited(self):
self.d.position=0
self.d.run_position_limited(position_sp=360, speed_sp=800,
stop_mode=Motor.STOP_MODE.BRAKE , ramp_up_sp=1000, ramp_down_sp=1000)
time.sleep(5)
def test_run_position_limited_relative (self):
self.d.position_mode=Motor.POSITION_MODE.RELATIVE
self.d.run_position_limited(position_sp=160, speed_sp=800,
stop_mode=Motor.STOP_MODE.BRAKE , ramp_up_sp=1000, ramp_down_sp=1000)
time.sleep(5)
class Lego:
def __init__(self):
self.idle = True
self.stagenum = 0
self.medium_motor = Motor(port=Motor.PORT.A)
self.left_large_motor = Motor(port=Motor.PORT.B)
self.right_large_motor = Motor(port=Motor.PORT.C)
def __del__(self):
self.stop_large_motors()
self.stop_medium_motors()
def detach(self):
if self.idle:
return
for i in range(2, 0, -1):
self.medium_motor.position = 0
self.medium_motor.run_position_limited(position_sp=((-1) ** i) * config.MEDIUM_MOTOR_ROTATION_ANGLE,
speed_sp=config.MEDIUM_MOTOR_SPEED,
stop_mode=Motor.STOP_MODE.BRAKE)
self.wait(self.medium_motor)
time.sleep(0.1)
def reset(self):
self.stop()
self.stagenum = 0
def run(self):
for i in range(self.stagenum, len(config.STAGES)):
if self.idle:
break
self.run_large_motors(config.STAGES[i])
self.stop_large_motors()
self.detach()
self.stagenum += 1
def run_large_motors(self, stage):
for s in stage:
if stage.index(s) == 0:
self.left_large_motor.run_forever(speed_sp=s[0])
self.right_large_motor.run_forever(speed_sp=s[1])
else:
self.set_speed(s[0], s[1])
self.sleep(s[2])
def set_speed(self, left_speed, right_speed):
if self.left_large_motor.regulation_mode:
self.left_large_motor.pulses_per_second_sp = left_speed
self.right_large_motor.pulses_per_second_sp = right_speed
else:
self.left_large_motor.duty_cycle_sp = left_speed
self.right_large_motor.duty_cycle_sp = right_speed
def sleep(self, secs):
x = math.floor(secs)
y = secs - x
if y != 0:
time.sleep(y)
for i in range(int(x)):
if self.idle:
break
time.sleep(1)
def start(self):
if self.idle:
self.idle = False
self.run()
def stop(self):
if not self.idle:
self.idle = True
def stop_large_motors(self):
self.left_large_motor.stop()
self.right_large_motor.stop()
def stop_medium_motors(self):
self.medium_motor.stop()
def wait(self, motor):
while motor.read_value('run') == '1':
pass
class LegoR2D2(Ev3Dev):
def __init__(self):
# Logger init
logging.basicConfig(filename='/var/log/r2d2.log',
level=logging.DEBUG,
format='%(asctime)s %(levelname)7s: %(message)s')
# Color the errors and warnings in red
logging.addLevelName( logging.ERROR, "\033[91m%s\033[0m" % logging.getLevelName(logging.ERROR))
logging.addLevelName( logging.WARNING, "\033[91m%s\033[0m" % logging.getLevelName(logging.WARNING))
# EV3 init
Ev3Dev.__init__(self)
self.head = Motor(port=Motor.PORT.A)
self.right_wheel = Motor(port=Motor.PORT.B)
self.left_wheel = Motor(port=Motor.PORT.C)
#print '%d%%' % self.get_battery_percentage(7945400)
# "kill/kill -9" init
signal.signal(signal.SIGTERM, self.signal_term_handler)
signal.signal(signal.SIGINT, self.signal_int_handler)
self.shutdown_flag = False
self.rest_server = RESTServer(self)
self.rest_server.start()
def signal_term_handler(self, signal, frame):
logger.error('Caught SIGTERM')
self.shutdown_flag = True
def signal_int_handler(self, signal, frame):
logger.error('Caught SIGINT')
self.shutdown_flag = True
def move(self, direction, speed):
if direction == 'forward':
self.left_wheel.run_forever(speed * -1, regulation_mode=False)
self.right_wheel.run_forever(speed * -1, regulation_mode=False)
elif direction == 'backward':
self.left_wheel.run_forever(speed, regulation_mode=False)
self.right_wheel.run_forever(speed, regulation_mode=False)
elif direction == 'left':
self.left_wheel.run_forever(speed, regulation_mode=False)
self.right_wheel.run_forever(speed * -1, regulation_mode=False)
elif direction == 'right':
self.left_wheel.run_forever(speed * -1, regulation_mode=False)
self.right_wheel.run_forever(speed, regulation_mode=False)
elif direction == 'lookleft':
self.head.position=0
self.head.run_position_limited(position_sp=45,
speed_sp=800,
stop_mode=Motor.STOP_MODE.COAST,
ramp_up_sp=100,
ramp_down_sp=100)
elif direction == 'lookright':
self.head.position=0
self.head.run_position_limited(position_sp=-45,
speed_sp=800,
stop_mode=Motor.STOP_MODE.COAST,
ramp_up_sp=100,
ramp_down_sp=100)
elif direction == 'stop':
self.left_wheel.stop()
self.right_wheel.stop()
# This looks a little silly but we leave this loop running so we
# can catch SIGTERM and SIGINT
def run(self):
while True:
sleep(1)
if self.shutdown_flag:
return
def shutdown(self):
self.move('stop', None)
logger.info('REST server shutdown begin')
self.rest_server.www.stop()
self.rest_server.join()
self.rest_server = None
logger.info('REST server shutdown complete')
