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 MyMotor(object):
def __init__(self,port):
self.motor=Motor(port=port)
self.motor.reset()
def reset(self):
self.motor.reset()
def run(self,duty):
self.motor.run_forever(duty, speed_regulation=False)
def stop(self):
self.motor.stop()
def position(self):
return self.motor.position
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)
def follow_line(Vref=0.0, colmax=0.0, colmin=0.0, distref=0.0, lKp=0.0, lKi=0.0, lKd=0.0, dKp=0.0, dKi=0.0, dKd=0.0):
cycle_t = 0.0
standing_t = 0.0
pid_line = PID(lKp, lKi, lKd)
pid_dist = PID(dKp, dKi, dKd)
ml = Motor(port=Motor.PORT.A)
mr = Motor(port=Motor.PORT.B)
cs = ColorSensor()
us = UltrasonicSensor()
hupe = Tone()
led = LED()
led.left.color = LED.COLOR.AMBER
led.right.color = LED.COLOR.AMBER
try:
ml.run_forever(Vref, speed_regulation=True)
mr.run_forever(Vref, speed_regulation=True)
while True:
cycle_t = time.time()
# die Farbwerte werden automatisch auf Bereich [0,100] abgebildet, Differenz zu 50 ist Fehler fuer PID
pid_line.calc( 50 - ( ( cs.reflect - colmin ) / ( colmax - colmin ) ) * 100 )
pid_dist.calc( distref - us.dist_cm )
# es soll nicht auf Hindernis zubeschleunigt werden
if pid_dist.correction < 0:
pid_dist.correction = 0
# Motoren werden invertiert angesteuert, da sie in anderer Richtung verbaut sind
if pid_line.correction > 0: # im dunklen Bereich
ml.run_forever( (-1) * ( Vref - pid_dist.correction ) )
mr.run_forever( (-1) * ( Vref - pid_dist.correction - pid_line.correction ) )
elif pid_line.correction < 0: # im hellen Bereich
ml.run_forever( (-1) * ( Vref - pid_dist.correction + pid_line.correction ) )
mr.run_forever( (-1) * ( Vref - pid_dist.correction ) )
# Startzeit des Zuckelns merken
if abs( Vref - pid_dist.correction ) < 100 and not standing_t:
standing_t = time.time()
if abs( Vref - pid_dist.correction ) > 100 and standing_t:
standing_t = 0
# Fahrtsteuerung komplett abschalten, wenn mind. 1 Sekunde gezuckelt wurde
if standing_t and ( time.time() - standing_t ) > 1:
ml.stop()
mr.stop()
led.left.color = LED.COLOR.RED
led.right.color = LED.COLOR.RED
hupe.play(100,500)
return
print "Zyklusdauer: " + str( ( time.time() - cycle_t ) * 1000 ) + "ms"
except:
ml.stop()
mr.stop()
print traceback.format_exc()
print "Programm wurde beendet"
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')
